This is an old revision of the document!


Using MoveIt! from CRAM

Description: In this tutorial you will be introduced to some of the functionalities offered by MoveIt! and the interfaces between it and CRAM.

Next Tutorial: (coming soon)

Setting up your system

Playing with the turtlesim package may be nice, but a simulated turtle will only take you so far. There are more complicated (and real) robots out there, together with a whole host of software packages to fulfill the functions needed to make a robot work, like perception, environment representation, motion, control etc. In the ROS world, using such other software packages involves exchanging messages through topics and service requests in a fairly standard fashion; you could write interfaces as needed.

Fortunately, CRAM already offers you interfaces to some other useful software packages, and this tutorial will present one of them: MoveIt!. Of course, to run the tutorial, you will have to make sure MoveIt! exists on your system. For installation guidelines, see here.

You will also need the tutorial initialization code. Go to the src folder of your ROS workspace and check whether the cram_tutorials folder exists, and whether it contains cram_intermediate_tutorials. If not, retrieve this code from github.

Initializing cram_bridge/cram_moveit

You will need three terminal tabs. In one of them, run

rosrun tf2_ros buffer_server

This will start a tf2 buffer server, which CRAM will call upon to see transformations between coordinate frames in the robot's world.

In another tab, run

roscd pr2_moveit_config
roslaunch ./launch/demo.launch

It will start move_group (an instance of the MoveIt! server node) and RViz (the Robot Visualization tool). The scenario for this demo is extremely simple– one PR2 in an empty world– but that's good enough for the purposes of this tutorial.

Finally, in the last tab run the roslisp REPL. Once it has started up, type the following at its command prompt:

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

This will load the tutorial code (and its dependencies, in particular cram-moveit), and will initialize a few variables we'll be using later. Also, it will start a ROS node from the REPL, which you need to do in order to send messages and service requests to other ROS nodes like move_group, and it will initialize the communication with MoveIt!. It defines publishers for updates to the planning scene (so you can modify what objects exist in the robot's environment), subscribes to robot joint state updates, and creates an actionclient object so that you can send requests to move_group.

For now, tuti:init-cram-moveit-tutorial will do all that for you. In your own programs, if you need to make sure those steps happen then you will have to call (or make sure that they are called)

  (roslisp:start-ros-node node-name)
  (moveit:init-moveit-bridge)

Using MoveIt!

Accessing the robot state

After all that setup, it's time to interact with MoveIt! Go to your REPL window and type:

(joint-states)

This function simply asks MoveIt! about where each joint on the robot is in its parameter space. The answer will likely be rather long, as the robot has many joints:

(("fl_caster_rotation_joint" . 0.0d0) ("fl_caster_l_wheel_joint" . 0.0d0)
 ("fl_caster_r_wheel_joint" . 0.0d0) ("fr_caster_rotation_joint" . 0.0d0)
 ("fr_caster_l_wheel_joint" . 0.0d0) ("fr_caster_r_wheel_joint" . 0.0d0)
 ("bl_caster_rotation_joint" . 0.0d0) ("bl_caster_l_wheel_joint" . 0.0d0)
 ("bl_caster_r_wheel_joint" . 0.0d0) ("br_caster_rotation_joint" . 0.0d0)
 ("br_caster_l_wheel_joint" . 0.0d0) ("br_caster_r_wheel_joint" . 0.0d0)
 ("torso_lift_joint" . 0.16825d0) ("torso_lift_motor_screw_joint" . 0.0d0)
 ("head_pan_joint" . 0.0d0) ("head_tilt_joint" . 0.0d0)
 ("laser_tilt_mount_joint" . 0.0d0) ("r_shoulder_pan_joint" . 0.0d0)
 ("r_shoulder_lift_joint" . 0.0d0) ("r_upper_arm_roll_joint" . 0.0d0)
 ("r_forearm_roll_joint" . 0.0d0) ("r_elbow_flex_joint" . -1.13565d0)
 ("r_wrist_flex_joint" . -1.05d0) ("r_wrist_roll_joint" . 0.0d0)
 ("r_gripper_motor_slider_joint" . 0.0d0)
 ("r_gripper_motor_screw_joint" . 0.0d0) ("r_gripper_l_finger_joint" . 0.0d0)
 ("r_gripper_r_finger_joint" . 0.0d0) ("r_gripper_l_finger_tip_joint" . 0.0d0)
 ("r_gripper_r_finger_tip_joint" . 0.0d0) ("r_gripper_joint" . 0.0d0)
 ("l_shoulder_pan_joint" . 0.0d0) ("l_shoulder_lift_joint" . 0.0d0)
 ("l_upper_arm_roll_joint" . 0.0d0) ("l_forearm_roll_joint" . 0.0d0)
 ("l_elbow_flex_joint" . -1.13565d0) ("l_wrist_flex_joint" . -1.05d0)
 ("l_wrist_roll_joint" . 0.0d0) ("l_gripper_motor_slider_joint" . 0.0d0)
 ("l_gripper_motor_screw_joint" . 0.0d0) ("l_gripper_l_finger_joint" . 0.0d0)
 ("l_gripper_r_finger_joint" . 0.0d0) ("l_gripper_l_finger_tip_joint" . 0.0d0)
 ("l_gripper_r_finger_tip_joint" . 0.0d0) ("l_gripper_joint" . 0.0d0))

