The current implementation turns off gravity on the specified bodies. This seems elegant, and is correct for many cases. But this has one problem: the effort limits on the input port are now incorrect.
To correctly model the input limits, we need to use the InverseDynamics system with InverseDynamicsMode == kGravityCompensation to compute the gravity comp forces, and pass those in to the input port. (And we should leave gravity enabled on the plant).
The current implementation turns off gravity on the specified bodies. This seems elegant, and is correct for many cases. But this has one problem: the effort limits on the input port are now incorrect.
To correctly model the input limits, we need to use the
InverseDynamicssystem withInverseDynamicsMode == kGravityCompensationto compute the gravity comp forces, and pass those in to the input port. (And we should leave gravity enabled on the plant).