In this tutorial we will discuss the simple collision avoidance system that runs as a part of Stretch Body.
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 in order to prevent self-collisions.
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.
**NOTE**: Self collisions are still possible while using the collision-avoidance system. The factory default collision models are coarse and not necessarily complete.
!!! 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:
@ -15,7 +16,7 @@ This system is turned off by default starting with Stretch 2. It may be turned o
If it is turned off you can enable it by adding the following to your stretch_user_yaml.py
If it is turned off you can enable it by adding the following to your stretch_user_yaml.py:
```bash
robot:
@ -24,24 +25,24 @@ robot:
## 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
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 DexWrist (if installed) and colliding with itself
* The DexWrist (if installed) and colliding with the base
* The DexWrist (if installed) colliding with itself
* The DexWrist (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 hardstop limits. For example, the lift has a mechanical throw of 1.1m:
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 hardstop limits. For example, the lift has a mechanical throw of 1.1m:
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 meter off the base:
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:
```python
import stretch_body.robot as robot
@ -50,9 +51,9 @@ r.startup()
r.lift.set_soft_motion_limit_min(0.3)
```
We see in the [API](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/lift.py), the value of `None` is used to designated no soft limit.
We see in the [API](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/lift.py), the value of `None` is used to designate no soft limit.
It is possible that when setting the Soft Motion Limit that the joint's current position is outside of the specified range. In this case, the joint will move to the nearest soft limit so as to comply with the limits. This can be demonstrated by:
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:
The [RobotCollision](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision.py) class manages a set of [RobotCollisionModels](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision.py). Each [RobotCollisionModel](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision.py) 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 the which collision models will execute when `use_collision_manager` is set to 1:
We can see which collision models will execute when `use_collision_manager` is set to 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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision.py) instance as a loadable plug-in. The [Robot](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot.py) 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 , set_soft_motion_limt_max` methods.
`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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision_models.py). As of this writing, the provide models are:
The default collision models for Stretch Body are found in [robot_collision_models.py](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision_models.py). As of this writing, the provided models are:
* [CollisionArmCamera](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision_models.py#L8): Avoid collision of the head camera with the arm
* [CollisionStretchGripper](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision_models.py#L75): Avoid collision of the wrist-yaw and gripper with the base and ground
**NOTE**: 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.
!!! 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.
**NOTE**: Additional collision models are provided for the DexWrist
!!! 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
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:
It can be straightforward to create your own custom collision model. As an example, we will create a model that avoids collision of the arm with a tabletop by
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
* Prevent the lift from descending below the table top when the arm is extended
* Allow the lift to descend below the tabletop so long as the arm retracted
* 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:
This assumes the arm is initially above the tabletop. To start, in a file `collision_arm_table.py` we add:
```python
from stretch_body.robot_collision import *
@ -211,16 +214,15 @@ class CollisionArmTable(RobotCollisionModel):
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.
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 in order to add the working directory to the PYTHONPATH env , This can also be added to our bashrc to permanently edit the path:
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:
You should see that the arm stops on contact when it extends, however it doesn't stop on contact when it then retracts. This is the guarded contact behavior in action.
You should see that the arm stops on contact when it extends, however, it doesn't stop on contact when it then retracts. This is the guarded contact behavior in action.
## Specifying Guarded Contacts
@ -49,14 +51,15 @@ The four stepper joints (base, arm, and lift) all support guarded contact settin
In this method you can optionally specify a contact threshold in the positive direction (`contact_thresh_pos`) and the negative direction `contact_thresh_neg`.
In this method, you can optionally specify a contact threshold in the positive and negative direction with `contact_thresh_pos` and `contact_thresh_neg` respectively.
**NOTE**: these optional parameters will default to `None`, in which case the motion will adopt the default settings as defined the robot's parameters
!!! note
These optional parameters will default to `None`, in which case the motion will adopt the default settings as defined by the robot's parameters.
**NOTE**: The parameters `contact_thresh_pos_N` and `contact_thresh_neg_N` are deprecate and no-longer supported.
!!! warning
The parameters `contact_thresh_pos_N` and `contact_thresh_neg_N` are deprecated and no longer supported.
A contact model is simply a function that, given a userspecified contact threshold, computes the motor current at which the motor controller will trigger a guarded contact. The following contact models are currently implemented:
A contact model is simply a function that, given a user-specified contact threshold, computes the motor current at which the motor controller will trigger a guarded contact. The following contact models are currently implemented:
### The Effort-Pct Contact Model
@ -77,13 +80,11 @@ A contact model is simply a function that, given a user specified contact thresh
robot.arm.move_by(0.1,contact_thresh_pos=50.0)
```
## Adjusting Contact Behaviors
The default factory settings for contact thresholds are tuned to allow Stretch to move throughout its workspace without triggering false-positive guarded contact events. These settings are the worstcase tuning as they account for the internal disturbances of the Stretch drive-train across its entire workspace.
The default factory settings for contact thresholds are tuned to allow Stretch to move throughout its workspace without triggering false-positive guarded contact events. These settings are the worst-case tuning as they account for the internal disturbances of the Stretch drive-train across its entire workspace.
It is possible to obtain greater contact sensitivity in carefully selected portions of the arm and lift workspace. Users who wish to programmatically adjust contact behaviors can create a simple test script and experiment with different values. For example:
It is possible to obtain greater contact sensitivity in carefully selected portions of the arm and lift workspace. Users who wish to programmatically adjust contact behaviors can create a simple test script and experiment with different values. For example:
```python
#!/usr/bin/env python
@ -109,7 +110,7 @@ robot.stop()
## Guarded Contact with the Base
Guarded contacts are enable by default for the arm and lift as they typically require safe and contact sensitive motion. They are turned off on the base by default as varying surface terrain can product undesired false-positive events.
Guarded contacts are enabled by default for the arm and lift as they typically require safe and contact-sensitive motion. They are turned off on the base by default as varying surface terrain can produce undesired false-positive events.
That being said, guarded contacts can be enabled on the base. They may be useful as a simple bump detector such that the base will stop when it runs into a wall.
The Stretch Factory package provides a tool to allow advanced users to recalibrate the default guarded contact thresholds. This tool can be useful if you've added additional payload to the arm and are experiencing false-positive guarded contact detections.
The tool sweeps the joint through its range-of-motion for `ncycle` iterations. It computes the maximum contact forces in both directions, adds a padding (`contact_thresh_calibration_margin`) to this value, and stores it to the robots Configuration yaml.
The tool sweeps the joint through its range of motion for `n-cycle` iterations. It computes the maximum contact forces in both directions, adds padding, `contact_thresh_calibration_margin`, to this value, and stores it to the robot's configuration YAML.
```bash
>>$ REx_calibrate_guarded_contact.py -h
@ -156,12 +155,8 @@ optional arguments:
-h, --help show this help message and exit
--lift Calibrate the lift joint
--arm Calibrate the arm joint
--ncycle NCYCLE Number of sweeps to run [4]
--ncycle NCYCLE Number of sweeps to run [4]
```
------
<divalign="center"> All materials are Copyright 2022 by Hello Robot Inc. Hello Robot and Stretch are registered trademarks.</div>
In this tutorial we explore how to add additional degrees of freedom to the Stretch wrist.
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](https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/) for the [WristYaw](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py) and the [StretchGripper](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/stretch_gripper.py) degrees of freedom that come standard with the robot.
Stretch exposes a Dynamixel X-Series TTL control bus at the end of its arm. It uses the [Dynamixel XL430-W250](https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/) for the [WristYaw](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py) and the [StretchGripper](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/stretch_gripper.py) that comes standard with the robot.
See the [Hardware User Guide](https://docs.hello-robot.com/0.2/stretch-hardware-guides/docs/hardware_guide_re2/#wrist-tool-plate) to learn how to mechanically attach additional DOFs to the robot.
**Note: Stretch is compatible with [any Dynamixel X Series servo](https://emanual.robotis.com/docs/en/dxl/x/) that utilizes the TTL level Multidrop Bus.**
!!! note
Stretch is compatible with any [Dynamixel X Series servo](https://emanual.robotis.com/docs/en/dxl/x/) 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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py)
* Adding YAML parameters to `stretch_user_params.yaml`that configure the servo as desired
* 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](./custom_wrist_dof/my_wrist_pitch.py). Place the file somewhere on the $PYTHONPATH.
Let's create a new DOF called MyWristPitch in a file named [my_wrist_pitch.py](./custom_wrist_dof/my_wrist_pitch.py). Place the file somewhere on the $PYTHONPATH.
```python
from stretch_body.dynamixel_hello_XL430 import DynamixelHelloXL430
@ -33,7 +34,7 @@ class MyWristPitch(DynamixelHelloXL430):
self.move_to(self.poses[p],v_r,a_r)
```
Now let's add the tools' parameters to your `stretch_user_params.yaml`in order to configure this servo. You may want to adapt these parameters to your application but the nominal values [found here](./custom_wrist_dof/stretch_user_params.yaml) usually work well. Below we highlight some of the more useful parameters.
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](./custom_wrist_dof/stretch_user_params.yaml) usually work well. Below we highlight some of the more useful parameters.
```yaml
my_wrist_pitch:
@ -46,9 +47,9 @@ my_wrist_pitch:
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 hardstop based homing. We also assume the servo has the Robotis default ID of 1.
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
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
```python
In [1]: import my_wrist_pitch
@ -64,7 +65,7 @@ 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](./custom_wrist_dof/stretch_user_params.yaml) to your `stretch_user_params.yaml`
Finally, you'll want to make your WristPitch available from `stretch_body.robot`. Add the following [YAML](./custom_wrist_dof/stretch_user_params.yaml) to your `stretch_user_params.yaml`
```yaml
end_of_arm:
@ -74,7 +75,7 @@ end_of_arm:
py_module_name: wrist_pitch
```
This tells `stretch_body.robot` to manage a wrist_`pitch.WristPitch`instance and add it to the [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) list of tools. Try it from iPython:
This tells `stretch_body.robot` to manage a `wrist_pitch.WristPitch`instance and add it to the [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) list of tools. Try it from iPython:
Typically, users will interact with these devices through either the [Head](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/head.py) or [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) interfaces. This tutorial is for users looking to work directly with the servos from the provided servo tools or through Stretch Body's lowlevel Dynamixel API.
Typically, users will interact with these devices through either the [Head](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/head.py) or the [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) interfaces. This tutorial is for users looking to work directly with the servos from the provided servo tools or through Stretch Body's low-level Dynamixel API.
## Servo Tools
**NOTE**: The servo tools here are part of the [Stretch Factory package](https://github.com/hello-robot/stretch_factory) which is installed as a part of Stretch Body.
!!! note
The servo tools here are part of the [Stretch Factory package](https://github.com/hello-robot/stretch_factory) which is installed as a part of Stretch Body.
### Jogging the Servos
@ -49,7 +49,7 @@ e: enable torque
### Rebooting the Servos
Under high-load conditions the servos may enter an error state to protect themselves from thermal overload. In this case, the red LED on the servo will flash (if visible). In addition, the servo will be unresponsive to motion commands. In this case, allow the overheating servo to cool down and reboot the servos using the `stretch_robot_dynamixel_reboot.py` tool:
Under high-load conditions, the servos may enter an error state to protect themselves from thermal overload. In this case, the red LED on the servo will flash (if visible). In addition, the servo will be unresponsive to motion commands. In this case, allow the overheating servo to cool down and reboot the servos using the `stretch_robot_dynamixel_reboot.py` tool:
```bash
$ stretch_robot_dynamixel_reboot.py
@ -65,10 +65,10 @@ For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.
### Identify Servos on the Bus
If it is unclear which servos are on the bus, and at what baud rate, you can use the `REx_dynamixel_id_scan.py` tool. Here we see that the two head servos are at ID 11 and 12 at baud 57600.
If it is unclear which servos are on the bus, and at what baud rate, you can use the `REx_dynamixel_id_scan.py` tool. Here we see that the two head servos are at ID `11` and `12` at baud `57600`.
@ -100,7 +100,7 @@ Scanning bus /dev/hello-dynamixel-head at baud rate 57600
### Setting the Servo Baud Rate
Stretch ships with its Dynamixels servos configured to baudrate=115200. When adding your own servos to the end-of-arm tool, you may want to set the servo baud using the `RE1_dynamixel_set_baud.py` tool. For example:
Stretch ships with its Dynamixel servos configured to `baudrate=115200`. When adding your servos to the end-of-arm tool, you may want to set the servo baud using the `REx_dynamixel_set_baud.py` tool. For example:
@ -110,13 +110,16 @@ Checking servo current baud for 57600
Identified current baud of 57600. Changing baud to 115200
Success at changing baud
```
**Note**: Earlier units of Stretch RE1 may be running Dynamixel servos at baud 57600
!!! note
Earlier units of Stretch RE1 may be running Dynamixel servos at baud 57600.
### Setting the Servo ID
Dynamixel servos come with ID=1 from the factory. When adding your own servos to the end-of-arm tool, you may want to set the servo ID using the `REx_dynamixel_id_change.py` tool. For example:
Dynamixel servos come with `ID=1` from the factory. When adding your servos to the end-of-arm tool, you may want to set the servo ID using the `REx_dynamixel_id_change.py` tool. For example:
**NOTE**: The naming of XL430 is for legacy reasons. These classes will work with all X Series servos.
!!! note
The naming of XL430 is for legacy reasons. These classes will work with all X Series servos.
### DynamixelXChain
DynamixelXChain manages a set of daisy-chained servos on a single bus (for example the head_pan and head_tilt servos). It allows for greater communication bandwidth by doing group read/write over USB.
DynamixelXChain manages a set of daisy-chained servos on a single bus (for example the head_pan and head_tilt servos). It allows for greater communication bandwidth by doing a group read/write over USB.
The [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) class derives from DynamixelXChain in order to provide an extensible interface that supports a user integrating additional DOF to the robot. The tutorial [Adding Custom Wrist DOF](./tutorial_custom_wrist_dof.md) explains how to do this.
The [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) class derives from DynamixelXChain to provide an extensible interface that supports a user in integrating additional degrees of freedom to the robot. The tutorial [Adding Custom Wrist DoF](./tutorial_custom_wrist_dof.md) explains how to do this.
### DynamixelHelloXL430
DynamixelHelloXL430 provides an interface to servo motion that is consistent with the Stretch Body lift, arm, and base joints. It also manages the servo parameters and calibration. Let's explore this interface further. From iPython let's look at the status message for DynamixelHelloXL430
DynamixelHelloXL430 provides an interface to servo motion that is consistent with the Stretch Body lift, arm, and base joints. It also manages the servo parameters and calibration. Let's explore this interface further. From iPython, let's look at the status message for DynamixelHelloXL430
```bash
import stretch_body.dynamixel_hello_XL430
@ -193,7 +197,7 @@ In addition to `move_to` and `move_by`, the class also implements a splined traj
### DynamixelXL430
DynamixelXL430 provides a thin wrapper to the [Robotis Dynamixel SDK](http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#control-table). You may chose to interact with the servo at this level as well. For example to jog the head_pan 200 ticks:
DynamixelXL430 provides a thin wrapper to the [Robotis Dynamixel SDK](http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#control-table). You may choose to interact with the servo at this level as well. For example, to jog the head_pan 200 ticks:
```python
import stretch_body.dynamixel_XL430
import time
@ -208,7 +212,5 @@ time.sleep(2.0)
m.stop()
```
------
<divalign="center"> All materials are Copyright 2022 by Hello Robot Inc. Hello Robot and Stretch are registered trademarks.</div>
In this tutorial we will discuss how parameters are managed in Stretch Body and show examples of how to customize your robot by overriding parameters.
In this tutorial, we will discuss how parameters are managed in Stretch Body and show examples of how to customize your robot by overriding parameters.
## Overview
Stretch Body shares a global set of parameters across all of the hardware it manages. All members of the [Device class](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/device.py) have an instance of [RobotParams](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_params.py). This class constructs a dictionary of the device parameters and well as the global parameters for each device. For example from iPython try:
Stretch Body shares a global set of parameters across all of the hardware it manages. All members of the [Device class](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/device.py) have an instance of [RobotParams](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_params.py). This class constructs a dictionary of the device parameters as well as the global parameters for each device. For example, from iPython try:
```python
import stretch_body.arm
@ -31,7 +31,7 @@ Out[7]:
'range_m': [0.0, 0.52]}
```
Or to access another device params:
or to access another device params:
```python
a.robot_params['lift']
@ -64,10 +64,10 @@ Stretch Body utilizes a prioritized parameter organization such that default set
| 1 | user_params | $HELLO_FLEET_PATH/$HELLO_FLEET_ID/ stretch_user_params.yaml | Yaml file for users to override default settings and to define custom configurations. |
| 2 | configuration_params | $HELLO_FLEET_PATH/$HELLO_FLEET_ID/ stretch_configuration_params.yaml | Robot specific data (eg, serial numbers and calibrations). Calibration tools may update these. |
| 3 | external_params | Imported via a list defined as `params` in stretch_user_params.yaml | External Python parameter dictionaries for 3rd party devices and peripherals. |
| 4 | nominal_params | stretch_body.robot_params_RE2V0.py | Generic systems settings (Common across all robots of a given model. |
| 5 | nominal_system_params | stretch_body.robot_params.py | Generic systems settings (Common across all robots models). |
| 4 | nominal_params | stretch_body.robot_params_RE2V0.py | Generic systems settings (common across all robots of a given model. |
| 5 | nominal_system_params | stretch_body.robot_params.py | Generic systems settings (common across all robot models). |
This allows the user to override any of the parameters by defining it in their `stretch_user_params.yaml`. It also allows Hello Robot to periodically update parameters defined in the Python files via Pip upadates.
This allows the user to override any of the parameters by defining it in their `stretch_user_params.yaml`. It also allows Hello Robot to periodically update parameters defined in the Python files via Pip updates.
The tool `stretch_params.py` will print out all of the robot parameters as well as their origin. For example:
The factory parameter settings should suffice for most use cases.
!!! note
The factory parameter settings should suffice for most use cases.
## Programmatically Modifying and Storing Parameters
A user want to compute the value of a parameter programmatically and modify the robot settings accordingly. For example, in the Stretch Factory tool [REx_base_calibrate_wheel_seperation.py](https://github.com/hello-robot/stretch_factory/blob/master/python/tools/REx_base_calibrate_wheel_separation.py) we see that the parameter `wheel_seperation_m` is recomputed as the variable `d_avg`. This new value could be used during the robot execution by simply
A user can compute the value of a parameter programmatically and modify the robot settings accordingly. For example, in the Stretch Factory tool [REx_base_calibrate_wheel_seperation.py](https://github.com/hello-robot/stretch_factory/blob/master/python/tools/REx_base_calibrate_wheel_separation.py) we see that the parameter `wheel_seperation_m` is recomputed as the variable `d_avg`. This new value could be used during the robot execution by simply:
As we've seen in previous tutorials, commanding robot motion can be simple and straight forward. For example, incremental motion of the arm can be commanded by:
As we've seen in previous tutorials, commanding robot motion is simple and straightforward. For example, the incremental motion of the arm can be commanded by:
```python linenums="1"
import stretch_body.robot
@ -14,7 +14,7 @@ time.sleep(2.0)
robot.stop()
```
Or, absolute motion can be commanded by:
The absolute motion can be commanded by:
```python linenums="1"
import stretch_body.robot
@ -30,9 +30,7 @@ robot.stop()
## Waiting on Motion
In the above examples we execute a `time.sleep()` after `robot.push_command()`. This allows the joint time to complete its motion. Instead we can use the `wait_until_at_setpoint()` method that polls the joint position versus the target position. We can also interrupt a motion by sending a new motion command at anytime. For example, try the following script:
In the above examples, we executed a `time.sleep()` after `robot.push_command()`. This allows the joint time to complete its motion. As an alternative, we can use the `wait_until_at_setpoint()` method that polls the joint position versus the target position. We can also interrupt a motion by sending a new motion command at any time. For example, try the following script:
```python linenums="1"
import stretch_body.robot
@ -60,13 +58,13 @@ You will see the arm fully retract, begin to extend, and then fully retract agai
## Motion Profiles
All joints support [trapezoidal based motion](https://www.motioncontroltips.com/what-is-a-motion-profile/) generation. Other types of controllers are available (splined trajectory, PID, velocity, etc) but they are not covered here. The trapezoidal motion controllers require three values:
All joints support [trapezoidal motion profile](https://www.motioncontroltips.com/what-is-a-motion-profile/) generation. Other types of controllers are available (splined trajectory, 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 queried using the `stretch_params.py` tool:
We provide 'defaults' for the velocity and acceleration settings, as well as 'fast', and 'slow' settings. These values have been tuned to be appropriate for the safe movement of the robot. These values can be queried using the `stretch_params.py` tool:
```bash
>>$stretch_params.py | grep arm | grep motion | grep default
@ -98,7 +96,7 @@ All joints obey motion limits which are specified in the robot parameters.
These are the mechanical limits of the joint. 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 factoryspecified values. However, they can be further limited if desired by setting soft motion limits:
These are the mechanical limits of the joints and 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 by setting soft motion limits:
The above examples have focused on the motion of the arm. Like the lift and the base, the arm utilizes Hello Robot's custom stepper motor controller. Control of the Dynamixels of the head and the end-of-arm is very similar to that of the arm (though not identical).
As we see here, the `robot.push_command` call is not required as the motion begins instantaneously and is not queued. In addition, the Dynamixel servos are managed as a chain of devices -- so we must pass in the joint name along with the command.
The above examples have focused on the motion of the arm. Like the lift and the base, the arm utilizes Hello Robot's custom stepper motor controller. Control of the Dynamixels of the head and the end-of-arm is very similar to that of the arm, though not identical.
As we see here, the `robot.push_command()` call is not required as the motion begins instantaneously and is not queued. In addition, the Dynamixel servos are managed as a chain of devices, so we must pass in the joint name along with the command.
```python
import stretch_body.robot
@ -151,7 +145,7 @@ time.sleep(3.0)
robot.stop()
```
Similarly to the stepper joints, the Dynamixel joints accept motion profile and motion limit commands. For example, here we restrict the head pan range of motion while executing both a fast and slow move:
Similar to the stepper joints, the Dynamixel joints accept motion profile and motion limit commands. For example, here we restrict the head pan range of motion while executing both a fast and slow move:
```python
import stretch_body.robot
@ -179,11 +173,9 @@ time.sleep(3.0)
robot.stop()
```
## Base Velocity Control
The Base also supports a velocity control mode which can be useful for use with navigation planner. The Base controllers will automatically switch between velocity and position based control. For example:
The Base also supports a velocity control mode which can be useful with navigation planners. The Base controllers will automatically switch between velocity and position control. For example:
As shown, care should be taken to set commanded velocities to zero on exit to avoid runaway.
!!! warning
As shown, care should be taken to set commanded velocities to zero on exit to avoid runaway.
## Advanced Topics
### Stepper Control Modes
Most users will control robot motion using the `move_to` and `move_by` commands as described above. There are numerous other low-level controller modes available. While these are a topic for advanced users, it is worth noting that each joint has a default safety mode and a default position control mode. These are:
Most users will control robot motion using the `move_to` and `move_by` commands as described above. However, there are numerous other low-level controller modes available. While this is a topic for advanced users, it is worth noting that each joint has a default safety mode and a default position control mode. These are:
| Joint | Default Safety Mode | Default Position Control Mode |
@ -217,11 +209,11 @@ Most users will control robot motion using the `move_to` and `move_by` commands
| wrist_yaw | Torque disabled | Trapezoidal position control |
| stretch_gripper | Torque disabled | Trapezoidal position control |
Each joint remains in Safety Mode when no program is running. When the `<device>.startup()` function is called, the joint controller transitions from Safety Mode to its Default Position Control Mode. It is then placed back in Safety Mode when `<device>.stop()` is called.
Each joint remains in its `Safety Mode` when no program is running. When the `<device>.startup()` function is called, the joint controller transitions from `Safety Mode` to its `Default Position Control Mode`. It is then placed back in `Safety Mode` when `<device>.stop()` is called.
### Motion Runstop
Runstop activation will cause the Base, Arm, and Lift to switch to Safety Mode and for subsequent 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. For example:
Runstop activation will cause the Base, Arm, and Lift to switch to `Safety Mode` and subsequent 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. For example:
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.
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 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
* Make a guarded motion where the robot reaches a surface and then stops
For more information on guarded motion, see the [Contact Models Tutorial](./tutorial_contact_models.md)
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:
It can be useful to poll the status of a joint during a motion to modify the robot's behavior, etc. The useful status values include:
```python
robot.arm.status['pos'] #Joint position
@ -306,11 +295,14 @@ The following update rates apply to Stretch:
| 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.
!!! note
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 control, and the USB based interface limits high rate communication.
!!! info
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 control, 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.
!!! tip
In practice, a Python-based control loop that calls push_command() at 1Hz to 10Hz is sufficiently matched to the robot's natural dynamics.
------
<divalign="center"> All materials are Copyright 2022 by Hello Robot Inc. Hello Robot and Stretch are registered trademarks.</div>
Stretch Body exposes a host of sensor data through the status dictionaries of its devices. In this tutorial we'll cover how to access, view, and configure this sensor data.
Stretch Body exposes a host of sensor data through the status dictionaries of its devices. In this tutorial, we'll cover how to access, view, and configure this sensor data.
## Tools to View Sensor Data
There are two useful tools for scoping Pimu and Wacc sensor data in real-time:
There are two useful tools for scoping Pimu and Wacc sensor data in real-time:
The base has a 9 DOF IMU using the 9 DOF FXOS8700 + FXAS21002 chipset. This is the same chipset as used on the [Adafruit NXP IMU board](https://www.adafruit.com/product/3463).
The base has a 9-DoF IMU using the 9-DoF FXOS8700 + FXAS21002 chipset. This is the same chipset used on the [Adafruit NXP IMU board](https://www.adafruit.com/product/3463).
The [Pimu](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/pimu.py) reports back the IMU sensor readings in its [IMU status dictionary](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/pimu.py#L27). For example, from iPython try:
@ -200,7 +200,7 @@ It reports:
These values are computed on the Pimu. As we can see in [its firmware code](https://github.com/hello-robot/stretch_firmware/blob/master/arduino/hello_pimu/IMU.cpp), a 100Hz Madgwick filter is used to compute the orientation.
Stretch Body also implements a bump detector using the the IMU accelerometers. This detector simply [computes the sum-of-squares of AX, AY, and AZ](https://github.com/hello-robot/stretch_firmware/blob/master/arduino/hello_pimu/IMU.cpp#L223). This value is then compared to the following threshold to determine if a bump is present:
Stretch Body also implements a bump detector using the IMU accelerometers. This detector simply [computes the sum of squares of AX, AY, and AZ](https://github.com/hello-robot/stretch_firmware/blob/master/arduino/hello_pimu/IMU.cpp#L223). This value is then compared to the following threshold to determine if a bump is detected:
**NOTE**: The IMU is calibrated by Hello Robot at the factory. Please contact Hello Robot support for details on recalibrating your IMU.
!!! note
The IMU is calibrated by Hello Robot at the factory. Please contact Hello Robot support for details on recalibrating your IMU.
## Wrist Accelerometer
The wrist includes a 3 axis [ADXL343](https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL343.pdf) accelerometer which provides bump and tap detection capabilities. The Wacc reports back AX, AY, and AZ [in its status dictionary](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wacc.py#L44) From iPython try:
The wrist includes a 3 axis [ADXL343](https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL343.pdf) accelerometer which provides bump and tap detection capabilities. The Wacc reports back AX, AY, and AZ [in its status dictionary](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wacc.py#L44). For example, from iPython try:
In addition to AX, AY, and AZ we also see the `single_tap_count` value which reports back a count of the number of single-tap contacts the accelerometer has experiences since power-up.
In addition to AX, AY, and AZ we also see the `single_tap_count` value which reports back a count of the number of single-tap contacts the accelerometer has experienced since power-up.
The following Wacc parameters configure the accelerometer low-pass filter and single-tap settings. See the [ADXL343](https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL343.pdf) datasheet for more details.
The following Wacc parameters configure the accelerometer low-pass filter and single-tap settings. See the [ADXL343](https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL343.pdf) datasheet for more details.
Stretch has four Sharp GP2Y0A51SK0F IR cliff sensors pointed towards the floor. These report the distance to the floor, allowing for detection of thresholds, stair edges, etc.
Stretch has four Sharp GP2Y0A51SK0F IR cliff sensors pointed toward the floor. These report the distance to the floor, allowing for the detection of thresholds, stair edges, etc.
Relevant parameters for the cliff sensors are:
@ -325,9 +324,10 @@ Calibration passed. Storing to YAML...
The `stop_at_cliff` field causes the robot to execute a Runstop when the cliff sensor readings exceed the value `cliff_thresh`. The parameter `cliff_LPF` defines the low-pass-filter rate (Hz) on the analog sensor readings.
**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.**
!!! 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 range values from the sensors can be read from the `robot.pimu.status` message. Relevant fields are:
The range values from the sensors can be read from the `robot.pimu.status` message. The relevant fields are:
```python
import stretch_body.robot
@ -345,7 +345,7 @@ Out[5]: False
```
The `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_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()` to reset the generated Runstop.
The cliff detection logic can be found in the [Pimu firmware](https://github.com/hello-robot/stretch_firmware/blob/master/arduino/hello_pimu/Pimu.cpp).
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 user parameters.
Stretch includes several built-in functions that help it maintain safe operating conditions. These functions can be disabled and enabled via the robot user parameters.
## Logging
@ -15,7 +15,7 @@ robot:
## Runstop Functions
The runstop deactivates all robot motion. It can be triggered by the physical button on the robot's head. It can also be triggered by internal monitors of the system state. The default configuration of these parameters is:
The Runstop deactivates all robot motion. It can be triggered by the physical button on the robot's head. It can also be triggered by internal monitors of the system state. The default configuration of these parameters is:
The [Pimu firmware](https://github.com/hello-robot/stretch_firmware/tree/master/arduino/hello_pimu) details the implementation of these functions.
**NOTE**: The `stop_at_cliff` and `stop_at_tilt` functions are disabled by default as they are not robust to normal operating conditions of the robot. Therefore do not rely on these functions for robot safety.
!!! warning
The `stop_at_cliff` and `stop_at_tilt` functions are disabled by default as they are not robust to the normal operating conditions of the robot. Therefore do not rely on these functions for robot safety.
## Robot Monitor
The [Robot Monitor](https://github.com/hello-robot/stretch_body/blob/master/python/stretch_body/robot_monitor.py) 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. T
The [Robot Monitor](https://github.com/hello-robot/stretch_body/blob/master/python/stretch_body/robot_monitor.py) 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.
The default parameters associated with RobotMonitor are:
@ -78,7 +79,7 @@ robot:
log_to_console: 1
```
The run the tool and hit the runstop button, then hold it down for 2 seconds:
Then run the tool and hit the Runstop button, and then hold it down for 2 seconds:
```bash
>>$ stretch_robot_monitor.py
@ -91,11 +92,9 @@ Starting Robot Monitor. Ctrl-C to exit
[INFO] [robot_monitor]: Runstop deactivated
```
## Robot Sentry
The [Robot Sentry](https://github.com/hello-robot/stretch_body/blob/master/python/stretch_body/robot_sentry.py) 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.
The [Robot Sentry](https://github.com/hello-robot/stretch_body/blob/master/python/stretch_body/robot_sentry.py) is a thread that can override and also generate commands to the robot hardware. Its 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 to reduce the chance of toppling. The Robot Sentry reports events to the log file as well.
@ -4,12 +4,11 @@ Stretch Body supports splined trajectory controllers across all of its joints. T
## What are Splined Trajectories?
A splined trajectory is a smooth path that a robot joint follows over a specific period of time. [Cubic or quintic splines](https://en.wikipedia.org/wiki/Spline_(mathematics)) are used to represent the trajectory. As shown below, the splines (blue) are defined by a series of userprovided waypoints (black dot). A waypoint is simply a target position, velocity, and optional acceleration at a given time. The spline ensures continuity and smoothness when interpolating between the waypoint targets.
A splined trajectory is a smooth path that a robot joint follows over a specific period of time. [Cubic or quintic splines](https://en.wikipedia.org/wiki/Spline_(mathematics)) are used to represent the trajectory. As shown below, the splines (blue) are defined by a series of user-provided waypoints (black dot). A waypoint is simply a target position, velocity, and optional acceleration at a given time. The spline ensures continuity and smoothness when interpolating between the waypoint targets.
During execution, the trajectory controller uses this splined representation to compute the instantaneous desired position, velocity, and acceleration of the joint (red). On Stretch, this instantaneous target is then passed to a lower-level position or velocity controller.
Splined trajectories are particularly useful when you want to coordinate motion across several joints. Because the trajectory representation is time based, it is straightforward to encode multi-joint coordination. Stretch Body supports both cubic and quintic spline. A quintic spline waypoint includes acceleration in the waypoint target, while a cubic spline does not.
Splined trajectories are particularly useful when you want to coordinate motion across several joints. Because the trajectory representation is time-based, it is straightforward to encode multi-joint coordination. Stretch Body supports both cubic and quintic splines. A quintic spline waypoint includes acceleration in the waypoint target, while a cubic spline does not.
@ -44,7 +43,8 @@ The tool GUI allows you to interactively construct a splined trajectory and then
**NOTE**: Use caution when commanding the base. Ensure that attached cables are long enough to support base motion. Alternatively you may want to put the base on top of a book so the wheel don't touch the ground.
!!! note
Use caution when commanding the base. Ensure that the attached cables are long enough to support the base motion. Alternatively, you may want to put the base on top of a book so the wheels don't touch the ground.
Finally, you can explore a full-body trajectory using the non-GUI version of the tool:
@ -52,11 +52,9 @@ Finally, you can explore a full-body trajectory using the non-GUI version of the
>>$ stretch_trajectory_jog.py --full_body
```
## Programming Trajectories
Programming a splined trajectory is straightforward. Try the following from iPython:
Programming a splined trajectory is straightforward. For example, try the following from iPython:
```python
import stretch_body.robot
@ -88,22 +86,21 @@ This will cause the arm to move from its current position to 0.45m, then back to
* This will execute a Cubic spline as we did not pass in accelerations to in `r.arm.trajectory.add`
* The call to `r.arm.follow_trajectory` is non-blocking and the trajectory generation is handled by a background thread of the Robot class
If you're interested in exploring the trajectory API further the [code for the `stretch_trajectory_jog.py`](https://github.com/hello-robot/stretch_body/blob/master/tools/bin/stretch_trajectory_jog.py)is a great reference to get started.
If you're interested in exploring the trajectory API further the [code for the `stretch_trajectory_jog.py`](https://github.com/hello-robot/stretch_body/blob/master/tools/bin/stretch_trajectory_jog.py)is a great reference to get started.
## Advanced: Controller Parameters
Sometimes the robot motion isn't quite what is expected when executing a splined trajectories. It is important that the trajectory be well-formed, meaning that it:
Sometimes the robot's motion isn't quite what is expected when executing a splined trajectory. It is important that the trajectory be well-formed, meaning that it:
* Respects the maximum velocity and accelerations limits of the joint
* Doesn't create a large 'excursion' outside of the acceptable range of motion in order to hit a target waypoint
* Doesn't create a large 'excursion' outside of the acceptable range of motion to hit a target waypoint
* Doesn't have waypoints so closely spaced together that it exceeds the nominal control rates of Stretch (~10-20 Hz)
For example, the arm trajectory below has a large excursion outside of the joints range of motion (white). This is because the second waypoint expects a non-zero velocity when the arm reaches full extension.
For example, the arm trajectory below has a large excursion outside of the joint's range of motion (white). This is because the second waypoint expects a non-zero velocity when the arm reaches full extension.
Often the trajectory waypoints will be generated from a motion planner. It is important that this planner incorporates the position, velocity, and acceleration constraints of the joint. These can be found by, for example
Often the trajectory waypoints will be generated from a motion planner. It is important for the planner to incorporate the position, velocity, and acceleration constraints of the joint. These can be found by, for example:
Fortunately the Stretch Body [Trajectory](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/trajectories.py) classes do some preliminary feasibility checking of trajectories using the [is_segment_feasible function](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/hello_utils.py#L290). This checks if the generated motions lie within the constraints of the `trajectory_max` parameters.
Fortunately, the Stretch Body [Trajectory](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/trajectories.py) classes do some preliminary feasibility checking of trajectories using the [is_segment_feasible function](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/hello_utils.py#L290). This checks if the generated motions lie within the constraints of the `trajectory_max` parameters.
It is generally important for the waypoints to be spaced far apart. Stretch isn't a dynamic and fast-moving robot, so there isn't a practical advantage to closely spaced waypoints at any rate.
It is generally important for the waypoints to be spaced far apart. Stretch isn't a dynamic and fast moving robot, so there isn't a practical advantage to closely spaced waypoints at any rate.
The stepper controllers (arm, lift, and base) can be updated at approximately 20 Hz maximum. Therefore, if your waypoints are spaced 50 ms apart, you run the risk of overflowing the stepper controller. Likewise, the Dynamixel joints can be updated at approximately 12 Hz.
The stepper controllers (arm, lift, and base) can be updated at approximately 20 Hz maximum. Therefore, if your waypoints are spaced 50 ms apart, you run the risk of overflowing the stepper controller. Likewise, the Dynamixel joints can be updated at approximately 12 Hz. As a rule of thumb, spacing the waypoints over 100 ms apart is a good idea.
!!! tip
As a rule of thumb, spacing the waypoints over 100 ms apart is a good idea.
------
<divalign="center"> All materials are Copyright 2022 by Hello Robot Inc. Hello Robot and Stretch are registered trademarks.</div>
@ -5,7 +5,7 @@ See the [Stretch Body Tutorials](https://docs.hello-robot.com/0.2/stretch-tutori
## The Robot Class
The most common interface to Stretch is the [Robot](#stretch_body.robot.Robot) class. It is typically initialized as:
The most common interface to Stretch is the [Robot](#stretch_body.robot.Robot) class. This class encapsulates all devices on the robot. It is typically initialized as:
```python linenums='1'
import stretch_body.robot
@ -38,15 +38,17 @@ r.pimu.pretty_print()
r.stop()
```
Each of these devices is defined in separate modules within `stretch_body`. In the following section, we'll look at the API of these classes. The [`stop()`](#stretch_body.robot.Robot.stop) method shuts down communication with the robot's devices. All methods in the [Robot](#stretch_body.robot.Robot) class are documented below.
Each of these devices is defined in separate modules within `stretch_body`. In the following section, we'll look at the API of these classes. The [`stop()`](#stretch_body.robot.Robot.stop) method shuts down communication with the robot's devices.
All methods in the [Robot](#stretch_body.robot.Robot) class are documented below.
::: stretch_body.robot.Robot
## The Device Class
The `stretch_body` library is modular in design. Each subcomponent of Stretch is defined in its class and the [Robot class](#the-robot-class) provides an interface that ties all of these classes together. This modularity allows users to plug in new/modified subcomponents into the [Robot](#stretch_body.robot.Robot) interface by extending a Device class.
The `stretch_body` library is modular in design. Each subcomponent of Stretch is defined in its class and the [Robot class](#the-robot-class) provides an interface that ties all of these classes together. This modularity allows users to plug in new/modified subcomponents into the [Robot](#stretch_body.robot.Robot) interface by extending the Device class.
It is possible to interface with a single subcomponent of Stretch by initializing its device class directly. In this section, we'll look at the API of seven subclasses of the Device class: the [arm](#stretch_body.arm.Arm), [lift](#stretch_body.lift.Lift), [base](#stretch_body.base.Base), [head](#stretch_body.head.Head), [end of arm](#stretch_body.end_of_arm.EndOfArm), [wacc](#stretch_body.wacc.Wacc), and [pimu](#stretch_body.pimu.Pimu) subcomponents of Stretch.
It is possible to interface with a single subcomponent of Stretch by initializing its device class directly. In this section, we'll look at the API of seven subclasses of the Device class: the [Arm](#stretch_body.arm.Arm), [Lift](#stretch_body.lift.Lift), [Base](#stretch_body.base.Base), [Head](#stretch_body.head.Head), [EndOfArm](#stretch_body.end_of_arm.EndOfArm), [Wacc](#stretch_body.wacc.Wacc), and [Pimu](#stretch_body.pimu.Pimu) subcomponents of Stretch.
### Using the Arm class
@ -88,7 +90,9 @@ a.push_command()
a.motor.wait_until_at_setpoint()
```
The [`move_to()`](#stretch_body.arm.Arm.move_to) and [`move_by()`](#stretch_body.arm.Arm.move_by) methods queue absolute and relative position commands to the arm, respectively, while the nonblocking [`push_command()`](#stretch_body.arm.Arm.push_command) method pushes the queued position commands to the hardware for execution. The attribute `motor`, an instance of the [Stepper](#stretch_body.stepper.Stepper) class, has the method [`wait_until_at_setpoint()`](#stretch_body.stepper.Stepper.wait_until_at_setpoint) which blocks program execution until the joint reaches the commanded goal. With [P1 or greater firmware](https://github.com/hello-robot/stretch_firmware/blob/master/tutorials/docs/updating_firmware.md) installed, it is also possible to queue a waypoint trajectory for the arm to follow:
The [`move_to()`](#stretch_body.arm.Arm.move_to) and [`move_by()`](#stretch_body.arm.Arm.move_by) methods queue absolute and relative position commands to the arm, respectively, while the nonblocking [`push_command()`](#stretch_body.arm.Arm.push_command) method pushes the queued position commands to the hardware for execution.
The attribute `motor`, an instance of the [Stepper](#stretch_body.stepper.Stepper) class, has the method [`wait_until_at_setpoint()`](#stretch_body.stepper.Stepper.wait_until_at_setpoint) which blocks program execution until the joint reaches the commanded goal. With [firmware P1 or greater](https://github.com/hello-robot/stretch_firmware/blob/master/tutorials/docs/updating_firmware.md) installed, it is also possible to queue a waypoint trajectory for the arm to follow:
```python linenums='26'
starting_position = a.status['pos']
@ -104,7 +108,9 @@ a.follow_trajectory()
import time; time.sleep(9)
```
The attribute `trajectory`, an instance of the [PrismaticTrajectory](#stretch_body.trajectories.PrismaticTrajectory) class, has the method [`add()`](#stretch_body.trajectories.PrismaticTrajectory.add) which adds a single waypoint in a linear sliding trajectory. For a well formed `trajectory` (see [`is_valid()`](#stretch_body.trajectories.Spline.is_valid)), the [`follow_trajectory()`](#stretch_body.arm.Arm.follow_trajectory) method kicks off trajectory following for the telescoping arm. It is also possible to dynamically restrict the arm joint's range:
The attribute `trajectory`, an instance of the [PrismaticTrajectory](#stretch_body.trajectories.PrismaticTrajectory) class, has the method [`add()`](#stretch_body.trajectories.PrismaticTrajectory.add) which adds a single waypoint in a linear sliding trajectory. For a well-formed `trajectory` (see [`is_valid()`](#stretch_body.trajectories.Spline.is_valid)), the [`follow_trajectory()`](#stretch_body.arm.Arm.follow_trajectory) method starts tracking the trajectory for the telescoping arm.
It is also possible to dynamically restrict the arm joint range:
```python linenums='37'
range_upper_limit = 0.3 # meters
@ -123,7 +129,9 @@ print(a.status['pos']) # we should expect to see ~0.3
a.stop()
```
The [`set_soft_motion_limit_min/max()`](#stretch_body.arm.Arm.set_soft_motion_limit_min) methods form the basis of an experimental [self-collision avoidance](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision.py#L47) system built into Stretch Body. All methods in the [Arm class](#stretch_body.arm.Arm) are documented below.
The [`set_soft_motion_limit_min/max()`](#stretch_body.arm.Arm.set_soft_motion_limit_min) methods form the basis of an experimental [self-collision avoidance](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_collision.py#L47) system built into Stretch Body.
All methods in the [Arm class](#stretch_body.arm.Arm) are documented below.
::: stretch_body.arm.Arm
@ -147,7 +155,7 @@ l.home()
# interact with the lift here
```
The [`startup()`](#stretch_body.lift.Lift.startup) and [`home()`](#stretch_body.lift.Lift.home) methods are extended from the [Device](#stretch_body.device.Device) class. Reading the lift's current state and sending commands to the joint occurs similarly as in the [Arm](#stretch_body.arm.Arm) class:
The [`startup()`](#stretch_body.lift.Lift.startup) and [`home()`](#stretch_body.lift.Lift.home) methods are extended from the [Device](#stretch_body.device.Device) class. Reading the lift's current state and sending commands to the joint occurs similarly to the [Arm](#stretch_body.arm.Arm) class:
```python linenums='11'
starting_position = l.status['pos']
@ -158,7 +166,7 @@ l.push_command()
l.motor.wait_until_at_setpoint()
```
The attribute `status` is a dictionary of the joint's current status. This state information is updated in the background in realtime by default (disable by initializing as [`startup(threading=False)`](#stretch_body.lift.Lift.startup)). Use the [`pretty_print()`](#stretch_body.lift.Lift.pretty_print) method to print out this state info in a humaninterpretable format. Setting up waypoint trajectories for the lift is also similar to the [Arm](#stretch_body.arm.Arm):
The attribute `status` is a dictionary of the joint's current status. This state information is updated in the background in real-time by default (disable by initializing as [`startup(threading=False)`](#stretch_body.lift.Lift.startup)). Use the [`pretty_print()`](#stretch_body.lift.Lift.pretty_print) method to print out this state info in a human-interpretable format. Setting up waypoint trajectories for the lift is also similar to the [Arm](#stretch_body.arm.Arm):
```python linenums='17'
starting_position = l.status['pos']
@ -173,7 +181,9 @@ l.follow_trajectory()
import time; time.sleep(6)
```
The attribute `trajectory` is also an instance of the [PrismaticTrajectory](#stretch_body.trajectories.PrismaticTrajectory) class, and by providing the instantaneous velocity argument `v_m` to the [`add()`](#stretch_body.trajectories.PrismaticTrajectory.add) method, a cubic spline can be loaded into the joint's `trajectory`. The call to [`follow_trajectory()`](#stretch_body.lift.Lift.follow_trajectory) begins hardware tracking of the spline. Finally, setting soft motion limits for the lift's range can be done using:
The attribute `trajectory` is also an instance of the [PrismaticTrajectory](#stretch_body.trajectories.PrismaticTrajectory) class, and by providing the instantaneous velocity argument `v_m` to the [`add()`](#stretch_body.trajectories.PrismaticTrajectory.add) method, a cubic spline can be loaded into the joint's `trajectory`. The call to [`follow_trajectory()`](#stretch_body.lift.Lift.follow_trajectory) begins hardware tracking of the spline.
Finally, setting soft motion limits for the lift's range can be done using:
```python linenums='27'
# cut out 0.2m from the top and bottom of the lift's range
@ -184,7 +194,9 @@ l.push_command()
l.stop()
```
The [`set_soft_motion_limit_min/max()`](#stretch_body.lift.Lift.set_soft_motion_limit_min) methods perform clipping of the joint's range at the firmware level (can persist across reboots). All methods in the [Lift](#stretch_body.lift.Lift) class are documented below.
The [`set_soft_motion_limit_min/max()`](#stretch_body.lift.Lift.set_soft_motion_limit_min) methods perform clipping of the joint's range at the firmware level (can persist across reboots).
All methods in the [Lift](#stretch_body.lift.Lift) class are documented below.
::: stretch_body.lift.Lift
@ -213,7 +225,7 @@ if not b.startup():
# interact with the base here
```
Stretch's mobile base is a differential drive configuration. The left and right wheels are accessible through [Base](#stretch_body.base.Base) `left_wheel` and `right_wheel` attributes, both of which are instances of the [Stepper](#stretch_body.stepper.Stepper) class. The [`startup()`](#stretch_body.base.Base.startup) method is extended from the [Device](#stretch_body.device.Device) class. Since the mobile base is unconstrained, there is no homing method. We can read the base's current state and send commands using:
Stretch's mobile base is a differential drive configuration. The left and right wheels are accessible through [Base](#stretch_body.base.Base) `left_wheel` and `right_wheel` attributes, both of which are instances of the [Stepper](#stretch_body.stepper.Stepper) class. The [`startup()`](#stretch_body.base.Base.startup) method is extended from the [Device](#stretch_body.device.Device) class. Since the mobile base is unconstrained, there is no homing method. The [`pretty_print()`](#stretch_body.base.Base.pretty_print) method prints out mobile base state information in a human-interpretable format. We can read the base's current state and send commands using:
```python linenums='10'
b.pretty_print()
@ -229,7 +241,9 @@ b.push_command()
b.left_wheel.wait_until_at_setpoint()
```
The [`pretty_print()`](#stretch_body.base.Base.pretty_print) method prints out mobile base state information in a human interpretable format. The [`translate_by()`](#stretch_body.base.Base.translate_by) and [`rotate_by()`](#stretch_body.base.Base.rotate_by) methods send relative commands similar to the way [`move_by()`](#stretch_body.lift.Lift.move_by) behaves for the single degree of freedom joints. The mobile base also supports velocity control:
The [`translate_by()`](#stretch_body.base.Base.translate_by) and [`rotate_by()`](#stretch_body.base.Base.rotate_by) methods send relative commands similar to the way [`move_by()`](#stretch_body.lift.Lift.move_by) behaves for the single degree of freedom joints.
The mobile base also supports velocity control:
```python linenums='21'
# command the base to translate forward at 5cm / second
@ -252,7 +266,9 @@ b.enable_freewheel_mode()
b.push_command()
```
The [`set_translate_velocity()`](#stretch_body.base.Base.set_translate_velocity) and [`set_rotational_velocity()`](#stretch_body.base.Base.set_rotational_velocity) methods give velocity control over the translational and rotational components of the mobile base independently. The [`set_velocity()`](#stretch_body.base.Base.set_velocity) method gives control over both of these components simultaneously. To halt motion, you can command zero velocities or command the base into freewheel mode using [`enable_freewheel_mode()`](#stretch_body.base.Base.enable_freewheel_mode). The mobile base also supports waypoint trajectory following, but the waypoints are part of the SE2 group (a.k.a. each of the base's desired waypoints is defined as an (x, y) point and a theta orientation):
The [`set_translate_velocity()`](#stretch_body.base.Base.set_translate_velocity) and [`set_rotational_velocity()`](#stretch_body.base.Base.set_rotational_velocity) methods give velocity control over the translational and rotational components of the mobile base independently. The [`set_velocity()`](#stretch_body.base.Base.set_velocity) method gives control over both of these components simultaneously.
To halt motion, you can command zero velocities or command the base into freewheel mode using [`enable_freewheel_mode()`](#stretch_body.base.Base.enable_freewheel_mode). The mobile base also supports waypoint trajectory following, but the waypoints are part of the SE2 group, where a desired waypoint is defined as an (x, y) point and a theta orientation:
```python linenums='39'
# reset odometry calculation
@ -273,9 +289,11 @@ b.stop()
```
!!! warning
The [Base](#stretch_body.base.Base) waypoint trajectory following has no notion of obstacles in the environment. It will blindly follow the commanded waypoints. For obstacle avoidance, perception and a path planner should be employed.
The [Base](#stretch_body.base.Base) waypoint trajectory following has no notion of obstacles in the environment. It will blindly follow the commanded waypoints. For obstacle avoidance, we recommend employing perception and a path planner.
The attribute `trajectory` is an instance of the [DiffDriveTrajectory](#stretch_body.trajectories.DiffDriveTrajectory) class. The call to [`follow_trajectory()`](#stretch_body.base.Base.follow_trajectory) begins hardware tracking of the spline.
The attribute `trajectory` is an instance of the [DiffDriveTrajectory](#stretch_body.trajectories.DiffDriveTrajectory) class. The call to [`follow_trajectory()`](#stretch_body.base.Base.follow_trajectory) begins hardware tracking of the spline. All methods of the [Base](#stretch_body.base.Base) class are documented below.
All methods of the [Base](#stretch_body.base.Base) class are documented below.
::: stretch_body.base.Base
@ -284,7 +302,7 @@ The attribute `trajectory` is an instance of the [DiffDriveTrajectory](#stretch_
The interface to Stretch's head is the [Head](#stretch_body.head.Head) class. The head contains an Intel Realsense D435i depth camera. The pan/tilt joints in the head allow Stretch to swivel and capture depth imagery of its surrounding. The head is typically initialized as:
The interface to Stretch's head is the [Head](#stretch_body.head.Head) class. The head contains an Intel Realsense D435i depth camera. The pan and tilt joints in the head allow Stretch to swivel and capture depth imagery of its surrounding. The head is typically initialized as:
```python linenums='1'
import stretch_body.head
@ -298,7 +316,7 @@ h.home()
# interact with the head here
```
[Head](#stretch_body.head.Head) is a subclass of the [DynamixelXChain](#stretch_body.dynamixel_X_chain.DynamixelXChain) class, which in turn is a subclass of the [Device](#stretch_body.device.Device) class. Therefore, some of [Head's](#stretch_body.head.Head) methods, such as [`startup()`](#stretch_body.head.Head.startup) and [`home()`](#stretch_body.head.Head.home) methods are extended from the [Device](#stretch_body.device.Device) class, while others come from the [DynamixelXChain](#stretch_body.dynamixel_X_chain.DynamixelXChain) class. Reading the head's current state and sending commands to its revolute joints (head pan and tilt) can be achieved using:
[Head](#stretch_body.head.Head) is a subclass of the [DynamixelXChain](#stretch_body.dynamixel_X_chain.DynamixelXChain) class, which in turn is a subclass of the [Device](#stretch_body.device.Device) class. Therefore, some of [Head's](#stretch_body.head.Head) methods, such as [`startup()`](#stretch_body.head.Head.startup) and [`home()`](#stretch_body.head.Head.home) are extended from the [Device](#stretch_body.device.Device) class, while others come from the [DynamixelXChain](#stretch_body.dynamixel_X_chain.DynamixelXChain) class. Reading the head's current state and sending commands to its revolute joints (head pan and tilt) can be achieved using:
```python linenums='10'
starting_position = h.status['head_pan']['pos']
@ -324,7 +342,7 @@ The attribute `status` is a dictionary of dictionaries, where each subdictionary
Commanding the head's revolute joints is done through the [`move_to()`](#stretch_body.head.Head.move_to) and [`move_by()`](#stretch_body.head.Head.move_by) methods. Notice that, unlike the previous joints, no push command call is required here. These joints are Dynamixel servos, which behave differently than the Hello Robot steppers. Their commands are not queued and are executed as soon as they're received.
[Head's](#stretch_body.head.Head) two joints, the 'head_pan' and 'head_tilt', are instances of the [DynamixelHelloXL430](#stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430) class, and are retreiveable using the [`get_joint()`](#stretch_body.head.Head.get_joint) method. They have the [`wait_until_at_setpoint()`](#stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430.wait_until_at_setpoint) method, which blocks program execution until the joint reaches the commanded goal.
[Head's](#stretch_body.head.Head) two joints, the 'head_pan' and 'head_tilt' are instances of the [DynamixelHelloXL430](#stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430) class and are retrievable using the [`get_joint()`](#stretch_body.head.Head.get_joint) method. They have the [`wait_until_at_setpoint()`](#stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430.wait_until_at_setpoint) method, which blocks program execution until the joint reaches the commanded goal.
The [`pose()`](#stretch_body.head.Head.pose) method makes it easy to command the head to common head poses (e.g. looking 'ahead', at the end-of-arm 'tool', obstacles in front of the 'wheels', or 'up').
@ -344,7 +362,9 @@ h.follow_trajectory()
import time; time.sleep(6)
```
The head pan and tilt [DynamixelHelloXL430](#stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430) instances have an attribute `trajectory`, which is an instance of the [RevoluteTrajectory](#stretch_body.trajectories.RevoluteTrajectory) class. The call to [`follow_trajectory()`](#stretch_body.dynamixel_X_chain.DynamixelXChain.follow_trajectory) begins software tracking of the spline. Finally, setting soft motion limits for the head's pan and tilt range can be achieved using:
The head pan and tilt [DynamixelHelloXL430](#stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430) instances have an attribute `trajectory`, which is an instance of the [RevoluteTrajectory](#stretch_body.trajectories.RevoluteTrajectory) class. The call to [`follow_trajectory()`](#stretch_body.dynamixel_X_chain.DynamixelXChain.follow_trajectory) begins software tracking of the spline.
Finally, setting soft motion limits for the head's pan and tilt range can be achieved using:
The [`set_soft_motion_limit_min/max()`](#stretch_body.dynamixel_X_chain.DynamixelXChain.set_soft_motion_limit_min) methods perform clipping of the joint's range at the software level (cannot persist across reboots). All methods of the [Head](#stretch_body.head.Head) class are documented below.
The [`set_soft_motion_limit_min/max()`](#stretch_body.dynamixel_X_chain.DynamixelXChain.set_soft_motion_limit_min) methods perform clipping of the joint's range at the software level (cannot persist across reboots).
All methods of the [Head](#stretch_body.head.Head) class are documented below.
::: stretch_body.head.Head
### Using the EndOfArm class
The interface to Stretch's end of arm is the [EndOfArm](#stretch_body.end_of_arm.EndOfArm) class. It is typically initialized as:
The interface to Stretch's end-of-arm is the [EndOfArm](#stretch_body.end_of_arm.EndOfArm) class. It is typically initialized as:
Many users will want to work with tools other than the default Stretch Gripper that ships with the robot. In this tutorial you will learn how to configure the Stretch software interfaces to support other tools.
Many users will want to work with tools other than the default Stretch Gripper that ships with the robot. In this tutorial, you will learn how to configure the Stretch software interfaces to support other tools.
## Changing Tool Interfaces in Stretch Body
Stretch Body supports a plug-inbased architecture for tools. A tool is an extension of the [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) class that supports additional degrees of freedom.
Stretch Body supports a plug-in-based architecture for tools. A tool is an extension of the [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) class that supports additional degrees of freedom.
### Standard Tools
@ -71,9 +71,9 @@ In [6]: r.end_of_arm.stow()
In [7]: r.stop()
```
## Loading Tool Interfaces from the Stretch Tool Share
## Loading Tool Interfaces from Stretch Tool Share
The [Stretch Tool Share](https://github.com/hello-robot/stretch_tool_share/) is an open Git repository for non-standard Stretch tools. It hosts the CAD, URDF, and Python files needed to integrate these tools onto your robot.
The [Stretch Tool Share](https://github.com/hello-robot/stretch_tool_share/) is an open Git repository for non-standard Stretch tools. It hosts the CAD, URDF, and Python files needed to integrate these tools with your robot.
To use Stretch Tool Share tools, first update your installation:
@ -81,7 +81,7 @@ To use Stretch Tool Share tools, first update your installation:
$ pip install -U hello-robot-stretch-tool-share
```
As an example, we see on the Tool Share that there is a tool, the [ToolDryEraseToolHolderV1](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/dry_erase_holder_v1/tool.py) which [extends the EndOfArm](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/usbcam_wrist_v1/tool.py) class. In order to load this tool interface , modify your `stretch_user_params.yaml` to load the tool as before. We will also need to tell it where to find the tool's [parameter file](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/dry_erase_holder_v1/params.py):
As an example, we see on the Tool Share that there is a tool, the [ToolDryEraseToolHolderV1](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/dry_erase_holder_v1/tool.py) which [extends the EndOfArm](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/usbcam_wrist_v1/tool.py) class. To load this tool interface, modify your `stretch_user_params.yaml` to load the tool as before. We will also need to tell it where to find the tool's [parameter file](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/dry_erase_holder_v1/params.py):
```yaml
robot:
@ -112,9 +112,9 @@ In [6]: r.end_of_arm.stow()
# Changing Tool Interfaces in Stretch ROS
Next we'll show how to change the ROS interface for a tool. Here we will continue with the [ToolDryEraseHolderV1](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/dry_erase_holder_v1/tool.py#L3) example. First, configure Stretch Body to use the tool as in the previous exercise.
Next, we'll see how to change the ROS interface for a tool. Here we will continue with the [ToolDryEraseHolderV1](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/dry_erase_holder_v1/tool.py#L3) example. First, configure Stretch Body to use the tool as in the previous exercise.
Now we will update the tool Xacro for Stretch. Open the file `~/catkin_ws/src/stretch_ros/stretch_description/urdf/stretch_description.xacro` in an editor. Comment out the current tool Xacro and include the Xacro for the dryerase holder.
Now we will update the tool Xacro for Stretch. Open the file `~/catkin_ws/src/stretch_ros/stretch_description/urdf/stretch_description.xacro` in an editor. Comment out the current tool Xacro and include the Xacro for the dry-erase holder.
```xml
<?xml version="1.0"?>
@ -153,7 +153,7 @@ Now we will update the tool Xacro for Stretch. Open the file `~/catkin_ws/src/st
</robot>
```
Finally, we'll update our already calibrated URDF to use this new tool:
Finally, we'll update the calibrated URDF to use this new tool:
```console
$ cd ~/catkin_ws/src/stretch_ros/stretch_description/urdf
For users looking to create their own custom tools it can be useful to understand how the tool plug-in architecture works. Here we will walk through the basics of the system for both Stretch Body and Stretch ROS
For users looking to create their custom tools, it can be useful to understand how the tool plug-in architecture works. Here we will walk through the basics of the system for both Stretch Body and Stretch ROS
### Stretch Body
The [Robot](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot.py#L97) class expects an instance of EndOfArm tool to be present. The EndOfArm tool is an extension of the [DynamixelXChain](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_X_chain.py#L16) class, which manages a chain of Dynamixel servos.
The [Robot](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot.py#L97) class expects an instance of the EndOfArm tool to be present. The EndOfArm tool is an extension of the [DynamixelXChain](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_X_chain.py#L16) class, which manages a chain of Dynamixel servos.
A tool is defined via its parameters (either in user YAML or Python). For example, the ToolStretchGripper is defined in [robot_params.py](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot_params.py). These parameters tell the plug-in which [DynamixelHelloXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) instances to load and manage. Here we see:
@ -203,9 +203,9 @@ A tool is defined via its parameters (either in user YAML or Python). For exampl
},
```
This dictionary defines a tool of class ToolStretchGripper with two [DynamixelHelloXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) devices on its bus (StretchGripper and WristYaw).
This dictionary defines a tool of the class ToolStretchGripper with two [DynamixelHelloXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) devices on its bus (StretchGripper and WristYaw).
We see that the [ToolStretchGripper](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm_tools.py#L7) class extends the EndOfArm class and provides its own stowing behavior:
We see that the [ToolStretchGripper](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm_tools.py#L7) class extends the EndOfArm class and provides its stowing behavior:
```python
class ToolStretchGripper(EndOfArm):
@ -220,7 +220,7 @@ class ToolStretchGripper(EndOfArm):
For tools that are not a part of Stretch Body, such as from the Tool Share, you must include the tool parameters as well in your `stretch_user_params.yaml`. A robot that must support many tools may have user YAML that looks like:
For tools that are not a part of Stretch Body, such as from the Tool Share, you must include the tool parameters as well in your `stretch_user_params.yaml`. A robot that supports many tools may have a user YAML that looks like:
```yaml
params:
@ -235,7 +235,8 @@ robot:
#tool: tool_stretch_dex_wrist_beta
```
For a more complex implementation of a tool we recommend reviewing the Stretch Dex Wrist implementation on the Stretch Tool Share.
!!! tip
For a more complex implementation of a tool, we recommend reviewing the Stretch Dex Wrist implementation on the Stretch Tool Share.
hello-chintan
1 year ago