Best way to limit Yaw and Pitch of Stretch RE1

Ask
1

Hi

I am working on attaching a stock to the hello-robot arm to fix a camera for ego-centric streaming (refer to the image attached).

There is a hurdle to this that hello-robot during calibration moves its arm in certain ways that can break the stock and camera. We also want to avoid any such unintentional movements that may be given manually through commands or teleop.

I think if we could limit the motion of yaw and pitch of the arm, it should solve the issue. So wanted to know what’s the best way to apply this safety limits and ensure they are universally applied.

Thanks

IMG_20221018_154321~2
IMG_20221018_154321~21920×1644 372 KB

2

Hey @Anant_Rai ,

You can create Custom Collision Models for the robot using the RobotCollisionModel class. The RobotCollisionModel is a base class to provide simple self-collision avoidance by defining derived (custom) classes that should implement the collision logic. It works by defining acceptable joint ranges for the joints based on the current kinematic state of the robot. And this collision model can be implemented all over stretch body movements.

You can find the Tutorial: Collision Avoidance, which will discuss the simple collision avoidance system that runs as a part of the Stretch Body and implementing custom collision models.

For the Yaw motion, you have to keep in mind that even if the Range of motion is clipped using an active custom collision model but during the homing process the Yaw moves CW to find the hard-stop which is required to calibrate its home position.

3

Thanks a lot for your reply. And really sorry to reach back this late. But I tried following the tutorial and had several issues:

So I first created my collision model class as follows

from stretch_body.robot_collision import *
from stretch_body.hello_utils import *
YAW_MAX = 1.5
YAW_MIN = -0.4
PITCH_MAX = 0.2
PITCH_MIN = -0.5
#TODO: Add a readme explaining how to apply this CollisionModel to the robot
class CamStockCollisionModel(RobotCollisionModel):
    def __init__(self, collision_manager):
        RobotCollisionModel.__init__(self, collision_manager, 'CamStockCollisionModel')

    def step(self, status):
        
        return {'head_pan': [None, None],'head_tilt': [None, None], 
                    'lift': [None, None],'arm': [None, None],
                    'wrist_yaw': [YAW_MIN, YAW_MAX], 'wrist_pitch': [PITCH_MIN, PITCH_MAX]}

In the tutorial it says to edit stretch_re1_user_params.yaml and add the above class which I did but nothing happened. Note: Intially there was no such file present. Instead there was stretch_re1_user_params.migration_backup.20220923150601.yaml. I renamed it to the required stretch_re1_user_params.yaml.

So instead I added the Collision class to stretch_user_params.yaml as below

#Parameters that are specific to this robot
#Do not edit, instead edit stretch_user_params.yaml
head:
  baud: 115200
head_pan:
  baud: 115200
head_tilt:
  baud: 115200
hello-motor-lift:
  gains:
    i_safety_feedforward: 0.75
lift:
  contact_models:
    effort_pct: {}
  i_feedforward: 0.75
params:
- stretch_tool_share.stretch_dex_wrist.params
robot:
  tool: tool_stretch_dex_wrist
  use_collision_manager: 1
stretch_gripper:
  baud: 115200
  range_t:
  - 0
  - 6415
  zero_t: 4017
tool_none:
  baud: 11520
tool_stretch_gripper:
  baud: 115200
wrist_yaw:
  baud: 115200

robot_collision:
  models:
  - CamStockCollisionModel

CamStockCollisionModel:
  enabled: 1
  py_class_name: 'CamStockCollisionModel'
  py_module_name: 'CamStockCollisionModel'

After this, it did look like the collision class was being loaded, but still when I am controlling with the joystick, I can still do full default motions and it seems the safety limits in the my class I created didnt work.

I would really appreciate if you could help me figure out what am I doing wrong.

Thanks!

4

Hey @Anant_Rai , apologies for this issue, and thank you for bringing this up.
We were able to reproduce this issue of joint soft limits defined in custom collision models not being applied to the end-of-arm tools joints.
This issue was due to a small bug in our Robot Collision module, and the bugfix is now pushed. You must upgrade your Stretch Body package to the latest version (v0.4.11) to reflect this fix.
You can upgrade your Stretch Body package to the latest version with the command:
pip3 install hello-robot-stretch-body --upgrade

And you are right the tutorial points to the config file stretch_re1_user_params.yaml which is our older naming convention. But from Stretch Body v0.4 and above it is renamed to stretch_user_params.yaml.

Please let us know if you do run into any issues, happy to help resolve them.

5

Thanks a lot. Will update and try again