Aaron Edsinger
2 years ago
21 changed files with 2599 additions and 97 deletions
Split View
Diff Options
-
+10 -10getting_started/README.md
-
+30 -30ros1/README.md
-
+18 -6ros1/follow_joint_trajectory.md
-
+30 -30ros2/README.md
-
+17 -14stretch_body/README.md
-
+184 -0stretch_body/extending_wrist_dof/README.md
-
+37 -0stretch_body/extending_wrist_dof/stretch_re1_tool_params.yaml
-
+11 -0stretch_body/extending_wrist_dof/stretch_re1_user_params.yaml
-
+12 -0stretch_body/extending_wrist_dof/wrist_pitch.py
-
BIN
-
+260 -0stretch_body/stretch_body_guide.md
-
+212 -1stretch_body/tutorial_collision_avoidance.md
-
+97 -1stretch_body/tutorial_command_line_tools.md
-
+165 -1stretch_body/tutorial_dynamixel_servos.md
-
+183 -0stretch_body/tutorial_extending_wrist_dof.md
-
+184 -0stretch_body/tutorial_introduction.md
-
+284 -1stretch_body/tutorial_parameter_management.md
-
+314 -1stretch_body/tutorial_robot_motion.md
-
+52 -1stretch_body/tutorial_robot_sensors.md
-
+79 -1stretch_body/tutorial_safe_coding.md
-
+420 -0stretch_body/tutorial_stretch_body_api.md
+ 10
- 10
getting_started/README.md
View File
+ 30
- 30
ros1/README.md
View File
+ 18
- 6
ros1/follow_joint_trajectory.md
View File
+ 30
- 30
ros2/README.md
View File
+ 17
- 14
stretch_body/README.md
View File
+ 184
- 0
stretch_body/extending_wrist_dof/README.md
View File
| @ -0,0 +1,184 @@ | |||
|  | |||
| # Extending the Wrist DOF | |||
| 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. | |||
| See the [Hardware User Guide](https://docs.hello-robot.com/hardware_user_guide/#wrist) 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.** | |||
| ## 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_re1_tool_params.yaml `that configure the servo as desired | |||
| * Adding YAML parameters to `stretch_re1_user_params.yaml` that tell Stretch to include this class in its EndOfArm list of servos | |||
| Let's create a new DOF called WristPitch in a file named [wrist_pitch.py](./wrist_pitch.py). Place the file somewhere on the $PYTHONPATH. | |||
| ```python | |||
| from stretch_body.dynamixel_hello_XL430 import DynamixelHelloXL430 | |||
| from stretch_body.hello_utils import * | |||
| class WristPitch(DynamixelHelloXL430): | |||
| def __init__(self, chain=None): | |||
| DynamixelHelloXL430.__init__(self, 'wrist_pitch', chain) | |||
| self.poses = {'tool_up': deg_to_rad(45), | |||
| 'tool_down': deg_to_rad(-45)} | |||
| def pose(self,p,v_r=None,a_r=None): | |||
| self.move_to(self.poses[p],v_r,a_r) | |||
| ``` | |||
| Now let's copy in [YAML parameters for your servo](./stretch_re1_tool_params.yaml) to your `stretch_re1_tool_params.yaml` in order to configure this servo. You may want to adapt these parameters to your application but the nominal values shown usually work well. Below we highlight some of the more useful parameters. | |||
| ```yaml | |||
| wrist_pitch: | |||
| id: 1 #ID on the Dynamixel bus | |||
| range_t: #Range of servo, in ticks | |||
| - 0 | |||
| - 4096 | |||
| req_calibration: 0 #Does the joint require homing after startup | |||
| use_multiturn: 0 #Single turn or multi-turn mode of rotation | |||
| 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. | |||
| At this point your WristPitch class is ready to use. Plug the servo into the cable leaving the Stretch WristYaw joint. Experiment with the API from iPython | |||
| ```python | |||
| Python 2.7.17 (default, Apr 15 2020, 17:20:14) | |||
| ... | |||
| In [1]: import wrist_pitch | |||
| In [2]: w=wrist_pitch.WristPitch() | |||
| In [3]: w.startup() | |||
| In [4]: w.move_by(0.1) | |||
| 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](./stretch_re1_user_params.yaml) to your `stretch_re1_user_params.yaml` | |||
| ```yaml | |||
| end_of_arm: | |||
| devices: | |||
| wrist_pitch: | |||
| py_class_name: WristPitch | |||
| 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: | |||
| ```python | |||
| Python 2.7.17 (default, Jul 20 2020, 15:37:01) | |||
| ... | |||
| In [1]: import stretch_body.robot as robot | |||
| In [2]: r=robot.Robot() | |||
| In [3]: r.startup() | |||
| In [4]: r.end_of_arm.move_by('wrist_pitch',0.1) | |||
| ``` | |||
| ## Additional Information | |||
| ### The [DynamixelHelloXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) Class | |||
| The DynamixelHelloXL430 class is the primary interface to the Dynamixel servos of the robot. While named 'XL430' for legacy reasons, this class should be compatible with other X Series servos so long as they have compatible Control Tables (see the Dynamixel documentation). | |||
| The role of DynamixelHelloXL430 is to provide a calibrated interface to the servo in radians, manage homing to hardstops, and to expose an interface to `stretch_body` that is consistent with the robot's other DOF. | |||
| A class, such as [WristYaw](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py), derives from DynamixelHelloXL430, and extends it to include functionality that is specific to that DOF. For example, we can see in [WristYaw](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py) that it provides an interface to move to predetermined poses. | |||
| ### The [DynamixelXChain](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_X_chain.py) Class and [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) Class | |||
| The DynamixelXChain class manages a set of DynamixelHelloXL430 devices for `stretch_body`. It also provides synchronized read access to the servos in order to reduce bus communication bottlenecks | |||
| The EndOfArm class is derived from DynamixelXChain. It includes functionality that allows it to read the YAML and instantiate custom DynamixelHelloXL430 devices as shown in the example above. | |||
| ### Setting the Servo ID | |||
| By default the Dynamixel servo has an ID of 1. Each servo on a bus must have a unique ID. You can scan the bus for IDs using the `RE1_dynamixel_id_scan.py` tool: | |||
| ```bash | |||
| >>$ RE1_dynamixel_id_scan.py /dev/hello-dynamixel-wrist | |||
| [Dynamixel ID:000] ping Failed. | |||
| [Dynamixel ID:001] ping Failed. | |||
| [Dynamixel ID:002] ping Failed. | |||
| [Dynamixel ID:003] ping Succeeded. Dynamixel model number : 1060 | |||
| [Dynamixel ID:004] ping Succeeded. Dynamixel model number : 1020 | |||
| [Dynamixel ID:005] ping Succeeded. Dynamixel model number : 1020 | |||
| [Dynamixel ID:006] ping Succeeded. Dynamixel model number : 1060 | |||
| ... | |||
| ``` | |||
| Here we see that devices with IDs 3-6 are on the bus. To change the ID for device 6 to 7 for example, use `RE1_dynamixel_id_change.py` | |||
| ```bash | |||
| >>$RE1_dynamixel_id_change.py /dev/hello-dynamixel-wrist 6 7 | |||
| [Dynamixel ID:006] ping Succeeded. Dynamixel model number : 1060 | |||
| Ready to change ID to 7 . Hit enter to continue | |||
| [Dynamixel ID:007] ping Succeeded. Dynamixel model number : 1060 | |||
| Success at setting ID to 7 | |||
| ``` | |||
| ### Homing a Multi-Turn Servo | |||
| By default the Dynamixel servos are configured to be single-turn devices. When in single turn mode, they do not require a homing procedure on startup. Multi-turn devices, such as the Stretch WristYaw and StretchGripper, require a homing procedure. | |||
| The homing procedure moves the joint to one or both mechanical limits of the joint and, based on the detected hardstops, sets the joint's 'zero' point. | |||
| Your custom DynamixelHelloXL430 device may use this functionality if desired. To do so, you'll modify the following YAML fields for the device: | |||
| ```yaml | |||
| wrist_pitch: | |||
| ... | |||
| pwm_homing: #Set the force that the joint approaches the hardstop | |||
| - -300 | |||
| - 300 | |||
| range_t: #Set the mechanical range of the joint in ticks | |||
| - 0 | |||
| - 9340 | |||
| req_calibration: 1 #Don't allow the joint to move until it has been homed | |||
| use_multiturn: 1 #Enable multi-turn mode on the servo | |||
| zero_t: 7175 #Mark the position in the joint range to call zero | |||
| ``` | |||
| As shown above, the robot expects a mechanical range of motion of 9340 ticks (as defined in the Dynamixel documentation). Once homed, the DynamixelHelloXL430 device will report `pos=0` radians when the servo is at tick 7175. | |||
| In order to home the joint you can simply: | |||
| ```python | |||
| import wrist_pitch | |||
| w=wrist_pitch.WristPitch() | |||
| w.startup() | |||
| w.home(single_stop=True, move_to_zero=True) | |||
| ``` | |||
| This will cause it to move to just the first stop (at -300 PWM), and the move to zero (eg tick 7175) when done. | |||
| Finally, if your custom device is registered with EndOfArm, it will automatically home along with the other joints when calling `stretch_robot_home.py`. | |||
+ 37
- 0
stretch_body/extending_wrist_dof/stretch_re1_tool_params.yaml
View File
| @ -0,0 +1,37 @@ | |||
| wrist_pitch: | |||
| flip_encoder_polarity: 0 | |||
| gr: 1.0 | |||
| id: 1 | |||
| max_voltage_limit: 15 | |||
| min_voltage_limit: 11 | |||
| motion: | |||
| default: | |||
| accel: 8.0 | |||
| vel: 3.0 | |||
| fast: | |||
| accel: 10.0 | |||
| vel: 5.0 | |||
| max: | |||
| accel: 14 | |||
| vel: 7 | |||
| slow: | |||
| accel: 4.0 | |||
| vel: 1.0 | |||
| pid: | |||
| - 640 | |||
| - 0 | |||
| - 0 | |||
| pwm_limit: 885 | |||
| range_t: | |||
| - 0 | |||
| - 4096 | |||
| req_calibration: 0 | |||
| return_delay_time: 0 | |||
| stall_max_effort: 20.0 | |||
| stall_max_time: 1.0 | |||
| stall_min_vel: 0.1 | |||
| temperature_limit: 72 | |||
| usb_name: /dev/hello-dynamixel-wrist | |||
| use_multiturn: 0 | |||
| zero_t: 2048 | |||
+ 11
- 0
stretch_body/extending_wrist_dof/stretch_re1_user_params.yaml
View File
| @ -0,0 +1,11 @@ | |||
| factory_params: stretch_re1_factory_params.yaml | |||
| tool_params: stretch_re1_tool_params.yaml | |||
| # You can override factory settings for each device here | |||
| # Otherwise defaults to the factory 'robot_params' | |||
| # USE WITH CAUTION. IT IS POSSIBLE TO CAUSE UNSAFE BEHAVIOR OF THE ROBOT # | |||
| end_of_arm: | |||
| devices: | |||
| wrist_pitch: | |||
| py_class_name: WristPitch | |||
| py_module_name: wrist_pitch | |||
+ 12
- 0
stretch_body/extending_wrist_dof/wrist_pitch.py
View File
| @ -0,0 +1,12 @@ | |||
| from stretch_body.dynamixel_hello_XL430 import DynamixelHelloXL430 | |||
| from stretch_body.hello_utils import * | |||
| class WristPitch(DynamixelHelloXL430): | |||
| def __init__(self, chain=None): | |||
| DynamixelHelloXL430.__init__(self, 'wrist_pitch', chain) | |||
| self.poses = {'tool_up': deg_to_rad(45), | |||
| 'tool_down': deg_to_rad(-45)} | |||
| def pose(self,p,v_r=None,a_r=None): | |||
| self.move_to(self.poses[p],v_r,a_r) | |||
BIN
View File
+ 260
- 0
stretch_body/stretch_body_guide.md
View File
| @ -0,0 +1,260 @@ | |||
|  | |||
| # Working with Stretch Body | |||
| 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](https://github.com/hello-robot/stretch_body). | |||
| 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](https://github.com/SkoltechRobotics/rplidar) | |||
| * Respeaker: [respeaker_python_library](https://github.com/respeaker/respeaker_python_library) | |||
| * D435i: [pyrealsense2](https://pypi.org/project/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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot.py). | |||
| As an example, the Python script below prints all Robot sensor and state data to the console every 250ms. | |||
| ```python linenums="1" | |||
| import time | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| for i in range(10): | |||
| robot.pretty_print() | |||
| time.sleep(0.25) | |||
| robot.stop() | |||
| ``` | |||
| Looking at this in detail: | |||
| ```python linenums="2" | |||
| import stretch_body.robot | |||
| ``` | |||
| The package stretch_body includes the Python module for Robot as well as other Devices such as Lift and Arm. | |||
| ```python linenums="4" | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| ``` | |||
| 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. | |||
| ```python linenums="7" | |||
| for i in range(10): | |||
| robot.pretty_print() | |||
| time.sleep(0.25) | |||
| ``` | |||
| The call to `pretty_print()` prints to console all of the robot's sensor and state data. | |||
| ```python linenums="11" | |||
| robot.stop() | |||
| ``` | |||
| 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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/device.py). It also encapsulates a number of other Devices: | |||
| * [robot.head](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/head.py) | |||
| * [robot.arm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/arm.py) | |||
| * [robot.lift](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/lift.py) | |||
| * [robot.base](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/base.py) | |||
| * [robot.wacc](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wacc.py) | |||
| * [robot.pimu](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/pimu.py) | |||
| * [robot.end_of_arm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) | |||
| 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: | |||
| ```python | |||
| 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: | |||
| ```python | |||
| 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: | |||
| ```python | |||
| 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: | |||
| ```python linenums="1" | |||
| import time | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| robot.arm.move_by(0.1) | |||
| robot.push_command() | |||
| time.sleep(2.0) | |||
| robot.arm.move_by(-0.1) | |||
| robot.push_command() | |||
| time.sleep(2.0) | |||
| robot.stop() | |||
| ``` | |||
| A few important things are going on: | |||
| ```python linenums="7" | |||
| robot.arm.move_by(0.1) | |||
| ``` | |||
| The `move_by()` method queues up an RPC command to the stepper motor controller. However, the command does not yet execute. | |||
| ```python linenums="8" | |||
| robot.push_command() | |||
| ``` | |||
| 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: | |||
| ```python | |||
| 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: | |||
| ```python | |||
| if not robot.is_calibrated(): | |||
| robot.home() #blocking | |||
| ``` | |||
| Or it can be done manually after boot using the command line tool: | |||
| ```console | |||
| $ 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. | |||
| ```python | |||
| robot.stow() #blocking | |||
| ``` | |||
| Or it can be done manually from the command line when needed: | |||
| ```console | |||
| $ stretch_robot_stow.py | |||
| ``` | |||
| ## Scripting the Robot | |||
| A simplified design pattern to script the Robot is as follows | |||
| ```python linenums="1" | |||
| #!/usr/bin/env python | |||
| import time | |||
| import stretch_body.robot | |||
| from stretch_body.hello_utils import ThreadServiceExit | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| x_move_base = 0 | |||
| x_move_arm = 0 | |||
| x_move_lift = 0 | |||
| x_move_head_pan = 0 | |||
| x_move_head_tilt = 0 | |||
| x_move_wrist_yaw = 0 | |||
| x_move_gripper = 0 | |||
| def update_my_behavior(status): | |||
| #Update the joint commands based on the status data | |||
| pass | |||
| try: | |||
| while True: | |||
| #Get a snapshot of the robot status data | |||
| status=robot.get_status() | |||
| #Compute new position targets based on sensor data | |||
| update_my_behavior(status) | |||
| #Queue new targets to devices | |||
| robot.base.translate_by(x_move_base) #or robot.base.rotate_by() | |||
| robot.arm.move_by(x_move_arm) | |||
| robot.lift.move_by(x_move_lift) | |||
| robot.head.move_by('head_pan', x_move_head_pan) | |||
| robot.head.move_by('head_tilt', x_move_head_tilt) | |||
| robot.end_of_arm.move_by('wrist_yaw', x_move_wrist_yaw) | |||
| robot.end_of_arm.move_by('stretch_gripper', x_move_gripper) | |||
| #Synchronized send of new position targets | |||
| robot.push_command() | |||
| #Wait for next control cycle | |||
| time.sleep(0.1) | |||
| except (KeyboardInterrupt, SystemExit, ThreadServiceExit) | |||
| pass | |||
| robot.stop() | |||
| ``` | |||
+ 212
- 1
stretch_body/tutorial_collision_avoidance.md
View File
| @ -1 +1,212 @@ | |||
| Coming soon | |||
|  | |||
| # Working with the Collision Avoidance System | |||
| In this tutorial we will discuss the simple collision avoidance system that runs as a part of Stretch Body. | |||
| **Note**: This tutorial applies to Stretch Body version 0.1.x or greater. | |||
| ## Overview | |||
| Stretch Body includes a system to prevent inadvertent 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. | |||
| ## 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: | |||
| ```bash | |||
| >>$stretch_params.py | grep range | grep lift | |||
| stretch_body.robot_params.factory_params param.lift.range_m [0.0, 1.1] | |||
| ``` | |||
| A reduced range-of-motion can be set at run-time by setting the Soft Motion Limit. For example, to limit the lift range of motion to 0.3 meter off the base: | |||
| ```python | |||
| import stretch_body.robot as robot | |||
| r=robot.Robot() | |||
| r.startup() | |||
| r.lift.set_soft_motion_limit_min(0.3) | |||
| ``` | |||
| We see in the [API](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. | |||
| It is possible that when setting the Soft Motion Limit that the joints 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: | |||
| ```python | |||
| import stretch_body.robot as robot | |||
| import time | |||
| r=robot.Robot() | |||
| r.startup() | |||
| #Move to 0.2 | |||
| r.lift.move_to(0.2) | |||
| r.push_command() | |||
| time.sleep(5.0) | |||
| #Will move to 0.3 | |||
| r.lift.set_soft_motion_limit_min(0.3) | |||
| ``` | |||
| ## Collision Models | |||
| The [RobotCollision](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. | |||
| The `step` method of a RobotCollisionModel returns the desired joint limits given that model. For example, the base class is simply: | |||
| ```python | |||
| class RobotCollisionModel(Device): | |||
| def step(self, status): | |||
| return {'head_pan': [None, None],'head_tilt': [None, None], | |||
| 'lift': [None, None],'arm': [None, None],'wrist_yaw': [None, None]} | |||
| ``` | |||
| , where the value of `None` specifies that no-limit is specified and the full range-of-motion for the joint is acceptable. | |||
| We could define a new collision model that simply limits the lift range of motion to 1 meter by: | |||
| ```python | |||
| class MyCollisionModel(Device): | |||
| def step(self, status): | |||
| return {'head_pan': [None, None],'head_tilt': [None, None], | |||
| 'lift': [None, 1.0],'arm': [None, None],'wrist_yaw': [None, None]} | |||
| ``` | |||
| 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. | |||
| ## 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: | |||
| * CollisionArmCamera: Avoid collision of the head camera with the arm | |||
| * CollisionStretchGripper: 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. | |||
| ### 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 | |||
| ```python | |||
| "robot_collision": {'models': ['collision_arm_camera']}, | |||
| ``` | |||
| We also see that model `collision_arm_camera` is defined as: | |||
| ```python | |||
| "collision_arm_camera": { | |||
| 'enabled': 1, | |||
| 'py_class_name': 'CollisionArmCamera', | |||
| 'py_module_name': 'stretch_body.robot_collision_models' | |||
| } | |||
| ``` | |||
| This instructs RobotCollision to construct a model of type `CollisionArmCamera` and enable it by default. One can disable this model by default by specifying the following `stretch_re1_user_params.yaml`: | |||
| ```yaml | |||
| collision_arm_camera: | |||
| enabled: 0 | |||
| ``` | |||
| The entire collision avoidance system can be disabled in `stretch_re1_user_params.yaml` by: | |||
| ```yaml | |||
| robot: | |||
| use_collision_manager: 0 | |||
| ``` | |||
| A specific collision model can be enabled or disabled during runtime by: | |||
| ```python | |||
| import stretch_body.robot | |||
| r=stretch_body.robot.Robot() | |||
| r.startup() | |||
| ... #Do some work | |||
| r.collision.disable_model('collsion_arm_camera') | |||
| ... #Do some work | |||
| r.collision.enable_model('collsion_arm_camera') | |||
| ``` | |||
| Finally, if we want to also use the CollisionStretchGripper model, we can add to `stretch_re1_user_params.py`: | |||
| ```yaml | |||
| robot_collision: | |||
| models: | |||
| - collision_arm_camera | |||
| - collision_stretch_gripper | |||
| ``` | |||
| ### Creating a Custom Collision Model | |||
| 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 table top 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 | |||
| This assumes the arm is initially above the table top. To start, in a file `collision_arm_table.py` we add: | |||
| ```python | |||
| from stretch_body.robot_collision import * | |||
| from stretch_body.hello_utils import * | |||
| class CollisionArmTable(RobotCollisionModel): | |||
| def __init__(self, collision_manager): | |||
| RobotCollisionModel.__init__(self, collision_manager, 'collision_arm_table') | |||
| def step(self, status): | |||
| limits = {'lift': [None, None],'arm': [None, None]} | |||
| table_height = 0.5 #m | |||
| arm_safe_retract = 0.1 #m | |||
| safety_margin=.05#m | |||
| x_arm = status['arm']['pos'] | |||
| x_lift = status['lift']['pos'] | |||
| #Force arm to stay retracted if below table | |||
| if x_lift<table_height: | |||
| limits['arm'] = [None, arm_safe_retract-safety_margin] | |||
| else: | |||
| limits['arm'] = [None, None] | |||
| #Force lift to stay above table unless arm is retracted | |||
| if x_arm<arm_safe_retract: | |||
| limits['lift'] =[None,None] | |||
| else: | |||
| limits['lift']=[table_height+safety_margin,None] | |||
| return limits | |||
| ``` | |||
| In this example we include the `safety_margin` as a way to introduce some hysteresis around state changes to avoid toggling between the soft limits. | |||
| The following command should be run in order to add the working directory to the PYTHONPATH env , This can also be added to our bashrc to permanently edit the path: | |||
| ```bash | |||
| >>$ export PYTHONPATH=$PYTHONPATH:/<path_to_modules> | |||
| ``` | |||
| Next we configure RobotCollision to use our CollisionArmTable model in `stretch_re1_user_yaml`: | |||
| ```yaml | |||
| robot_collision: | |||
| models: | |||
| - collision_arm_table | |||
| collision_arm_table: | |||
| enabled: 1 | |||
| py_class_name: 'CollisionArmTable' | |||
| py_module_name: 'collision_arm_table' | |||
| ``` | |||
| Finally, test out the model by driving the arm and lift around using the XBox teleoperation tool: | |||
| ```bash | |||
| >>$ stretch_xbox_controller_teleop.py | |||
| ``` | |||
+ 97
- 1
stretch_body/tutorial_command_line_tools.md
View File
| @ -1 +1,97 @@ | |||
| Coming soon | |||
| # Tutorial: Stretch Body Command Line Tools | |||
| Stretch Body includes the package [hello-robot-stretch-body-tools](https://github.com/hello-robot/stretch_body/tree/master/tools) -- a suite of command line tools that allow direct interaction with hardware subsystems. | |||
| These tools are 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. | |||
| ```console | |||
| $ stretch_ | |||
| stretch_about.py | |||
| stretch_about_text.py | |||
| stretch_arm_home.py | |||
| stretch_arm_jog.py | |||
| stretch_audio_test.py | |||
| stretch_base_jog.py | |||
| stretch_gripper_home.py | |||
| stretch_gripper_jog.py | |||
| stretch_hardware_echo.py | |||
| stretch_head_jog.py | |||
| stretch_lift_home.py | |||
| stretch_lift_jog.py | |||
| stretch_params.py | |||
| stretch_pimu_jog.py | |||
| stretch_pimu_scope.py | |||
| stretch_realsense_visualizer.py | |||
| stretch_respeaker_test.py | |||
| stretch_robot_battery_check.py | |||
| stretch_robot_dynamixel_reboot.py | |||
| stretch_robot_home.py | |||
| stretch_robot_jog.py | |||
| stretch_robot_keyboard_teleop.py | |||
| stretch_robot_monitor.py | |||
| stretch_robot_stow.py | |||
| stretch_robot_system_check.py | |||
| stretch_robot_urdf_visualizer.py | |||
| stretch_rp_lidar_jog.py | |||
| stretch_trajectory_jog.py | |||
| stretch_version.sh | |||
| stretch_wacc_jog.py | |||
| stretch_wacc_scope.py | |||
| stretch_wrist_yaw_home.py | |||
| stretch_wrist_yaw_jog.py | |||
| stretch_xbox_controller_teleop.py | |||
| ``` | |||
| All tools accept '--help' as a command line argument to learn its function. For example: | |||
| ```console | |||
| >>$ stretch_pimu_scope.py --help | |||
| For use with S T R E T C H (R) RESEARCH EDITION from Hello Robot Inc. | |||
| --------------------------------------------------------------------- | |||
| usage: stretch_pimu_scope.py [-h] [--cliff] [--at_cliff] [--voltage] [--current] [--temp] [--ax] [--ay] [--az] [--mx] [--my] [--mz] [--gx] [--gy] [--gz] [--roll] [--pitch] [--heading] [--bump] | |||
| Visualize Pimu (Power+IMU) board data with an oscilloscope | |||
| optional arguments: | |||
| -h, --help show this help message and exit | |||
| --cliff Scope base cliff sensors | |||
| --at_cliff Scope base at_cliff signal | |||
| --voltage Scope bus voltage (V) | |||
| --current Scope bus current (A) | |||
| --temp Scope base internal temperature (C) | |||
| --ax Scope base accelerometer AX | |||
| --ay Scope base accelerometer AY | |||
| --az Scope base accelerometer AZ | |||
| --mx Scope base magnetometer MX | |||
| --my Scope base magnetometer MY | |||
| --mz Scope base magnetometer MZ | |||
| --gx Scope base gyro GX | |||
| --gy Scope base gyro GY | |||
| --gz Scope base gyro GZ | |||
| --roll Scope base imu Roll | |||
| --pitch Scope base imu Pitch | |||
| --heading Scope base imu Heading | |||
| --bump Scope base imu bump level | |||
| ``` | |||
| ### 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. | | |||
| Take a minute to explore each of these tools from the console. | |||
+ 165
- 1
stretch_body/tutorial_dynamixel_servos.md
View File
| @ -1 +1,165 @@ | |||
| Coming soon | |||
| # Working with Dynamixel Servos | |||
| In this tutorial we will go into the details with Dynamixel servos and Stretch. | |||
| ## Overview | |||
| Stretch comes with two Dynamixel buses - one for the head and one for the end-of-arm: | |||
| ```bash | |||
| >>$ ls /dev/hello-dynamixel-* | |||
| /dev/hello-dynamixel-head /dev/hello-dynamixel-wrist | |||
| ``` | |||
| 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 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. | |||
| ### Jogging the Servos | |||
| You can directly command each servo using the command line tool `REx_dynamixel_servo_jog.py`. This can be useful for debugging new servos added to the end-of-arm tool during system bring-up. For example, to command the head pan servo: | |||
| ```bash | |||
| $ REx_dynamixel_jog.py /dev/hello-dynamixel-head 11 | |||
| [Dynamixel ID:011] ping Succeeded. Dynamixel model number : 1080 | |||
| ------ MENU ------- | |||
| m: menu | |||
| a: increment position 50 tick | |||
| b: decrement position 50 tick | |||
| A: increment position 500 ticks | |||
| B: decrement position 500 ticks | |||
| v: set profile velocity | |||
| u: set profile acceleration | |||
| z: zero position | |||
| h: show homing offset | |||
| o: zero homing offset | |||
| q: got to position | |||
| p: ping | |||
| r: reboot | |||
| w: set max pwm | |||
| t: set max temp | |||
| i: set id | |||
| d: disable torque | |||
| 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: | |||
| ```bash | |||
| $ stretch_robot_dynamixel_reboot.py | |||
| For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc. | |||
| ---- Rebooting Head ---- | |||
| [Dynamixel ID:011] Reboot Succeeded. | |||
| [Dynamixel ID:012] Reboot Succeeded. | |||
| ---- Rebooting Wrist ---- | |||
| [Dynamixel ID:013] Reboot Succeeded. | |||
| [Dynamixel ID:014] Reboot Succeeded. | |||
| ``` | |||
| ### 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. | |||
| ```bash | |||
| $ RE1_dynamixel_id_scan.py /dev/hello-dynamixel-head --baud 57600 | |||
| Scanning bus /dev/hello-dynamixel-head at baud rate 57600 | |||
| ---------------------------------------------------------- | |||
| [Dynamixel ID:000] ping Failed. | |||
| [Dynamixel ID:001] ping Failed. | |||
| [Dynamixel ID:002] ping Failed. | |||
| [Dynamixel ID:003] ping Failed. | |||
| [Dynamixel ID:004] ping Failed. | |||
| [Dynamixel ID:005] ping Failed. | |||
| [Dynamixel ID:006] ping Failed. | |||
| [Dynamixel ID:007] ping Failed. | |||
| [Dynamixel ID:008] ping Failed. | |||
| [Dynamixel ID:009] ping Failed. | |||
| [Dynamixel ID:010] ping Failed. | |||
| [Dynamixel ID:011] ping Succeeded. Dynamixel model number : 1080 | |||
| [Dynamixel ID:012] ping Succeeded. Dynamixel model number : 1060 | |||
| [Dynamixel ID:013] ping Failed. | |||
| [Dynamixel ID:014] ping Failed. | |||
| [Dynamixel ID:015] ping Failed. | |||
| [Dynamixel ID:016] ping Failed. | |||
| [Dynamixel ID:017] ping Failed. | |||
| [Dynamixel ID:018] ping Failed. | |||
| [Dynamixel ID:019] ping Failed. | |||
| [Dynamixel ID:020] ping Failed. | |||
| [Dynamixel ID:021] ping Failed. | |||
| [Dynamixel ID:022] ping Failed. | |||
| [Dynamixel ID:023] ping Failed. | |||
| [Dynamixel ID:024] ping Failed. | |||
| ``` | |||
| ### 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: | |||
| ```bash | |||
| $ REx_dynamixel_set_baud.py /dev/hello-dynamixel-wrist 13 115200 | |||
| --------------------- | |||
| 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 | |||
| ### 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: | |||
| ```bash | |||
| $ RE1x_dynamixel_id_change.py /dev/hello-dynamixel-wrist 1 13 --baud 115200 | |||
| [Dynamixel ID:001] ping Succeeded. Dynamixel model number : 1080 | |||
| Ready to change ID 1 to 13. Hit enter to continue: | |||
| [Dynamixel ID:013] ping Succeeded. Dynamixel model number : 1080 | |||
| Success at setting ID to 13 | |||
| ``` | |||
| ## Stretch Body Dynamixel API | |||
| Stretch Body's low level Dynamixel API includes a hierarchy of three classes | |||
| | Class | | |||
| | ------------------------------------------------------------ | | |||
| | [DynamixelXChain](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_X_chain.py) | | |||
| | [DynamixelHelloXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) | | |||
| | [DynamixelXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_XL430.py) | | |||
| **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. | |||
| 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_extending_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. | |||
| ```bash | |||
| import stretch_body.dynamixel_hello_XL430 | |||
| m = stretch_body.dynamixel_hello_XL430.DynamixelHelloXL430('head_pan') | |||
| m.startup() | |||
| m.pretty_print() | |||
| ``` | |||
| ### DynamixelXL430 | |||
| Provides a thin wrapper to the Robotis Dynamixel SDK | |||
| ------ | |||
| <div align="center"> All materials are Copyright 2022 by Hello Robot Inc. The Stretch RE1 robot has patents pending</div> | |||
+ 183
- 0
stretch_body/tutorial_extending_wrist_dof.md
View File
| @ -0,0 +1,183 @@ | |||
| # Adding Custom Wrist DOF | |||
| 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. | |||
| See the [Hardware User Guide](https://docs.hello-robot.com/hardware_user_guide/#wrist) 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.** | |||
| ## 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_re1_tool_params.yaml `that configure the servo as desired | |||
| * Adding YAML parameters to `stretch_re1_user_params.yaml` that tell Stretch to include this class in its EndOfArm list of servos | |||
| Let's create a new DOF called WristPitch in a file named [wrist_pitch.py](./wrist_pitch.py). Place the file somewhere on the $PYTHONPATH. | |||
| ```python | |||
| from stretch_body.dynamixel_hello_XL430 import DynamixelHelloXL430 | |||
| from stretch_body.hello_utils import * | |||
| class WristPitch(DynamixelHelloXL430): | |||
| def __init__(self, chain=None): | |||
| DynamixelHelloXL430.__init__(self, 'wrist_pitch', chain) | |||
| self.poses = {'tool_up': deg_to_rad(45), | |||
| 'tool_down': deg_to_rad(-45)} | |||
| def pose(self,p,v_r=None,a_r=None): | |||
| self.move_to(self.poses[p],v_r,a_r) | |||
| ``` | |||
| Now let's copy in [YAML parameters for your servo](./stretch_re1_tool_params.yaml) to your `stretch_re1_tool_params.yaml` in order to configure this servo. You may want to adapt these parameters to your application but the nominal values shown usually work well. Below we highlight some of the more useful parameters. | |||
| ```yaml | |||
| wrist_pitch: | |||
| id: 1 #ID on the Dynamixel bus | |||
| range_t: #Range of servo, in ticks | |||
| - 0 | |||
| - 4096 | |||
| req_calibration: 0 #Does the joint require homing after startup | |||
| use_multiturn: 0 #Single turn or multi-turn mode of rotation | |||
| 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. | |||
| At this point your WristPitch class is ready to use. Plug the servo into the cable leaving the Stretch WristYaw joint. Experiment with the API from iPython | |||
| ```python | |||
| Python 2.7.17 (default, Apr 15 2020, 17:20:14) | |||
| ... | |||
| In [1]: import wrist_pitch | |||
| In [2]: w=wrist_pitch.WristPitch() | |||
| In [3]: w.startup() | |||
| In [4]: w.move_by(0.1) | |||
| 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](./stretch_re1_user_params.yaml) to your `stretch_re1_user_params.yaml` | |||
| ```yaml | |||
| end_of_arm: | |||
| devices: | |||
| wrist_pitch: | |||
| py_class_name: WristPitch | |||
| 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: | |||
| ```python | |||
| Python 2.7.17 (default, Jul 20 2020, 15:37:01) | |||
| ... | |||
| In [1]: import stretch_body.robot as robot | |||
| In [2]: r=robot.Robot() | |||
| In [3]: r.startup() | |||
| In [4]: r.end_of_arm.move_by('wrist_pitch',0.1) | |||
| ``` | |||
| ## Additional Information | |||
| ### The [DynamixelHelloXL430](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) Class | |||
| The DynamixelHelloXL430 class is the primary interface to the Dynamixel servos of the robot. While named 'XL430' for legacy reasons, this class should be compatible with other X Series servos so long as they have compatible Control Tables (see the Dynamixel documentation). | |||
| The role of DynamixelHelloXL430 is to provide a calibrated interface to the servo in radians, manage homing to hardstops, and to expose an interface to `stretch_body` that is consistent with the robot's other DOF. | |||
| A class, such as [WristYaw](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py), derives from DynamixelHelloXL430, and extends it to include functionality that is specific to that DOF. For example, we can see in [WristYaw](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py) that it provides an interface to move to predetermined poses. | |||
| ### The [DynamixelXChain](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_X_chain.py) Class and [EndOfArm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) Class | |||
| The DynamixelXChain class manages a set of DynamixelHelloXL430 devices for `stretch_body`. It also provides synchronized read access to the servos in order to reduce bus communication bottlenecks | |||
| The EndOfArm class is derived from DynamixelXChain. It includes functionality that allows it to read the YAML and instantiate custom DynamixelHelloXL430 devices as shown in the example above. | |||
| ### Setting the Servo ID | |||
| By default the Dynamixel servo has an ID of 1. Each servo on a bus must have a unique ID. You can scan the bus for IDs using the `RE1_dynamixel_id_scan.py` tool: | |||
| ```bash | |||
| >>$ RE1_dynamixel_id_scan.py /dev/hello-dynamixel-wrist | |||
| [Dynamixel ID:000] ping Failed. | |||
| [Dynamixel ID:001] ping Failed. | |||
| [Dynamixel ID:002] ping Failed. | |||
| [Dynamixel ID:003] ping Succeeded. Dynamixel model number : 1060 | |||
| [Dynamixel ID:004] ping Succeeded. Dynamixel model number : 1020 | |||
| [Dynamixel ID:005] ping Succeeded. Dynamixel model number : 1020 | |||
| [Dynamixel ID:006] ping Succeeded. Dynamixel model number : 1060 | |||
| ... | |||
| ``` | |||
| Here we see that devices with IDs 3-6 are on the bus. To change the ID for device 6 to 7 for example, use `RE1_dynamixel_id_change.py` | |||
| ```bash | |||
| >>$RE1_dynamixel_id_change.py /dev/hello-dynamixel-wrist 6 7 | |||
| [Dynamixel ID:006] ping Succeeded. Dynamixel model number : 1060 | |||
| Ready to change ID to 7 . Hit enter to continue | |||
| [Dynamixel ID:007] ping Succeeded. Dynamixel model number : 1060 | |||
| Success at setting ID to 7 | |||
| ``` | |||
| ### Homing a Multi-Turn Servo | |||
| By default the Dynamixel servos are configured to be single-turn devices. When in single turn mode, they do not require a homing procedure on startup. Multi-turn devices, such as the Stretch WristYaw and StretchGripper, require a homing procedure. | |||
| The homing procedure moves the joint to one or both mechanical limits of the joint and, based on the detected hardstops, sets the joint's 'zero' point. | |||
| Your custom DynamixelHelloXL430 device may use this functionality if desired. To do so, you'll modify the following YAML fields for the device: | |||
| ```yaml | |||
| wrist_pitch: | |||
| ... | |||
| pwm_homing: #Set the force that the joint approaches the hardstop | |||
| - -300 | |||
| - 300 | |||
| range_t: #Set the mechanical range of the joint in ticks | |||
| - 0 | |||
| - 9340 | |||
| req_calibration: 1 #Don't allow the joint to move until it has been homed | |||
| use_multiturn: 1 #Enable multi-turn mode on the servo | |||
| zero_t: 7175 #Mark the position in the joint range to call zero | |||
| ``` | |||
| As shown above, the robot expects a mechanical range of motion of 9340 ticks (as defined in the Dynamixel documentation). Once homed, the DynamixelHelloXL430 device will report `pos=0` radians when the servo is at tick 7175. | |||
| In order to home the joint you can simply: | |||
| ```python | |||
| import wrist_pitch | |||
| w=wrist_pitch.WristPitch() | |||
| w.startup() | |||
| w.home(single_stop=True, move_to_zero=True) | |||
| ``` | |||
| This will cause it to move to just the first stop (at -300 PWM), and the move to zero (eg tick 7175) when done. | |||
| Finally, if your custom device is registered with EndOfArm, it will automatically home along with the other joints when calling `stretch_robot_home.py`. | |||
+ 184
- 0
stretch_body/tutorial_introduction.md
View File
| @ -0,0 +1,184 @@ | |||
| # Tutorial: Introduction to Stretch Body | |||
| The Stretch_Body package provides a low level Python API to the Stretch hardware. 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. | |||
| The package is available on [Git and installable via Pip](https://github.com/hello-robot/stretch_body). | |||
| It encapsulates the: | |||
| * Mobile base | |||
| * Arm | |||
| * Lift | |||
| * Head actuators | |||
| * End -of-arm-actuators | |||
| * Wrist board with accelerometer (Wacc) | |||
| * Base power and IMU board (Pimu) | |||
| As shown below, the primary programming interface to Stretch Body s the [Robot class](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot.py). This class encapsulates the various hardware module classes (e.g. Lift, Arm, etc). Each of these modules then communicate the robot's firmware over USB using various utility classes. | |||
|  | |||
| Stretch also includes 3rd party hardware devices that are not accessible through Stretch_Body. However, it is possible to directly access this hardware through open-source Python packages: | |||
| * Laser range finder: [rplidar](https://github.com/SkoltechRobotics/rplidar) | |||
| * Respeaker: [respeaker_python_library](https://github.com/respeaker/respeaker_python_library) | |||
| * D435i: [pyrealsense2](https://pypi.org/project/pyrealsense2/) | |||
| ## Robot Interface | |||
| The primary developer interface to Stretch_Body is the [Robot class](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/robot.py). Let's write some code to explore the interface. Launch an interactive Python terminal: | |||
| ```bash | |||
| hello-robot@stretch-re2-9999:~$ ipython | |||
| In [1]: | |||
| ``` | |||
| And type in the following: | |||
| ```python linenums="1" | |||
| import time | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| for i in range(10): | |||
| robot.pretty_print() | |||
| time.sleep(0.25) | |||
| robot.stop() | |||
| ``` | |||
| As you can see, this prints all Robot sensor and state data to the console every 250ms. | |||
| Looking at this in detail: | |||
| ```python linenums="4" | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| ``` | |||
| Here we instantiated 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. | |||
| ```python linenums="7" | |||
| for i in range(10): | |||
| robot.pretty_print() | |||
| time.sleep(0.25) | |||
| ``` | |||
| The call to `pretty_print()` prints to console all of the robot's sensor and state data. The sensor data is automatically updated in the background by a helper thread. | |||
| ```python linenums="11" | |||
| robot.stop() | |||
| ``` | |||
| 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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/device.py). It also encapsulates a number of other Devices: | |||
| * [robot.head](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/head.py) | |||
| * [robot.arm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/arm.py) | |||
| * [robot.lift](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/lift.py) | |||
| * [robot.base](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/base.py) | |||
| * [robot.wacc](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wacc.py) | |||
| * [robot.pimu](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/pimu.py) | |||
| * [robot.end_of_arm](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/end_of_arm.py) | |||
| 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: | |||
| ```python | |||
| 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 around 20Hz. The Status data can be accessed via the Robot. For example: | |||
| ```python | |||
| 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: | |||
| ```python | |||
| 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: | |||
| ```python linenums="1" | |||
| import time | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| robot.arm.move_by(0.1) | |||
| robot.push_command() | |||
| time.sleep(2.0) | |||
| robot.arm.move_by(-0.1) | |||
| robot.push_command() | |||
| time.sleep(2.0) | |||
| robot.stop() | |||
| ``` | |||
| A few important things are going on: | |||
| ```python linenums="7" | |||
| robot.arm.move_by(0.1) | |||
| ``` | |||
| The `move_by()` method queues up the command to the stepper motor controller. However, the command does not yet execute. | |||
| ```python linenums="8" | |||
| robot.push_command() | |||
| ``` | |||
| The `push_command()` causes all queued up commands to be executed at once. This allows for synchronization of motion across joints. For example, the following code will cause the base, arm, and lift to initiate motion simultaneously: | |||
| ```python | |||
| robot.arm.move_by(0.1) | |||
| robot.lift.move_by(0.1) | |||
| robot.base.translate_by(0.1) | |||
| robot.push_command() | |||
| ``` | |||
| **NOTE**: 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 communication protocol. As such, the head, wrist, and gripper will move immediately upon issuing a motion command. | |||
+ 284
- 1
stretch_body/tutorial_parameter_management.md
View File
| @ -1 +1,284 @@ | |||
| Coming soon | |||
|  | |||
| # Working With 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. | |||
| **Note**: This tutorial applies to Stretch Body version 0.1.x or greater. | |||
| ## Overview | |||
| Stretch Body shares a global set of parameters across all of its Python Devices. A quick way to see what parameters are available are with the `stretch_params.py` tool: | |||
| ```console | |||
| $ stretch_params.py | |||
| ################################### Parameters for stretch-re1-1039 ###################################### | |||
| Origin Parameter Value | |||
| ---------------------------------------------------------------------------------------------------------- | |||
| stretch_re1_factory_params.yaml param.head_pan.req_calibration 0 | |||
| stretch_re1_factory_params.yaml param.head_tilt.pwm_limit 885 | |||
| ... | |||
| ``` | |||
| The tool display each parameter's value as well as which parameter file it was loaded from. | |||
| By grepping the tool's output you can query specific settings. For example, to query contact thresholds for the arm: | |||
| ```console | |||
| $ stretch_params.py | grep arm | grep contact_thresh | |||
| stretch_re1_factory_params.yaml param.arm.contact_thresh_max_N [-100, 100] | |||
| stretch_re1_factory_params.yaml param.arm.contact_thresh_N [-64.46241590881348, 66.51084520568847] | |||
| ``` | |||
| Now if you want to override the default contact thresholds for the arm, you could add the following to your `stretch_re1_user_params.yaml`: | |||
| ```yaml | |||
| arm: | |||
| contact_thresh_N: [-80.0,80.0] | |||
| ``` | |||
| Run the tool again and we see: | |||
| ```console | |||
| $ stretch_params.py | grep arm | grep contact_thresh | |||
| stretch_re1_factory_params.yaml param.arm.contact_thresh_max_N [-100, 100] | |||
| stretch_re1_factory_params.yaml param.arm.contact_thresh_N [-80, 80] | |||
| ``` | |||
| ## Data Organization | |||
| Parameters' data sources may be either a Python dictionary or a YAML file. The robot's YAML files are found under `$HELLO_FLEET_PATH/$HELLO_FLEET_ID`. The Python dictionaries may come from any Python module on the Python path. | |||
| There are three types of parameters: | |||
| * Factory Parameters | |||
| * `stretch_re1_factory_params.yaml `: These are common factory settings shared across RE1s as well as factory calibration data that is specific to your robot. | |||
| * `stretch_body.robot_params.py`: These are common factory settings shared across RE1s that may be automatically updated over time as new versions of Stretch Body are released. | |||
| * External Parameters | |||
| * `<external_module>.params.py`: These are parameters for devices outside of Stretch Body (such as the devices found in the Stretch Tool Share) | |||
| * User Parameters | |||
| * `stretch_re1_user_params.yaml`: These are user customizations of the robot's settings | |||
| As shown below, the parameters are loaded into the `robot_params` dictionary in a specific order, starting with `stretch_re1_factory_params.yaml`. Loading of subsequent parameters overwrites earlier parameters. As such, the `stretch_re1_user_params.yaml` parameters can overwrite any factory parameters or external parameters. | |||
|  | |||
| You can configure which external parameters' data sources are loaded, and their order, in your `stretch_re1_user_params.yaml`. For example: | |||
| ```yaml | |||
| factory_params: stretch_re1_factory_params.yaml | |||
| params: | |||
| - stretch_tool_share.usbcam_wrist_v1.params | |||
| - stretch_tool_share.stretch_dex_wrist_beta.params | |||
| ``` | |||
| Here we see that the name of the factory parameters file is `stretch_re1_factory_params.yaml`. We also see that two external parameter sets will be loaded. In this example, the order of parameter precedence would be: | |||
| 1. `stretch_re1_user_params.yaml` | |||
| 2. `stretch_tool_share.stretch_dex_wrist_beta.params.py` | |||
| 3. `stretch_tool_share.usbcam_wrist_v1.params.py` | |||
| 4. `stretch_body.robot_params.py` | |||
| 5. `stretch_re1_factory_params.yaml` | |||
| ## Working with Parameters Programmatically | |||
| Stretch Body is organized as a set of classes that extend the base [Device](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/device.py#L27) class. Each Device has access to a set of global parameters that are stored within the Device as a dictionary. These are: | |||
| * `device.params` : the parameters for the specific device | |||
| * `device.robot_params`: global set of parameters | |||
| For example, let's look at the Robot class: | |||
| ```python | |||
| class Robot(Device): | |||
| def __init__(self): | |||
| Device.__init__(self,'robot') | |||
| ... | |||
| ``` | |||
| When instantiating its Device base class it loads the robot parameters of the various YAML files and Python dictionaries. | |||
| ```python | |||
| class Device: | |||
| def __init__(self,name): | |||
| self.name=name | |||
| self.user_params, self.robot_params = RobotParams.get_params() | |||
| try: | |||
| self.params=self.robot_params[self.name] | |||
| except KeyError: | |||
| print('No device params found for %s'%name) | |||
| self.params={} | |||
| ``` | |||
| We can explore these parameters via iPython. | |||
| ```python | |||
| In [1]: import stretch_body.robot as robot | |||
| In [2]: r=robot.Robot() | |||
| In [3]: r.params | |||
| Out[3]: | |||
| {'batch_name': 'Irma', | |||
| 'log_to_console': 0, | |||
| 'serial_no': 'stretch-re1-1039', | |||
| 'stow': {'arm': 0.0, | |||
| 'head_pan': 0.0, | |||
| 'head_tilt': 0.0, | |||
| 'lift': 0.2, | |||
| 'stretch_gripper': 0, | |||
| 'wrist_yaw': 3.4}, | |||
| 'tool': 'tool_none', | |||
| 'use_arm': 1, | |||
| 'use_base': 1, | |||
| 'use_end_of_arm': 1, | |||
| 'use_head': 1, | |||
| 'use_lift': 1, | |||
| 'use_monitor': 1, | |||
| 'use_pimu': 1, | |||
| 'use_sentry': 1, | |||
| 'use_wacc': 1, | |||
| 'verbose': 0} | |||
| In [4]: r.arm.params | |||
| Out[4]: | |||
| {'chain_pitch': 0.0167, | |||
| 'chain_sprocket_teeth': 10, | |||
| 'contact_thresh_N': [-80, 80], | |||
| 'contact_thresh_max_N': [-100, 100], | |||
| 'force_N_per_A': 55.9, | |||
| 'gr_spur': 3.875, | |||
| 'homing_force_N': [-60, 80], | |||
| 'i_feedforward': 0, | |||
| 'motion': {'default': {'accel_m': 0.14, 'vel_m': 0.14}, | |||
| 'fast': {'accel_m': 0.2, 'vel_m': 0.25}, | |||
| 'max': {'accel_m': 0.3, 'vel_m': 0.3}, | |||
| 'slow': {'accel_m': 0.07, 'vel_m': 0.06}}, | |||
| 'range_m': [0.0, 0.5202755326289126], | |||
| 'verbose': 0} | |||
| ``` | |||
| All devices have access the global parameter set, `robot_params`. For example, the arm can access the lift parameters: | |||
| ```python | |||
| In [5]: r.arm.robot_params['lift'] | |||
| Out[5]: | |||
| {'belt_pitch_m': 0.005, | |||
| 'contact_thresh_N': [-72.45217552185059, 65.6581787109375], | |||
| 'contact_thresh_max_N': [-100, 100], | |||
| 'force_N_per_A': 75.0, | |||
| 'homing_force_N': [-70, 70], | |||
| 'i_feedforward': 0.75, | |||
| 'motion': {'default': {'accel_m': 0.15, 'vel_m': 0.095}, | |||
| 'fast': {'accel_m': 0.2, 'vel_m': 0.12}, | |||
| 'max': {'accel_m': 0.3, 'vel_m': 0.15}, | |||
| 'slow': {'accel_m': 0.05, 'vel_m': 0.05}}, | |||
| 'pinion_t': 12, | |||
| 'range_m': [0.0, 1.0983598164609882], | |||
| 'verbose': 0} | |||
| ``` | |||
| You can set any of the `robot_params` programmatically. For example to adjust the contact sensitivity for the arm: | |||
| ```python | |||
| In [6]: r.arm.params['contact_thresh_N'] | |||
| Out[6]: [-64.46241590881348, 66.51084520568847] | |||
| In [7]: r.arm.params['contact_thresh_N']=[-80.0, 80.0] | |||
| In [8]: r.arm.params['contact_thresh_N'] | |||
| Out[8]: [-80.0, 80.0] | |||
| ``` | |||
| ## Robot Parameters | |||
| All robot data is stored in the stretch_user directory. The location of this directory can be found by: | |||
| ```console | |||
| $ echo $HELLO_FLEET_PATH | |||
| /home/hello-robot/stretch_user | |||
| ``` | |||
| The robot data stored here is identified by the robot ID (eg, stretch-re1-1002) | |||
| ```console | |||
| $ 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. | |||
| ```yaml | |||
| 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](https://github.com/hello-robot/stretch_body/tree/master/tutorial/extending_wrist_dof) | |||
| <div align="center"> All materials are Copyright 2020 by Hello Robot Inc. The Stretch RE1 robot has patents pending</div> | |||
| ------ | |||
| <div align="center"> All materials are Copyright 2020 by Hello Robot Inc. The Stretch RE1 robot has patents pending</div> | |||
+ 314
- 1
stretch_body/tutorial_robot_motion.md
View File
| @ -1 +1,314 @@ | |||
| Coming soon | |||
| # Tutorial: Robot Motion | |||
| 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: | |||
| ```python linenums="1" | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| robot.arm.move_by(0.1) | |||
| robot.push_command() | |||
| time.sleep(2.0) | |||
| robot.stop() | |||
| ``` | |||
| Or, absolute motion can be commanded by: | |||
| ```python linenums="1" | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| robot.arm.move_to(0.1) | |||
| robot.push_command() | |||
| time.sleep(2.0) | |||
| 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: | |||
| ```python linenums="1" | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| #Move to full retraction | |||
| robot.arm.move_to(0.0) | |||
| robot.push_command() | |||
| robot.arm.wait_until_at_setpoint() | |||
| #Move to full extension | |||
| robot.arm.move_to(0.5) | |||
| robot.push_command() | |||
| #Interrupt motion midway and retract again | |||
| time.sleep(2.0) | |||
| robot.arm.move_to(0.0) | |||
| robot.arm.wait_until_at_setpoint() | |||
| robot.stop() | |||
| ``` | |||
| You will see the arm fully retract, begin to extend, and then fully retract again. | |||
| ## 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: | |||
| * 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: | |||
| ```bash | |||
| >>$stretch_params.py | grep arm | grep motion | grep default | |||
| stretch_body.robot_params.nominal_params param.arm.motion.fast.accel_m 0.14 | |||
| stretch_body.robot_params.nominal_params param.arm.motion.fast.vel_m 0.14 | |||
| ``` | |||
| We see that the arm motion in 'default' mode will move with a velocity of 0.14 m/s and an acceleration of 0.14 m/s^2. | |||
| The `move_by` and `move_to` commands support optional motion profile parameters. For example, for a fast motion: | |||
| ```python | |||
| v = robot.arm.params['motion']['fast']['vel_m'] | |||
| a = robot.arm.params['motion']['fast']['accel_m'] | |||
| robot.arm.move_by(x_m=0.1, v_m=v, a_m=a) | |||
| robot.push_command() | |||
| ``` | |||
| The motion will use the 'default' motion profile settings if no values are specified. | |||
| ## Motion Limits | |||
| All joints obey motion limits which are specified in the robot parameters. | |||
| ```bash | |||
| >>$ stretch_params.py | grep arm | grep range_m | |||
| stretch_user_params.yaml param.arm.range_m [0.0, 0.515] | |||
| ``` | |||
| 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 factory specified values. However, they can be further limited if desired by setting soft motion limits: | |||
| ```python | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| robot.arm.set_soft_motion_limit_min(0.2) | |||
| robot.arm.set_soft_motion_limit_max(0.4) | |||
| #Will move to position 0.2 | |||
| robot.arm.move_to(0.0) | |||
| robot.push_command() | |||
| robot.arm.wait_until_at_setpoint() | |||
| #Will move to position 0.4 | |||
| robot.arm.move_to(0.5) | |||
| robot.push_command() | |||
| robot.arm.wait_until_at_setpoint() | |||
| robot.stop() | |||
| ``` | |||
| ## Controlling Dynamixel Motion | |||
| 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 | |||
| from stretch_body.hello_utils import deg_to_rad | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| robot.head.move_to('head_pan',0) | |||
| robot.head.move_to('head_tilt',0) | |||
| time.sleep(3.0) | |||
| robot.head.move_to('head_pan',deg_to_rad(90.0)) | |||
| robot.head.move_to('head_tilt',deg_to_rad(45.0)) | |||
| 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: | |||
| ```python | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| #Limit range of motion of head_pan | |||
| robot.head.get_motor('head_pan').set_soft_motion_limit_min(deg_to_rad(-45.0)) | |||
| robot.head.get_motor('head_pan').set_soft_motion_limit_max(deg_to_rad(45.0)) | |||
| #Do a fast motion | |||
| v = robot.params['head_pan']['motion']['fast']['vel'] | |||
| a = robot.params['head_pan']['motion']['fast']['accel'] | |||
| robot.head.move_to('head_pan',deg_to_rad(-90.0),v_r=v, a_r=a) | |||
| time.sleep(3.0) | |||
| #Do a slow motion | |||
| v = robot.params['head_pan']['motion']['slow']['vel'] | |||
| a = robot.params['head_pan']['motion']['slow']['accel'] | |||
| robot.head.move_to('head_pan',deg_to_rad(90.0),v_r=v, a_r=a) | |||
| 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: | |||
| ```python | |||
| 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 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: | |||
| | Joint | Default Safety Mode | Default Position 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 | | |||
| 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. | |||
| ### 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: | |||
| ```python | |||
| import stretch_body.robot | |||
| robot=stretch_body.robot.Robot() | |||
| robot.startup() | |||
| #Move to full retraction | |||
| robot.arm.move_to(0.0) | |||
| robot.push_command() | |||
| robot.arm.wait_until_at_setpoint() | |||
| #Move to full extension | |||
| robot.arm.move_to(0.5) | |||
| robot.push_command() | |||
| #Runstop motion midway through the motion | |||
| input('Hit enter to runstop motion') | |||
| robot.pimu.runstop_event_trigger() | |||
| robot.push_command() | |||
| input('Hit enter to restart motion') | |||
| robot.pimu.runstop_event_reset() | |||
| robot.push_command() | |||
| robot.arm.move_to(0.5) | |||
| robot.push_command() | |||
| robot.arm.wait_until_at_setpoint() | |||
| robot.stop() | |||
| ``` | |||
| ### 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 | |||
| For more information on guarded motion, see the [Contact Models Tutorial](./tutorial_contact_models.md) | |||
| ### Synchronized Motion | |||
| The Arm, Lift, and Base actuators have a hardware synchronization mechanism. This allows for stepper controller commands to be time synchronized across joints. This behavior can be disabled via the user YAML. By default the settings are: | |||
| ```bash | |||
| >>$ stretch_params.py | grep enable_sync_mode | |||
| stretch_body.robot_params.nominal_params param.hello-motor-arm.gains.enable_sync_mode 1 | |||
| stretch_body.robot_params.nominal_params param.hello-motor-left-wheel.gains.enable_sync_mode 1 | |||
| stretch_body.robot_params.nominal_params param.hello-motor-lift.gains.enable_sync_mode 1 | |||
| stretch_body.robot_params.nominal_params param.hello-motor-right-wheel.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: | |||
| ```python | |||
| robot.arm.status['pos'] #Joint position | |||
| robot.arm.status['vel'] #Joint velocity | |||
| robot.arm.motor.status['effort_pct'] #Joint effort (-100 to 100) (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 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. | |||
+ 52
- 1
stretch_body/tutorial_robot_sensors.md
View File
| @ -1 +1,52 @@ | |||
| Coming soon | |||
| ## Sensors | |||
| ### Base IMU | |||
| Coming soon. | |||
| ### Wrist Accelerometer | |||
| Coming soon. | |||
| ### Cliff Sensors | |||
| Stretch has [four IR cliff sensors](https://docs.hello-robot.com/hardware_user_guide/#base) 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 | |||
| ```yaml | |||
| 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: | |||
| ```python | |||
| 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](https://github.com/hello-robot/stretch_firmware/blob/master/arduino/hello_pimu/Pimu.cpp). | |||
+ 79
- 1
stretch_body/tutorial_safe_coding.md
View File
| @ -1 +1,79 @@ | |||
| Coming soon | |||
| ## 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: | |||
| ```yaml | |||
| 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](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. | |||
| | 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. | |||
| ```yaml | |||
| 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](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. | |||
| | 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 | | |||
| ------ | |||
+ 420
- 0
stretch_body/tutorial_stretch_body_api.md
View File
| @ -0,0 +1,420 @@ | |||
| # Stretch Body API Reference | |||
| Stretch Body is the Python interface to working with the Stretch RE1. This page serves as a reference of the interfaces defined in the `stretch_body` library. | |||
| See the [Stretch Body Tutorials](shttps://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/) for additional information on working with this library. | |||
| ## The Robot Class | |||
| ### Using the Robot class | |||
| The most common interface to Stretch is the [Robot](#stretch_body.robot.Robot) class. It is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.robot | |||
| r = stretch_body.robot.Robot() | |||
| if not r.startup(): | |||
| exit() # failed to start robot! | |||
| # home the joints to find zero, if necessary | |||
| if not r.is_calibrated(): | |||
| r.home() | |||
| # interact with the robot here | |||
| ``` | |||
| The [`startup()`](#stretch_body.robot.Robot.startup) and [`home()`](#stretch_body.robot.Robot.home) methods starts communication with and homes each of the robot's devices, respectively. Through the [Robot](#stretch_body.robot.Robot) class, users can interact with all other devices on the robot. For example, continuing the example above: | |||
| ```python linenums='12' | |||
| # moving joints on the robot | |||
| r.arm.pretty_print() | |||
| r.lift.pretty_print() | |||
| r.base.pretty_print() | |||
| r.head.pretty_print() | |||
| r.end_of_arm.pretty_print() | |||
| # other devices on the robot | |||
| r.wacc.pretty_print() | |||
| r.pimu.pretty_print() | |||
| r.stop() | |||
| ``` | |||
| Each of these devices are defined in other modules within `stretch_body`. In the [following section](#the-device-classes), 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 of [Robot's](#stretch_body.robot.Robot) subroutines are documented below. | |||
| ::: stretch_body.robot.Robot | |||
| ## The Device Classes | |||
| The `stretch_body` library is modular in design. Each subcomponent of Stretch is defined in its own 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. | |||
| 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 device classes: 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. | |||
| ### Using the Arm class | |||
|  | |||
|  | |||
| The interface to Stretch's telescoping arm is the [Arm](#stretch_body.arm.Arm) class. It is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.arm | |||
| a = stretch_body.arm.Arm() | |||
| a.motor.disable_sync_mode() | |||
| if not a.startup(): | |||
| exit() # failed to start arm! | |||
| a.home() | |||
| # interact with the arm here | |||
| ``` | |||
| Since both [Arm](#stretch_body.arm.Arm) and [Robot](#stretch_body.robot.Robot) subclass [Device](#stretch_body.device.Device), the same [`startup()`](#stretch_body.arm.Arm.startup) and [`stop()`](#stretch_body.arm.Arm.stop) methods are available here, as well as other [Device](#stretch_body.device.Device) methods such as [`home()`](#stretch_body.arm.Arm.home). Using the [Arm](#stretch_body.arm.Arm) class, we can read the arm's current state and send commands to the joint. For example, continuing the example above: | |||
| ```python linenums='11' | |||
| starting_position = a.status['pos'] | |||
| # move out by 10cm | |||
| a.move_to(starting_position + 0.1) | |||
| a.push_command() | |||
| a.motor.wait_until_at_setpoint() | |||
| # move back to starting position quickly | |||
| a.move_to(starting_position, v_m=0.2, a_m=0.25) | |||
| a.push_command() | |||
| a.motor.wait_until_at_setpoint() | |||
| a.move_by(0.1) # move out by 10cm | |||
| 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 commands to the arm respectively, while the nonblocking [`push_command()`](#stretch_body.arm.Arm.push_command) method pushes the queued command to the hardware for execution. Arm's attribute `motor`, an instance of the [Stepper](#stretch_body.stepper.Stepper) class, has [`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: | |||
| ```python linenums='26' | |||
| starting_position = a.status['pos'] | |||
| # queue a trajectory consisting of four waypoints | |||
| a.trajectory.add(t_s=0, x_m=starting_position) | |||
| a.trajectory.add(t_s=3, x_m=0.15) | |||
| a.trajectory.add(t_s=6, x_m=0.1) | |||
| a.trajectory.add(t_s=9, x_m=0.2) | |||
| # trigger trajectory execution | |||
| a.follow_trajectory() | |||
| import time; time.sleep(9) | |||
| ``` | |||
| Arm's attribute `trajectory`, an instance of the [PrismaticTrajectory](#stretch_body.trajectories.PrismaticTrajectory) class, has [`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: | |||
| ```python linenums='37' | |||
| range_upper_limit = 0.3 # meters | |||
| # set soft limits on arm's range | |||
| a.set_soft_motion_limit_min(0) | |||
| a.set_soft_motion_limit_max(range_upper_limit) | |||
| a.push_command() | |||
| # command the arm outside the valid range | |||
| a.move_to(0.4) | |||
| a.push_command() | |||
| a.motor.wait_until_at_setpoint() | |||
| 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 of [Arm's](#stretch_body.arm.Arm) subroutines are documented below. | |||
| ::: stretch_body.arm.Arm | |||
| ### Using the Lift class | |||
|  | |||
|  | |||
| The interface to Stretch's lift is the [Lift](#stretch_body.lift.Lift) class. It is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.lift | |||
| l = stretch_body.lift.Lift() | |||
| l.motor.disable_sync_mode() | |||
| if not l.startup(): | |||
| exit() # failed to start lift! | |||
| 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 to the [Arm](#stretch_body.arm.Arm): | |||
| ```python linenums='11' | |||
| starting_position = l.status['pos'] | |||
| # move up by 10cm | |||
| l.move_to(starting_position + 0.1) | |||
| l.push_command() | |||
| l.motor.wait_until_at_setpoint() | |||
| ``` | |||
| [Lift's](#stretch_body.lift.Lift) 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'] | |||
| # queue a trajectory consisting of three waypoints | |||
| l.trajectory.add(t_s=0, x_m=starting_position, v_m=0.0) | |||
| l.trajectory.add(t_s=3, x_m=0.5, v_m=0.0) | |||
| l.trajectory.add(t_s=6, x_m=0.6, v_m=0.0) | |||
| # trigger trajectory execution | |||
| l.follow_trajectory() | |||
| import time; time.sleep(6) | |||
| ``` | |||
| [Lift's](#stretch_body.lift.Lift) 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 has been 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 happens using: | |||
| ```python linenums='27' | |||
| # cut out 0.2m from the top and bottom of the lift's range | |||
| l.set_soft_motion_limit_min(0.2) | |||
| l.set_soft_motion_limit_max(0.8) | |||
| 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 of [Lift's](#stretch_body.lift.Lift) subroutines are documented below. | |||
| ::: stretch_body.lift.Lift | |||
| ### Using the Base class | |||
|  | |||
|  | |||
| | | Item | Notes | | |||
| | ------- | -------------- | --------------------------------------------------------- | | |||
| | A | Drive wheels | 4 inch diameter, urethane rubber shore 60A | | |||
| | B | Cliff sensors | Sharp GP2Y0A51SK0F, Analog, range 2-15 cm | | |||
| | C | Mecanum wheel | Diameter 50mm | | |||
| The interface to Stretch's mobile base is the [Base](#stretch_body.base.Base) class. It is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.base | |||
| b = stretch_body.base.Base() | |||
| b.left_wheel.disable_sync_mode() | |||
| b.right_wheel.disable_sync_mode() | |||
| if not b.startup(): | |||
| exit() # failed to start base! | |||
| # interact with the base here | |||
| ``` | |||
| Stretch's mobile base is a differential drive configuration. The left and right wheels are accessible through [Base's](#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: | |||
| ```python linenums='10' | |||
| b.pretty_print() | |||
| # translate forward by 10cm | |||
| b.translate_by(0.1) | |||
| b.push_command() | |||
| b.left_wheel.wait_until_at_setpoint() | |||
| # rotate counter-clockwise by 90 degrees | |||
| b.rotate_by(1.57) | |||
| 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 info 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: | |||
| ```python linenums='21' | |||
| # command the base to translate forward at 5cm / second | |||
| b.set_translate_velocity(0.05) | |||
| b.push_command() | |||
| import time; time.sleep(1) | |||
| # command the base to rotate counter-clockwise at 0.1rad / second | |||
| b.set_rotational_velocity(0.1) | |||
| b.push_command() | |||
| time.sleep(1) | |||
| # command the base with translational and rotational velocities | |||
| b.set_velocity(0.05, 0.1) | |||
| b.push_command() | |||
| time.sleep(1) | |||
| # stop base motion | |||
| b.enable_freewheel_mode() | |||
| b.push_command() | |||
| ``` | |||
| The [`set_translate_velocity()`](#stretch_body.base.Base.set_translate_velocity)/[`set_rotational_velocity()`](#stretch_body.base.Base.set_rotational_velocity) give velocity control over the translational/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 a (x, y) point and a theta orientation): | |||
| ```python linenums='39' | |||
| # reset odometry calculation | |||
| b.first_step = True | |||
| b.pull_status() | |||
| # queue a trajectory consisting of three waypoints | |||
| b.trajectory.add(time=0, x=0.0, y=0.0, theta=0.0) | |||
| b.trajectory.add(time=3, x=0.1, y=0.0, theta=0.0) | |||
| b.trajectory.add(time=6, x=0.0, y=0.0, theta=0.0) | |||
| # trigger trajectory execution | |||
| b.follow_trajectory() | |||
| import time; time.sleep(6) | |||
| print(b.status['x'], b.status['y'], b.status['theta']) # we should expect to see around (0.0, 0.0, 0.0 or 6.28) | |||
| b.stop() | |||
| ``` | |||
| !!! warning | |||
| The [Base's](#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. | |||
| [Base's](#stretch_body.base.Base) 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 of [Base's](#stretch_body.base.Base) subroutines are documented below. | |||
| ::: stretch_body.base.Base | |||
| ### Using the Head class | |||
|  | |||
|  | |||
| The interface to Stretch's head is the [Head](#stretch_body.head.Head) class. Stretch's head contains a Intel Realsense D435i depth camera, so the pan/tilt joints in the head allows Stretch to swivel and capture depth imagery of its surrounding. The head is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.head | |||
| h = stretch_body.head.Head() | |||
| if not h.startup(): | |||
| exit() # failed to start head! | |||
| h.home() | |||
| # interact with the head here | |||
| ``` | |||
| [Head](#stretch_body.head.Head) is a subclass of [DynamixelXChain](#stretch_body.dynamixel_X_chain.DynamixelXChain), which in turn subclasses 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 other 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) happens using: | |||
| ```python linenums='10' | |||
| starting_position = h.status['head_pan']['pos'] | |||
| # look right by 90 degrees | |||
| h.move_to('head_pan', starting_position + 1.57) | |||
| h.get_joint('head_pan').wait_until_at_setpoint() | |||
| # tilt up by 30 degrees | |||
| h.move_by('head_tilt', -1.57 / 3) | |||
| h.get_joint('head_tilt').wait_until_at_setpoint() | |||
| # look down towards the wheels | |||
| h.pose('wheels') | |||
| import time; time.sleep(3) | |||
| # look ahead | |||
| h.pose('ahead') | |||
| time.sleep(3) | |||
| ``` | |||
| [Head's](#stretch_body.head.Head) attribute `status` is a dictionary of dictionaries, where each subdictionary is the status of one of the head's joints. This state information is updated in the background in real time by default (disable by initializing as [`startup(threading=False)`](#stretch_body.head.Head.startup)). Use the [`pretty_print()`](#stretch_body.head.Head.pretty_print) method to print out this state info in a human interpretable format. 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; they're 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. 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'). The head supports waypoint trajectories as well: | |||
| ```python linenums='27' | |||
| # queue a trajectory consisting of three waypoints | |||
| h.get_joint('head_tilt').trajectory.add(t_s=0, x_r=0.0) | |||
| h.get_joint('head_tilt').trajectory.add(t_s=3, x_r=-1.0) | |||
| h.get_joint('head_tilt').trajectory.add(t_s=6, x_r=0.0) | |||
| h.get_joint('head_pan').trajectory.add(t_s=0, x_r=0.1) | |||
| h.get_joint('head_pan').trajectory.add(t_s=3, x_r=-0.9) | |||
| h.get_joint('head_pan').trajectory.add(t_s=6, x_r=0.1) | |||
| # trigger trajectory execution | |||
| h.follow_trajectory() | |||
| import time; time.sleep(6) | |||
| ``` | |||
| The head pan/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/tilt range happens using: | |||
| ```python linenums='38' | |||
| # clip the head_pan's range | |||
| h.get_joint('head_pan').set_soft_motion_limit_min(-1.0) | |||
| h.get_joint('head_pan').set_soft_motion_limit_max(1.0) | |||
| # clip the head_tilt's range | |||
| h.get_joint('head_tilt').set_soft_motion_limit_min(-1.0) | |||
| h.get_joint('head_tilt').set_soft_motion_limit_max(0.1) | |||
| h.stop() | |||
| ``` | |||
| 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 of [Head's](#stretch_body.head.Head) subroutines 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: | |||
| ```python linenums='1' | |||
| import stretch_body.end_of_arm | |||
| e = stretch_body.end_of_arm.EndOfArm() | |||
| if not e.startup(threaded=True): | |||
| exit() # failed to start end of arm! | |||
| # interact with the end of arm here | |||
| e.stop() | |||
| ``` | |||
| [EndOfArm's](#stretch_body.end_of_arm.EndOfArm) subroutines are documented below. | |||
| ::: stretch_body.end_of_arm.EndOfArm | |||
| ### Using the Wacc class | |||
| The interface to Stretch's wrist board is the [Wacc](#stretch_body.wacc.Wacc) (wrist + accelerometer) class. This board provides an Arduino and accelerometer sensor that is accessible from the [Wacc](#stretch_body.wacc.Wacc) class. It is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.wacc | |||
| w = stretch_body.wacc.Wacc() | |||
| if not w.startup(threaded=True): | |||
| exit() # failed to start wacc! | |||
| # interact with the wacc here | |||
| w.stop() | |||
| ``` | |||
| [Wacc's](#stretch_body.wacc.Wacc) subroutines are documented below. | |||
| ::: stretch_body.wacc.Wacc | |||
| ### Using the Pimu class | |||
| The interface to Stretch's power board is the [Pimu](#stretch_body.pimu.Pimu) (power + IMU) class. This board provides an 9 DOF IMUthat is accessible from the [Pimu](#stretch_body.pimu.Pimu) class. It is typically initialized as: | |||
| ```python linenums='1' | |||
| import stretch_body.pimu | |||
| p = stretch_body.pimu.Pimu() | |||
| if not p.startup(threaded=True): | |||
| exit() # failed to start pimu! | |||
| # interact with the pimu here | |||
| p.stop() | |||
| ``` | |||
| [Pimu's](#stretch_body.pimu.Pimu) subroutines are documented below. | |||
| ::: stretch_body.pimu.Pimu | |||
| ------ | |||
| <div align="center"> All materials are Copyright 2020 by Hello Robot Inc. The Stretch RE1 robot has patents pending</div> | |||