Add constraints and transforms to a motion plan | Tutorial

Add constraints and transforms to a motion plan

Say you want your robot to pass you a cup of tea, but you don’t want it to spill the water or bump into other objects on the table.

If you followed along with the Plan Motion with an Arm tutorial, you used the motion service to move a robot arm and end effector to desired positions. This tutorial builds on this foundation.

In this tutorial, you will learn about transforms and constraints with the example of moving a cup across a table without hitting another object, and while remaining upright.

Learning Goals

After following this tutorial, you will be able to:

use transforms to represent objects that are connected to a machine without being components of the machine
use constraints to control the way your machine moves between its start and end position

The full code is available at the end of this page.

Caution

Be careful when instructing robot arms to move. Before running any code, ensure your robotic arm has enough space and that there are no obstacles. Also pay attention to your surroundings, double-check your code for correctness, and make sure anyone nearby is aware and alert before issuing commands to your robot.

Prerequisites

Before starting this tutorial, you must:

Install the Viam Python SDK.
If you are connecting to a real robotic arm during this tutorial, make sure your computer can communicate with the arm controller before continuing. Code examples in this tutorial use a UFACTORY xArm 6, but you can use any arm model including a fake arm model.
Complete the previous tutorial, Plan Motion with an Arm and a Gripper, which configures the robot, client and service access, and other infrastructure we’ll need for this tutorial. For reference, see the full code sample from the prior tutorial.

Configure your robot

Use the same machine configuration from the previous tutorial for this tutorial, including the arm and gripper components with frames configured. Make one change: Change the Z translation of the gripper frame from 90 to 0.

The motion service is one of the “built-in” services, so you don’t need to do anything to enable it on your robot.

Click to see what your raw JSON config should look like.

If you completed the previous tutorial, your robot’s configuration should match the following. You can view your machine configuration on the CONFIGURE tab by selecting JSON mode in the left-hand menu.

If instead you create a new machine for this tutorial, copy and paste the following configuration into the JSON field:

Modify your robot’s working environment

In the previous tutorial, you defined a table obstacle to prevent the arm and gripper from hitting the table upon which the arm is mounted. In this tutorial, you’ll expand on the code that describes your robot’s working environment in two ways:

Define an additional obstacle: a tissue box sitting on the table.
Add a z_offset parameter to the table_origin and to the new box_origin. This makes it easier to calibrate your motion plans based on how high or low your arm is mounted compared to the table.

The following example shows the two obstacles defined such that the table’s top surface is at z=0 and the tissue box sits on top of the table. Adjust the dimensions and positions of the obstacles to describe your own scenario:

Determine the gripper’s axes

You need to identify how the gripper’s frame corresponds to its hardware so that you can write code that creates your desired behavior. That is, you need to determine which axis points from the “wrist” to the end of the gripper, which axis lines up with the direction the jaws actuate, and, when the gripper is holding a cup, which axis passes vertically through the cup.

All example code below assumes +Z points from the base of the gripper to the point where its jaws close, +Y points towards the ground when the gripper is holding a cup from the side, and the X axis is the axis along which the jaws close, following the right-hand rule. The following diagram shows this, as well as the global coordinate system.

If you are using a fake gripper, there is no real hardware to calibrate and you can continue to the next section, imagining that your fake gripper corresponds to the diagram above.

If you are using a real arm and gripper, use the CONTROL tab to move the gripper, look at its reported orientations, and map them to its orientation in the real world. If the axes are different from those described above, take these differences into account in your code.

Use a transform to represent a drinking cup

Imagine your cup is 120 millimeters tall with a radius of 45 millimeters. You need to take this space into account to avoid bumping objects on the table with the cup.

You can pass transforms to the motion service move method to represent objects that are connected to the robot but are not actual robotic components. To represent the drinking cup held in your robot’s gripper, create a transform with the cup’s measurements:

Here, we use a 155mm offset along the gripper’s Z axis to represent the distance from the base of the gripper to the center of the cup. We assume the cup is situated against the palm of the gripper’s hand. This offset is then equal to the radius of the cup (45mm) plus the distance from the gripper’s origin (where it meets the arm) to its palm (110mm in this example). If your gripper has different dimensions, change the offset accordingly.

Now that you have created the table and tissue box obstacles as well as the cup transform, create a WorldState that includes all of them:

Define start and end poses

You need to tell the robot where to pick up the cup, and where to put it down. The previous tutorial introduced the concept of defining poses. For this tutorial, you’ll define a pose for the gripper that is just above the table on one side of the tissue box, and another pose on the other side of the tissue box.

You’ll also want to define some waypoints. The motion service has the mathematical ability to plan complex motion from one position to another, even around an obstacle. However, this is more computationally intensive than planning a linear path from one point to another. To increase your program’s efficiency, add waypoints such that the path between any two consecutive points can be linear, without intersecting the tissue box or the table:

Though we are passing these waypoints in manually, the motion service will throw an error if we accidentally pass in a pose that would cause the arm or gripper to hit any obstacles we’ve defined.

Note that the orientations of all the poses are the same. If we changed the orientation along the way, we might spill the tea!

Tip

You may be wondering how the orientations of the poses are determined. Our example gripper’s frame is defined such that its orientation vector points from its “wrist” to the tip of its jaws. In the example code above, all poses have an orientation vector pointing along the positive X axis of the world frame, which is a horizontal orientation pointing “forwards” with respect to the xArm 6 base. When we tell the gripper to move to such a pose, its orientation vector moves to align with the orientation vector of the pose, so its jaws end up pointing along the global X axis, “forwards” from the robot base. This puts it in a good position for picking up and moving a cup.

Additionally, for our gripper, setting theta=0 about this particular orientation vector orients the gripper such that its jaws open and close horizontally. If we changed it to theta=90 or theta=270, the gripper jaws would open vertically, not ideal for picking up a cup!

Add a motion constraint

To keep the cup upright as the arm moves it from one place on the table to another, create a linear constraint. When you tell the robot to move the cup from one upright position to another, the linear constraint forces the gripper to move linearly and to maintain the upright orientation of the cup throughout the planned path.

You could try using an orientation constraint instead, which would also constrain the orientation. However, since this opens up many more options for potential paths, it is much more computationally intensive than the linear constraint.

The code below creates a linear constraint and then uses that constraint to keep the cup upright and move it in a series of linear paths along the predetermined route while avoiding the obstacles we’ve defined:

Full code

The following code contains everything covered in this tutorial in addition to the connect() function, and the resource access code from the last tutorial that you need here as well. Be sure to change the placeholders shown in the code to match your actual machine credentials, and change all relevant parameters such as z_offset and other dimensions and poses to match your hardware.

Next steps

If you would like to continue onto working with Viam’s motion service, check out this tutorial:

[There should have been a video here but your browser does not seem to support it.

Claw Game

Create your own version of the famous arcade claw machine using a robotic arm and a claw grabber.](https://docs.viam.com/tutorials/projects/claw-game/)