如何在pydrake中使用minDistanceConstraint

Question

In this repo , @Gizatt uses the following command to assemble collision constraints for the kuka iiwa:
ik.MinDistanceConstraint(tree, collision_tol, list(), set())

Here, what do list() and set() signify. Both seem to be empty here.

Let's just say I have an item (item 1) that consists of 6 bodies (within one urdf) and another object (item 2) in my RigidBodyTree that consists of one body (within a separate urdf) and I only want to check for collisions between any of the 6 bodies that make up item 1 and item 2. Is there a way to set this function so that it doesn't check for collisions within the all the bodies in item 1 but only for collisions between item 1 and item 2?

Finally, I currently have the following error when I use this function: [2018-11-14 19:39:20.812] [console] [warning] Attempting to compute distance between two collision elements, at least one of which is non-convex. I took @gizatt's advice and converted the meshes of each link within item 1 to convex hulls using meshlab and when I look at each mesh using the visualizer, they all appear to be convex to me. However I still get this error. Is there any other reason this error would pop up?

Answer 1

The documentation for that method is here: https://drake.mit.edu/pydrake/pydrake.solvers.ik.html?highlight=mindistanceconstraint#pydrake.solvers.ik.MinDistanceConstraint

The last two arguments that greg used are about the "active" bodies. This will help you if you want to ignore some bodies entirely from the collision computation.

If you want to ignore some collision pairs, then use collision filter groups: https://drake.mit.edu/pydrake/pydrake.multibody.rigid_body_tree.html?highlight=collision%20filter#pydrake.multibody.rigid_body_tree.RigidBodyTree.DefineCollisionFilterGroup

Our RBT/Bullet wrapper assumes every mesh is convex EXCEPT static/anchored geometry. It seems likely that you are getting that warning because you are collision checking against the static/anchored geometry?

FWIW -- the documentation is MUCH more complete on multibodytree vs rigidbodytree, but for this particular query I think you're right to use RBT -- multibody is not quite there yet.

Answer 2

Here, what do list() and set() signify. Both seem to be empty here.

These arguments are historical artifacts and are almost never what you want to use. I would recommend leaving them empty as in the example you provided. The first restricts the constraint to consider only the bodies specified by active_bodies_idx . The second restricts the constraint to consider only the collision groups whose names are contained in active_group_names . Note that the "collision group" concept for the active_group_names argument is not the same as the "collision filter group" concept.

Is there a way to set this function so that it doesn't check for collisions within the all the bodies in item 1 but only for collisions between item 1 and item 2?

You were on the right track. You want to add a collision filter group that contains all of the bodies in item 1 and then set that collision group to ignore itself. The following code requires the AddCollisionFilterIgnoreTarget() binding added by PR #10085 .

tree.DefineCollisionFilterGroup("1_filtergroup")

tree.AddCollisionFilterGroupMember("1_filtergroup", "base_link", 1_model_id)
# Repeat the previous line for all bodies in item 1.

tree.AddCollisionFilterIgnoreTarget("1_filtergroup", "1_filtergroup")

You can then create a constraint with the desired behavior by calling ik.MinDistanceConstraint(tree, collision_tol, list(), set()) .

Finally, I currently have the following error when I use this function: [2018-11-14 19:39:20.812] [console] [warning] Attempting to compute distance between two collision elements, at least one of which is non-convex. ... Is there any other reason this error would pop up?

As @Russ Tedrake mentioned , all mesh collision elements attached to anchored (welded to the world) bodies are converted to non-convex mesh objects in the Bullet backend, regardless of the convexity of the original mesh. This is unfortunate, but will most likely not be addressed, as all RigidBodyTree -related code is nearing end-of-life. For IK purposes, you can work around this by attaching the mesh to a dummy body that is connected to the world by a prismatic or revolute joint whose upper and lower limits are both 0. That will force the backend to work with the convex-hull of the provide mesh. You'll then need to remove the element corresponding to the dummy joint from the configuration returned by IK.