Notice that it's a list of lists (joint-name joint-values), which you could search for the joints that interest you. Or, better, you could just ask MoveIt! for the joints you're interested in:

(joint-states (list "r_wrist_roll_joint" "l_wrist_roll_joint"))

in which case the answer will be much more concise

(("r_wrist_roll_joint" . 0.0d0) ("l_wrist_roll_joint" . 0.0d0))

What happens if you make a mistake and ask MoveIt! for a joint name that doesn't exist? It will simply return an empty list for that joint, and only return values for the joints that are actually there:

(joint-states (list "fictional_joint_that_isnt_there" "l_wrist_roll_joint"))
 
(NIL ("l_wrist_roll_joint" . 0.0d0))

Play a bit with the robot in RViz: move one arm (remember to click Plan and Execute to make sure the movement actually happens!), then use joint-states to look at some joint on the arm and see that it changes.

Moving the robot

But actually, you can ask the robot to move from CRAM as well. In the REPL window, try the following:

(move-link-pose "r_wrist_roll_link" "right_arm" tuti:*pose-mid*)

and look in the RViz window for a reaction. After some planning time, usually a fraction of a second, the robot's right wrist will move to the requested location.

The parameters to move-link-pose are, in order, a link name, a planning group, and a pose-stamped object representing the pose to move to. The pose is something you will typically define yourself in your programs; either hard-coded (as in this tutorial) or through some generation process like location designator resolution.

Link names and planning groups are found in the *.srdf file for the robot you're working with. For this tutorial, this is the default *.srdf file in the pr2_moveit_config package. You can run MoveIt! with other robots as well (here's a tutorial on how to configure it, even if it shows another configuration for the PR2), in which case you'll just have to use the *.srdf defined for that robot. Link names are self explanatory, and defined in the robot definition files. Planning groups are defined in the *.srdf, and represent kinematic chains on the robot: lists of links that will typically move together to achieve some goal.

Sometimes you just want to know a trajectory between a robot's state and some goal, but you don't want to also execute the motion. For such situations, you can run something like:

(multiple-value-bind (start-robot-state-p planned-trajectory-p) 
                     (plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right*) 
                     (setf tuti:*start-robot-state* start-robot-state-p) 
                     (setf tuti:*planned-trajectory* planned-trajectory-p))

Let's first look into the RViz window to see what happens. After some planning time, you will see RViz show you the trajectory– but notice that the robot hasn't actually moved to the goal location yet. If you check to see what its current state is, you will notice it hasn't changed.

Now let's look at the plan-link-movement function itself. Its required parameters are identical to those of move-link-pose; however, it returns a multi-value (NOT a list of values): the robot state at the start of the trajectory, and the planned trajectory itself. We already defined tuti:*planned-trajectory* and tuti:*start-robot-state* through the call to init-moveit-tutorial, so we can now use them to catch the multiple values.

