Collisions and Constraints in MoveIt!

Description: This tutorial describes how MoveIt!'s state validation services are used, which includes collision and constraint checking. The tutorial also looks at how collision checking and constraints are employed in MoveIt!'s motion generation. CRAM interfaces to MoveIt!'s functionality are presented.

Previous Tutorial: Using MoveIt! from CRAM

Next Tutorial: (coming soon)

Let's restart the tutorial. If you have the moveit pr2_config demo running, close it and rerun it. Once the RViz window appears, make sure you have the “Loop animation” option ticked in the Planned Path submenu on the window's left side.

Ensure you have tf running. You don't need to restart it, but if it is not running then open a terminal tab and run

rosrun tf2_ros buffer_server

If you have a REPL window running, close it, restart the REPL, then run

(swank:operate-on-system-for-emacs "cram-intermediate-tutorial" (quote load-op))
(in-package :moveit)
(tuti:init-cram-moveit-tutorial)

You should now see only the PR2, in the default configuration, in the RViz window; there are no other objects in the world. We'll add one by running the following in the REPL:

(register-collision-object "cube" :mesh-shapes (list tuti:*cube-mesh*))

(add-collision-object "cube" tuti:*pose-cube*)

We'll now create two robot state messages, representing joint states at two poses. One pose puts the right end effector to the left of the cube, and the other puts the end effector inside it.

(defvar rs-mid nil)
(defvar rs-cube nil)
 
(setf rs-mid (compute-ik "r_wrist_roll_link" "right_arm" tuti:*pose-mid* :avoid-collisions nil))
(setf rs-cube (compute-ik "r_wrist_roll_link" "right_arm" tuti:*pose-cube* :avoid-collisions nil))

Note that for this example we run compute-ik with state validation disabled, but even so it is in general good practice to validate robot states when you need to move to them, even if they were good at generation time. The environment may have changed since then.

To check whether a state is valid, we use the cram-moveit:check-state-validity function. Let's try it:

(check-state-validity rs-mid "right_arm" (roslisp:make-msg "moveit_msgs/Constraints"))

First, let's look t the parameters. The first is the robot state we wish to check, and the second names the planning group we specifically want to verify. For speed reasons, state checking can be restricted to a subset of the robot's links; if you want to check the entire robot state, you would use a planning group made up of all the robot's links. Third parameter is a Constraints message through which you specify what other kinematic constraints you wish the state to satisfy.

We'll look at constraints later. For now let's just see what the response from check-state-validity was, for a simple collision check. The response should look something like this:

