The SO-101 Robot Arms

For this module, we will be writing code to calculate the forward kinematics for our SO-101 robot arms and execute a simple “Pick and Place” maneuver from pre-determined joint angles. This approach is often used for manufacturing robots where the starting and ending configurations are known apriori. But once we learn inverse kinematics, we will repeat this module using only the desired starting and ending cartesian positions of our end-effector.

Outline

Assignment Part 1: Forward Kinematics

1a) Write Forward Kinematics Helper Function

This is the first time we will be implementing forward-kinematics for our SO-101 robot arms! To do this, write a helper script named so101_forward_kinematics.py that takes computes the tool frame position and orientation given a dictionary item of the robot configuration. To help you write this, I’ve provided a start to this script below that shows how to compute the first transformation in our product of Lie groups method:

import time
import mujoco
import numpy as np

def Rx(thetadeg):
    thetarad = np.deg2rad(thetadeg)
    c = np.cos(thetarad)
    s = np.sin(thetarad)
    return np.array([[1, 0, 0],
                     [0, c, -s],
                     [0, s, c]])

def Ry(thetadeg):
    thetarad = np.deg2rad(thetadeg)
    c = np.cos(thetarad)
    s = np.sin(thetarad)
    return np.array([[c, 0, s],
                     [0, 1, 0],
                     [-s, 0, c]])
    
def Rz(thetadeg):
    thetarad = np.deg2rad(thetadeg)
    c = np.cos(thetarad)
    s = np.sin(thetarad)
    return np.array([[c, -s, 0],
                     [s, c, 0],
                     [0, 0, 1]])

BASE_HEIGHT = 0.0047625  # meters, 3/16 in metal base under the robot

def get_gw1(theta1_deg):
    displacement = (0.0388353, 0.0, BASE_HEIGHT + 0.0624)
    rotation = Rz(180) @ Rx(180) @ Rz(theta1_deg)
    pose = np.block([[rotation, np.array(displacement).reshape(3,1)], [0, 0, 0, 1]])
    return pose

Note that every SO-101 arm in the lab is mounted on a 3/16” metal base, which raises the robot 0.0047625 m above the table and acrylic grid. The BASE_HEIGHT term in get_gw1 accounts for this so that all heights are measured from the table. This is why the z-displacement in the code is larger than the 0.0624 shown on the diagram.

Your forward kinematics will use the frames shown in the diagram below. The information shown on the diagram was obtained directly from the so-101 urdf used in Assignment 5.

SO-101 Forward Kinematics Diagrams

Since we are interested in the position of objects within the moving jaw’s grasp, we added an object frame as shown on the diagram. This object frame has the same orientation as joint 5 and is only translated from it, so when the gripper is pointed downwards the object frame lines up with the world frame. This means that in our forward kinematics mapping we will actually ignore joint 6 (since the moving jaw does not influence where the object would be located). This is the same procedure/methodology as what we’ve been doing with the tool frame in class.

Using this diagram, complete all of the get_gxx() functions in so101_forward_kinematics.py.

Once you complete all of the get_gxx() functions, you can obtain the entire forward kinematics as follows:

def get_forward_kinematics(position_dict):
    gw1 = get_gw1(position_dict['shoulder_pan'])
    g12 = get_g12(position_dict['shoulder_lift'])
    g23 = get_g23(position_dict['elbow_flex'])
    g34 = get_g34(position_dict['wrist_flex'])
    g45 = get_g45(position_dict['wrist_roll'])
    g5t = get_g5t()
    gwt = gw1 @ g12 @ g23 @ g34 @ g45 @ g5t
    position = gwt[0:3, 3]
    rotation = gwt[0:3, 0:3]
    return position, rotation

1b) Test Forward Kinematics in MuJoCo

To test whether or not your forward kinematics are valid, we will use a method similar to that done for the homework component this week. This method will plot an additional cylinder along the z-axis of the tool frame. If you’ve done your forward kinematics correctly, you should see the cylinder in the gripper when you run the following test script. Since the MuJoCo model does not include the metal base, this script also raises the robot by BASE_HEIGHT right after loading the model so that the simulation matches both your forward kinematics and the real robot. The robot will appear to float slightly above the floor, which is expected. Keep this line in all of your simulation scripts from now on:

import time
import mujoco
import mujoco.viewer
from so101_mujoco_utils import set_initial_pose, send_position_command, move_to_pose, hold_position
from so101_forward_kinematics import get_forward_kinematics, BASE_HEIGHT
import numpy as np

