Collision filter groups for C++

[MarcoCar]

The current approach for filtering collisions based on collision groups is to include the groups in the urdf and parse them with matlab. I can't find an equivalent procedure to filter collision when using collisionDetect directly from C++. Is there a plan to implement such functionality? Or does it already exist and I simply missed it?

[RussTedrake]

We definitely want to implement it in c++. Would be thrilled if you beat me to it.

[mauricefallon]

But is inserting these in urdf the right approach? users will have practical issues in modifying the urdf to inject these tags. and the tags are non standard. others include arbitrary foot contact points.

my first reaction would be to implement them in libbot's config parser (which we did use in DRC) but implementing them in json (a la Director) would also work.

[wxmerkt]

@rdeits included the filtered collision groups for the C++ QP Controller in a YAML file and then filtered them like this. Corresponding YAML files showing how to keep collision groups are e.g. here.

[rdeits]

Thanks, @iamwolf! @MarcoCar is that mechanism workable for you?

[RussTedrake]

i prefer the urdf approach. yaml is almost certainly better than xml as a language, but i don't think users will have more practical issues with xml than yaml. and I dislike the idea of having to create a second config file for every robot. it's ok for big robots like atlas and val, but people who aren't as focused on a single robot probably won't like it as much...

[patmarion]

It's not clear that the YAML file is encoding the same information that is in the URDF. The URDF associates link names with collision groups. The parsing of this information is handled by Matlab and collision filters are created at runtime and represented with bit masks (used by bullet). In the past, I was able to initialize the collision filter groups by dumping information from matlab and generating c++ code that looks like this (valkyrie example):

findModelLink(model, "pelvis")->setCollisionFilter(1, 1); findModelLink(model, "torsoYawLink")->setCollisionFilter(1, 1); findModelLink(model, "torsoPitchLink")->setCollisionFilter(1, 1); findModelLink(model, "torso")->setCollisionFilter(1, 1);

findModelLink(model, "lowerNeckPitchLink")->setCollisionFilter(1, 1); findModelLink(model, "neckYawLink")->setCollisionFilter(1, 1); findModelLink(model, "upperNeckPitchLink")->setCollisionFilter(1, 1); //findModelLink(model, "hokuyo_link")->setCollisionFilter(1, 1);

findModelLink(model, "rightShoulderPitchLink")->setCollisionFilter(264, 257); findModelLink(model, "rightShoulderRollLink")->setCollisionFilter(264, 257); findModelLink(model, "rightShoulderYawLink")->setCollisionFilter(256, 256); findModelLink(model, "rightElbowPitchLink")->setCollisionFilter(256, 256); findModelLink(model, "rightForearmLink")->setCollisionFilter(256, 256); findModelLink(model, "rightWristRollLink")->setCollisionFilter(256, 256);

findModelLink(model, "rightPalm")->setCollisionFilter(256, 256); findModelLink(model, "rightPinkyPitch1Link")->setCollisionFilter(256, 256); findModelLink(model, "rightPinkyPitch2Link")->setCollisionFilter(256, 256); findModelLink(model, "rightPinkyPitch3Link")->setCollisionFilter(256, 256); findModelLink(model, "rightMiddleFingerPitch1Link")->setCollisionFilter(256, 256); findModelLink(model, "rightMiddleFingerPitch2Link")->setCollisionFilter(256, 256); findModelLink(model, "rightMiddleFingerPitch3Link")->setCollisionFilter(256, 256); findModelLink(model, "rightIndexFingerPitch1Link")->setCollisionFilter(256, 256); findModelLink(model, "rightIndexFingerPitch2Link")->setCollisionFilter(256, 256); findModelLink(model, "rightIndexFingerPitch3Link")->setCollisionFilter(256, 256); findModelLink(model, "rightThumbRollLink")->setCollisionFilter(256, 256); findModelLink(model, "rightThumbPitch1Link")->setCollisionFilter(256, 256); findModelLink(model, "rightThumbPitch2Link")->setCollisionFilter(256, 256); findModelLink(model, "rightThumbPitch3Link")->setCollisionFilter(256, 256);

findModelLink(model, "leftShoulderPitchLink")->setCollisionFilter(24, 17); findModelLink(model, "leftShoulderRollLink")->setCollisionFilter(24, 17); findModelLink(model, "leftShoulderYawLink")->setCollisionFilter(16, 16); findModelLink(model, "leftElbowPitchLink")->setCollisionFilter(16, 16); findModelLink(model, "leftForearmLink")->setCollisionFilter(16, 16); findModelLink(model, "leftWristRollLink")->setCollisionFilter(16, 16);

