Ask Your Question

Obstacles management in Moveit

asked 2014-04-26 08:39:17 -0600

McMurdo gravatar image

I would be delighted if someone can explain very clearly.

I have recently started working on a robot that has a Jaco Arm. (MetraLab Scitos G5).

I am using the ASUS xtion pro for the obstacle avoidance. Once this is done, I am able to view the Octree in the monitored_planning_scene topic (using rviz).

1. I also found that I could specify collision objects using the PlanningSceneInterface Class. What is the difference between the two approaches?

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?

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?

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?

Please clarify!

Thank you so much!

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2014-06-20 10:16:34 -0600

McMurdo gravatar image

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:

As an inspiration, I found digging through this code helpful:

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(


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 ... (more)

edit flag offensive delete link more


Quoted from this answer: "It's important that you really get the current allowed collision matrix first and then modify it, as shown above." Why is this the case? What is the meaning of the is_diff argument, if not to indicate that the acm published is the diff between current and desired acm?

dinosaur gravatar image dinosaur  ( 2015-05-19 15:43:55 -0600 )edit

answered 2017-01-20 13:21:52 -0600

AndyZe gravatar image

updated 2017-01-20 13:27:46 -0600

A hacky way to ignore collisions with the environment (i.e. collisions that are caused by Octomap) is to clear octomap just before planning the motion. For C++:

system("rosservice call /clear_octomap");

I'm using this just for my final Cartesian approach to a grasp. Obviously it's kinda dangerous.

edit flag offensive delete link more



Please don't call services using a system(..) call. If at all possible, use a proper ServiceClient.

gvdhoorn gravatar image gvdhoorn  ( 2017-01-20 15:14:25 -0600 )edit

OK. Here's a better answer from the Moveit Google group:

planning_scene->getAllowedCollisionMatrixNonConst().setEntry("<octomap>", left_hand_links_vector_, true);
AndyZe gravatar image AndyZe  ( 2017-01-21 09:17:31 -0600 )edit

planning_scene->getAllowedCollisionMatrixNonConst().setEntry("<octomap>", left_hand_links_vector_, true);

This is giving segfault, can yoy help me out of this.

Manish_Soni_1908 gravatar image Manish_Soni_1908  ( 2017-01-21 12:59:37 -0600 )edit

Maybe you don't have a vector of links called left_hand_links_vector_? Try just a single link from the urdf.

AndyZe gravatar image AndyZe  ( 2017-01-21 13:56:13 -0600 )edit

but if we do not write anything in place of left_hand_links_vector , then it should avoid collision for all robot links from octomap. but i tried that, it is not doing that, and can we give multiple links name without defining a vector

Manish_Soni_1908 gravatar image Manish_Soni_1908  ( 2017-01-21 14:23:12 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2014-04-26 08:39:17 -0600

Seen: 5,129 times

Last updated: Jan 20 '17