m = mujoco.MjModel.from_xml_path('model/scene.xml')
m.body('base').pos[2] = BASE_HEIGHT  # raise the robot to match the metal base
d = mujoco.MjData(m)

def show_frame(viewer, position, rotation, size=0.02):
    # Draw a 3-axis frame using cylinders and cones as arrows.
    # Each axis is aligned with the local z-direction of a geom, then
    # rotated into the object's frame so x, y, and z are shown.
    axis_length = 0.12
    shaft_radius = max(0.003, size * 0.2)
    cone_height = 0.03

    axis_rotations = {
        'x': np.array([[0.0, 0.0, 1.0],
                       [0.0, 1.0, 0.0],
                       [-1.0, 0.0, 0.0]], dtype=float),
        'y': np.array([[1.0, 0.0, 0.0],
                       [0.0, 0.0, 1.0],
                       [0.0, -1.0, 0.0]], dtype=float),
        'z': np.eye(3, dtype=float),
    }
    axis_colors = {'x': [1.0, 0.0, 0.0, 1.0],
                   'y': [0.0, 1.0, 0.0, 1.0],
                   'z': [0.0, 0.0, 1.0, 1.0]}

    geoms_used = 0
    for axis_name, local_axis_rot in axis_rotations.items():
        world_rot = rotation @ local_axis_rot
        axis_dir = world_rot[:, 2]

        # Shaft: cylinder extending from the base to the tip.
        shaft_half = axis_length / 2.0
        shaft_center = position + axis_dir * shaft_half
        mujoco.mjv_initGeom(
            viewer.user_scn.geoms[geoms_used],
            type=mujoco.mjtGeom.mjGEOM_CYLINDER,
            size=[shaft_radius, shaft_half, 0.0],
            pos=shaft_center,
            mat=world_rot.flatten(),
            rgba=axis_colors[axis_name]
        )
        geoms_used += 1

    viewer.user_scn.ngeom = geoms_used
    viewer.sync()
    return

def show_cylinder(viewer, position, rotation, radius=0.0245, halfheight=0.05, rgba=[1, 0, 0, 1]):
    # Add a cylinder aligned with z-axis
    mujoco.mjv_initGeom(
        viewer.user_scn.geoms[0],
        type=mujoco.mjtGeom.mjGEOM_CYLINDER,   # cylinder type
        size=[radius, halfheight, 0],                  # [radius, half-height, ignored]
        pos=position,                         # center position
        mat=rotation.flatten(),              # orientation matrix (identity = z-up)
        rgba=rgba                           # color
    )
    viewer.user_scn.ngeom = 1
    viewer.sync()
    return

test_configuration = {
    'shoulder_pan': -45.0,   # in radians for mujoco! 
    'shoulder_lift': 45.0,
    'elbow_flex': -45.00,
    'wrist_flex': 90.0,
    'wrist_roll': 0.0,
    'gripper': 10          
}

set_initial_pose(d, test_configuration)
send_position_command(d, test_configuration)

object_position, object_orientation = get_forward_kinematics(test_configuration)

with mujoco.viewer.launch_passive(m, d) as viewer:
  # Close the viewer automatically after 30 wall-seconds.
  start = time.time()
  
  # Add a cylinder as a site for visualization
  show_frame(viewer, object_position, object_orientation)
  
  # Hold Starting Position for 2 seconds
  hold_position(m, d, viewer, 20.0)

If your forward kinematics are correct, you should see the following:

Forward Kinematics Solution

Assignment Part 2: Write Pick and Place code

2a) Test Pick and Place in MuJoCo

We will first conduct the pick and place maneuver in simulation. This will require modifying the code above to have six different joint configurations, and transitioning between these joint configurations using our hold_position and move_to_pose functions from the last two assignments. The first and last configurations should be the following:

starting_configuration = {
    'shoulder_pan': -45.0,   # in radians for mujoco! 
    'shoulder_lift': 45.0,
    'elbow_flex': -45.00,
    'wrist_flex': 90.0,
    'wrist_roll': 0.0,
    'gripper': 50
}
final_configuration = {
    'shoulder_pan': 45.0,   # in radians for mujoco! 
    'shoulder_lift': 45.0,
    'elbow_flex': -45.00,
    'wrist_flex': 90.0,
    'wrist_roll': 0.0,
    'gripper': 50       
}

The second and second-to-last configurations should then be these same configurations but with the gripper closed:

