stretch_tutorials/stretch_body/tutorial_collision_avoidance.md

Tutorial: Collision Avoidance

In this tutorial, we will discuss the simple collision avoidance system that runs as a part of Stretch Body.

Overview

Stretch Body includes a system to prevent inadvertent self-collisions. It will dynamically limit the range of motion of each joint to prevent self-collisions.

!!! warning Self collisions are still possible while using the collision-avoidance system. The factory default collision models are coarse and not necessarily complete.

This system is turned off by default starting with Stretch 2. It may be turned off by default on many RE1 systems. First check if the collision detection system is turned on:

stretch_params.py | grep use_collision_manager

Output:

stretch_body.robot_params.nominal_params       param.robot.use_collision_manager         1

If it is turned off you can enable it by adding the following to your stretch_user_yaml.py:

robot:
  use_collision_manager: 1

Common Self Collisions

Fortunately, the simple kinematics of Stretch make self-collisions fairly uncommon and simple to predict. The primary places where self-collisions may occur are

• The lift lowering the wrist or tool into the base
• The arm retracting the wrist or tool into the base
• The head_pan at pos==0 and head_tilt at pos=-90 deg and the lift raising the arm into the camera (minor collision)
• The Dex Wrist (if installed) colliding with itself
• The Dex Wrist (if installed) colliding with the base

Joint Limits

The collision avoidance system works by dynamically modifying the acceptable range of motion for each joint. By default, a joint's range is set to the physical hard stop limits. For example, the lift has a mechanical throw of 1.1m:

stretch_params.py | grep range | grep lift

Output:

stretch_body.robot_params.factory_params       param.lift.range_m      [0.0, 1.1]                

A reduced range of motion can be set at run-time by setting the Soft Motion Limit. For example, to limit the lift range of motion to 0.3 meters off the base:

import stretch_body.robot as robot
r=robot.Robot()
r.startup()
r.lift.set_soft_motion_limit_min(0.3)

We see in the API, the value of None is used to designate no soft limit.

It is possible that when setting the Soft Motion Limit the joint's current position is outside of the specified range. In this case, the joint will move to the nearest soft limit to comply with the limits. This can be demonstrated by:

import stretch_body.robot as robot
import time

r=robot.Robot()
r.startup()

#Move to 0.2
r.lift.move_to(0.2)
r.push_command()
time.sleep(5.0) 

#Will move to 0.3
r.lift.set_soft_motion_limit_min(0.3)

Collision Models

The RobotCollision class manages a set of RobotCollisionModels. Each RobotCollisionModel computes the soft limits for a subset of joints based on a simple geometric model. This geometric model captures the enumerated set of potential collisions listed above.

We can see which collision models will execute when use_collision_manager is set to 1:

stretch_params.py | grep collision | grep enabled

Output:

stretch_body.robot_params.nominal_params    param.collision_arm_camera.enabled           1                             
stretch_body.robot_params.nominal_params    param.collision_stretch_gripper.enabled      1  

We see two models. One that protects the camera from the arm, and one that protects the base from the gripper. Each model is registered with the RobotCollision instance as a loadable plug-in. The Robot class calls the RobotCollision.step method periodically at approximately 10hz.

RobotCollision.step computes the 'AND' of the limits specified across each Collision Model such that the most restrictive joint limits are set for each joint using the set_soft_motion_limit_min and set_soft_motion_limt_max methods.

Default Collision Models

The default collision models for Stretch Body are found in robot_collision_models.py. As of this writing, the provided models are:

• CollisionArmCamera: Avoid collision of the head camera with the arm
• CollisionStretchGripper: Avoid collision of the wrist-yaw and gripper with the base and ground

!!! warning The provided collision models are coarse and are provided to avoid common potentially harmful collisions only. Using these models it is still possible to collide the robot with itself in some cases.

!!! info Additional collision models are provided for the DexWrist

Working with Models

The collision models to be used by Stretch Body are defined with the robot_collision parameter. For example, we see in robot_params.py that the CollisionArmCamera is loaded by default:

