How to locally allow object/gripper collision in electric ?

edit

Hi Tom,

The system is pretty complicated, and these are good questions. To give you a bit clearer idea what's going on, I'll first refer you this document. When you publish a collision object on the /collision_object topic, it enters into the main planning scene - the one held by the environment server. This can be considered the current state of the world. It does not, however, automatically get pushed to the arm_navigation components - so collision aware IK will not know about the object if you start making calls to it directly. The planning scene only gets pushed to the various components (via the SyncPlanningScene action) when someone calls the SetPlanningSceneDiff service in the environment server. Note that this is NOT the service called in the get_state_validity.cpp function, though they have the same fields. Thus to get a collision object to the different components via the method of publishing a collision_object you then need to call the SetPlanningSceneDiff action afterwards with empty arguments - this will send the current planning scene held by the environment server to the different components.

The system now also provides an alternate way to populate arm navigation components with a collision object - instead of publishing the object on the collision_object topic, you can include it in the planning_scene_diff field in the service request. What this does is say to the environment server "take the current scene, apply the diff that I'm providing, and sync all arm navigation components to the diffed scene". Until the next time you call SetPlanningSceneDiff, all arm navigation components will contain the diffed scene, even if the current scene held by the environment server changes (a new collision map, published objects). There is only one scene held in the arm navigation components at a time, so if you are calling SetPlanningSceneDiff from multiple nodes with different scenes you may not get the scene that you are expecting.

The service called in get_state_validity is a little bit different. It tells the environment server "take the current scene, apply whatever diff I'm handing you, and return the result". This call doesn't affect the arm navigation components in any way. You then take this result and use it populate your own internal CollisionModel class, which provides a whole bunch of functions for collision checking and validity queries. In Electric for stuff like collision-aware IK and planning you need to use the SetPlanningSceneDiff method, but there's a lot you can do internally with CollisionModels.

Nodes will filter the collision_map for objects at the time their planning scene is set - if everyone agrees on what objects are in the world, then I think you'll start getting consistent behavior in that respect.