0

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?

Rickyduh
  • 1
  • 1
  • Thanks for the information. So, what I currently have is: For item 1 (that has 6 links/bodies) and item 2 (that has 1 link/body), I defined two different collision groups as follows: tree.DefineCollisionFilterGroup("1_filtergroup") tree.DefineCollisionFilterGroup("2_filtergroup"). I then added each link in each item to the respective collision filter group using: tree.AddCollisionFilterGroupMember("1_filtergroup", "base_link", 1_model_id) and did this for every link in the respective models. – Rickyduh Nov 21 '18 at 15:49
  • I then used constraints.append(ik.MinDistanceConstraint(tree, collision_tol, id_collision_body, {"1_filtergroup", "2_filtergroup"})). I noticed that even after doing so, the q.sol (the ik solution) will return 1 even though I purposely directed the end-effector in item 1 to a position that collides with item 2 in the rigid body tree. – Rickyduh Nov 21 '18 at 15:50
  • In addition, when I direct the end-effector from a position that is on the inside of a wall of item 2 to a position that is on the outside of the wall of item 2, I would expect that it would take the shortest path around the wall with collision constraints turned on, but it instead goes right through the wall. – Rickyduh Nov 21 '18 at 15:50
  • I am using that if id_collision_body in my code is the index of every body in by tree, then I am saying that all bodies are active in the collision check. In addition, I am also assuming that including {"1_filtergroup", "2_filtergroup"} as the last argument in minDistanceConstraint means that I am stating that only collisions between any body in 1_filtergroup and any body in 2_filtergroup, should be checked. Is this a correct assumption? – Rickyduh Nov 21 '18 at 15:54

2 Answers2

0

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.

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
0

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.

Andrés Valenzuela
  • 88
  • 5
  • Hi Andres, thanks for the pointers. I tried adding a "world" link to my urdf and then attaching a "dummy_link" to the world link with a "world_joint" that is revolute. Then I attached that to my "base_link" through a "dummy_joint" that is revolute. However, I am still getting the error stated above. Is there something I am doing incorrectly? – Rickyduh Dec 11 '18 at 19:52
  • Are there any mesh collision geometries attached to bodies that are rigidly fixed to the world? That's the only case in which I've seen the "Attempting to compute distance between two collision elements ..." error message." Without seeing the URDF in question, I'm hesitant to speculate further. – Andrés Valenzuela Dec 12 '18 at 20:26