The equation of motion from Lesson 7.1 can be read in two directions. Forward dynamics takes the state and the applied forces and returns accelerations: it is what mj_forward and every mj_step compute. Inverse dynamics takes the state and a desired acceleration and returns the forces that would produce it: mj_inverse, with the result in qfrc_inverse.
Inverse dynamics is how a planned motion becomes motor torques (feedforward and computed-torque control, Level 8), how a recorded motion is checked against a robot’s torque limits, and how physical parameters are identified from data (Level 18). This lesson shows that in a simulator the two directions agree exactly only if the inverse accounts for the integrator’s discrete step, and that with contacts the inverse is decided by MuJoCo’s soft-contact model.
[!established] What
mj_inversecomputes Fromqpos,qvelandqacc(plus mocap poses and other inputs),mj_inversereturnsqfrc_inverse, the generalized force that must be applied, by actuators,qfrc_appliedandxfrc_appliedtogether, to produceqacc. Constraint forces, including contacts, are computed from the motion through the soft-constraint model, so inverse dynamics is well defined even with contacts. Theinvdiscreteenable flag makes it invert the discrete-time update of theEulerand implicit integrators instead of the continuous-time equation; thefwdinvflag makesmj_forwardalso run the inverse and report the discrepancy insolver_fwdinv.
The lab plans a 2 s motion of arm7, computes the joint torques with mj_inverse before anything moves, then replays those torques with no feedback and plots the largest joint error on a logarithmic scale.
invdiscrete off). The error starts below $10^{-7}$ rad in the first steps and climbs to a few hundredths of a radian by the end: the torques were right for the continuous motion, not for the steps the simulator takes.invdiscrete on, then replay. The error stays at about $10^{-16}$ rad for the whole motion: the replay lands on every planned sample to rounding error.```lab dynamics {“dock”: true, “title”: “Torque from motion, replayed open loop”, “height”: 280}
## Mathematics
### The two directions
With the terms of Lesson 7.1,
$$\text{forward:}\quad \ddot q = M^{-1}\left(\tau + \tau_\text{passive} + J_c^\top f_c - c\right), \qquad \text{inverse:}\quad \tau = M\ddot q + c - \tau_\text{passive} - J_c^\top f_c .$$
Without constraints these are exact inverses of each other, and output (1) confirms it on `arm7`: `qfrc_inverse` equals the actuator plus applied forces that produced the acceleration, to $10^{-11}$ N m, and it decomposes exactly as the inverse formula says.
### What the discrete step needs
A simulator does not follow the continuous equation; it takes steps. MuJoCo's default `Euler` integrator is semi-implicit:
$$v_{k+1} = v_k + h\,a_k, \qquad q_{k+1} = q_k + h\,v_{k+1},$$
and it treats joint damping implicitly, which replaces $a_k$ by the solution of $(M + hD)\,a_k = f_k$ (Lesson 5.2). For the replay to land exactly on planned samples $q^\star_k$, the inverse must be asked for the acceleration this update needs, not the trajectory's analytic one:
$$v_k = \frac{q^\star_k - q^\star_{k-1}}{h}, \qquad a_k = \frac{q^\star_{k+1} - 2q^\star_k + q^\star_{k-1}}{h^2},$$
and it must invert the implicit-damping update too, which is what `invdiscrete` does. The analytic $\ddot q(t_k)$ and $\dot q(t_k)$ differ from these by terms of order $h$; each step's small mismatch is integrated into position error, and without feedback nothing removes it.
> [!derivation] Why both fixes are needed
> Output (2) separates the causes. Analytic accelerations with `invdiscrete` on still drift, because the velocities and accelerations do not match the discrete update. Integrator-consistent accelerations with `invdiscrete` off still drift, because the inverse ignores the $hD$ term that `Euler` adds for the joints' damping (0.5 N m s/rad on every `arm7` joint). With both, the forward step reproduces each sample to $10^{-16}$ rad: forward and inverse are exact inverses of the discrete map.
### Inverse dynamics with contact
A rigid contact model cannot say how hard a box presses on the floor from its motion alone: any force between zero and infinity is consistent with "not moving". MuJoCo's soft constraints remove the ambiguity. A contact's force depends on its penetration and velocity through `solref` and `solimp` (Level 9), so the force needed to hold a box still at a given height is a function of that height. Output (3) maps it: lifted 1 mm, the box needs exactly its weight, 9.81 N; at its rest depth, zero; pushed 0.5 mm deeper, $-80$ N with the default time constant and $-178$ N with a four-times-stiffer one. Inverse dynamics with contact reports a property of the contact model as much as of the motion.
## Implementation
```io
INPUT: arm7.xml; a 1 kg box on a plane
PROCESS: check that mj_inverse undoes mj_forward; compute torques for a planned motion four ways and replay them open loop; map the holding force of a box against its height for two contact stiffnesses, and check forward-inverse consistency with contacts
OUTPUT: printed tables
```python file=examples/l7_2_forward_inverse.py “"”Lesson 7.2: forward and inverse dynamics, and making them agree through a timestep.
INPUT arm7.xml; a 1 kg box on a plane, written below PROCESS (1) mj_forward then mj_inverse at random states: the inverse recovers the applied forces; the forward-inverse check flag; the terms of qfrc_inverse; (2) torques for a 2 s minimum-jerk motion from mj_inverse, replayed open loop with mj_step, four ways: analytic or integrator-consistent accelerations, with or without the discrete-time inverse flag; then the exact recipe under the other integrators; (3) inverse dynamics with contact: the force needed to hold a box still at heights around its rest pose, for two contact stiffnesses OUTPUT printed tables
Run: python examples/l7_2_forward_inverse.py “””
import os
import mujoco import numpy as np
from mjcourse import model_path
N = 200 if os.environ.get(“MJC_FAST”) == “1” else 1000
INVDISCRETE = int(mujoco.mjtEnableBit.mjENBL_INVDISCRETE)
BOX = “”“
</worldbody></mujoco>”””
def inverse_undoes_forward() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) data = mujoco.MjData(model) rng = np.random.default_rng(0) worst = 0.0 for _ in range(N): data.qpos[:] = rng.uniform(model.jnt_range[:, 0], model.jnt_range[:, 1]) data.qvel[:] = rng.normal(size=model.nv) data.ctrl[:] = rng.uniform(model.actuator_ctrlrange[:, 0], model.actuator_ctrlrange[:, 1]) data.qfrc_applied[:] = rng.normal(scale=2.0, size=model.nv) mujoco.mj_forward(model, data) # torques -> qacc applied = data.qfrc_actuator + data.qfrc_applied mujoco.mj_inverse(model, data) # qacc -> the forces that must have been applied worst = max(worst, np.abs(data.qfrc_inverse - applied).max()) print(f” {N} random states: max |qfrc_inverse - (qfrc_actuator + qfrc_applied)| = {worst:.1e} N m”) mass = np.zeros((model.nv, model.nv)) mujoco.mj_fullM(model, data, mass) terms = mass @ data.qacc + data.qfrc_bias - data.qfrc_passive - data.qfrc_constraint print(f” qfrc_inverse = M qacc + qfrc_bias - qfrc_passive - qfrc_constraint: max difference “ f”{np.abs(terms - data.qfrc_inverse).max():.1e} N m”)
def min_jerk(q0, q1, duration): def at(t): s = np.clip(t / duration, 0.0, 1.0) p = 10 * s3 - 15 * s4 + 6 * s5 dp = (30 * s2 - 60 * s3 + 30 * s4) / duration ddp = (60 * s - 180 * s2 + 120 * s3) / duration**2 return q0 + (q1 - q0) * p, (q1 - q0) * dp, (q1 - q0) * ddp return at
def torque_from_motion() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) h, duration = model.opt.timestep, 2.0 q0 = model.key_qpos[model.key(“home”).id].copy() q1 = q0 + np.array([0.6, -0.3, 0.4, -0.5, 0.5, 0.4, 0.8]) path = min_jerk(q0, q1, duration) n = round(duration / h) samples = np.array([path(k * h)[0] for k in range(n + 2)]) # the positions the replay must hit
def torques(accel: str, invdiscrete: bool) -> np.ndarray:
data = mujoco.MjData(model)
model.opt.enableflags = (model.opt.enableflags | INVDISCRETE) if invdiscrete else (model.opt.enableflags & ~INVDISCRETE)
tau = np.zeros((n, model.nv))
for k in range(n):
if accel == "analytic": # the continuous-time trajectory at t_k
q, v, a = path(k * h)
else: # what semi-implicit Euler needs to land on the samples:
q = samples[k] # v_{k+1} = v_k + h a_k, q_{k+1} = q_k + h v_{k+1}
v = (samples[k] - samples[k - 1]) / h if k else np.zeros(model.nv)
a = ((samples[k + 1] - samples[k]) / h - v) / h
data.qpos[:], data.qvel[:], data.qacc[:] = q, v, a
mujoco.mj_inverse(model, data)
tau[k] = data.qfrc_inverse
model.opt.enableflags &= ~INVDISCRETE
return tau
def replay(tau: np.ndarray) -> np.ndarray:
data = mujoco.MjData(model)
data.qpos[:] = q0
err = np.zeros(n)
for k in range(n):
data.ctrl[:] = tau[k] # motors with gear 1: ctrl is joint torque
mujoco.mj_step(model, data)
err[k] = np.abs(data.qpos - samples[k + 1]).max()
return err
print(f" motion: home to home + [0.6, -0.3, 0.4, -0.5, 0.5, 0.4, 0.8] rad in {duration} s, Euler, h = {h} s")
for accel, inv in (("analytic", False), ("analytic", True), ("integrator-consistent", False), ("integrator-consistent", True)):
tau = torques(accel, inv)
err = replay(tau)
print(f" {accel:<22} invdiscrete {str(inv):<5}: max joint error at 0.5 s {err[n // 4 - 1]:.1e}, "
f"1 s {err[n // 2 - 1]:.1e}, 2 s {err[-1]:.1e} rad")
print(f" largest torque over the motion / motor limit: {np.max(np.abs(tau) / model.actuator_ctrlrange[:, 1]):.2f}")
for name in ("mjINT_IMPLICITFAST", "mjINT_IMPLICIT", "mjINT_RK4"):
model.opt.integrator = getattr(mujoco.mjtIntegrator, name)
try:
err = replay(torques("integrator-consistent", True))
print(f" {name[6:].lower():<13} integrator-consistent, invdiscrete True: max joint error over 2 s {err.max():.1e} rad")
except mujoco.FatalError as exc:
model.opt.enableflags &= ~INVDISCRETE
print(f" {name[6:].lower():<13} mj_inverse with invdiscrete raises: {exc}")
def contact_inverse() -> None: for tc in (0.02, 0.005): model = mujoco.MjModel.from_xml_string(BOX.format(tc=tc)) data = mujoco.MjData(model) for _ in range(2000): # settle onto the plane mujoco.mj_step(model, data) rest = data.qpos[2] row = [] for dz in (1e-3, 0.0, -2e-4, -5e-4, -1e-3): probe = mujoco.MjData(model) probe.qpos[:] = data.qpos probe.qpos[2] = rest + dz probe.qvel[:] = 0 probe.qacc[:] = 0 # “hold it perfectly still here” mujoco.mj_inverse(model, probe) row.append(f”{1000 * dz:+.1f} mm: {probe.qfrc_inverse[2]:+8.2f} N”) print(f” solref time constant {tc} s (rests {1000 * (0.05 - rest):.3f} mm deep): “ + “, “.join(row)) model = mujoco.MjModel.from_xml_string(BOX.format(tc=0.02)) model.opt.enableflags |= int(mujoco.mjtEnableBit.mjENBL_FWDINV) data = mujoco.MjData(model) worst = np.zeros(2) for k in range(1000): # rest, then push it into sliding data.xfrc_applied[1, :3] = [12.0, 0.0, 0.0] if k > 500 else 0.0 mujoco.mj_step(model, data) worst = np.maximum(worst, data.solver_fwdinv) print(f” with contacts active (box at rest, then sliding under a 12 N push, above its 9.81 N friction limit), the forward-inverse check “ f”(mjENBL_FWDINV) stays at most {worst[0]:.1e} in qfrc and {worst[1]:.1e} in constraint force”)
if name == “main”: print(“(1) mj_inverse undoes mj_forward (arm7, no contacts)”) inverse_undoes_forward() print(“(2) torque from motion, replayed open loop”) torque_from_motion() print(“(3) vertical force needed to hold a 1 kg box still, by height offset from its rest pose”) contact_inverse()
Output:
```text
(1) mj_inverse undoes mj_forward (arm7, no contacts)
1000 random states: max |qfrc_inverse - (qfrc_actuator + qfrc_applied)| = 1.6e-11 N m
qfrc_inverse = M qacc + qfrc_bias - qfrc_passive - qfrc_constraint: max difference 7.1e-15 N m
(2) torque from motion, replayed open loop
motion: home to home + [0.6, -0.3, 0.4, -0.5, 0.5, 0.4, 0.8] rad in 2.0 s, Euler, h = 0.002 s
analytic invdiscrete False: max joint error at 0.5 s 3.1e-04, 1 s 7.2e-04, 2 s 4.8e-02 rad
analytic invdiscrete True : max joint error at 0.5 s 3.1e-04, 1 s 7.2e-04, 2 s 2.8e-02 rad
integrator-consistent invdiscrete False: max joint error at 0.5 s 6.2e-04, 1 s 1.4e-03, 2 s 2.0e-02 rad
integrator-consistent invdiscrete True : max joint error at 0.5 s 6.9e-18, 1 s 5.6e-17, 2 s 1.1e-16 rad
largest torque over the motion / motor limit: 0.32
implicitfast integrator-consistent, invdiscrete True: max joint error over 2 s 5.4e-20 rad
implicit integrator-consistent, invdiscrete True: max joint error over 2 s 6.9e-18 rad
rk4 mj_inverse with invdiscrete raises: mj_discreteAcc: discrete inverse dynamics is not supported by RK4 integrator
(3) vertical force needed to hold a 1 kg box still, by height offset from its rest pose
solref time constant 0.02 s (rests 0.108 mm deep): +1.0 mm: +9.81 N, +0.0 mm: +0.00 N, -0.2 mm: -21.35 N, -0.5 mm: -80.15 N, -1.0 mm: -211.74 N
solref time constant 0.005 s (rests 0.043 mm deep): +1.0 mm: +9.81 N, +0.0 mm: -0.00 N, -0.2 mm: -50.19 N, -0.5 mm: -177.52 N, -1.0 mm: -524.00 N
with contacts active (box at rest, then sliding under a 12 N push, above its 9.81 N friction limit), the forward-inverse check (mjENBL_FWDINV) stays at most 1.0e-12 in qfrc and 3.0e-08 in constraint force
Reading the output:
qfrc_inverse is exactly $M\ddot q + c - \tau_\text{passive} - \tau_\text{constraint}$.invdiscrete replay exactly, under Euler, implicitfast and implicit alike; under RK4 the flag is refused. In every other case the error is already $10^{-4}$ rad at 0.5 s and grows faster than linearly, to 2 to 5 hundredths of a radian (1 to 3 degrees) at 2 s: open-loop errors compound. The torques stay under a third of the motor limits, so clamping plays no part.fwdinv) agrees to $10^{-12}$ in generalized force; the $3 \times 10^{-8}$ in constraint force is the forward solver’s convergence tolerance, since the inverse is computed in closed form.Torques from inverse dynamics make the simulated robot drift away from the plan. Feed mj_inverse the velocities and accelerations of the discrete update and enable invdiscrete (output 2). If the plan itself is continuous, sample it at the timestep first.
mj_inverse returns enormous forces for an object resting on a table. The state you gave it has the object slightly inside the table, or the object’s height comes from a different contact setting. Inverse forces with contact depend on penetration (output 3): take resting states from a simulation of the same model.
Inverse dynamics asks for force on a joint that has no actuator. The motion is not feasible for this robot: an unactuated joint, or a free-floating base, cannot receive the force qfrc_inverse demands there. Check those entries before trusting the actuated ones.
mj_inverse stops with “discrete inverse dynamics is not supported by RK4 integrator”. invdiscrete inverts the single-stage update of Euler, implicitfast and implicit (output 2 shows exact replay under all three). RK4 evaluates the dynamics four times per step with the control held constant, so exact replay needs a different inverse (the Challenge).
For the two-link arm, plan a 1 s minimum-jerk motion from $(0, 0)$ to $(0.8, -1.0)$ rad. Compute the torques three ways: from the Lagrangian of Lesson 7.1 with analytic accelerations, from mj_inverse with analytic accelerations, and from mj_inverse with integrator-consistent accelerations and invdiscrete. Show that the first two agree to rounding error and that only the third replays exactly.
Switch arm7 to the RK4 integrator and find torques whose open-loop replay lands on the planned samples to $10^{-10}$ rad. Hint: for each step, solve for the constant ctrl that maps the current state to the next sample under one mj_step, with a few Newton iterations whose Jacobian you estimate by finite differences of mj_step. Compare the torques with the Euler ones.
[!research] Why soft contacts make inverse dynamics useful Because MuJoCo’s contact forces are a function of the state and acceleration, inverse dynamics with contacts has a unique answer, which makes it usable inside trajectory optimization and for estimating contact forces from motion (Todorov, 2014, on the Reading list, describes the formulation). The price is visible in output (3): the answer depends on contact parameters that are themselves modelling choices. Learned inverse-dynamics models trained on simulated data inherit that dependence (inference); identifying contact parameters from real data is part of Level 18.
{"id": "7.2-check", "title": "Knowledge check", "questions": [
{"kind": "mcq", "q": "With semi-implicit Euler (v⁺ = v + h a, q⁺ = q + h v⁺), which acceleration makes a step from sample q_k land exactly on q_{k+1}, given that the velocity arrived at q_k from q_{k−1}?",
"options": ["The analytic second derivative of the trajectory at t_k", "(q_{k+1} − 2 q_k + q_{k−1}) / h²", "(q_{k+1} − q_k) / h", "Zero"],
"answer": 1,
"explain": "<p>With $v_k = (q_k - q_{k-1})/h$, landing on $q_{k+1}$ needs $v_{k+1} = (q_{k+1} - q_k)/h$, so $a_k = (v_{k+1} - v_k)/h$, the central second difference.</p>"},
{"kind": "mcq", "q": "Integrator-consistent accelerations alone still leave arm7's replay drifting. What else must the inverse account for under Euler?",
"options": ["Gravity", "The implicit treatment of joint damping, via the invdiscrete flag", "The actuator gear ratio", "The contact model"],
"answer": 1,
"explain": "<p>Euler solves $(M + hD)a = f$ for joints with damping. <code>invdiscrete</code> makes <code>mj_inverse</code> invert that update; output (2) shows both fixes are needed.</p>"},
{"kind": "numeric", "q": "A 1 kg box rests on the floor. You ask mj_inverse for the vertical force needed to hold it still (qacc = 0) 1 mm above its rest height, where it touches nothing. What is the answer, in N? (g = 9.81 m/s²)",
"answer": 9.81, "tol": 0.001, "unit": "N",
"explain": "<p>No contact, so the whole weight must be supplied: +9.81 N, as output (3) prints.</p>"},
{"kind": "mcq", "q": "Why is inverse dynamics with contacts well defined in MuJoCo but not in a rigid-contact model?",
"options": ["MuJoCo ignores contacts in mj_inverse", "MuJoCo's soft constraints make contact force a function of penetration and velocity", "MuJoCo uses smaller timesteps", "Rigid-contact models have no inverse dynamics at all"],
"answer": 1,
"explain": "<p>A rigid contact admits any force consistent with zero penetration; the soft model picks one through <code>solref</code> and <code>solimp</code>, which is also why the answer depends on them.</p>"}
]}
Level 8 uses these pieces to control the arm: joint-space PD and PID, gravity compensation and computed torque, and operational-space control in the task space.