Tutorial: Custom Wrist DOF

In this tutorial, we explore how to add additional degrees of freedom to the Stretch wrist.

Stretch exposes a Dynamixel X-Series TTL control bus at the end of its arm. It uses the Dynamixel XL430-W250 for the Wrist Yaw and the Stretch Gripper that comes standard with the robot.

See the Hardware User Guide to learn how to mechanically attach additional DOFs to the robot.

Note

Stretch is compatible with any Dynamixel X Series servo that utilizes the TTL level Multidrop Bus.

Adding a Custom DOF

Adding one or more custom Dynamixel X Series servos to Stretch wrist involves:

Creating a new class that derives from DynamixelHelloXL430
Adding YAML parameters to stretch_user_params.yaml that configure the servo as desired
Adding YAML parameters to stretch_user_params.yaml that tell Stretch to include this class in its EndOfArm list of servos

Let's create a new DOF called MyWristPitch in a file named my_wrist_pitch.py. Place the file somewhere on the $PYTHONPATH.

from stretch_body.dynamixel_hello_XL430 import DynamixelHelloXL430
from stretch_body.hello_utils import *

class MyWristPitch(DynamixelHelloXL430):
    def __init__(self, chain=None):
        DynamixelHelloXL430.__init__(self, 'my_wrist_pitch', chain)
        self.poses = {'tool_up': deg_to_rad(45),
                      'tool_down': deg_to_rad(-45)}

    def pose(self,p,v_r=None,a_r=None):
        self.move_to(self.poses[p],v_r,a_r)

Now let's add the tools' parameters to your stretch_user_params.yaml to configure this servo. You may want to adapt these parameters to your application but the nominal values found here usually work well. Below we highlight some of the more useful parameters.

my_wrist_pitch:
  id: 1                         #ID on the Dynamixel bus
  range_t:                      #Range of servo, in ticks
  - 0
  - 4096
  req_calibration: 0            #Does the joint require homing after startup
  use_multiturn: 0              #Single turn or multi-turn mode of rotation
  zero_t: 2048                  #Position in ticks that corresponds to zero radians

For this example, we are assuming a single-turn joint that doesn't require hard stop-based homing. We also assume the servo has the Robotis default ID of 1.

At this point, your MyWristPitch class is ready to use. Plug the servo into the cable leaving the Stretch WristYaw joint. Experiment with the API from iPython

In [1]: import my_wrist_pitch

In [2]: w=wrist_pitch.WristPitch()

In [3]: w.startup()

In [4]: w.move_by(0.1)

In [5]: w.pose('tool_up')

In [6]: w.pose('tool_down')

Finally, you'll want to make your WristPitch available from stretch_body.robot. Add the following YAML to your stretch_user_params.yaml

end_of_arm:
  devices:
    wrist_pitch:
      py_class_name: WristPitch
      py_module_name: wrist_pitch

This tells stretch_body.robot to manage a wrist_pitch.WristPitch instance and add it to the EndOfArm list of tools. Try it from iPython:

In [1]: import stretch_body.robot as robot

In [2]: r=robot.Robot()

In [3]: r.startup()

In [4]: r.end_of_arm.move_by('wrist_pitch',0.1)