From 5ce0be8a7e7c7ac0078c0abdc9012cd9688fae07 Mon Sep 17 00:00:00 2001 From: Lukas Woodtli Date: Fri, 29 Dec 2023 21:44:27 +0100 Subject: [PATCH] Capstone: Milestone 1: Odometry --- luki_modern_robotics/capstone/__init__.py | 0 .../capstone/mobile_manipulation.py | 95 +++++++++++++++++++ luki_modern_robotics/test/.gitignore | 1 + luki_modern_robotics/test/test_capstone.py | 29 ++++++ 4 files changed, 125 insertions(+) create mode 100644 luki_modern_robotics/capstone/__init__.py create mode 100644 luki_modern_robotics/capstone/mobile_manipulation.py create mode 100644 luki_modern_robotics/test/.gitignore create mode 100644 luki_modern_robotics/test/test_capstone.py diff --git a/luki_modern_robotics/capstone/__init__.py b/luki_modern_robotics/capstone/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/luki_modern_robotics/capstone/mobile_manipulation.py b/luki_modern_robotics/capstone/mobile_manipulation.py new file mode 100644 index 0000000..c2f8560 --- /dev/null +++ b/luki_modern_robotics/capstone/mobile_manipulation.py @@ -0,0 +1,95 @@ +import numpy as np + + +def _get_chassis_config(state): + """Get the chassis configuration from the state""" + assert len(state) == 12 + return state[:3] + + +def _get_angles(state): + """Get all the joint and wheel angles from the state""" + assert len(state) == 12 + return state[3:] + + +def _euler_step(angles, speeds, delta_t): + """Calculate new angles of joints and wheels for a time step""" + return angles + speeds * delta_t + + +def _odometry(q_k, delta_theta): + """Calculate odometry of mobile base. + :param q_k: The current chassis config + :param delta_theta: The wheel angle step + :return: The new configuration of the mobile base""" + + r = 0.0475 # wheel radius (meters) + l = 0.235 # center to the wheel axis (meters) + omega = 0.15 # half wheel axis (meters) + H_0 = 1/r * np.array([[-l - omega, 1, -1], + [ l + omega, 1, 1], + [ l + omega, 1, -1], + [-l - omega, 1, 1], + ]) + # calculate twist + V_b = np.dot(np.linalg.pinv(H_0), delta_theta.T) + + # elements of the twist + omega_bz = V_b[0] + v_bx = V_b[1] + v_by = V_b[2] + + # coordinate changes relative to body frame + if np.isclose(0.0, omega_bz): + delta_qb = V_b + else: + delta_qb = np.array([omega_bz, + (v_bx * np.sin(omega_bz) + v_by * (np.cos(omega_bz) - 1))/omega_bz, + (v_by * np.sin(omega_bz) + v_bx * (1 - np.cos(omega_bz)))/omega_bz, + ]) + + # transforming from body frame to fixed frame + phi_k = q_k[0] + transf = np.array([[1, 0, 0], + [0, np.cos(phi_k), -np.sin(phi_k)], + [0, np.sin(phi_k), np.cos(phi_k)]]) + delta_q = transf @ delta_qb + + # return updated estimate of chassis configuration + return q_k + delta_q + + +def NextState(current_configuration, + wheel_and_joint_controls, + delta_t: float, + speed_max: float): + """ + Calculates the next state of the robot with: euler step for the angles and odometry for the chassis configuration + :param current_configuration: 12-vector: + - phi, x, y (chassis configuration) + - 5 arm configuration angles + - 4 wheel angles + :param wheel_and_joint_controls: 9-vector: + - 5 arm joint speeds: theta_dot + - 4 wheel speeds: u + :param delta_t: time step + :param speed_max: maximum speed of wheels and joints + """ + + assert len(current_configuration) == 12 + assert len(wheel_and_joint_controls) == 9 + # limit speed controls + controls = np.clip(wheel_and_joint_controls, -speed_max, speed_max) + + # angles + new_angles = _euler_step(_get_angles(current_configuration), controls, delta_t) + assert len(new_angles) == 9 + # config + current_config = _get_chassis_config(current_configuration) + new_configuration = _odometry(current_config, controls[5:] * delta_t) + assert len(new_configuration) == 3 + + new_state = np.r_[new_configuration, new_angles] + assert len(new_state) == 12 + return new_state diff --git a/luki_modern_robotics/test/.gitignore b/luki_modern_robotics/test/.gitignore new file mode 100644 index 0000000..c4467f8 --- /dev/null +++ b/luki_modern_robotics/test/.gitignore @@ -0,0 +1 @@ +output-*.csv diff --git a/luki_modern_robotics/test/test_capstone.py b/luki_modern_robotics/test/test_capstone.py new file mode 100644 index 0000000..57bca48 --- /dev/null +++ b/luki_modern_robotics/test/test_capstone.py @@ -0,0 +1,29 @@ +import os + +import numpy as np +import pytest +from ..capstone.mobile_manipulation import NextState + +DIR_PATH = os.path.dirname(os.path.realpath(__file__)) + +testdata = [ + ("x-movement", np.array([10, 10, 10, 10])), + ("y-movement", np.array([-10, 10, -10, 10])), + ("rotation", np.array([-10, 10, 10, -10])) +] + + +@pytest.mark.parametrize("name,u", testdata) +def test_NextState(name, u): + delta_t = 0.01 + steps = 100 + current_config = np.zeros(12) + theta_dot = np.zeros(5) + speeds = np.r_[theta_dot, u] + gripper_state = np.ones(0) + all_states = np.array(np.r_[current_config, gripper_state]) + for _ in range(steps): + current_config = NextState(current_config, speeds, delta_t, 5) + current_state = np.r_[current_config, gripper_state] + all_states = np.vstack([all_states, current_state]) + np.savetxt(os.path.join(DIR_PATH, f"output-{name}.csv"), all_states, delimiter=",")