Analytical MuJoCo MPC

Exploratory research, in progress (2026). This page summarizes work on computing exact analytical derivatives of MuJoCo’s dynamics — now including its soft contact dynamics — and using them inside a model-predictive-control loop, instead of MuJoCo’s default finite differences. Measured results are labeled as such; the one remaining projection is called out explicitly.

📄 Technical draft (PDF) — Exact Analytical First-Order Derivatives of MuJoCo's Smooth and Contact Dynamics for Model-Based Control.

TL;DR

MuJoCo is the de-facto simulator for robot control, but its dynamics Jacobians are computed by finite differencing — which is both slow (it re-simulates the system many times per derivative) and accuracy-limited. This project derives and implements the exact analytical rigid-body-dynamics (RBD) derivatives of MuJoCo’s own smooth dynamics and drops them into the MPC solver.

Three contributions:

• First order (FO), smooth dynamics: the missing analytical \(\partial/\partial q\) half of MuJoCo’s smooth dynamics — the position derivatives MuJoCo computes only by finite difference today.
• Contact dynamics: the first exact analytical derivative through MuJoCo’s soft, elliptic-friction-cone contact solve — covering stuck, slipping, and separated contacts with a single factorization, plus a closed-form contact-Jacobian derivative for the floating base and sphere/capsule feet.
• Second order (SO): the exact analytical Hessian, enabling true exact-Hessian Newton steps in MPC (rather than Gauss–Newton / iLQG approximations).

Headline measured results: analytical FO matches finite-difference to 1e-8…1e-10 and is up to 11.6× faster than MuJoCo’s mjd_transitionFD at humanoid scale; the contact derivative matches a clean finite difference to machine precision, and a fully analytical closed-loop quadruped MPC (state, control, and cost Jacobians \(A,B,C,D\) — zero finite differences, counter-verified) computes its derivatives 10.6× faster than FD / 8.8× faster than WASP and reaches a 37× lower control cost — the latter isolated to the analytical cost-Jacobians, which fix a real finite-difference corruption of MJPC’s time-scheduled cost gradient; analytical SO is bit-identical to compiled CasADi autodiff while needing zero code generation and zero build RAM (CasADi runs out of memory at humanoid scale).

The quadruped MPC solution

The analytical derivatives drive a whole-body MPC controller on a Unitree B2 quadruped + Z1 arm (a loco-manipulation platform). The MPC plans over a 14-node horizon with a Fatrop OCP solver.

The problem

For MPC, you need the dynamics Jacobians \(\partial f/\partial x\) at every node, every tick. In MuJoCo today:

• mjd_smooth_vel gives only the velocity half of the smooth dynamics analytically — i.e. \(\partial\,qacc/\partial v\).
• The position half \(\partial\,qacc/\partial q\) has no analytical path — it is recovered by central finite differences via mjd_transitionFD, which costs \(O(n^2)\) forward simulations and is limited to ~1e-6 accuracy.
• Contact/constraint derivatives are omitted entirely.

So the position Jacobian — the load-bearing term for MPC stability — is exactly the piece MuJoCo computes least accurately and most slowly.

Old vs. new derivative expressions

MuJoCo’s smooth forward dynamics (contacts disabled), with \(c \equiv qfrc\_bias\) the bias (Coriolis/centrifugal/gravity) force:

Old (baseline) — what MuJoCo uses today

New (this work) — the analytical position half

The \(\tfrac{\partial M}{\partial q}\,qacc\) term is not negligible — dropping it is a common and silent source of error. A clean, numerically-robust equivalent reuses the inverse-dynamics map \(\mathrm{id}(q,v,a) = M(q)\,a + c(q,v)\):

which is evaluated in a single \(O(n^2)\) RNEA-style spatial recursion rather than \(O(n^2)\) full forward simulations.

Second order (the moat) — analytical Lagrangian Hessian

For a multiplier \(\lambda\), the four Hessian blocks needed by an exact-Hessian MPC:

