search

gizatt / pydrake_kuka

MIT License
5 stars 5 forks source link

Questions about Rigid Body Tree, MinDistanceConstraint and IK #7

Open rickyduh opened 5 years ago

rickyduh commented 5 years ago

I currently have a function that computes q trajectory of a robot arm from point A to point B where point A and B are very close in space. For my constraints matrix, I have a WorldPositionConstraint and a WorldEulerConstraint. Whenever I add a MinDistanceConstraint:

constraints.append(ik.MinDistanceConstraint(tree, collision_tol, id_collision_body, set()))

where id_collision_body is the collision index of all the bodies in the rigid body tree and collision tol is a very small number (ie 0.0001), I get the following warning when I run the script:

[2018-11-13 18:06:07.338] [console] [warning] Attempting to compute distance between two collision elements, at least one of which is non-convex. [2018-11-13 18:06:07.338] [console] [warning] Attempting to compute distance between two collision elements, at least one of which is non-convex. [2018-11-13 18:06:07.342] [console] [warning] Attempting to compute distance between two collision elements, at least one of which is non-convex.

then whenever I output result.info[0] of my ik script, I get a number not equal to 0. (I am guessing 1 means there was an IK solution and a number that is not equal to 1 means there is no IK solution for the given point).

However, when I visibly look at the UR in meshcat, it seems to be a doable position. Would you have any information on this?

When I take out MinDistanceConstraint, I don't get this problem and the result.info[0] is equal to 1.

gizatt commented 5 years ago

Likewise, this one's a good fit for the Drake stack-overflow. They may be a little hesitant to answer in this case, as this is getting into the meat of a system (RigidBodyTree and its collision handling backend) that is almost ready to be replaced...

As (vaguely) hinted by the error, RBT collision handling doesn't support nonconvex moving bodies. If nonconvex geometry is welded to the world, that's OK, but all mobile bodies need to be convex. This is a regression from previous functionality -- I'm not sure when it changed, it used to automatically take convex hulls of nonconvex moving geometry. Fixing your geometry so it has no nonconvex parts is an important first step, to make those errors go away and ensure you can trust the collision checks.

rickyduh commented 5 years ago

Hi, great thanks for letting me know! I converted all of my links to convex hulls using meshlab and still have that error:

I posted here but haven't gotten a response yet. If you know any of the drake developers that might know more about this, that would be really helpful.