Suppose later you want to execute a trajectory you have previously planned. Try the following in the REPL window:

(execute-trajectory tuti:*planned-trajectory*)

And if you look at the RViz window you should see that this time the robot's state does eventually become the goal state.

Forward and Inverse Kinematics

MoveIt! also allows you to do Forward and Inverse Kinematics calculations on the robot, so lets look at how to access these services from CRAM. Try the following in the REPL window:

(compute-fk (list "r_wrist_roll_link" "l_wrist_roll_link" "fictional_link_that_isnt_there"))

You will see something like the following as a response:

(("r_wrist_roll_link"
  [GEOMETRY_MSGS-MSG:POSESTAMPED
     HEADER:
       (STD_MSGS-MSG:HEADER (:SEQ . 0) (:STAMP . 1.4331643594920259d9)
        (:FRAME_ID . "/odom_combined"))
     POSE:
       (GEOMETRY_MSGS-MSG:POSE
        (:POSITION
         . [GEOMETRY_MSGS-MSG:POINT
              X:
                0.5853153338934672d0
              Y:
                -0.188d0
              Z:
                1.2500104864353416d0])
        (:ORIENTATION
         . [GEOMETRY_MSGS-MSG:QUATERNION
              X:
                0.0d0
              Y:
                -0.8879298959569948d0
              Z:
                0.0d0
              W:
                0.45997880371360633d0]))])
 ("l_wrist_roll_link"
  [GEOMETRY_MSGS-MSG:POSESTAMPED
     HEADER:
       (STD_MSGS-MSG:HEADER (:SEQ . 0) (:STAMP . 1.4331643594920301d9)
        (:FRAME_ID . "/odom_combined"))
     POSE:
       (GEOMETRY_MSGS-MSG:POSE
        (:POSITION
         . [GEOMETRY_MSGS-MSG:POINT
              X:
                0.5853153338934672d0
              Y:
                0.188d0
              Z:
                1.2500104864353416d0])
        (:ORIENTATION
         . [GEOMETRY_MSGS-MSG:QUATERNION
              X:
                0.0d0
              Y:
                -0.8879298959569948d0
              Z:
                0.0d0
              W:
                0.45997880371360633d0]))]))

As you can see, compute-fk returns a list of lists (link-name pose-stamped). Also notice that if you ask about a link that doesn't exist, MoveIt! simply returns nothing for that link– in the example above, there is no pose returned for “fictional_link_that_isnt_there”.

compute-fk, by default, returns the link poses in the global frame given that the robot joints are set at their current values. Sometimes, you would like to know where a link would be if the robot was in some other joint state. In that case, you should use something like

(compute-fk link-names-list :robot-state robot-state)

Quite often you will be interested in knowing what values the joint states should have so that a link is at a requested pose. In such cases, you can use

(compute-ik "r_wrist_roll_link" "right_arm" tuti:*pose-mid*)

The answer is a robot state message:

[MOVEIT_MSGS-MSG:ROBOTSTATE
   JOINT_STATE:
     (SENSOR_MSGS-MSG:JOINTSTATE
      (:HEADER
       . [STD_MSGS-MSG:HEADER
            SEQ:
              0
            STAMP:
              0.0d0
            FRAME_ID:
              "/odom_combined"])
      (:NAME
       . #("bl_caster_rotation_joint" "bl_caster_l_wheel_joint"
           "bl_caster_r_wheel_joint" "br_caster_rotation_joint"
           "br_caster_l_wheel_joint" "br_caster_r_wheel_joint"
           "fl_caster_rotation_joint" "fl_caster_l_wheel_joint"
           "fl_caster_r_wheel_joint" "fr_caster_rotation_joint"
           "fr_caster_l_wheel_joint" "fr_caster_r_wheel_joint"
           "torso_lift_joint" "head_pan_joint" "head_tilt_joint"
           "l_shoulder_pan_joint" "l_shoulder_lift_joint"
           "l_upper_arm_roll_joint" "l_elbow_flex_joint" "l_forearm_roll_joint"
           "l_wrist_flex_joint" "l_wrist_roll_joint" "l_gripper_l_finger_joint"
           "l_gripper_l_finger_tip_joint" "l_gripper_motor_slider_joint"
           "l_gripper_motor_screw_joint" "l_gripper_r_finger_joint"
           "l_gripper_r_finger_tip_joint" "l_gripper_joint"
           "laser_tilt_mount_joint" "r_shoulder_pan_joint"
           "r_shoulder_lift_joint" "r_upper_arm_roll_joint"
           "r_elbow_flex_joint" "r_forearm_roll_joint" "r_wrist_flex_joint"
           "r_wrist_roll_joint" "r_gripper_l_finger_joint"
           "r_gripper_l_finger_tip_joint" "r_gripper_motor_slider_joint"
           "r_gripper_motor_screw_joint" "r_gripper_r_finger_joint"
           "r_gripper_r_finger_tip_joint" "r_gripper_joint"
           "torso_lift_motor_screw_joint"))
      (:POSITION
       . #(0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0
           0.0d0 0.16825d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 -1.13565d0 0.0d0
           -1.05d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0
           0.2554085145507399d0 0.4421557221482706d0 0.0d0
           -0.9362545530404258d0 2.638296338238714d0 -0.5513148794272151d0
           -2.703040545168192d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0 0.0d0
           0.0d0))
      (:VELOCITY . #()) (:EFFORT . #()))
   MULTI_DOF_JOINT_STATE:
     (SENSOR_MSGS-MSG:MULTIDOFJOINTSTATE
      (:HEADER
       . [STD_MSGS-MSG:HEADER
            SEQ:
              0
            STAMP:
              0.0d0
            FRAME_ID:
              "/odom_combined"])
      (:JOINT_NAMES . #("world_joint"))
      (:TRANSFORMS
       . #([GEOMETRY_MSGS-MSG:TRANSFORM
              TRANSLATION:
                (GEOMETRY_MSGS-MSG:VECTOR3 (:X . 0.0d0) (:Y . 0.0d0)
                 (:Z . 0.0d0))
              ROTATION:
                (GEOMETRY_MSGS-MSG:QUATERNION (:X . 0.0d0) (:Y . 0.0d0)
                                              (:Z . 0.0d0) (:W . 1.0d0))]))
      (:TWIST . #()) (:WRENCH . #()))
   ATTACHED_COLLISION_OBJECTS:
     #()
   IS_DIFF:
     NIL]

You can extract the required joint values from the returned message.

compute-ik takes a link name, a planning group, and a pose as parameters. Link and pose are self explanatory; planning groups define what joints the IK solver is allowed to move. See above for where you can find the link name and planning group definitions.

If the group name is not recognized MoveIt will signal an INVALID-GROUP-NAME error. If the pose is unreachable, the answer is nil. Take care however about the link name: if it is not recognized, a robot state is nonetheless returned (TODO: there is a bug in MoveIt!; in theory, MoveIt! should return an INVALID-LINK-NAME error in this case or, perhaps, default to the planning group tip. I've sent a pull request with a fix).

Note that at this time MoveIt!'s IK solver does not make use of the often present nullspace in redundant robot arms.

Just as compute-fk, compute-ik will, by default, assume you start with the current robot state. If you want to specify another, use something like this:

(compute-ik link-name planning-group-name pose :robot-state robot-state)

You may use robot states different than the current one when, for example, you want to see whether the robot can reach a location on a table with the robot base placed at various positions around that table.

compute-ik will also account for collisions with objects in the environment, as well as with the robot itself; the solution it returns (if any) is guaranteed to be collision free. You can also tell MoveIt! that some collisions are ok and will not invalidate a solution. This may happen for example when you want to grab an item from the environment: the item and the gripper will be in collision, but that's the goal!

Manipulating the environment

So far we've worked with an empty environment in RViz, but usually if you want something interesting done with the robot, you need some oject in its environment that it could interact with, or at least avoid. There are several ways to populate the robot's environment (what MoveIt! refers to as the “planning scene”), but here we will focus on what CRAM can do.

CRAM, in its cram-moveit package, will maintain a list of named collision objects. As far as cram-moveit knows and cares about, these are just pairs of a name and some type of shape– a geometric primitive, a mesh, or a plane– because the only object property that matters for cram-moveit (and for MoveIt! as well) is how it interferes with collision detection. Its shape, in other words. Any more advanced properties of objects like semantic annotations about their use are the scope of other packages.

Let's define an object for cram-moveit. In the REPL window, try:

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

And notice that the response is … not a lot. Certainly nothing happens in RViz– we haven't told MoveIt! about this object yet, it's only defined inside cram-moveit. The return value is nil. Parameters are an object name, and you can choose among a few &key parameters to provide the shape information. For this example, we chose a mesh defined in init-moveit-tutorial.

IMPORTANT note about cram-moveit's collision objects: names are internally converted to uppercase, and if a name is already found in the known object list, then the shape is NOT updated. Suppose you ran something like the following:

(register-collision-object "cube" :mesh-shapes (list tuti:*cube-mesh*))
(register-collision-object "CUBE" :mesh-shapes (list some-mesh))
(register-collision-object "CubE" :mesh-shapes (list some-other-mesh))

then only the first call to register-collision-object does anything. The second and third will discover that there already is a “CUBE” registered, and will just throw away the request so no new shapes are registered.

To actually see something in RViz, and inform MoveIt! that we want an object in the planning scene, we need to run this:

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

As a response you should see

[(MOVEIT) INFO] 1433167655.149: Added `CUBE' to environment server.
1

Also, note that a small green cube appeared in the RViz window. You can change an object's color if you like. The following line will make the cube turn blue:

(set-object-color "cube" '(0 0 128 1))

This is just cosmetic; it sometimes helps to have different colored objects so that a human user might have a quick visual cue about their uses in the scenario.

Try running a move-link-pose request for the robot. For example:

(move-link-pose "r_wrist_roll_link" "right_arm" tuti:*pose-right*)
(move-link-pose "r_wrist_roll_link" "right_arm" tuti:*pose-mid*)

Notice that the robot arm will not go directly between the two poses; it will avoid collisions with the cube. (You may need to use RViz's Loop Animation or Show Trail to see this, as sometimes the robot configuration jumps to the goal without showing you the path it took to get there.)

Not all objects in the environment are to be avoided however, and not all objects are stationary. In particular, sometimes the robot grabs objects and moves them around. You can signal to MoveIt! that this is about to happen by calling something like:

(attach-collision-object-to-link "cube" "r_wrist_roll_link")

Notice that the cube's color changed again; that is just to give you a quick visual cue that this object was attached to the robot. What this means is that from now on whenever the link that it is attached to moves (in this case “r_wrist_roll_link”), the object moves as well, as if connected rigidly to that link.

Note that you can attach an object to any link on the robot, and it will track that link's movement. While hilarious to attach an object to the laser scanner joint, you'll typically just want to attach objects when gripping them; in the case of the PR2, you might use “r_wrist_roll_link”, “l_wrist_roll_link”, and any of the links marked with “_gripper” for that purpose.

When you're done moving an object with the robot, you can detach it:

(detach-collision-object-from-link "cube" "r_wrist_roll_link")

Note that the cube's color changed back to the original blue.

Finally, when you're done with an object you can also remove it from MoveIt!'s planning scene:

(remove-collision-object "cube")

and even from cram-moveit's list of known collision-objects:

(unregister-collision-object "cube")

It's good practice to remove the collision objects you defined, at least from the MoveIt! planning scene (so that if you restart your scenario in CRAM, your initialization won't add redundant objects to the MoveIt! planning scene). For this purpose you can use

(clear-collision-environment)

when you want to remove objects both from the MoveIt! planning scene and cram-moveit's list, or

(clear-collision-objects)

when you want to clear the objects you've added to the MoveIt! planning scene from CRAM, but want to keep cram-moveit's list of objects intact.

Collision checking

(Work in progress: MoveIt! offers a service to check whether a robot state satisfies a set of constraints (e.g., non-collision with obstacles), and allows adjustments to the Allowed Collision Matrix. However CRAM doesn't yet have a function to wrap calls to this service. Coming soon!)

Next

(coming soon)