(("state validity" T) ("contacts" #()) ("cost sources" #())
 ("constraint check result" #()))

As you can see, it is a list of named pairs, and you can rely on the pairs being in the same order always (the names are just there for user readability). First pair contains a boolean value indicating whether the state respects the defined constraints AND does not collide with any obstacles. In the simplest use cases, that's all you need. The other named pairs contain vectors of ROS messages that describe what, if any, violations occur, whether these are contacts/collisions with objects in the environment or constraint violations.

Let's try to check the other state:

(check-state-validity rs-cube "right_arm" (roslisp:make-msg "moveit_msgs/Constraints"))

You should see as a response something like:

(("state validity" NIL)
 ("contacts"
  #([MOVEIT_MSGS-MSG:CONTACTINFORMATION
       HEADER:
         (STD_MSGS-MSG:HEADER (:SEQ . 0) (:STAMP . 1.4334884644863384d9)
          (:FRAME_ID . "/odom_combined"))
       POSITION:
         (GEOMETRY_MSGS-MSG:POINT (:X . 0.626802326696518d0)
                                  (:Y . -0.19797068080424754d0)
                                  (:Z . 0.9014074405165766d0))
       NORMAL:
         (GEOMETRY_MSGS-MSG:VECTOR3 (:X . -1.0d0) (:Y . 0.0d0) (:Z . 0.0d0))
       DEPTH:
         0.006802310752275553d0
       CONTACT_BODY_1:
         "CUBE"
       BODY_TYPE_1:
         1
       CONTACT_BODY_2:
         "r_forearm_link"
       BODY_TYPE_2:
         0]))
 ("cost sources"
  #(<a long vector of messages>))
 ("constraint check result" #()))

Notice that this state is invalid, as expected. Also, notice that the contacts vector tells you what objects collide– in this case, a link on the robot and the cube– as well as information about the contact: depth of interpenetration, a contact position and a normal. The last two are well defined for shallow interpenetration depths, and you can think of them as describing a point and direction for a resistance force to be applied from the first contact body to the second.

MoveIt! should return all contacts it finds between links in the planning group and other objects in the environment (including other links that are not adjacent to each other in the robot's kinematic chain). If links from other groups are also in contact with obstacles, these will not be reported– those links didn't get checked.

(Author's note: cost sources, defined and computed in the package FCL that MoveIt! calls for collision checks, try to approximate contact regions via axis aligned bounding boxes. It's not clear however which features are being used when creating the AABBs; the features are certainly not the complete mesh, and seem instead to center around face intersections.)

Collision and constraint checking are considered by MoveIt! while doing motion generation as well. For example, let's try this now:

(compute-cartesian-path "/odom_combined"
                        rs-mid
                        "right_arm" 
                        "r_wrist_roll_link" 
                        (list tuti:*pose-right-msg*)
                        0.1 
                        1.5 
                        t 
                        (roslisp:make-msg "moveit_msgs/Constraints"))

We ask MoveIt! to create a simple, linear path for the right wrist roll link that will take it from one side of the cube to the other. But that means the motion must pass through the cube, so obviously it will fail; you can see in the RViz window that MoveIt created an incomplete path that stops just before hitting the cube, and you can see in the return values in the REPL window that the completion fraction is below 1.

Sometimes however collisions between the robot and objects in the environment are ok, and even desired. To handle such cases, we will look at the Allowed Collision Matrix next.

MoveIt!'s Allowed Collision Matrix (or acm) is a datastructure containing pairs of object names associated to boolean values. If the value is true, then its associated object pair is allowed to collide without invalidating a state. Allowed collisions do not show up in responses from check-state-validity. If an object pair does not appear in the acm, it is assumed that collisions between those objects is not tolerated.

The acm is useful when you want a more fine-tuned collision checking behavior. Perhaps your robot should touch certain objects (this is a certain requirement for manipulation, unless the PR2 is telekinetic), or perhaps you have some objects in the planning scene that are there just for visualization purposes. In any case, you can specify what collisions to ignore.

Let's remember first how we can retrieve the acm from the planning scene. In the REPL, run the following:

(defvar ps-acm nil)
(setf ps-acm (second (first (get-planning-scene-info :allowed-collision-matrix t))))

Let's query the matrix first. Try:

(get-collision-matrix-entry ps-acm "CUBE" "r_forearm_link")

(Note: cram-moveit stores objects with names in all-caps, and those are the names it sends to MoveIt! when adding/removing objects. Therefore, the object that MoveIt! knows about is “CUBE”, not “cube”. Also, cram-moveit cannot send names of objects for acm queries in all caps, because the entries of the acm are mostly populated by MoveIt!, which doesn't follow the all-caps names convention. As a result, you'll have to manually all-caps cram-moveit object names in acm queries. TODO: perhaps remove the cram-moveit all-caps convention?)

The response you should get is nil, because this entry wasn't defined yet. Since we'll want to allow some links of the robot to pass through the cube, we'll need to define that entry (and a few others) so we run:

(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_forearm_link" T))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_roll_link" T))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_flex_link" T))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_palm_link" T))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_r_finger_link" T))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_l_finger_link" T))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_motor_accelerometer_link" T))

(Note that set-collision-matrix-entry returns a modified acm, but does not change the original acm however.)

You can try get-collision-matrix-entry on one of those pairs to verify that its value is now T.

The above calls only affected cram-moveit variables, so we now need to make the MoveIt! planning scene aware of the changes. Run the following in the REPL:

(set-planning-scene-collision-matrix ps-acm)

and lets try the cartesian path request again:

(compute-cartesian-path "/odom_combined"
                        rs-mid
                        "right_arm" 
                        "r_wrist_roll_link" 
                        (list tuti:*pose-right-msg*)
                        0.1 
                        1.5 
                        t 
                        (roslisp:make-msg "moveit_msgs/Constraints"))

