Environment Representation/C++ API

The [/classplanning__scene_1_1PlanningScene.html PlanningScene] class provides the main interface that you will use for collision checking and constraint checking. In this tutorial, we will explore the C++ interface to this class.

Setup
The [/classplanning__scene_1_1PlanningScene.html PlanningScene] class can be easily setup and configured using a [/classmoveit_1_1core_1_1RobotModel.html RobotModel] or a URDF and SRDF. This is, however, not the recommended way to instantiate a PlanningScene. The [/classplanning__scene__monitor_1_1PlanningSceneMonitor.html PlanningSceneMonitor] is the recommended method to create and maintain the current planning scene (and is discussed in detail in the next tutorial) using data from the robot's joints and the sensors on the robot. In this tutorial, we will instantiate a PlanningScene class directly, but this method of instantiation is only intended for illustration.

Collision Checking


Self-collision checking
The first thing we will do is check whether the robot in its current state is in self-collision, i.e. whether the current configuration of the robot would result in the robot's parts hitting each other. To do this, we will construct a CollisionRequest object and a CollisionResponse object and pass them into the collision checking function. As you can note, the result of whether the robot is in self-collision or not is contained within the result. Self collision checking uses an unpadded version of the robot, i.e. it directly uses the collision meshes provided in the URDF with no extra padding added on.

Change the state
Now, let's change the current state of the robot. The planning scene maintains the current state internally. We can get a reference to it and change it and then check for collisions for the new robot configuration. Note in particular that we need to clear the collision_result before making a new collision checking request.

Checking for a group
Now, we will do collision checking only for the right_arm of the PR2, i.e. we will check whether there are any collisions between the right arm and other parts of the body of the robot. We can ask for this specifically by adding the group name "right_arm" to the collision request.

Getting Contact Information
First, manually set the right arm to a position where we know internal (self) collisions do happen. Note that this state is now actually outside the joint limits of the PR2, which we can also check for directly.

Now, we can get contact information for any collisions that might have happened at a given configuration of the right arm. We can ask for contact information by filling in the appropriate field in the collision request and specifying the maximum number of contacts to be returned as a large number.

Modifying the Allowed Collision Matrix
The Allowed Collision Matrix (ACM) provides a mechanism to tell the collision world to ignore collisions between certain object: both parts of the robot and objects in the world. We can tell the collision checker to ignore all collisions between the links reported above, i.e. even though the links are actually in collision, the collision checker will ignore those collisions and return not in collision for this particular state of the robot.

Note also in this example how we are making copies of both the allowed collision matrix and the current state and passing them in to the collision checking function.



Full Collision Checking
While we have so far been only using self-collision checking, we can collision check for objects in the world as well. The API calls are exactly the same, except we will be using the checkCollision functions instead of the checkSelfCollision functions:



Constraint Checking
The PlanningScene class also includes easy to use function calls for checking constraints. The constraints can be of two types: (a) constraints chosen from the [/classkinematic__constraints_1_1KinematicConstraint.html KinematicConstraint set]: i.e. [/classkinematic__constraints_1_1JointConstraint.html JointConstraint], [/classkinematic__constraints_1_1PositionConstraint.html PositionConstraint], [/classkinematic__constraints_1_1OrientationConstraint.html OrientationConstraint] and [/classkinematic__constraints_1_1VisibilityConstraint.html VisibilityConstraint] and (b) user defined constraints specified through a callback. We will first look at an example with a simple KinematicConstraint.

Checking Kinematic Constraints
We will first define a simple position and orientation constraint on the end-effector of the right_arm of the PR2 robot. Note the use of convenience functions for filling up the constraints (these functions are found in the [/utils_8h_source.html utils.h] file from the kinematic_constraints directory in moveit_core).

Now, we can check a state against this constraint using the isStateConstrained functions in the PlanningScene class.

There's a more efficient way of checking constraints (when you want to check the same constraint over and over again, e.g. inside a planner). We first construct a KinematicConstraintSet which pre-processes the ROS Constraints messages and sets it up for quick processing.

There's a direct way to do this using the KinematicConstraintSet class.



User-defined constraints
User defined constraints can also be specified to the PlanningScene class. This is done by specifying a callback using the setStateFeasibilityPredicate function. Here's a simple example of a user-defined callback that checks whether the "r_shoulder_pan" of the PR2 robot is at a positive or negative angle:

Now, whenever isStateFeasible is called, this user-defined callback will be called.

Whenever isStateValid is called, three checks are conducted: (a) collision checking (b) constraint checking and (c) feasibility checking using the user-defined callback.

Note that all the planners available through MoveIt! and OMPL will currently perform collision checking, constraint checking and feasibility checking using user-defined callbacks.

The Entire Code
The entire code can be seen here in the moveit_pr2 github project.

Compiling the code
Follow the instructions for compiling code from source.

The launch file
The entire launch file is here on github. All the code in this tutorial can be compiled and run from the pr2_moveit_tutorials package that you have as part of your MoveIt! setup.

Running the code

 * Roslaunch the launch file to run the code directly from pr2_moveit_tutorials:

Additional Reading

 * [/classplanning__scene_1_1PlanningScene.html PlanningScene Code API]

Links

 * Back to Planning Scene