findModelLink(model, "leftPalm")->setCollisionFilter(16, 16); findModelLink(model, "leftPinkyPitch1Link")->setCollisionFilter(16, 16); findModelLink(model, "leftPinkyPitch2Link")->setCollisionFilter(16, 16); findModelLink(model, "leftPinkyPitch3Link")->setCollisionFilter(16, 16); findModelLink(model, "leftMiddleFingerPitch1Link")->setCollisionFilter(16, 16); findModelLink(model, "leftMiddleFingerPitch2Link")->setCollisionFilter(16, 16); findModelLink(model, "leftMiddleFingerPitch3Link")->setCollisionFilter(16, 16); findModelLink(model, "leftIndexFingerPitch1Link")->setCollisionFilter(16, 16); findModelLink(model, "leftIndexFingerPitch2Link")->setCollisionFilter(16, 16); findModelLink(model, "leftIndexFingerPitch3Link")->setCollisionFilter(16, 16); findModelLink(model, "leftThumbRollLink")->setCollisionFilter(16, 16); findModelLink(model, "leftThumbPitch1Link")->setCollisionFilter(16, 16); findModelLink(model, "leftThumbPitch2Link")->setCollisionFilter(16, 16); findModelLink(model, "leftThumbPitch3Link")->setCollisionFilter(16, 16);

findModelLink(model, "rightHipYawLink")->setCollisionFilter(1024, 1089); findModelLink(model, "rightHipRollLink")->setCollisionFilter(1024, 1089); findModelLink(model, "rightHipPitchLink")->setCollisionFilter(1024, 1089); findModelLink(model, "rightKneePitchLink")->setCollisionFilter(512, 1536); findModelLink(model, "rightAnklePitchLink")->setCollisionFilter(512, 1536); findModelLink(model, "rightFoot")->setCollisionFilter(516, 1540);

findModelLink(model, "leftHipYawLink")->setCollisionFilter(64, 1089); findModelLink(model, "leftHipRollLink")->setCollisionFilter(64, 1089); findModelLink(model, "leftHipPitchLink")->setCollisionFilter(64, 1089); findModelLink(model, "leftKneePitchLink")->setCollisionFilter(32, 96); findModelLink(model, "leftAnklePitchLink")->setCollisionFilter(32, 96); findModelLink(model, "leftFoot")->setCollisionFilter(36, 100);

[mauricefallon]

The point I'd make is that collision groups are algorithm configuration - they aren't really fundamental properties of the robot so URDF isn't the right place for non-standard tags.

For Atlas we had a 1000 line config file and the same existed for the DGC. SDF is basically a configuration format for Gazebo.

[skohlbr]

For completeness let me mention that collision group setup is done via the SRDF format in the ROS/MoveIt world. There, per default any link can collide with any other per default, and collision checking can be explicitly disabling between two links by using the <disable_collisions> tag.

Examples: https://github.com/ros-planning/moveit_pr2/blob/indigo-devel/pr2_moveit_config/config/pr2.srdf https://github.com/team-vigir/vigir_atlas_planning/blob/master/vigir_atlas_moveit_config/config/vigir_atlas.srdf.xacro (using xacro macros for launch time configuration)

[RussTedrake]

@mauricefallon -- i don't actually agree. I think they are properties of the robot. you could define multiple groups that get enabled/disabled in an application -- just like an end-effector frame. But defining that frame is done once per robot.

[MarcoCar]

I wrote a collision filter group parser that replicates the one for matlab. For this to work though, all links connected through fixed joints need to be added to a collision filter in the urdf. This is not needed in matlab since the RigidBodyManipulator.removeFixedJoints function creates a single body from rigidly connected links, which prevents collisions between them from being detected, but I couldn't find a similar functionality for the RigidBodyTree in C++.

[RussTedrake]

i hadn't thought of that. i would almost say that it's a bug in the matlab (which would be hard to fix) that collision filters pass directly through fixed joints. it seems like the author should have to specify that explicitly.

if we did want to provide that functionality, we could presumably assemble a filter group programmatically upon parsing a fixed joint.

[liangfok]

Can somebody point me to a few survey papers describing the state-of-the-art in automatic collision-group determination?