As you can see, the robot arm now passes through the cube with no protest.

Before we proceed, let's remove the new entries from the collision matrix because we will want the cube to function as an obstacle again.

(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_forearm_link" nil))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_roll_link" nil))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_flex_link" nil))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_palm_link" nil))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_r_finger_link" nil))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_l_finger_link" nil))
(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_motor_accelerometer_link" nil))
(set-planning-scene-collision-matrix ps-acm)

NOTE: MoveIt! will NOT remove entries in the allowed collision matrix when an object is removed from the planning scene. For clean removal, you should remember to do this yourself.

Sometimes motion must satisfy more requirements than just getting from A to B without bumping into anything. Suppose, for example, that the robot is carrying a glass full of water, in which case it shouldn't tilt the glass too hard or it will spill. MoveIt! allows you to impose constraints on motion planning, cartesian trajectory generation, and state validity checking. We'll first look at what kinds of constraints MoveIt! supports, and then show an example of how to use a constraint type for motion planning.

MoveIt! defines several type of constraints, which are detailed below. Every type of constraint has a weight parameter, and checking for constraints will return a boolean value indicating whether the constraint is satisfied, and a “distance” (better thought of as cost) from perfect constraint satisfaction. The distance is computed by multiplying the weight parameter with some relevant metric of constraint satisfaction; different constraint types use different metrics.

joint constraints: specify a value and an interval around this value that a joint should be at. A JointConstraint message contains the following:

• joint_name
• position, the joint value to reach
• tolerance_above and tolerance_below
• and the weight parameter.

If joint_name is not a recognized joint name, the constraint evaluates to perfectly satisfied.

position constraints: specify a region that a given link reference point should be at. A PositionConstraint message contains:

• header (whose frame_id specifies the frame name in which the poses for the constraint region shapes are given)
• link_name
• target_offset which is a Vector3 message giving the target point in the link_name frame
• constraint_region, which is a BoundingVolume message containing:
  • primitives
  • primitive_poses
  • meshes
  • mesh_poses, all of which are vectors of messages specifying primitives (BOX, SPHERE, CYLINDER or CONE), meshes, and their poses in the frame given by frame_id from the PositionConstraint header
• and the weight parameter.

The constraint is satisfied if the link reference point is in at least one of the regions specified by constraint_region. The “distance” part of the result is then the weight multiplied by the distance between the link's reference point and the target point in the constraint. If link_name is not recognized or constraint_region is empty, then the constraint is considered perfectly satisfied.

orientation constraints: specify an orientation for a link, with tolerances in the Roll-Pitch-Yaw angles. An OrientationConstraint message contains:

• header (whose frame_id names the frame in which the orientation is given)
• link_name
• orientation
• absolute_{x, y, z}_axis_tolerance give the tolerances for roll, pitch, and yaw respectively
• and the weight parameter.

The constraint is satisfied if the link orientation RPY value doesn't differ from the target one by amounts larger than those given by the tolerances. The “distance” part of the result is then the weight multiplied by the sum of absolute values of the errors in roll, pitch, yaw. If link_name is not recognized, the constraint is considered to be satisfied perfectly.

visibility constraints: test whether the robot can see a specified region. IMPORTANT: only tests whether the robot obstructs its own line of sight, does not take into account world objects or sensor minimum/maximum range. For the latter, use PositionConstraint. A VisibilityConstraint message contains:

• target_radius, the radius of the area to see
• target_pose, pose-stamped of the area center that should be seen, includes orientation
• cone_sides, number of sides to approximate the base of a visibility cone with
• sensor_pose, pose-stamped of the sensor
• max_view_angle, highest tolerated incidence angle of rays from the sensor to the area center
• max_range_angle, highest angular size that the target area is allowed to occupy in the sensor's field of view
• sensor_view_direction, 0 for Z, 2 for X
• and the weight parameter.

The constraint is satisfied if there is no collision between a visibility cone (placed between the sensor and the target) and the robot, AND the incidence angle of the ray to the target area center is below max_view_angle, AND the target area occupies less than max_range_angle from the sensor's field of view, AND the sensor faces toward the target area. If these conditions apply, the constraint is considered perfectly satisfied.

If the angle conditions are met, but there is a collision between the visibility cone and the robot, then the constraint fails, and the “distance” part of the result is the depth of the collision (NOTE: not multiplied by the weight parameter, which may be a MoveIt! bug). If one of the angle conditions fails, then the constraint fails, and the “distance” part of the result is set to 0.

If the target radius is on the order of machine epsilon or smaller, the constraint is considered perfectly satisfied.

From the previous part of the tutorial, we should have a running REPL with the cram moveit tutorial loaded and initialized, an RViz window showing a PR2 and an environment containing only a cube (which is now an obstacle: robot links are not allowed to collide with it), and MoveIt! running in the background.

cram-moveit also allows you to define path constraints for compute-cartesian-path, move-link-pose, as well as imposing constraints for state validity checking, but for the example below we will issue a planning request with constraints.

Let's first try a simple motion plan request, in which we ask the robot to move from one pose around the cube to another:

(plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right* :start-robot-state rs-mid)

(Note the use of the &key parameter start-robot-state to plan a motion from a state that might not be the robot's current state. Also, we're not interested in executing the planned trajectory; we just want to see it in RViz. That's why we don't have the multiple value bind construct around this call, like we did in the previous tutorial.)

Run that plan request a few times and watch the path taken by the end effector in RViz. You'll notice that the planner has no objection to rotating the gripper in various ways; it hasn't been told not to do it.

So let's do just that:

(defvar or-con nil)
(setf or-con 
      (roslisp:make-msg "moveit_msgs/Constraints"
                        :orientation_constraints (vector 
                                                   (roslisp:make-msg "moveit_msgs/OrientationConstraint"
                                                                     :header (roslisp:make-msg "std_msgs/Header"
                                                                                               :frame_id "/odom_combined")
                                                                     :orientation (roslisp:make-msg "geometry_msgs/Quaternion"
                                                                                                    :x 0.0
                                                                                                    :y 0.0
                                                                                                    :z 0.0
                                                                                                    :w 1.0)
                                                                     :link_name "r_wrist_roll_link"
                                                                     :absolute_x_axis_tolerance 0.01
                                                                     :absolute_y_axis_tolerance 0.01
                                                                     :absolute_z_axis_tolerance 0.01
                                                                     :weight 1.0))))
(plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right* :start-robot-state rs-mid 
                    :path-constraints-msg or-con)

(We leave some tolerance in the axis to help the sampler; a too strict constraint may be impossible to satisfy anyway.)

Unfortunately, if you try the code above, and assuming you've made no changes to the pr2_moveit_config package, you will see an error signal in the REPL window, and if you look in the RViz window you'll see that the planned path attempts to pass through the cube. What happened?

There is a solution however– look below.

MoveIt! does not do continuous collision checking, which means, instead, that it chooses some points along a robot trajectory segment and does collision checking on them. Usually this won't affect you, unless you work with very narrow objects and/or constraints.

Narrow objects are a problem because the poses MoveIt! chooses to check along a trajectory segment may just miss the object, even if, in fact, the trajectory passes through the object. Kinematic constraints seem to be a problem for a more complicated reason: there tends to be more space, and longer segments, between samples produced by constrained samplers.

There is a tradeoff between increasing the resolution of the collision checks along a trajectory segment and planning time. Sadly, MoveIt! doesn't yet have a way for you to set the resolution at run-time.

For the example above to work, you will have to close move_group, go to pr2_moveit_config/config and edit the ompl_planning.yaml file. Notice that each planning group has a variable longest_valid_segment_fraction. Set its value to 0.005 (rather than the original 0.05), then rerun the pr2_moveit_config/launch/demo.launch file.

From the REPL, rerun the lisp code we tried above, for sending a constrained plan request. This time you should receive a multi-value consisting of a robot state and a robot trajectory as answer– and the trajectory is a valid solution. You can also observe it in RViz (remember to enable Loop Animation from the Planned Path submenu on the left): notice how the planned path keeps the end effector in the same orientation while it moves around the cube.

Coming soon!