Capstone: Milestone 2: Trajectory Generation

luki_modern_robotics/capstone/mobile_manipulation.py

@@ -1,4 +1,5 @@
 import numpy as np
+from modern_robotics import ScrewTrajectory
 
 
 def _get_chassis_config(state):
@@ -93,3 +94,162 @@ def NextState(current_configuration,
     new_state = np.r_[new_configuration, new_angles]
     assert len(new_state) == 12
     return new_state
+
+
+# Motion Planning #
+
+# Frames:
+#  `{s}`: fixed space frame
+#  `{b}`: mobile base (body) frame
+#  `{0}`: base of arm frame
+#  `{e}`: end effector frame
+#  `{c}`: cube (manipulated object) frame
+
+
+def T_se_initial():
+    """initial configuration of the end-effector in the reference trajectory"""
+    return _T_sb(np.array([0, 0, 0])) @ _T_b0() @ _M_0e()
+
+
+def _T_sb(q):
+    """configuration of the frame `{b}` of the mobile base, relative to the frame `{s}` on the floor"""
+    phi = q[0]
+    x = q[1]
+    y = q[2]
+    z = 0.0963  # meters is the height of the `{b}` frame above the floor
+    se3 = np.array([
+        [np.cos(phi), -np.sin(phi), 0, x],
+        [np.sin(phi),  np.cos(phi), 0, y],
+        [          0,            0, 1, z],
+        [          0,            0, 0, 1],
+    ])
+    return se3
+
+
+def _T_b0():
+    """The fixed offset from the chassis frame `{b}` to the base frame of the arm `{0}`"""
+    return np.array([
+        [1, 0, 0, 0.1662],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0.0026],
+        [0, 0, 0, 1]])
+
+
+def _M_0e():
+    """Arm at home configuration (all joint angles zero).
+    end-effector frame `{e}` relative to the arm base frame `{0}`"""
+    return np.array([
+        [1, 0, 0, 0.033],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0.6546],
+        [0, 0, 0, 1]])
+
+
+def T_sc_initial():
+    """Initial configuration of the cube `{c}` relative to the fixed frame `{s}`"""
+    return np.array([
+        [1, 0, 0, 1],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0.025],
+        [0, 0, 0, 1]])
+
+
+def T_sc_goal():
+    """Goal configuration of the cube `{c}` relative to the fixed frame `{s}`"""
+    return np.array([
+        [0, 1, 0, 0],
+        [-1, 0, 0, -1],
+        [0, 0, 1, 0.025],
+        [0, 0, 0, 1]])
+
+
+def _standoff_from_cube(cube_conf):
+    """End effector standoff configuration, relative to cube frame `{c}`"""
+    theta = 3. / 4 * np.pi
+    d = 0.1
+    standoff = np.array([
+        [np.cos(theta), 0, np.sin(theta), 0],
+        [0, 1, 0, 0],
+        [-np.sin(theta), 0, np.cos(theta), d],
+        [0, 0, 0, 1]
+    ])
+
+    return cube_conf @ standoff
+
+
+def _grasp_from_cube(cube_config):
+    """End effector configuration for grasping cube, relative to cube frame `{c}`"""
+    theta = 3. / 4 * np.pi
+
+    grasp = np.array([
+        [np.cos(theta), 0, np.sin(theta), 0],
+        [0, 1, 0, 0],
+        [-np.sin(theta), 0, np.cos(theta), -0.0215],
+        [0, 0, 0, 1]])
+
+    return cube_config @ grasp
+
+
+def _generate_waypoints(T_se_initial, T_sc_initial, T_sc_goal):
+    """Generate all the main positions of the end-effector including gripper state and times required for the step"""
+    GRIPPER_OPEN = 0
+    GRIPPER_CLOSED = 1
+    _ = "unused"
+
+    standoff_1 = _standoff_from_cube(T_sc_initial)
+    grip_1 = _grasp_from_cube(T_sc_initial)
+    standoff_2 = _standoff_from_cube(T_sc_goal)
+    grip_2 = _grasp_from_cube(T_sc_goal)
+
+    waypoints = []  # (config, gripper, time_in_s)
+
+    waypoints.append((T_se_initial, GRIPPER_OPEN, _))
+    waypoints.append((standoff_1, GRIPPER_OPEN, 7))
+    waypoints.append((grip_1, GRIPPER_OPEN, 5))
+    waypoints.append((grip_1, GRIPPER_CLOSED, 2))
+    waypoints.append((grip_1, GRIPPER_CLOSED, 2))
+    waypoints.append((standoff_1, GRIPPER_CLOSED, 5))
+    waypoints.append((standoff_2, GRIPPER_CLOSED, 7))
+    waypoints.append((grip_2, GRIPPER_CLOSED, 5))
+    waypoints.append((grip_2, GRIPPER_OPEN, 2))
+    waypoints.append((grip_2, GRIPPER_OPEN, 2))
+    waypoints.append((standoff_2, GRIPPER_OPEN, 5))
+
+    return waypoints
+
+
+def _generate_trajectory(X_from, X_to, gripper, time_in_s):
+    """Generate trajectory from one position to another with a given time in seconds.
+    Also handle the given gripper state."""
+    dt = 0.01
+    N = time_in_s / dt
+    trajectory = ScrewTrajectory(X_from, X_to, time_in_s, N, 3)
+
+    t = []
+    # flat list for serialization to csv
+    for traj in trajectory:
+        r = traj[:-1, :-1]
+        p = traj[:-1, -1]
+        s = np.concatenate((r.flatten(), p.flatten(), np.array([gripper])))
+        t.append(s)
+
+    return t
+
+
+def TrajectoryGenerator(T_se_initial, T_sc_initial, T_sc_final):
+    """Generate trajectory for the pick and place task.
+    Intermediate positions (standoff, picking, placing) are calculated with the given positions.
+    :param T_se_initial: the initial position of the end effector
+    :param T_sc_initial: the initial position of the cube to be picked
+    :param T_sc_final: the goal position of the cube
+    :return: The trajectory for the given task"""
+    waypoints = _generate_waypoints(T_se_initial, T_sc_initial, T_sc_final)
+    traj = []
+    for i in range(len(waypoints) - 1):
+        waypoint_from, gripper, _ = waypoints[i]
+        waypoint_to, _, time = waypoints[i + 1]
+        traj.extend(_generate_trajectory(waypoint_from,
+                                         waypoint_to,
+                                         gripper,
+                                         time))
+    return traj

luki_modern_robotics/test/test_capstone.py

@@ -2,7 +2,7 @@
 
 import numpy as np
 import pytest
-from ..capstone.mobile_manipulation import NextState
+from ..capstone.mobile_manipulation import NextState, TrajectoryGenerator, T_se_initial, T_sc_initial, T_sc_goal
 
 DIR_PATH = os.path.dirname(os.path.realpath(__file__))
 
@@ -27,3 +27,9 @@ def test_NextState(name, u):
         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=",")
+
+
+def test_TrajectoryGenerator():
+    traj = TrajectoryGenerator(T_se_initial(), T_sc_initial(), T_sc_goal())
+
+    np.savetxt(os.path.join(DIR_PATH, "output-trajectory.csv"), traj, delimiter=",")