computed as a directional quantity in one adjoint pass — \(O(n^2)\) instead of forming the full \(O(n^3)\) tensor.

Analytical contact derivatives — the soft-cone IFT

Smooth dynamics is only half of a contact-rich step. MuJoCo resolves contacts with a soft, convex, acceleration-level solve, \(qacc = qacc_{s} + M^{-1} J^{\top} f\), where \(qacc_{s}=M^{-1}\,qfrc\_smooth\), \(J=\texttt{efc\_J}\) is the constraint Jacobian, and the contact force \(f\) solves a regularized elliptic friction cone. MuJoCo ships no analytical derivative of this step — only finite difference.

One formula for every contact state. Differentiating the solve’s optimality condition by the implicit function theorem gives, for any state component \(\theta\),

The system matrix \(M + J^{\top} H J\) uses the per-contact cost Hessian \(H\): the diagonal stiffness when a contact is stuck, MuJoCo’s dense elliptic cone-Hessian when slipping, and zero when separated. A single Cholesky factorization therefore spans all zones — no special-casing. The last term is the regularization derivative (MuJoCo’s soft stiffness depends on penetration); omitting it is a silent source of error.

Closed-form \(\partial\,\texttt{efc\_J}/\partial q\). The contact Jacobian’s configuration derivative is the material-point kinematic Hessian plus geometry. Three pieces are MuJoCo-specific and were the technical crux: the floating base’s three rotation dofs mutually drag one another’s motion axes; a sphere/capsule on a plane reduces to its center / lower endpoint (with a ½ penetration term on the normal); and a capsule’s friction frame tracks its axis (an extra \(\partial(\text{frame})/\partial q\) on the tangent rows). Validated against finite difference of efc_J to ~1e-11.

Fully analytical MPC (Unitree A1, MJPC iLQG). The backend now computes all four transition blocks analytically — the state Jacobian \(A\), the control Jacobian \(B\), and the cost/sensor Jacobians \(C=\partial\text{residual}/\partial x\), \(D=\partial\text{residual}/\partial u\) — across 100% of a trot (sphere feet, capsule shanks, slipping contacts, joint limits). An always-on counter confirms literally zero finite-difference evaluations enter the result (no mjd_transitionFD, no mj_differentiatePos, no central differences). Same task, same settings (Quadruped Flat, 250 planning steps, single thread):

Two results, both measured:

• Speed. The analytical derivative phase is 10.6× faster than FD and 8.8× faster than WASP (3.9× / 3.5× end-to-end). The gap jumped from the earlier 1.7× because the analytical \(C,D\) remove all \(2n_v\) state-rollouts — FD still re-simulates the system to get the cost Jacobian; the analytical backend does zero forward rollouts.
• Accuracy → control quality. The analytical run reaches a 37× lower control cost (\(0.0085\) vs. \(0.31\)). This is not a confound: flipping only \(C,D\) from analytical to FD — keeping \(A,B\) analytical — reproduces the FD cost exactly (\(0.3106\)). The reason is that mjd_transitionFD computes \(C,D\) by differencing through a full mj_step, which re-fires MJPC’s time-driven gait-schedule residual and injects \(\sim\!1/\epsilon\) discontinuity noise into the cost gradient (spurious \(\mathcal{O}(10^4)\) entries on the gait rows). The analytical \(C,D\) evaluate the instantaneous residual Jacobian directly and avoid it. So the analytical derivatives don’t merely match FD — for time-scheduled costs they fix a real defect in FD-based MPC.

Per-block accuracy vs. a clean central-difference reference (machine precision): \(A\!=\!6\!\times\!10^{-10}\), \(B\!=\!7\!\times\!10^{-12}\), \(C\!=\!2\!\times\!10^{-10}\), \(D\!=\!9\!\times\!10^{-12}\). The capsule friction-frame singularity (axis \(\parallel\) normal) is handled by a gauge-invariance argument — the tangent-frame derivative cancels in \(\partial\,qacc/\partial q\) — not a finite-difference fallback. Methodology: the cost finding above is isolated (same \(A,B\), only \(C,D\) swapped); the end-to-end walltimes are each backend planning its own trajectory (the honest realistic number), while accuracy and the cost isolation are measured at identical states.