I don't think collision groups should be part of the model because I believe they depend both on the configuration of the robot and the world in which the robot resides. They should ideally be automatically derived at run-time or, failing that, as a "pre-processing" step prior to run-time. Also, I believe collision groups should be probabilistic in the sense of saying "the probability of these two links colliding is so small the collision checker can ignore them during this round of the simulation".

[liangfok]

Just in case my terminology is unclear, by "configuration" I mean the current position, velocity, acceleration, etc., of the joints.

[sherm1]

Liang, typically pose-dependent collision avoidance is not handled through collision groups at all. It is done in what's called the "broad phase" of collision detection. That is a pre-pass done on the entire scene in which the O(n^2) potential collision are reduced to a much, much smaller number of possible collision pairs. That can be done with extremely fast algorithms. After that, a "narrow phase" is performed in which possible pairs are examined for actual collisions.

The collision groups are used (1) to eliminate actually-overlapping contact geometry that nevertheless should not collide (such as on adjacent links), and (2) as an accelerator for the broad phase to weed out contact geometry that can't possibly overlap, typically things fixed to the same link or to the world.

[liangfok]

Thanks for the info. Is the "broad phase" of collision detection typically done using collision groups that are specified manually?

Sorry I just realized that you said extremely fast algorithms can perform the broad phase of collision detection.

[avalenzu]

@RussTedrake, the behavior in MATLAB is a design choice we had to make due to our previous (perhaps misguided) decision to implement the collision filtering at the body level rather than the collision element level. The alternative was to lose the collision filtering info for both bodies when they were merged.

On Mon, Mar 28, 2016, 1:03 AM Michael Sherman notifications@github.com wrote:

Liang, typically pose-dependent collision avoidance is not handled through collision groups at all. It is done in what's called the "broad phase" of collision detection. That is a pre-pass done on the entire scene in which the O(n^2) potential collision are reduced to a much, much smaller number of possible collision pairs. That can be done with extremely fast algorithms. After that, a "narrow phase" is performed in which possible pairs are examined for actual collisions.

The collision groups are used (1) to eliminate actually-overlapping contact geometry that nevertheless should not collide (such as on adjacent links), and (2) as an accelerator for the broad phase to weed out contact geometry that can't possibly overlap, typically things fixed to the same link or to the world.

— You are receiving this because you are subscribed to this thread.

Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1793#issuecomment-202234504

[RussTedrake]

thanks @avalenzu -- we don't have that exact problem in the C++ classes, because we don't merge the fixed joints.
what's your feeling about whether the information lives in the urdf/sdf file or someplace separate?

[liangfok]

Upon further thought, collision groups are essentially hints for a collision checker to optimize the amount of computations needed. I am thus agnostic as to whether this information is included as part of a URDF/SDF file or in a separate file.

[amcastro-tri]

Yes, collision groups/masks are assigned by the user in order to avoid unnecessary checks between objects in the model. @sherm1's application to the filtering of two adjacent links (for instance in a manipulator) is a good example. Another example that @david-german-tri is working on is the filtering of unnecessary collisions between objects in the scene (for instance you don't want to check for collisions between a tree and the ground). The solver however has no way to know about these cases and the user needs to provide the information.

Yes, a good place where to provide them is the SDF file. Actually the SDF specification supports for this here.

I am currently working on adding Bullet's dispatcher to our collision model in #1940. Hopefully that will give us the mechanism to include filters.

However, as for now, it seems like Drake has a similar mechanism. Take a look at RigidBody::collidesWith.

[sherm1]

collision groups are essentially hints for a collision checker to optimize the amount of computations needed

To clarify: the primary purpose of collision groups is to signal the modeler's intent that two contact surfaces, despite appearances, do not collide. That contact exclusion is not an optimization but rather part of the model so it has to go in the model file. The canonical example is when link collision geometry is simplified to cylinders. In that case adjacent cylinders will overlap as the joints rotate but there is no intent to have them collide.

The optimization aspect of collision groups is a nice benefit but not critical. You can get most of the benefit by noticing which collision geometries are fixed to the same rigid body and treating that as a group. Broad phase processing will quickly weed out other non-participants. I don't think it is a good idea for the modeler to attempt these optimizations and they are not logically part of the model in the same way as the contact exclusions are.

[tri-ltyyu]

This is likely specific to RigidBodyPlant. If this is applicable to MultBodyPlant/SceneGraph, please file new issue. Closing for now.