Frame mismatch in MultibodyPlant::CalcRevoluteJointPenaltyParameters()

[avalenzu]

Consider the following lines from multibody_plant.cc: https://github.com/RobotLocomotion/drake/blob/59de1fd0bb25b0628b598d4a89cddde99b3e6f53/multibody/plant/multibody_plant.cc#L208-L209

M_PPo_P.Shift(p_PJ) yields M_PJo_P, upon which we then call ReExpress(R_PJ). However, M_PJo_P.ReExpress(R_PJ) does not match the order of frames given in the documentation: https://github.com/RobotLocomotion/drake/blob/59de1fd0bb25b0628b598d4a89cddde99b3e6f53/multibody/tree/spatial_inertia.h#L312-L317

Per that description, the "expressed-in after" frame (A in the doc) should come first, and the "expressed in before" frame (E in the doc) should come second. Thus, the call to go from M_PJo_P to M_PJo_J ought to be M_PJo_P.ReExpress(R_JP), which would make line 209 in the snippet from multibody_plant.cc read M_PPo_P.Shift(p_PJ).ReExpress(R_PJ.inverse()).

[sherm1]

@amcastro-tri this does look wrong, though I think it may get saved a few lines later by the fact that the rotational axis is the same in either the parent or child frame?

[amcastro-tri]

that's correct, but it should still get fixed.

[mitiguy]

For what it is worth -- copying a comment that I made in a related Slack conversation:

Feedback on using operator*().

Using a single operator*() in connection with to reexpress a RotationalInertia (or SpatialInertia) seems odd to me. My understanding is re-expressing a RotationalInertia is equivalent to pre and post-multiplication of the 3x3 inertia matrix by the RotationMatrix and its transpose.

Related: Using the operator*() in conjunction with multiplying a RotationMatrix by a Vector3 is equivalent to the dot-product of the RotationMatrix regarded as a dyadic (tensor) with a vector.

[sherm1]

@mitiguy where did you see someone using operator*() to reexpress an inertia? The above uses the inertia.ReExpress() operator which applies the rotation from both sides.

[mitiguy]

There was a mention of considering using operator() in slack. "I believe it'd be nicer if we had operator(RotationMatrix, SpatialInertia) so that pattern matching is even more obvious" Hopefully, the link below guides you directly to it: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1598383411039200?thread_ts=1598377324.037100&cid=C2WBPQDB7

[amcastro-tri]

In case my Slack comment gets lost:

We can always impose our own semantics on the operators @mitiguy. I like that idea because then that way Vector3, SpatialVector(s) and SpatialInertia (also RotationalInertia) would re-express with the same c++ operator and pattern matching would work for them all the same.

The odd pattern matching in ReExpress() caused my own confusion in those lines of code @avalenzu reported.

Thoughts?

[sherm1]

I agree with @mitiguy that writing R_AB * I_BB (for an inertia I in frame B; I'm emphasizing that it is a tensor) would be misleading and we ought to avoid it. The rotation is R_AB * I_BB * R_BA; I'd rather not make anyone think it is just like rotating a vector.

[sherm1]

More from Slack: alejandro 1 minute ago

I'd accept that claim if R_AB * I_BB meant something else, but it doesn't. So if we introduce it, it means the same for every spatial quantity; re-express, ALWAYS. no?

sherm1 < 1 minute ago

It's usually a bad idea to overload operators unless the meaning is very conventional and obvious. In this case it is not so I think it is better to have the confusing I_BB.ReExpress(R_AB) that might lead someone to go read the documentation! Also this is a VERY expensive operation (though we can still speed it up with Featherstone's hack).

[mitiguy]

To me, R_AB I_BB has a different meaning, namely, the result is a dyadic with units of inertia (e.g., kg m^2) -- and the resulting dyadic has a mixed basis. I interpret R_AB I_BB as a dot product between the rotation dyadic R_AB and the inertia dyadic I_BB, and the result of the dot-product of two dyadics is a dyadic - but not the one proposed.