Lack of force introspection makes model debugging hard

[ggould-tri]

This is a feature request, not a bug -- I have no reason to believe that Drake was doing anything odd, just that I could not figure out why my code using drake was buggy.

Trying to debug why a joint in my simulation reached equilibrium at an unexpected position, I found myself wanting to know all of the forces acting on it. But this turns out to be very difficult, as there is no complete list of the forces acting on a joint and each individual thing that can act on a joint presents its force in a different way (if at all):

• Joint internal forces:
  • Joint friction -- no way to get this at all?
  • Joint limit -- no way to get this at all?
• Mobilizer forces:
  • You have to find something to compute an output port on?
• Contact forces:
  • You have to find something to compute an output port on?
• JointActuator and ForceElement forces:
  • If you know what force elements act on your joint you can CalcConservativePower and CalcNonConservative power, multiply them onto joint velocity, and hope you get the geometry right I guess?
• Other sources of force: Are there any? There doesn't seem to be a way for me to know if this list is complete.

A similar situation applies to free bodies.

@amcastro-tri suggests that this is related to #12886

[amcastro-tri]

Thank you @ggould-tri, this is a great list! I'd love to hear if you have any thoughts on a preferred API to request these.

[ggould-tri]

The two problems are (1) what are all the things exerting a force on this joint or body, and (2) what force is each of those things exerting.

So for example right now I have a joint that I believe has a gravity force, a joint limit force and a LinearSpringDamper force, and which is in equilibrium in a position that seems wrong to me. I wish to debug this situation. So I would want roughly:

• A function "what is exerting forces on this joint" -- Joint::GetForceSources() -> vector<pair<Tag, Identifier>> perhaps, assuming that there's a TypeSafeIndex type for every source of force?
• A GetForce method on each such source of force. For instance, LinearSpringDamper::GetForce, Joint::GetJointLimitForce, etc...

[sherm1]

Forces (including torques) along the joint axes come in two flavors:

• Forces directly applied to the axes, like actuators and torsional springs, and
• Body forces that are transmitted through the joint. (That includes gravity, centrifugal and coriolis force, contact forces, etc.)

Similarly, joints apply forces to bodies (both along actuated axes and as reaction forces in the non-moving directions).

Any thoughts on how you would like those untangled?

[ggould-tri]

From a debugging perspective, what I ideally want is a free body diagram like kids draw in high school physics :-) "What are the forces" and "where is this particular force coming from" are the questions and the rest is detail.

It sounds to me from your description like the forces in the first category are forces on the joint (either from actuation, constraint, or topology), and the forces in the second category are forces on links (which are the things that have mass and can collide and occupy space). So possibly asking for the forces on a joint should give the individual items in the first category and the total force of the second category coming in, and likewise asking for the forces on a link (or free body) the itemized second category and the sum of the inputs from the first category?

I think the main thing should be that the Newtonian accounting should work -- the forces on a body or joint at rest must sum to zero. That's the only way to be sure you've got them all.