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: Collisions and Constraints in MoveIt!

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.

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

roslaunch pr2_moveit_config 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:

CL-USER> (ros-load:load-system "cram_intermediate_tutorial" :cram-intermediate-tutorial)
CL-USER> (tuti:init-cram-moveit-tutorial)
CL-USER> (in-package :cram-moveit)

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 ActionLib client 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 you will have to do the initialization yourself by calling:

  (roslisp-utilities:startup-ros)
  (cram-moveit:init-moveit-bridge)

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.

Another function you can use to get the current robot state is get-planning-scene-info. To get a robot state ROS message (as opposed to a list of name-value pairs), use the following:

(second (first (get-planning-scene-info :robot-state T)))

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.

So far all movement requests have involved a beneath-the-hood call to MoveIt!'s motion planning, which makes motion generation more capable to handle complicated environments with obstacles in them, and allow motion trajectories to be generated when needed rather than pre-programmed.

However, motion planning has its own problems; it often produces strange looking trajectories and takes an unpredictable amount of time to complete, usually several tenths of a second. Put another way, motion planning is often overkill for the task you want the robot to do. Perhaps you have some other, simpler method to generate some waypoints, for example, based on some features of the environment, and are just interested to know if some “simple” movements can connect the waypoints.

For such purposes, you can use the compute-cartesian-path function. Here's an example call:

(move-link-pose "r_wrist_roll_link" "right_arm" tuti:*pose-mid*)
(compute-cartesian-path "/odom_combined"
                        (second (first (get-planning-scene-info :robot-state T)))
                        "right_arm" 
                        "r_wrist_roll_link" 
                        (list tuti:*pose-right-msg*)
                        0.1 
                        1.5 
                        t 
                        :path-constraints-msg (roslisp:make-msg "moveit_msgs/Constraints"))

which you are encouraged to try in the REPL window. The snippet above first makes sure that the robot goes to a certain position, then issues a request to compute a cartesian path to another pose. get-planning-scene-info is another cram-moveit function that we'll look at in the next section, for now we're just using it here to retrieve the current robot state.

First, let's see what happened in the RViz window. Notice that the robot state hasn't changed yet- compute-cartesian-path simply computes a trajectory, it won't execute it. You can do that with execute-trajectory as described above. In order to see the generated trajectory, enable the “Loop animation” option in RViz's “Planned Path” section of the side menu. You should see the robot sweeping from center to right with its right arm.

Notice how the arm's wrist moves in a straight line between the start and endpoint. In general, compute-cartesian-path will generate trajectories that link the waypoints via linear motions in cartesian space; that is, the link you specify waypoints for will move in a line between them.

Let's now look at the return values provided by compute-cartesian-path. In the REPL window, you should see something like this

(("start state" <a robot_state ROS message here>) 
 ("trajectory" <a robot_trajectory ROS message>) 
 ("completion fraction" <a number, 1.0 if trajectory successful>))

You can rely on the order of the named pairs in the response to always be the same (“start state”, then “trajectory”, then “completion fraction”); the names are there simply for user readability. So if you wanted, for example, to simply extract the trajectory from the response (so that you could execute it later) you could write something like

(second (second (compute-cartesian-path "/odom_combined"
                                        (second (first (get-planning-scene-info :robot-state T)))
                                        "right_arm" 
                                        "r_wrist_roll_link" 
                                        (list tuti:*pose-right-msg*) 
                                        0.1 
                                        1.5 
                                        t 
                                        (roslisp:make-msg "moveit_msgs/Constraints"))))

The third part of the return value (“completion fraction”) tells you whether the trajectory was successfully generated and if not, how much of it could be done. A complete trajectory will have a completion fraction of 1.0. A failed trajectory will be somewhere between 1.0 and 0.0. Failures in generating a trajectory can have several causes, but the most important ones are either collisions with obstacles (the robot can't move along a line to the next waypoint because an obstacle blocks the way) or IK failures: either the IK solver found no solution for a point along the trajectory, or it did find one but it requires the arm configuration to “jump” and is therefore an impossible motion.

Now let's look at the parameters. The first one (“/odom_combined” in the example) is the frame in which the waypoints are given. The second parameter is a robot state message that represents the state at the start of the trajectory. The next two parameters are strings that identify the planning group (the set of links to move) and respectively the link for which we define waypoints.

The next parameter is a list of waypoints (represented by pose messages); in our example, there's only one waypoint, but there can be several. The trajectory of the named link will be made of line segments connecting the waypoints.

The next two parameters are maximum distance (in Cartesian space) between points on the generated trajectory (so the smaller this number, the more points on the generated trajectory), and respectively a jump threshold, a distance in joint space that, if exceeded between consecutive points, is interpreted as a jump in IK solutions. Setting this number may require some fine tuning.

Finally, we have a true/false value indicating whether trajectory generation should care about obstacle collisions, and a ROS message describing what other kinematic constraints the trajectory should satisfy. We'll look at constraints in more depth in the next tutorial.

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 (we will look into this in the next tutorial). 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!

Sometimes you may want to tell MoveIt! to not do any collision checking during IK queries. You can do this by using the key parameter :avoid-collisions like so:

(compute-ik link-name planning-group-name pose :avoid-collisions nil)

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.

So far we've only written to MoveIt!'s planning scene. It may be useful however to also query it for its current state. To this end, cram-moveit provides the function get-planning-scene-info. Let's look at an example query:

(get-planning-scene-info :scene-settings T)

for which the response will look something like

(("planning scene name" "(noname)+") ("robot model name" "pr2"))

Notice how the response is again a list of named pairs. What is in the list depends on the nature of the query. For a query like

(get-planning-scene-info :world-object-names T)

you will instead get as a response (assuming you still have the cube object in the planning scene)

(("world object names" ("CUBE")))

so as you can see the partner for “world object names” is a list of object names.

Earlier in this tutorial we've also seen

(get-planning-scene-info :robot-state T)

which puts a pair formed of “robot state” and a robot state message describing the current robot state on the returned list.

You can combine several queries about the planning scene into one:

(get-planning-scene-info :world-object-names T :scene-settings T)

and the response will be a concatenation of the responses of the individual queries:

(("planning scene name" "(noname)+") ("robot model name" "pr2")
 ("world object names" ("CUBE")))

When you issue several queries in a get-planning-scene-info call, do NOT rely on the order of the pairs in the list; use the first element of the pair to identify it. The docstring for get-planning-scene-info will explain what queries are available and what responses they produce.

We've mentioned the topic of obstacles and collisions, it's time to look at it more in-depth …