Derivative accuracy

FO accuracy gate (KS0). Analytical \(\partial\,qacc/\partial\{q,v\}\) vs. MuJoCo’s own central-difference qacc. Pre-registered gate: max relative error ≤ 1e-5 on every robot. Result — passes by 3–6 orders of magnitude:

SO accuracy. Analytical second-order vs. CasADi autodiff and finite difference on B2+Z1 (n_v=25) — essentially exact (machine precision against CasADi):

Speed

FO speed vs. finite difference (KS1). Wall-clock per derivative evaluation; the gap grows with DoF as expected (exact \(O(n^2)\) RBD vs. \(O(n)\times\) forward-sim FD):

Binding-level speed vs. compiled CasADi (same quantities, both compiled to C, B2+Z1, n_v=25):

…and crucially, the one-time codegen wall: analytical needs 0 generated code, 0 compile time, 0 peak RAM, whereas CasADi SO generates 8.83 MB of C, takes 63 s to compile, and runs out of memory (>32 GB) at humanoid scale.

MPC results

Measured end-to-end (B2+Z1, 14-node Fatrop OCP, CasADi derivatives). Derivatives dominate — ~80% of every control tick is spent in the Jacobian and Hessian callbacks:

Projected with analytical derivatives (component-wise measurements applied to the same loop — projection, not yet a closed-loop measurement): replacing nlp_jac_g (3.7 → 0.36 ms, ~10×) and nlp_hess_l (7.8 → 1.5 ms, ~5×) brings the tick to ~20 ms (~3× faster).

Scaling / buildability. Analytical builds and runs at every scale; CasADi’s SO tensor cannot be built at humanoid scale:

How it compares

(End-to-end on the Unitree A1 trot, 250 planning steps. The 37× lower cost is isolated to the analytical cost-Jacobians \(C,D\) — see above. End-to-end walltime is 3.9× / 3.5× faster; at humanoid scale the smooth derivative alone reaches 11.6× over FD.)

Models tested

Status & next steps

• ✅ FO well-posedness (KS0) — analytical matches FD to 1e-8…1e-10 across n_v = 9 → 38.
• ✅ FO speed (KS1) — up to 11.6× over FD at humanoid scale.
• ✅ Contact derivative — exact analytical derivative through MuJoCo’s soft elliptic cone (stuck/slip/separated), with closed-form ∂efc_J/∂q for floating-base sphere/capsule feet; matches a clean FD to machine precision.
• ✅ Fully analytical closed-loop (A, B, C, D) — wired into MJPC, 100% analytic coverage of a quadruped trot with counter-verified zero finite differences; 10.6× faster derivatives than FD (8.8× than WASP) and 37× lower control cost (isolated to the analytical cost-Jacobians, which fix FD’s corruption of the time-scheduled cost gradient).
• ✅ SO correctness — bit-identical to CasADi, validated against FD, with no codegen wall.
• ⏳ Capsule side / self-collision & other geom pairs — current contact closed form covers sphere/capsule-endpoint on a plane; cylinder/box and capsule–capsule contacts fall back to FD.
• ⏳ Exact-Hessian vs. Gauss–Newton — does the exact Hessian buy faster MPC convergence? (the flagship SO question)

Stack: MuJoCo, rigid-body-dynamics derivatives (RNEA/ABA-style recursions), CasADi (baseline), Fatrop OCP, whole-body MPC, C++/Python.

Exploratory research conducted independently. Numbers above are taken from the project’s pre-registered result logs; the end-to-end analytical MPC tick time is a projection from per-component measurements and is labeled accordingly.