How to compute the spatial forces acting on a body/ manipulator end-effector?

32 views Asked by At

I want to obtain the spatial forces acting on a body from MultibodyPlant without simulating the plant. In particular, I have a 7 DoF manipulator and wish to obtain the spatial forces acting on its last link (operational space form of the inverse dynamics).

I could obtain these using the pseudo-inverse of the spatial Jacobian (F = J^{-T} @ tau), where generalized forces tau are computed using CalcInverseDynamics. However, this is rather inefficient, especially as I want to differentiate through them with respect to plant parameters (system ID).

There is also get_reaction_forces_output_port. However, this would require simulating the plant.

There exists an internal C++ overload of CalcInverseDynamics that is called by the public version of CalcInverseDynamics and computes the spatial forces (see here). Moreover, there is an even closer function to what I want, called CalcJointReactionForces that computes the spatial forces for each joint of the iiwa manipulator. However, it lives in benchmarks and is not general enough to be useful in wider contexts (e.g. no way to initialize the context).

How can I achieve this with a public API method?

0

There are 0 answers