starting_configuration_closed = {
    'shoulder_pan': -45.0,   # in radians for mujoco! 
    'shoulder_lift': 45.0,
    'elbow_flex': -45.00,
    'wrist_flex': 90.0,
    'wrist_roll': 0.0,
    'gripper': 5
}
final_configuration_closed = {
    'shoulder_pan': 45.0,   # in radians for mujoco! 
    'shoulder_lift': 45.0,
    'elbow_flex': -45.00,
    'wrist_flex': 90.0,
    'wrist_roll': 0.0,
    'gripper': 5    
}

The middle two configurations should be determined by you in order to move the manipulator arm upwards so that it doesn’t interfere with the table or any other obstacles that might be on the table.

Your final code should produce the video shown below:

The following code was used to visualize the cubes in the simulation video:

def show_cubes(viewer, starting_config, final_config, halfwidth=0.013):
    # Use forward kinematics
    starting_object_position, starting_object_orientation = get_forward_kinematics(starting_config)
    final_object_position, final_object_orientation = get_forward_kinematics(final_config)
  
    # Add starting cube
    mujoco.mjv_initGeom(
        viewer.user_scn.geoms[0],
        type=mujoco.mjtGeom.mjGEOM_BOX, 
        size=[halfwidth, halfwidth, halfwidth],                 
        pos=starting_object_position,                         
        mat=starting_object_orientation.flatten(),              
        rgba=[1, 0, 0, 0.2]                           
    )
    # Add final cube
    mujoco.mjv_initGeom(
        viewer.user_scn.geoms[1],
        type=mujoco.mjtGeom.mjGEOM_BOX, 
        size=[halfwidth, halfwidth, halfwidth],                 
        pos=final_object_position,                         
        mat=final_object_orientation.flatten(),              
        rgba=[0, 1, 0, 0.2]                           
    )
    viewer.user_scn.ngeom = 2
    viewer.sync()
    return

2b) Execute Pick and Place on Hardware

Once you’re comfortable with this process of moving from joint configuration to joint coniguration, try it out on the real robot! You should mostly be able to use your code from Assignment 3 with the addition of a few more target joint configurations and move_to_pose calls. The general procedure is the following.

  1. Determine the object locations using our predictions from the forward kinematics mapping and place the objects in this position using our acrylic cartesian grids.
  2. Start your code as in Lab 3 where we store the robot’s actual starting position and move to the first desired joint configuration using:
starting_pose = bus.sync_read("Present_Position")
move_to_pose(bus, starting_configuration, 2.0)

At the end of your code, you should then transition back to the initial starting position using

move_to_pose(bus, starting_pose, 2.0)

This will prevent the robot from slamming into the table when it terminates the script.

  1. Execute the full sequence and verify that it visually matches what we see in simulation. In particular, does the first desired configuration align with our expected object location? In your writeup, comment on whether or not this prediction was accurate.
  2. If the initial object location is not accurate, you may need to recalibrate. If it is still not accurate, you can use the read_position script from Assignment 3 to find a more appropriate starting and final joint configuration. Note that this will require also modifying the in-between joint configurations.

Your final robotic demonstration should look like the following:

Please put away all supplies and return the desk to a clean position when you are done with lab. Please also let us know if anything breaks so that we can maintain the robots.

Optional: Offset the End-Effector Frame

The end-effector frame used above is located on the wrist roll axis, which is centered between the jaws when the gripper is fully closed. However, only one of the jaws moves when the gripper opens and closes, so an object held in the gripper will sit slightly off-center from this frame.

As something to consider, you could instead define the end-effector frame on the inside edge of the fixed jaw by adding an offset in the -x axis of joint 5’s frame, as shown in the diagram below. The benefit of this frame is that it makes it easier to align the gripper to the edges of blocks. The drawback is that the end-effector frame is no longer on the wrist roll axis, which makes the inverse kinematics that we will derive later slightly more involved.

SO-101 Forward Kinematics Diagram with Offset

If you try this frame, you should see the following result from your forward kinematics test script:

Forward Kinematics Solution with Offset

This is optional and is not part of the submission for this lab. Future labs will use the end-effector frame from Part 1.

Submission Details

To document the lab for this week, please record a video of your robot executing the full motion and upload it to youtube (it can be as an unlisted video). Then please include a link to the video with the homework submission. If you scan your homework, please make sure that the link is legible. As usual, also include in your submission a writeup (included in the homework submission) about what was implemented, and any challenges/difficulties that you had when writing/executing the code.