Stretch Body Guide
The Stretch_Body package provides a low level Python API to the Stretch RE1 hardware.
The package is available on Git and installable via Pip.
It encapsulates
- Mobile base
- Arm
- Lift
- Head actuators
- Wrist and tool actuators
- Wrist accelerometer and Arduino
- Base power and IMU board
The robot's 3rd party hardware devices are intended to be accessed through ROS and not Stretch_Body. However, it is possible to directly access this hardware through open-source Python packages:
- Laser range finder: rplidar
- Respeaker: respeaker_python_library
- D435i: pyrealsense2
The Stretch_Body package is intended for advanced users who prefer to not use ROS to control the robot. It assumes a moderate level of experience programming robot sensors and actuators.
Robot Interface
The primary developer interface to Stretch_Body is the Robot class.
As an example, the Python script below prints all Robot sensor and state data to the console every 250ms.
1 2 3 4 5 6 7 8 9 10 11 | |
Looking at this in detail:
2 | |
The package stretch_body includes the Python module for Robot as well as other Devices such as Lift and Arm.
4 5 | |
Here we instantiate an instance of our Robot. The call to startup() opens the serial ports to the various devices, loads the Robot YAML parameters, and launches a few helper threads.
7 8 9 | |
The call to pretty_print() prints to console all of the robot's sensor and state data.
11 | |
Finally, the stop() method shuts down the Robot threads and cleanly closes the open serial ports.
Units
The Robot API uses SI units of:
- meters
- radians
- seconds
- Newtons
- Amps
- Volts
Parameters may be named with a suffix to help describe the unit type. For example:
- pos_m : meters
- pos_r: radians
The Robot Status
The Robot derives from the Device class. It also encapsulates a number of other Devices:
All devices contain a Status dictionary. The Status contains the most recent sensor and state data of that device. For example, looking at the Arm class we see:
class Arm(Device):
def __init__(self):
...
self.status = {'pos': 0.0, 'vel': 0.0, 'force':0.0, \
'motor':self.motor.status,'timestamp_pc':0}
The Status dictionaries are automatically updated by a background thread of the Robot at 25Hz. The Status data can be accessed via the Robot. For example:
if robot.arm.status['pos']>0.25:
print('Arm extension greater than 0.25m')
If an instantaneous snapshot of the entire Robot Status is needed, the get_status() method can be used instead:
status=robot.get_status()
if status['arm']['pos']>0.25:
print('Arm extension greater than 0.25m')
The Robot Command
In contrast to the Robot Status which pulls data from the Devices, the Robot Command pushes data to the Devices.
Consider the following example which extends and then retracts the arm by 0.1 meters:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | |
A few important things are going on:
7 | |
The move_by() method queues up an RPC command to the stepper motor controller. However, the command does not yet execute.
8 | |
The push_command() causes all queued up RPC commands to be executed at once. In this example we call sleep() to allow time for the motion to complete before initiating a new motion.
NOTE: The Dynamixel servos do not use the Hello Robot RPC protocol. As such, the head, wrist, and gripper will move immediately upon issuing a motion command.
The stepper actuators support a synchronous mode, allowing the base, arm, and lift to synchronously track trajectories. Thus, the following code will cause the base, arm, and lift to initiate motion simultaneously:
robot.arm.move_by(0.1)
robot.lift.move_by(0.1)
robot.base.translate_by(0.1)
robot.push_command()
Commanding robot motion through the Stretch_Body interface is covered in more detail in the Robot Motion section.
Stowing and Homing
After power up the robot requires homing in order for its joint encoders to find their zero position. The homing procedure will run the robot through a series of moves to find these zeros. It can be done programatically:
if not robot.is_calibrated():
robot.home() #blocking
Or it can be done manually after boot using the command line tool:
$ stretch_robot_home.py
Likewise, stowing is a robot procedure that will cause it to move its arm and tool safely within the footprint of the base.
robot.stow() #blocking
Or it can be done manually from the command line when needed:
$ stretch_robot_stow.py
Scripting the Robot
A simplified design pattern to script the Robot is as follows
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 | |
Command Line Tools
The Stretch_Body package comes with a suite of command line tools that allow direct interaction with hardware subsystems. These can be useful when developing and debugging applications. They also serve as code examples when developing applications for Stretch_Body.
These tools can be found by tab completion of 'stretch_' from a terminal.
$ stretch_
stretch_about.py
stretch_gripper_home.py
stretch_lift_jog.py
stretch_robot_dynamixel_reboot.py
stretch_robot_stow.py
stretch_wacc_scope.py
stretch_arm_home.py
stretch_gripper_jog.py
stretch_pimu_jog.py
stretch_robot_home.py
stretch_robot_system_check.py
stretch_wrist_yaw_home.py
stretch_arm_jog.py
stretch_hardware_echo.py
stretch_pimu_scope.py
stretch_robot_jog.py
stretch_rp_lidar_jog.py
stretch_wrist_yaw_jog.py
stretch_audio_test.py
stretch_head_jog.py
stretch_respeaker_test.py
stretch_robot_keyboard_teleop.py
stretch_urdf_show.py
stretch_xbox_controller_teleop.py
stretch_base_jog.py
stretch_lift_home.py
stretch_robot_battery_check.py
stretch_robot_monitor.py
stretch_wacc_jog.py
All tools accept '--help' as a command line argument to learn its function. For example:
$ stretch_wacc_scope.py --help
usage: stretch_wacc_scope.py [-h] [--ax] [--ay] [--az] [--a0] [--d0] [--d1]
[--tap]
Visualize Wacc (Wrist+Accel) board data with an oscilloscope
optional arguments:
-h, --help show this help message and exit
--ax Scope accelerometer AX
--ay Scope accelerometer AY
--az Scope accelerometer AZ
--a0 Scope analog-in-0
--d0 Scope digital-in-0
--d1 Scope digital-in-1
--tap Scope single tap
Commonly Used Tools
These are the tools a typical user will want to become familiar with.
| Tool | Utility |
|---|---|
| stretch_robot_home.py | Commonly run after booting up the robot in-order to calibrate the joints |
| stretch_robot_system_check.py | Scans for all hardware devices and ensure they are present on the bus and reporting valid values. Useful to verify that the robot is in good working order prior to commanding motion. It will report all success in green, failures in red. |
| stretch_robot_stow.py | Useful to return the robot arm and tool to a safe position within the base footprint. It can also be useful if a program fails to exit cleanly and the robot joints are not backdriveable. It will restore them to their 'Safety' state. |
| stretch_robot_battery_check.py | Quick way to check the battery voltage / current consumption |
| stretch_xbox_controller_teleop.py | Useful to quickly test if a robot can achieve a task by manually teleoperating the robot |
| stretch_robot_dynamixel_reboot.py | This will reset all Dynamixels in the robot, which may be needed if a servo overheats during high use and enters an error state. |
The other tools are fairly self explanatory. They allow the user to quickly read a sensor value or control an individual hardware subsystem.
Robot Motion
Controlling the motion of the robot's actuators is typically done through the ROS interfaces. However it is also possible to control the robot directly through the stretch_body interfaces.
Actuators are commanded by either a move_by or move_to command (the former being incremental, the latter being absolute). For example, a relative move using the default motion parameters of the arm looks like:
1 2 3 4 5 6 7 8 9 | |
Motion Profiles
All joints support trapezoidal based motion generation. Other types of controllers are available (PID, velocity, etc) but they are not covered here . The trapezoidal motion controllers require three values:
- x: target position of joint
- v: maximum velocity of motion
- a: acceleration of motion
We provide 'default' settings for the velocity and acceleration settings, as well as 'fast', and 'slow' settings. These values have been tuned to be appropriate for safe motion of the robot. These values can be seen in the 'stretch_re1_factory_params.yaml'. For example:
arm:
motion:
fast: {accel_m: 0.2, vel_m: 0.2}
default: {accel_m: 0.14, vel_m: 0.14}
max: {accel_m: 1.0, vel_m: 1.0}
slow: {accel_m: 0.07, vel_m: 0.06}
To move the arm quickly instead:
vel_fast_m = robot.arm.params['motion']['fast']['vel_m']
accel_fast_m = robot.arm.params['motion']['fast']['accel_m']
robot.arm.move_by(x_m=0.1, v_m=vel_fast_m, a_m=accel_fast_m)
robot.push_command()
The motion will fall back to the 'default' settings found in the YAML if no parameters are provided.
Range of Motion
All joints obey motion limits which are specified in the factory YAML. These limits have been set at the factory to prevent damage to the hardware. It is not recommended to set them to be greater than the factory specified values. However, they can be further limited if desired.
For example. to prevent the lift from descending within 100mm of the base, one can override the factory setting in stretch_re1_user_params.yaml
lift:
range_m: [0.1, 1.095]
Control Modes
Each joint has a default safety mode and default control mode. These are:
| Joint | Default Safety Mode | Default Control Mode |
|---|---|---|
| left_wheel | Freewheel | Trapezoidal position control |
| right_wheel | Freewheel | Trapezoidal position control |
| lift | Gravity compensated 'float' | Trapezoidal position control |
| arm | Freewheel | Trapezoidal position control |
| head_pan | Torque disabled | Trapezoidal position control |
| head_tilt | Torque disabled | Trapezoidal position control |
| wrist_yaw | Torque disabled | Trapezoidal position control |
| stretch_gripper | Torque disabled | Trapezoidal position control |
The actuator remains in Safety Mode when no program is running. When the <device>.startup() function is called it transitions to its Control Mode. It is placed back in Safety Mode when <device>.stop() is called.
In addition, the Base supports a velocity control mode. The Base controllers will automatically switch between velocity and position based control. For example:
robot.base.translate_by(x_m=0.5)
robot.push_command()
time.sleep(4.0) #wait
robot.base.set_rotational_velocity(v_r=0.1) #switch to velocity controller
robot.push_command()
time.sleep(4.0) #wait
robot.base.set_rotational_velocity(v_r=0.0) #stop motion
robot.push_command()
As shown, care should be taken to reduce commanded velocities to zero to avoid runaway.
Runstop
Runstop activation will cause the Base, Arm, and Lift to switch to Safety Mode and for motion commands will be ignored. The motion commands will resume smoothly when the runstop is deactivated. This is usually done via the runstop button. However, it can also be done via the Pimu interface:
if robot.pimu.status['runstop_event']:
robot.pimu.runstop_event_reset()
robot.push_command()
Guarded Motion
The Arm, Lift, and Base support a guarded motion function. It will automatically transition the actuator from Control mode to Safety mode when the exerted motor torque exceeds a threshold.
This functionality is most useful for the Lift and the Arm. It allows these joints to safely stop upon contact. It can be used to:
- Safely stop when contacting an actuator hardstop
- Safely stop when making unexpected contact with the environment or a person
- Make a guarded motion where the robot reaches to a surface and then stops
Each of these tasks have different force characteristics and may require different threshold settings. The factory defaults are set so as to allow freespace motion without a payload (without triggering a false positive stop). These thresholds are set in the factory YAML. For example:
arm:
contact_thresh_N: [-50, 50]
contact_thresh_max_N: [-80, 80]
A user can dynamically set the contact thresholds depending on the task requirements. For example, to make the arm extension motion more sensitive:
robot.arm.move_by(x_m=0.1, contact_thresh_pos_N=30.0)
If too sensitive, the joint may trigger false positives (e.g., stop without contact), and different thresholds may be needed in different portions of the workspace.
When a guarded motion event has occurred it is reported in the Status:
if robot.arm.motor.status['in_guarded_event']:
print('Arm has made contact')
The guarded event can be reset and motion resumed by simply sending a new motion command to the joint (that is not identical to the previous command). Here is a simple example of moving to contact, then moving back:
robot.arm.move_to(0.5) #Reach all the way out
robot.push_command()
while robot.arm.status['pos']<0.5:
if robot.arm.motor.status['in_guarded_event']:
print('Contact made at', robot.arm.status['pos'])
break
time.sleep(0.1)
print('Retracting...')
robot.arm.move_to(0.0)
robot.push_command()
Note: The units of Newtons are approximations only and may not be accurate to real world contact forces.
Synchronized Motion
The Arm, Lift, and Base actuators have a hardware synchronization mechanism. This allows for controller commands to be time synchronized across joints. By default these are turned out in the factory YAML:
hello-motor-arm:
gains: {enable_sync_mode: 1,...}
Motion Status
It can be useful to poll the status of a joint during motion in order to modify the robot behavior, etc. The useful status values include:
robot.arm.status['pos'] #Joint position
robot.arm.status['vel'] #Joint velocity
robot.arm.status['force'] #Joint force (derived from motor current)
robot.arm.motor.status['near_pos_setpoint'] #Is sensed position near commanded position
robot.arm.motor.status['near_vel_setpoint'] #Is sensed velocity near commanded velocity
robot.arm.motor.status['is_moving'] #Is the joint in motion
robot.arm.motor.status['in_guarded_event'] #Has a guarded event occured
robot.arm.motor.status['in_safety_event'] #Has a safety event occured
Update Rates
The following update rates apply to Stretch:
| Item | Rate | Notes |
|---|---|---|
| Status data for Arm, Lift, Base, Wacc, and Pimu | 25Hz | Polled automatically by Robot thread |
| Status data for End of Arm and Head servos | 15Hz | Polled automatically by Robot thread |
| Command data for Arm, Lift, Base, Wacc, Pimu | N/A | Commands are queued and executed upon calling robot.push_command( ) |
| Command data for End of Arm and Head servos | N/A | Commands execute immediately |
Motion commands are non-blocking and it is the responsibility of the user code to poll the Robot Status to determine when and if a motion target has been achieved.
The Stretch_Body interface is not designed to support high bandwidth control applications. The natural dynamics of the robot actuators do not support high bandwidth contorl, and the USB based interface limits high rate communication.
In practice, a Python based control loop that calls push_command( ) at 1Hz to 10Hz is sufficiently matched to the robot natural dynamics.
Sensors
Base IMU
Coming soon.
Wrist Accelerometer
Coming soon.
Cliff Sensors
Stretch has four IR cliff sensors pointed towards the floor. These report the distance to the floor, allowing for detection of thresholds, stair edges, etc.
Relevant parameters in the factory YAML are
pimu:
config:
cliff_LPF: 10.0
cliff_thresh: -50
cliff_zero:
- 523.7940936279297
- 508.10246490478517
- 496.55742706298827
- 525.149652709961
stop_at_cliff: 0
The stop_at_cliff field causes the robot to execute a Runstop when the cliff sensor readings are out of bounds.
Note: As configured at the factory, stop_at_cliff is set to zero and Stretch does not stop its motion based on the cliff sensor readings. Hello Robot makes no guarantees as to the reliability of Stretch's ability to avoid driving over ledges and stairs when this flag is enabled.
The sensors are calibrated such that a zero value indicates the sensor is at the correct height from the floor surface. A negative value indicates a drop off such as a stair ledge while a positive value indicates an obstacle like a threshold or high pile carpet.
The calibrated range values from the sensors can be read from the robot.pimu.status message. Relevant fields are:
In [1]: robot.pimu.pretty_print()
------ Pimu -----
...
At Cliff [False, False, False, False]
Cliff Range [2.043212890625, 3.710906982421875, 1.6026611328125, 1.95098876953125]
Cliff Event False
...
A Cliff Event flag is set when any of the four sensor readings exceed cliff_thresh and stop_at_cliff is enabled. In the event of a Cliff Event, it must be reset by robot.pimu.cliff_event_reset()in order to reset the generated Runstop.
The cliff detection logic can be found in the Pimu firmware.
Robot Parameters
All robot data is stored in the stretch_user directory. The location of this directory can be found by:
$ echo $HELLO_FLEET_PATH
/home/hello-robot/stretch_user
The robot data stored here is identified by the robot ID (eg, stretch-re1-1002)
$ cd $HELLO_FLEET_PATH/$HELLO_FLEET_ID
$ ls
calibration_base_imu
calibration_guarded_contact
calibration_steppers
calibration_D435i
calibration_ros
export_urdf
udev
stretch_re1_factory_params.yaml
stretch_re1_user_params.yaml
stretch_re1_tool_params.yaml
A factory image of this data (as shipped), is stored read-only under /etc/hello-robot . This is only for backup and to support cloning the user environment for new users.
Calibration Data
The raw calibration data that was used in production for the robot is also stored for reference within the stretch_user directory. It isn't generally required for development.
URDF Data
A calibrated URDF, and associated mesh files, are provided in the 'export_urdf' directory. This is provided for users who don't wish to use ROS yet still want an accurate model of the robot. The stretch_urdf_view.py tool demonstrates how to visualize the URDF from Python.
YAML Data
Stretch_Body relies upon the following three primary YAML files:
| File | Purpose |
|---|---|
| stretch_re1_factory_params.yaml | Factory settings for controller gains, calibrations, and system configuration. Read only |
| stretch_re1_user_params.yaml | User parameters that override the factory parameters |
| stretch_re1_tool_params.yaml | Settings and configuration data for optional 3rd party end-of-arm tools. |
Factory Parameters
This stretch_re1_factory_params.yaml file contains the robot's 'factory' settings. This includes things such as PID parameters for motor controllers, calibration constants, and default joint velocities and accelerations.
The user should not edit this file. Hello Robot retains an 'as shipped' version of this file should it ever get corrupted. It can be instructive to review this file when getting to know the Stretch_Body code base.
User Parameters
The factory settings should suffice for most use cases. However, the user is allowed to override the factory settings. This is done by using same YAML structure and name as is used in the stretch_re1_user_params.yaml file as in the factory file.
For example, heres the stretch_re1_user_params.yaml file is overriding the factory default contact thresholds and motion speeds.
factory_params: stretch_re1_factory_params.yaml
tool_params: stretch_re1_tool_params.yaml
lift:
contact_thresh_N: [-60, 60]
motion:
default: {accel_m: 0.15, vel_m: 0.095}
arm:
contact_thresh_N: [-80, 80]
motion:
default: {accel_m: 0.14, vel_m: 0.14}
base:
motion:
default: {accel_m: 0.1, vel_m: 0.15}
End of Arm Tool Parameters
The stretch_re1_tool_params.yaml file stores configuration parameters specific to the user's custom end-of-arm-tools. It is read by the Robot class and the parameter data is made accessible to the user's end-of-arm-tool class.
More information on integrating custom hardware on the End of Arm Dynamixel bus can be found at the Extending Wrist DOF Tutorial
Safe Operation Features
Stretch includes a number of built-in functions that help it maintain safe operating conditions. These functions can be disabled and enabled via the robot YAML parameters.
Logging
Upon instantiation, the Robot class opens a new log file for warning and informational messages to be written to. These timestamped logs are found under $HELLO_FLEET_DIRECTORY/log.
The logging messages can be echoed to the console by setting:
robot:
log_to_console: 1
Runstop Functions
| YAML | Function |
|---|---|
| stop_at_low_voltage | Trigger runstop / beep when voltage too low |
| stop_at_high_current | Trigger runstop when bus current too high |
| stop_at_cliff | Trigger runstop when a cliff sensor is outside of range |
| stop_at_runstop | Allow runstop to disable motors |
| stop_at_tilt | Trigger runstop when robot tilts too far |
Robot Monitor
The Robot Monitor is a thread that monitors the Robot Status data for significant events. For example, it can monitor the error flags from the Dynamixel servos and notify when a thermal overload occurs. The Robot Monitor logs warnings to a log file by default.
| YAML | Function |
|---|---|
| monitor_base_bump_event | Report when the accelerometer detects a bump event |
| monitor_base_cliff_event | Report when a cliff sensor event occurs |
| monitor_current | Report when the battery current exceeds desired range |
| monitor_dynamixel_flags | Report when a Dynamixel servo enters an error state |
| monitor_guarded_contact | Report when a guarded contact event occurs |
| monitor_over_tilt_alert | Report when an over-tilt event occurs |
| monitor_runstop | Report when the runstop is activated / deactivated |
| monitor_voltage | Report when the battery voltage is out of range |
| monitor_wrist_single_tap | Report when the wrist accelerometer reports a single tap event |
The YAML below illustrates the types of events that are can be configured.
robot:
log_to_console: 0
use_monitor: 1
use_sentry: 1
robot_monitor:
monitor_base_bump_event: 1
monitor_base_cliff_event: 1
monitor_current: 1
monitor_dynamixel_flags: 1
monitor_guarded_contact: 1
monitor_over_tilt_alert: 1
monitor_runstop: 1
monitor_voltage: 1
monitor_wrist_single_tap: 1
robot_sentry:
base_fan_control: 1
base_max_velocity: 1
stretch_gripper_overload: 1
wrist_yaw_overload: 1
Robot Sentry
The Robot Sentry is a thread that can override and also generate commands to the robot hardware. It's purpose is to keep the robot operating within a safe regime. For example, the Robot Sentry monitors the position of the Lift and Arm and limits the maximum base velocity and acceleration (in order to reduce the chance of toppling). The Robot Sentry reports events to the log file as well.
| YAML | Function |
|---|---|
| base_fan_control | Turn the fan on when CPU temp exceeds range |
| base_max_velocity | Limit the base velocity when robot CG is high |
| stretch_gripper_overload | Reset commanded position to prevent thermal overload during grasp |
| wrist_yaw_overload | Reset commanded position to prevent thermal overload during pushing |