Contact dynamics

[tkoolen]

• [ ] Store collision geometry in Mechanism (current status: contact points stored in RigidBody, world-fixed environment in the form of halfspaces stored in Mechanism)
• [ ] Decide how to do distance queries (GJK implementation from GeometryTypes?).
• [x] Penalty-based contact dynamics
• [ ] LCP-style contact dynamics
• [ ] Parsing collision elements from URDF.

[rdeits]

Having geometry stored inside the mechanism would make RigidBodyTreeInspector's job much easier 👍

[rdeits]

Also, the GJK implementation from GeometryTypes will need to be improved before it can be useful here. It's got two important features missing:

• it repeatedly searches over all of the nodes in the mech to find the closest points, which is extremely inefficient. Better implementations use the mesh edges to only look at adjacent points in the mesh.
• it doesn't compute the penetration distance when the objects are in collision

Both of these are on my radar for my research too, since GJK is the limiting step in my tracking code.

[beel7217]

Where are the current contact dynamics implemented? The only thing I can tell from the documentation is that contact points can be added to a RigidBody, but it is very unclear how the contact or 'ground' plane is defined and accessed. Is the computation of contact forces implicit when using dynamics! with a floating base model? Any clarification would be greatly appreciated.