Forward dynamics, ABA, and simulator pipelines

Learning outcomes

Solve unconstrained forward dynamics.
Explain why ABA avoids a dense matrix inverse.
Identify the main stages of an engine step.

Prerequisites: Linear algebra, ODEs, and earlier lectures in this sequence.

Force to acceleration

For an unconstrained robot,

\dot v=M(q)^{-1}(\tau-c(q,v)).

This equation describes a mathematical solution, not an instruction to form a numerical inverse. For a small model, use a factorization and linear solve. Reuse a factorization when M is unchanged and multiple right-hand sides are needed.

A positive-definite mass matrix gives a unique acceleration for a specified generalized force. If a model's inertia is singular because masses or inertias are invalid, an arbitrary regularization can hide a modeling mistake.

Articulated-body algorithm

ABA eliminates internal force/acceleration dependencies recursively along a tree. A backward pass forms articulated-body inertias and bias quantities. A forward pass recovers joint accelerations. For fixed-size joints, unconstrained tree forward dynamics can have linear complexity without explicitly building dense M.

An engine with contacts may instead benefit from factoring the joint-space inertia because constraint solvers need repeated applications of $M^{-1}$ . Consequently, using a sophisticated engine does not imply every forward step uses textbook ABA.

Worked coupled solve

With

M=\begin{bmatrix}5&2\\2&1\end{bmatrix},\qquad\tau-c=(1,0),

the solution is

\dot v=(1,-2)

. Applying force to the first joint accelerates both coordinates. Dividing each force by the corresponding diagonal inertia gives the wrong answer because it ignores coupling.

A residual $\|M\dot v-(\tau-c)\|$ tests the solve. Compare a relative residual as well when force magnitudes vary substantially.

Engine pipeline

A typical step updates kinematics, computes inertia and bias, evaluates passive forces and actuation, detects or assembles constraints, solves accelerations, then integrates state. Derived arrays refer to particular stages. After advancing qpos, a cached Jacobian may describe the previous configuration until refreshed.

MuJoCo's mj_forward evaluates dynamics at a state without integration. mj_step also advances time. Practice 4 explicitly refreshes before every measurement and checks the documented force decomposition MuJoCo computation.

Timing experiments

Compare equal workloads: one inverse-dynamics call, one M assembly, one factorized solve, or a full constrained step. Include model size and contact count. Python overhead dominates tiny examples, so do not infer asymptotic algorithmic complexity from a two-link timing plot.

Exercise: Why can an implementation that constructs M still outperform a recursive alternative in a particular application? Answer guide: factorization reuse, vectorization, compiled code, repeated constraint solves, and overhead can dominate. Theoretical complexity describes scaling assumptions, not every finite workload.

The optional Pinocchio comparison calls ABA on an equivalent model and checks acceleration agreement. It is a separate extension so the core course remains runnable with MuJoCo alone.