"robot_collision": {'models': ['collision_arm_camera']},

We also see that model collision_arm_camera is defined as:

  "collision_arm_camera": {
        'enabled': 1,
        'py_class_name': 'CollisionArmCamera',
        'py_module_name': 'stretch_body.robot_collision_models'
    }

This instructs RobotCollision to construct a model of type CollisionArmCamera and enable it by default. One can disable this model by default by specifying the following stretch_re1_user_params.yaml:

collision_arm_camera:
  enabled: 0

The entire collision avoidance system can be disabled in stretch_re1_user_params.yaml by:

robot:
  use_collision_manager: 0

A specific collision model can be enabled or disabled during runtime by:

import stretch_body.robot
r=stretch_body.robot.Robot()
r.startup() 
... #Do some work
r.collision.disable_model('collsion_arm_camera')
... #Do some work
r.collision.enable_model('collsion_arm_camera')

Finally, if we want to also use the CollisionStretchGripper model, we can add to stretch_re1_user_params.py:

robot_collision:
  models:
  - collision_arm_camera
  - collision_stretch_gripper

Creating Custom Collision Models

The step method of a RobotCollisionModel returns the desired joint limits given that model. For example, the base class is simply:

    class RobotCollisionModel(Device):
    	def step(self, status):
        	return {'head_pan': [None, None],'head_tilt': [None, None], 
                    'lift': [None, None],'arm': [None, None],'wrist_yaw': [None, None]}

where the value of None specifies that no limit is specified and the full range of motion for the joint is acceptable.

We could define a new collision model that simply limits the lift range of motion to 1 meter by:

    class MyCollisionModel(Device):
    	def step(self, status):
        	return {'head_pan': [None, None],'head_tilt': [None, None], 
                    'lift': [None, 1.0],'arm': [None, None],'wrist_yaw': [None, None]}

It is straightforward to create a custom collision model. As an example, we will create a model that avoids collision of the arm with a tabletop by

• Preventing the lift from descending below the table top when the arm is extended
• Allowing the lift to descend below the tabletop so long as the arm retracted

This assumes the arm is initially above the tabletop. To start, in a file collision_arm_table.py we add:

from stretch_body.robot_collision import *
from stretch_body.hello_utils import *

class CollisionArmTable(RobotCollisionModel):
    def __init__(self, collision_manager):
        RobotCollisionModel.__init__(self, collision_manager, 'collision_arm_table')

    def step(self, status):
        limits = {'lift': [None, None],'arm': [None, None]}
        table_height = 0.5 #m
        arm_safe_retract = 0.1 #m
        safety_margin=.05#m

        x_arm = status['arm']['pos']
        x_lift = status['lift']['pos']

        #Force arm to stay retracted if below table
        if x_lift<table_height:
            limits['arm'] = [None, arm_safe_retract-safety_margin]
        else:
            limits['arm'] = [None, None]

        #Force lift to stay above table unless arm is retracted
        if x_arm<arm_safe_retract:
            limits['lift'] =[None,None]
        else:
            limits['lift']=[table_height+safety_margin,None]
        return limits

In this example, we include the safety_margin as a way to introduce some hysteresis around state changes to avoid toggling between the soft limits.

The following command should be run to add the working directory to the PYTHONPATH env. This can also be added to our .bashrc to permanently edit the path:

export PYTHONPATH=$PYTHONPATH:/<path_to_modules>

Next, we configure RobotCollision to use our CollisionArmTable model in stretch_re1_user_yaml:

robot_collision:
  models:
  - collision_arm_table

collision_arm_table:
  enabled: 1
  py_class_name: 'CollisionArmTable'
  py_module_name: 'collision_arm_table'

Finally, test out the model by driving the arm and lift around using the Xbox teleoperation tool:

stretch_xbox_controller_teleop.py

---

All materials are Copyright 2022 by Hello Robot Inc. Hello Robot and Stretch are registered trademarks.