Calibrating zeros for the Dex Wrist roll/pitch/yaw joints

[Anant_Rai]

Hi,

I have a hello robot whose 0, 0, 0 rpy doesn’t seem aligned with gravity. I feel this could be cause of overuse but not sure.

Wanted to know how can we calibrate the dexterous wrist manually so as to fix this misalignment? Thanks in advance for the help.

Best,
Anant

[bshah]

Hi @Anant_Rai, good question! Each of the roll/pitch/yaw Dex Wrist joints have offset parameters you can use to correct any misalignment. Here’s a manual procedure you could use to fix the offsets:

1. Home the robot using stretch_robot_home.py
   • The Dex Wrist should be pointed out like:
     
     
2. Login and open iPython in a terminal. Create an instance of the Robot class using:

   • import stretch_body.robot
     r = stretch_body.robot.Robot()
     r.startup()
     

   • Now, the roll/pitch/yaw joints should be ‘locked’, i.e. they shouldn’t be backdrive-able.
3. Put a level tool on the Dex Wrist. You might be able to find a level tool app on your phone’s app store (e.g. I used this one).
   • 
4. Try to get the bubble centered by commanding small increments to the pitch/roll joints using the iPython terminal:

   • r.end_of_arm.get_joint('wrist_pitch').move_by(0.05)
     r.end_of_arm.get_joint('wrist_roll').move_by(-0.03)
     

   • Command small increments, less than 0.1 rad, through move_by() while checking the bubble. Note: you may find the pitch joint won’t perfectly get the bubble centered vertically because it keeps slipping a small amount. This happens because there’s a sentry in Stretch Body dedicated to preventing the Dynamixel joints from “overloading”, i.e. a soft failure state where the Dxl motor uses more current to hold a position than is safe. The sentry asks the pitch joint to back off a small amount, which greatly reduces the current used, while remaining close to the commanded position (source code for the sentry here). You can temporarily turn off the sentry in iPython using the code below, or turn it off permanently if you don’t want that behavior to affect your experiments (keeping in mind that if a Dxl motor overloads, it will drop its position and become uncommand-able until reset) by setting the robot_sentry.wrist_<pitch/roll/yaw>_overload parameter in your “stretch_user_params.yaml”.

     • r.robot_params['robot_sentry']['wrist_pitch_overload'] = 0
       r.robot_params['robot_sentry']['wrist_roll_overload'] = 0
       r.robot_params['robot_sentry']['wrist_yaw_overload'] = 0
       

   • 
     
5. The offset parameter for Dxl joints is defined in “ticks”, which is the smallest unit of motion for the motor. We can read the joint’s current position in ticks using the status dictionary:

   • In [30]: r.end_of_arm.get_joint('wrist_pitch').status['pos_ticks']
     Out[30]: 993
     
     In [31]: r.end_of_arm.get_joint('wrist_roll').status['pos_ticks']
     Out[31]: 2080
     

   • The output above tells me that my Dex Wrist is leveled at pitch position 993 ticks and roll position 2080 ticks.
6. Since the yaw joint doesn’t affect the level tool’s reading, you can visually calibrate it by using step 4-5 until the wrist is pointed out and away from the robot.
7. You can now close the Robot class using r.stop() and exit out of iPython.
8. The offset parameter for a Dxl joint is called zero_t. We can query the current offsets for the roll/pitch/yaw joints using:

   • $ stretch_params.py | grep wrist_pitch.zero_t
     stretch_tool_share.stretch_dex_wrist.params                            param.wrist_pitch.zero_t                                               1024                          
     
     $ stretch_params.py | grep wrist_roll.zero_t
     stretch_tool_share.stretch_dex_wrist.params                            param.wrist_roll.zero_t                                                2048                          
     
     $ stretch_params.py | grep wrist_yaw.zero_t
     stretch_configuration_params.yaml                                      param.wrist_yaw.zero_t                                                 7175                          
     

   • Currently, my offsets are set to pitch position 1024 ticks, roll position 2048 ticks, and yaw position 7175 ticks.
9. Open “~/stretch_user/stretch-rey-xxxx/stretch_user_params.yaml” in a file editor and set the new offsets.
   • 
   • Use stretch_params.py again to verify the changes took effect.
10. Open iPython again and command the joints to 0.0 rad. Then verify the Dex Wrist joints are leveled correctly.

    • import stretch_body.robot
      r = stretch_body.robot.Robot()
      r.startup()
      r.end_of_arm.get_joint('wrist_roll').move_to(0.0)
      r.end_of_arm.get_joint('wrist_pitch').move_to(0.0)
      r.end_of_arm.get_joint('wrist_yaw').move_to(0.0)
      r.stop()
      

Hope this helps! Let me know if you have any questions!

[Anant_Rai]

Sorry for the late response. We tried this earlier and it works. Forgot to get back. Thanks a lot for the detailed guide!