You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
542 lines
18 KiB
542 lines
18 KiB
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Stretch Body Guide: Commanding Motions\n",
|
|
"\n",
|
|
"The Stretch Body package provides a low level Python API to the Stretch RE1 hardware. In this guide, we'll look at using the package to command motions to the robot."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## Setup\n",
|
|
"\n",
|
|
"Stretch Body is available on PyPi as [hello-robot-stretch-body](https://pypi.org/project/hello-robot-stretch-body/). An accompanying command-line tools package called [hello-robot-stretch-body-tools](https://pypi.org/project/hello-robot-stretch-body-tools/) uses Stretch Body to provide convenient scripts to interact with the robot from the command-line. Both come preinstalled on Stretch RE1, but the following command can be used to ensure you have the latest version.\n",
|
|
"\n",
|
|
"In Jupyter notebooks, code preceded by `!` are run in the command-line instead of the Python interpreter."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"!python -m pip install -q -U hello-robot-stretch-body\n",
|
|
"!python -m pip list | grep hello-robot-stretch-body"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# The Robot Class\n",
|
|
"\n",
|
|
"Stretch Body exposes many classes, but we'll focus on the `stretch_body.robot.Robot` class to start. This Python class allows users to command motions to the entire robot. Additionally, it allows the user to access the current state of the robot.\n",
|
|
"\n",
|
|
"We'll import the `stretch_body.robot` module, as well as the `numpy` and `time` modules for other utility methods."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import time\n",
|
|
"import numpy as np\n",
|
|
"import stretch_body.robot"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Now, we'll instantiate an object of robot and call it `r`."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r = stretch_body.robot.Robot()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The first method we'll look at is called `startup()`. The cell below displays the docstring for `startup()`. As you can see, the method returns a boolean depending on whether or not the class startup procedure succeeded. Only one instance of the robot class can exist at once, so if another instance is running elsewhere, the method returns false."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.startup?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Below, we make the call to `startup()`. As part of the startup procedure, this method opens serial ports to the hardware devices, loads the parameters that dictate robot behavior, and launches a few helper threads to poll for status in the background."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.startup()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"If startup fails unexpectedly, the first thing to check is whether a background process is already running an instance of the robot class. Below we use the `pstree` command to list the tree of background processes, and `grep` to filter for scripts starting with \"stretch_\" (often the \"stretch_xbox_controller_teleop.py\" scripts is running in the background). If we see output below, we should use the `pkill` command to [terminate the conflicting process](https://docs.hello-robot.com/troubleshooting_guide/#rpc-transport-errors-stretch-doesnt-respond-to-commands)."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"!pstree | grep stretch_"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The second method we'll look at is called `stop()`. This method closes connections to all serial ports, releases other resources, and shuts down background threads. We'll wait until the end of the notebook to actually call the method."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.stop?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"With the startup procedure completed, we must check that the robot is \"homed\". The relevant method is called `is_homed()` and returns a boolean depending on whether the hardware has been homed."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.is_calibrated?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The robot must be homed once every time it has been turned on. The homing procedure allows the robot to determine where its joints are with respect to the limits of motion."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.is_calibrated()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The `home()` method puts the Stretch RE1 through the homing procedure. The method is blocking and will not return until all joints on the robot is homed. It returns a boolean depending on whether the robot homes successfully."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {
|
|
"scrolled": true
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.home()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The hardware now reports that the robot is homed."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.is_calibrated()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Next, we'll send the robot to its \"stow\" position with the `stow()` method. A custom stow position can be defined for any end-effector attached to the robot, however, the default position retracts the Arm and Wrist Yaw, and sends the Lift to 0.2m above the base. `stow()` is blocking as well and returns a boolean depending on whether stowing succeeds."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.stow()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The next method we'll look at is called `pretty_print()`. This method prints out the entire state of the robot in a human readable format."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.pretty_print()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"However, we'd often like to be able to access the robot's state programmatically. The `get_status()` method returns a dictionary with a snapshot of the current state of the robot."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.get_status()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Since we stowed the robot before, we expect the Lift to be at 0.2 meters. We can verify it like this:"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"stow_status = r.get_status()\n",
|
|
"lift_pos_m = stow_status['lift']['pos']\n",
|
|
"is_lift_close = np.isclose(lift_pos_m, 0.2, atol=1e-3)\n",
|
|
"\n",
|
|
"print('Lift position {0}m is near 0.2m? {1}'.format(lift_pos_m, is_lift_close))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Note that the status dictionary is a snapshot of the state of the robot when the method was called. If we change the state of the robot, the dictionary will be out of date.\n",
|
|
"\n",
|
|
"We'll look at one last method from the robot class called `push_command()`. This method takes commands queued at from the Python API and pushes it out to the hardware drivers."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.push_command?"
|
|
]
|
|
},
|
|
{
|
|
"attachments": {},
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Joint Classes\n",
|
|
"\n",
|
|
"Next, let's look at the classes within Stretch Body that we can send motion commands to.\n",
|
|
"\n",
|
|
" * `stretch_body.lift.Lift`: maintains the lift, a vertical prismatic joint\n",
|
|
" * `stretch_body.arm.Arm`: maintains the arm, a telescoping prismatic joint\n",
|
|
" * `stretch_body.base.Base`: maintains the mobile base, containing left and right motors in a diff drive configuration\n",
|
|
" * `stretch_body.head.Head`: maintains the head, containing tilt and pan revolute joints\n",
|
|
" * `stretch_body.end_of_arm.EndOfArm`: maintains the end-effector, typically a wrist yaw joint and a compliant gripper\n",
|
|
"\n",
|
|
"The robot class has an instance of each one of these joints, however, we could have instantiated a single joint to interact with just it. For example, we could create a lift object using `l = stretch_body.lift.Lift()`. Then, most of the methods we covered with the robot class can be used with the lift class, including `l.startup()`, `l.stop()`, `l.home()`, and `l.pretty_print()`. The same applies to the other joint classes too.\n",
|
|
"\n",
|
|
"The robot class makes an instance of each joint on the robot and handles startup, homing, and stopping automatically. Therefore, to interact with the lift joint, we can access its class from `r.lift`."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.lift"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Let's use robot's joint classes to determine the range of motion of each joint on the robot. Each joint class has an attribute called `params`, which is a dictionary of parameters dictating how the class behavies. We can find the joint range in these dictionaries."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"lift_range_m = r.lift.params['range_m']\n",
|
|
"arm_range_m = r.arm.params['range_m']\n",
|
|
"wrist_yaw_range_rad = r.end_of_arm.motors['wrist_yaw'].soft_motion_limits\n",
|
|
"stretch_gripper_range_rad = r.end_of_arm.motors['stretch_gripper'].soft_motion_limits\n",
|
|
"head_tilt_range_rad = r.head.motors['head_tilt'].soft_motion_limits\n",
|
|
"head_pan_range_rad = r.head.motors['head_pan'].soft_motion_limits\n",
|
|
"\n",
|
|
"is_lift_within_range = lambda pos: pos >= lift_range_m[0] and pos <= lift_range_m[1]\n",
|
|
"is_arm_within_range = lambda pos: pos >= arm_range_m[0] and pos <= arm_range_m[1]\n",
|
|
"is_wrist_yaw_within_range = lambda pos: pos >= wrist_yaw_range_rad[0] and pos <= wrist_yaw_range_rad[1]\n",
|
|
"is_stretch_gripper_within_range = lambda pos: pos >= stretch_gripper_range_rad[0] and pos <= stretch_gripper_range_rad[1]\n",
|
|
"is_head_tilt_within_range = lambda pos: pos >= head_tilt_range_rad[0] and pos <= head_tilt_range_rad[1]\n",
|
|
"is_head_pan_within_range = lambda pos: pos >= head_pan_range_rad[0] and pos <= head_pan_range_rad[1]\n",
|
|
"\n",
|
|
"print('Lift range is {0} meters'.format(lift_range_m))\n",
|
|
"print('Arm range is {0} meters'.format(arm_range_m))\n",
|
|
"print('Mobile base has no joint limits')\n",
|
|
"print('End effector wrist_yaw range is {0} radians'.format(wrist_yaw_range_rad))\n",
|
|
"print('End effector gripper range is {0} radians'.format(stretch_gripper_range_rad))\n",
|
|
"print('Head tilt range is {0} radians'.format(head_tilt_range_rad))\n",
|
|
"print('Head pan range is {0} radians'.format(head_pan_range_rad))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The joint classes make two methods available to allow users to issue position commands. The `move_to()` and `move_by()` methods move a joint to a specific position value and by a position delta respectively."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.lift.move_to?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"We combine `move_to()` and `move_by()` commands with `push_command()` to trigger execution of the motion command. In the cell below, we command the robot to move to 0.3 meters above the base. Since the robot had been in the stow position, we expect it to move upwards by 10 centimeters."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.lift.move_to(0.3)\n",
|
|
"r.push_command() # pushes command to the hardware"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Let's confirm that lift is at 0.3m and within the valid range of the lift (typically 0.0 to 1.1 meters)."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"moveto_status = r.get_status()\n",
|
|
"lift_pos_m = moveto_status['lift']['pos']\n",
|
|
"is_lift_close = np.isclose(lift_pos_m, 0.3, atol=1e-3)\n",
|
|
"lift_within_range = is_lift_within_range(lift_pos_m)\n",
|
|
"\n",
|
|
"print('Lift position {0}m is near 0.3m and within lift range of {1}? {2}'.format(lift_pos_m, lift_range_m, is_lift_close and lift_within_range))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Note that `push_command()` doesn't block until the motion is completed. This allows your code to monitor the robot state and even send overriding commands while the robot is executing a motion. Below, we send the lift to 0.2m again and monitor the lift position until it reaches the desired goal. We loop until the lift has stopped moving by checking the \"is_moving_filtered\" flag."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {
|
|
"scrolled": true
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.lift.move_by(-0.1)\n",
|
|
"r.push_command()\n",
|
|
"\n",
|
|
"time.sleep(0.4) # give the lift some time to start moving\n",
|
|
"while r.get_status()['lift']['motor']['is_moving_filtered']:\n",
|
|
" time.sleep(0.2)\n",
|
|
" print(r.get_status()['lift']['pos'])"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"While `r.lift`, `r.arm`, `r.end_of_arm`, and `r.head` have the `move_to()` and `move_by()` methods, the mobile base `r.base` has slightly different methods. Because the mobile base can translate and rotate, the mobile base has `translate_by()` and `rotate_by()`."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.base.rotate_by?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The cell below commands the robot to rotate 45 degrees. A commented command can send the robot in a full circle. Be wary of cables attached to the base before uncommenting this command."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.base.rotate_by(np.pi / 4)\n",
|
|
"# r.base.rotate_by(np.pi * 2) # rotates a full circle, check no cables will get caught\n",
|
|
"r.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The `push_command()` method triggers synchronous motion of all issued commands. This is often important in robotics because tasks like grasping required coordinated execution of all joints together. Below, we issue a whole body coordinated command."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.base.rotate_by(-np.pi / 4)\n",
|
|
"r.lift.move_by(0.3)\n",
|
|
"r.arm.move_to(0.2)\n",
|
|
"r.end_of_arm.move_to(\"stretch_gripper\", 0.0)\n",
|
|
"r.end_of_arm.move_to(\"wrist_yaw\", 0.0)\n",
|
|
"r.head.move_to(\"head_tilt\", -np.pi / 4)\n",
|
|
"r.head.move_to(\"head_pan\", -np.pi)\n",
|
|
"r.push_command()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.stow()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Wrapping Up\n",
|
|
"\n",
|
|
"In this notebook, we've covered:\n",
|
|
"\n",
|
|
" * Classes and methods available in Stretch Body \n",
|
|
" * Fetching robot state from the `stretch_body.robot.Robot` class\n",
|
|
" * Commanding motions to the various joints on the robot.\n",
|
|
"\n",
|
|
"For more information on Stretch Body API, take a look at the [API Documentation](https://docs.hello-robot.com/stretch_body_guide/). To reports bugs or contribute to the library, visit the [Stretch Body Github repo](https://github.com/hello-robot/stretch_body/) where development on the library happens. Also, feel free to join our community on the [forum](https://forum.hello-robot.com/) and learn about research/projects happening with Stretch."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"r.stop()"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "Python 2",
|
|
"language": "python",
|
|
"name": "python2"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 2
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython2",
|
|
"version": "2.7.17"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 2
|
|
}
|