Revision history [back]

The octomap is updated live from your Xtion sensor data. The collision objects are added by you. They are used to add collision objects that you know the position of; the most common way is to have some sort of object recognition node that recognizes, for example, tabletop objects, and adds these objects as collision objects to the PlanningSceneWorld. (If you look at moveit_msgs/PlanningSceneWorld, you can see that it has one octomap and a list of collision objects).

You can add objects by either publishing to the "/collision_object" topic, or by using a convenience function from the move_group C++ interface.

Once a collision object is added to the PlanningScene, the voxels that overlap with it are automatically filtered out of the octomap. You do not need to call excludeWorldObjectFromOctree() yourself (that's probably why it's protected).

By default, collision checking is done between all robot links and the octomap, and between all robot links and each collision object. However, you can disable collision checks between certain robot links and certain collision objects, and that's exactly what you should do: disable collision checking between the fingers and the object you're trying to pick up. There are basically two ways to do it:

1. Disable the collision checks before calling the move_group action. All calls to the move_group action after that will ignore those collisions. You'll have to remember to re-enable the collisions afterwards.

2. Specify the allowed collisions for that specific move_group call only, as a parameter of that action call. This is somewhat nicer in my opinion.

For (1), there are a couple of options:

1a. Using the PlanningSceneMonitor. From C++, I think this is better than the solution (1b) below. It should go something like this (warning, completely untested!):

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

planning_scene_monitor::PlanningSceneMonitorPtr psm = planning_scene_monitor::PlanningSceneMonitorPtr(
    new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf, "name"));

planning_scene_monitor::LockedPlanningSceneRW planning_scene = planning_scene_monitor::LockedPlanningSceneRW(psm);

collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();

// TODO: modify acm here as explained in the PlanningScene tutorial: http://docs.ros.org/api/pr2_moveit_tutorials/html/planning/src/doc/planning_scene_tutorial.html

As an inspiration, I found digging through this code helpful: https://github.com/ros-planning/moveit_ros/tree/hydro-devel/visualization/planning_scene_rviz_plugin

1b. Publishing to the "/planning_scene" topic. From Python, it looks like this:

    self._pubPlanningScene = rospy.Publisher('planning_scene', PlanningScene)
    rospy.wait_for_service('/get_planning_scene', 10.0)
    get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
    request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
    response = get_planning_scene(request)

    acm = response.scene.allowed_collision_matrix
    if not 'button' in acm.default_entry_names:
        # add button to allowed collision matrix
        acm.default_entry_names += ['button']
        acm.default_entry_values += [True]

        planning_scene_diff = PlanningScene(
                is_diff=True,
                allowed_collision_matrix=acm)

        self._pubPlanningScene.publish(planning_scene_diff)
        rospy.sleep(1.0)

For (2), you can use the same planning_scene_diff that I computed above by putting it into your MoveGroupGoal.planning_options.planning_scene_diff. It's important that you really get the current allowed collision matrix first and then modify it, as shown above.

Note that I added the collision object to the default_entry_names, which means that collisions between that object and all robot links are allowed, not just the fingers. That's bad, but I couldn't make it work otherwise.

Another option for you is to use the pick action instead of implementing it yourself with move_group calls. It wraps the move_group action, and it automatically handles the allowed collision matrix etc. for you. One small example of using it is here:

https://github.com/uos/calvin_robot/blob/hydro/calvin_pick_n_place/src/calvin_pick_n_place.cpp

2. Also, what is the exact approach to using a PlanningSceneMonitor. In the planning scene tutorial, it is mentioned that the PlanninSceneMonitor is the elegant way to manage a Planning Scene. But, is there a tutorial for the PlanningSceneMonitor?

Not that I know of. The Planning Scene tutorial says that it's "discussed in detail in the next tutorial", but that never got written if I'm correct. (Please, everyone, correct me if I'm wrong!)

3. I saw that the PlanningSceneMonitor has a protected function called "excludeWorldObjectFromOctree()" .

Sometimes when I try to grasp an object (using xtion for obstacle avoidance), the plan fails since there is a potential collision between the fingers of the JACO arm and the object that I intend to pickup. I thought I could add it as a World object using the Planning Scene Interface (this I am able to do) and then use the "excludeWorldObjectFromOctree()" method to exclude the object from being considered for collision checking. Is there a more straight forward way of excluding a world object from collision checking?

See above. Once you add a CollisionObject, it is automatically excluded from the octomap, but of course not from collision checking.

4. I have somewhat understood how you could modify the AllowedCollisionMatrix. But then, how do I ensure that this ACM is updated to the current Planning Scene? I expect that there is a method in PlanningSceneInterface like "updateAllowedCollisionMatrix()" ? Am I guessing right? or is there a serious error in my understanding?

See above.

Please clarify!

Thank you so much!

Chittaranjan S Srinivas

Orebro University

Cheers, Martin

I have posted this from the reply on moveit-users google mailing list.