[MuJoCo] Inverse Dynamics API

mj_inverse() calculates the inverse dynamics of the dynamics system, similar to inverseDynamics() function in Matlab Robotics Toolbox.

Input to the function need to be pre-assigned to the sim.data struct (mjData.qpos, mjData.qvel, mjData.qacc, mjData.mocap_pos, mjData.mocap_quat) and the result can be obtained from sim.data.qfrc_inverse.

qfrc_inverse is the sum of all applied forces, namely

qfrc_applied and Jx' * xfrc_applied are user defined external forces or forces from spring damper mechanisms. If no additional force is exerted on either joint or body, joints has no friction, the first two terms become 0 and qfrc_inverse = qfrc_acturator.

The contact forces are treated as internal forces of the system and can be handled implicitly by the inverse dynamics solver.

As for actuation, we have

in simple cases, but MuJoCo can also model more elaborate actuators.

mj_inverse() does not currently attempt to infer actuator forces. It just computes qfrc_inverse which is the sum of all external and actuator forces. On the bright side, the constraint forces are the output of the computation and not the input. So all you need to provide is the current position, velocity and acceleration (qpos, qvel, qacc). 1

It may seem magic, but the reason it is doable is because the constraint model is soft, so the kinematics uniquely determine both the applied force and the constraint force.2

The top-level function mj_inverse invokes the following sequence of computations.

  1. Compute the forward kinematics.
  2. Compute the body inertias and joint axes.
  3. Compute the tendon lengths and moment arms.
  4. Compute the actuator lengths and moment arms.
  5. Compute the composite rigid body inertias and form the joint-space inertia matrix $M(\qb)$.
  6. Compute the sparse factorization of the joint-space inertia matrix.
  7. Construct the list of active contacts (sim.data.contact).
  8. Construct the constraint Jacobian and compute the constraint residuals.
  9. Compute sensor data that only depends on position, and the potential energy if enabled (sim.data.sensordata).
  10. Compute the tendon and actuator velocities (sim.data.qvel).
  11. Compute the body velocities (sim.data.body_xvelp , sim.data.body_xvelr) and joint axes rates of change.
  12. Compute sensor data that depends on velocity, and the kinetic energy if enabled (sim.data.sensordata).
  13. Compute all passive forces (sim.data.qfrc_passive).
  14. Compute the reference constraint acceleration.
  15. Compute the constraint force. This is done analytically, without using a numerical solver.
  16. Compute the inverse dynamics for the unconstrained system.
  17. Compute sensor data that depends on force and acceleration if enabled (sim.data.sensordata).
  18. Compute the vector mjData.qfrc_inverse by combining all results. This is the main output of inverse dynamics. It equals the sum of external and actuation forces.
1. http://www.mujoco.org/forum/index.php?threads/qfrc_inverse.3463/
2. http://www.mujoco.org/forum/index.php?threads/3d-inverse-dynamics-on-experimental-data.3377/