The Physics of 2D Multibody Systems

Top-level map: Field Guide

Companion chapter: The Computational Machinery: 2D MBSD in Python — how each piece of the physics below is implemented in code.

A consolidated reference for the mechanical-dynamics theory underlying every simulation in this series: rigid-body mechanics, generalised coordinates, constraints, forces, energy, and the Lagrangian formulation that ties them all together.

Pure physics and maths, no code.

Intended as a clean refresher for a mechanical engineer who wants to understand what the simulator is computing before (or after) looking at how.

1. Rigid bodies in the plane

A rigid body in 2D is described by three coordinates:

$\mathbf{q}_\text{body} = [x,\; y,\; \theta]$

$(x, y)$ is the position of a reference point on the body (usually an attachment point where a joint lives), and $\theta$ is the body’s angular orientation measured counter-clockwise from the world x-axis.

The body’s centre of mass sits at a body-fixed offset $\mathbf{r}_G = [r_{G_x},\, r_{G_y}]$ relative to that reference point. In world coordinates, the CG is at

$\mathbf{R}_\text{CG} = (x,\, y) + \mathbf{A}(\theta)\cdot \mathbf{r}_G$

where $\mathbf{A}(\theta)$ is the $2\times 2$ rotation matrix:

$\mathbf{A}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}$

So $\mathbf{r}_G$ is a model input per body; setting $\mathbf{r}_G = \mathbf{0}$ means the body’s reference point coincides with its CG.

A crank whose reference point is at the pivot but whose CG is half-way along its length has $\mathbf{r}_G = [L/2,\; 0]$ — exactly our slider-crank convention.

Velocities and accelerations follow by time differentiation.

In particular, the CG velocity couples translation and rotation:

$\mathbf{V}_\text{CG} = (\dot{x},\, \dot{y}) + \mathbf{A}'(\theta)\cdot \mathbf{r}_G\cdot \dot{\theta}$

where $\mathbf{A}'(\theta) = d\mathbf{A}/d\theta$ . When $\mathbf{r}_G \neq \mathbf{0}$ , any rotation contributes to CG motion, and the body’s linear and angular equations of motion become coupled.

2. Mass and inertia — the body mass matrix

For a single body with mass $m$ , central inertia $I_\text{cg}$ , and CG offset $\mathbf{r}_G$ , Newton–Euler equations expressed at the reference point (not the CG) give a $3\times 3$ mass matrix:

\mathbf{M}_\text{body} = \begin{bmatrix} m & 0 & -m\,(\mathbf{A}'\mathbf{r}_G)_x \\ 0 & m & -m\,(\mathbf{A}'\mathbf{r}_G)_y \\ -m\,((\mathbf{A}'\mathbf{r}_G)^T)_x & -m\,((\mathbf{A}'\mathbf{r}_G)^T)_y & I_\text{ref} \end{bmatrix}

with $I_\text{ref} = I_\text{cg} + m\,\|\mathbf{r}_G\|^2$ by Steiner’s parallel-axis theorem. Two observations:

The off-diagonal terms vanish only when $\mathbf{r}_G = \mathbf{0}$ (reference point = CG).
For a system of $n_b$ bodies, the global mass matrix is block-diagonal, one $3\times 3$ per body. Size: $3n_b \times 3n_b$ .

The mass matrix is configuration-dependent (through $\mathbf{A}'$ ) but the structure is fixed; it only depends on the geometric constants $m$ , $I_\text{cg}$ , $\mathbf{r}_G$ per body.

3. Generalised coordinates

Stacking all body coordinates gives the system’s generalised coordinate vector:

$\mathbf{q} = [x_1,\; y_1,\; \theta_1,\quad x_2,\; y_2,\; \theta_2,\quad \ldots,\quad x_{n_b},\; y_{n_b},\; \theta_{n_b}]$

Plus, for joint types that use extra algebraic parameters (the cam/leva constraint’s surface parameters, for instance), additional entries appended at the end.

$\mathbf{q}$ has $n_\text{coord}$ entries total.
$\mathbf{v} = d\mathbf{q}/dt$ (generalised velocities).
$\mathbf{a} = d\mathbf{v}/dt$ (generalised accelerations).

A mechanism with no constraints would have $n_\text{coord}$ independent degrees of freedom; reality imposes constraint equations that remove some of these.

4. Constraints

A holonomic constraint is an equation of the form $\mathbf{C}(\mathbf{q}, t) = \mathbf{0}$ that must hold at every time.

“Holonomic” means it depends only on positions (and possibly explicit time), not on velocities. All the joints in this series are holonomic.

Common 2D joint types and the number of equations each contributes:

Joint	Geometric meaning	Equations
Revolute (pin)	Two body-fixed points coincide in world	2
Prismatic (slider)	Same angle + one point stays on a fixed line	2
Point-on-line	One body-fixed point stays on a line defined by another body	1
Cam/Leva	Two parameterised surfaces stay in tangent contact	3 (+ 2 extra coords per joint)
Gear	$\theta_i - \tau\,\theta_j = 0$ (fixed transmission ratio)	1
User	Any scalar equation $C_\text{user}(\mathbf{q}, t) = 0$ (driving motion, locks, …)	1 or more

Plus three equations pinning the ground body at $x_0 = y_0 = \theta_0 = 0$ .

Stacking all constraint equations gives the constraint vector $\mathbf{C}(\mathbf{q}, t) \in \mathbb{R}^{n_\text{restr}}$ . The system’s physical degrees of freedom are

$n_\text{DOF} = n_\text{coord} - n_\text{restr}$

For a slider-crank driven by a user constraint that prescribes the crank angle, $n_\text{DOF} = 0$ — the mechanism’s motion is completely determined.

Without that user constraint, $n_\text{DOF} = 1$ — the crank can spin freely.

Velocity and acceleration constraints

Differentiating $\mathbf{C}(\mathbf{q}, t) = \mathbf{0}$ once in time gives the velocity-level constraint:

$\mathbf{C}_q \cdot \mathbf{v} + \frac{\partial \mathbf{C}}{\partial t} = \mathbf{0} \qquad \bigl(\text{equivalently:}\;\; \mathbf{C}_q \cdot \mathbf{v} = -\mathbf{C}_t\bigr)$

where $\mathbf{C}_q = \partial\mathbf{C}/\partial\mathbf{q}$ is the constraint Jacobian, a matrix of size $n_\text{restr} \times n_\text{coord}$ . Differentiating a second time gives the acceleration-level constraint:

$\mathbf{C}_q \cdot \mathbf{a} = \boldsymbol{\gamma} \qquad \text{where} \quad \boldsymbol{\gamma} = -\!\left(\frac{d\mathbf{C}_q}{dt}\cdot \mathbf{v} + \frac{\partial^2 \mathbf{C}}{\partial t^2}\right)$

These are not independent of the original position-level $\mathbf{C} = \mathbf{0}$ — they are consequences of it. But they are what the solver actually uses.

5. Forces

Generalised forces have the same dimension as $\mathbf{q}$ — one entry per coordinate.

5.1 Gravity

If $\mathbf{g} = [0,\; -g_0]$ is the uniform world gravity vector, the gravitational potential energy of body $i$ is

$V_i = -m_i\,\mathbf{g}\cdot\mathbf{R}_{\text{CG},i} = m_i\,g_0\,y_{\text{CG},i}$

The generalised gravity force on $\mathbf{q}$ is $-\partial V/\partial\mathbf{q}$ . In practice:

Force on $(x_i, y_i)$ : $[0,\; -m_i g_0]$ — the usual weight.
Torque on $\theta_i$ : $m_i\,\mathbf{r}_{G_i}^T \mathbf{A}'(\theta_i)^T\,\mathbf{g}$ — non-zero only when the body has an offset CG ( $\mathbf{r}_G \neq \mathbf{0}$ ), capturing gravity’s lever arm.

5.2 Springs and dampers

A linear spring-damper element connects point $\mathbf{p}_i$ on body $i$ to point $\mathbf{p}_j$ on body $j$ . Its instantaneous length is $L = \|\mathbf{P}_i - \mathbf{P}_j\|$ ; the force magnitude along that line is

$F = k\,(L - L_0) + c\,\frac{dL}{dt}$

Projecting this scalar force onto $\mathbf{q}$ uses the spring Jacobian (derivative of $L$ with respect to the body coordinates). The rate of length change $dL/dt$ involves both translation and rotation of the attachment points.

5.3 Elastic (penalty) contact

Unilateral contact between two surfaces is modelled as a Hertzian penalty force that only acts when the surfaces interpenetrate:

$F_n = \begin{cases} K\,\delta^{3/2} + C_\text{damp}\,v_\text{pen}\,\delta & \delta > 0 \\ 0 & \delta \leq 0 \end{cases}$

where $\delta$ is the signed penetration depth (positive when interpenetrating) and $v_\text{pen} = -d\delta/dt$ is the penetration velocity. The normal force points along the surface normal; it always pushes (never pulls), which is the defining property of unilateral contact.

Optional Coulomb friction along the surface tangent is regularised using a smooth $\tanh$ :

$F_t = -\mu\,F_n\,\tanh\!\left(\frac{v_\text{slip}}{v_\text{lim}}\right)$

$v_\text{slip}$ is the tangential relative velocity at the contact point; $v_\text{lim}$ is a regularisation scale that replaces the discontinuous $\operatorname{sign}(v_\text{slip})$ of exact Coulomb friction with a smooth S-curve, essential for ODE integrators.

5.4 Other force types

Centrifugal / Coriolis pseudo-forces in a rotating reference frame.
User-defined forces — any function $\mathbf{Q}_\text{user}(t, \mathbf{q}, \mathbf{v})$ returning a generalised force vector.

5.5 Total generalised force

The total applied force on the system is the sum of all contributions:

$\mathbf{Q}_\text{total} = \mathbf{Q}_\text{gravity} + \mathbf{Q}_\text{springs} + \mathbf{Q}_\text{contacts} + \mathbf{Q}_\text{user} + \mathbf{Q}_\text{centrifugal}$

This is what appears on the right-hand side of the equations of motion.

6. The Lagrangian formulation

For an unconstrained system the Lagrangian is $\mathcal{L} = T - V$ where

$T = \tfrac{1}{2}\,\mathbf{v}^T \mathbf{M}(\mathbf{q})\,\mathbf{v} \qquad \text{(kinetic energy)}$

$V = V_\text{gravity} + V_\text{springs} + \cdots \qquad \text{(potential energy)}$

The Euler–Lagrange equations give the unconstrained equations of motion:

$\frac{d}{dt}\!\left(\frac{\partial T}{\partial \mathbf{v}}\right) - \frac{\partial T}{\partial \mathbf{q}} + \frac{\partial V}{\partial \mathbf{q}} = \mathbf{Q}_\text{non-potential}$

For a system with position-dependent mass matrix this would produce velocity-quadratic “Christoffel” terms, but because we work in reference coordinates and the mass matrix varies only through the rotation angles (and even then smoothly), for rigid-body dynamics in 2D the formulation simplifies to

$\mathbf{M}(\mathbf{q})\,\mathbf{a} = \mathbf{Q}_\text{total}(\mathbf{q}, \mathbf{v}, t) - (\text{velocity-quadratic terms})$

For planar mechanisms with bodies whose $\mathbf{r}_G = \mathbf{0}$ the velocity-quadratic terms vanish entirely; for bodies with $\mathbf{r}_G \neq \mathbf{0}$ they appear as small correction terms that we absorb into the numerical formulation (the mass-matrix time derivative when assembling $\boldsymbol{\gamma}$ for the constraint right-hand side).

6.1 Adding constraints: the Lagrange-multiplier augmentation

To enforce $\mathbf{C}(\mathbf{q}, t) = \mathbf{0}$ , we augment the Lagrangian with a multiplier vector $\boldsymbol{\lambda} \in \mathbb{R}^{n_\text{restr}}$ :

$\tilde{\mathcal{L}} = \mathcal{L} - \boldsymbol{\lambda}^T \mathbf{C}(\mathbf{q}, t)$

Stationarity with respect to $\mathbf{q}$ gives

$\mathbf{M}\,\mathbf{a} + \mathbf{C}_q^T\,\boldsymbol{\lambda} = \mathbf{Q}_\text{total}$

and stationarity with respect to $\boldsymbol{\lambda}$ recovers the constraint $\mathbf{C}(\mathbf{q},t) = \mathbf{0}$ itself.

Physical meaning of the multipliers. The term $\mathbf{C}_q^T\,\boldsymbol{\lambda}$ is the constraint reaction force: the force the constraint applies to the system to keep $\mathbf{C} = \mathbf{0}$ satisfied. Each individual $\lambda_k$ is the generalised force associated with constraint equation $k$ . For a revolute joint, the multiplier pair $(\lambda_x, \lambda_y)$ is the bearing reaction; for a user constraint that drives the crank angle, the multiplier is (minus) the required driving torque.

7. The equations of motion: an index-1 DAE

Putting everything together, the continuous-time equations of motion are a differential-algebraic system (DAE) of three coupled equations:

$\mathbf{M}(\mathbf{q})\,\mathbf{a} + \mathbf{C}_q^T\,\boldsymbol{\lambda} = \mathbf{Q}_\text{total}(\mathbf{q}, \mathbf{v}, t) \qquad \text{(Newton's law with constraint force)}$

$\mathbf{C}(\mathbf{q}, t) = \mathbf{0} \qquad \text{(position constraint, index-3)}$

The second equation is algebraic — it has no time derivatives of $\boldsymbol{\lambda}$ — which is what makes this a DAE rather than an ODE. The “index” refers to how many times the algebraic equation must be differentiated to recover an ODE in all variables.

Index reduction. Differentiating $\mathbf{C}(\mathbf{q}, t) = \mathbf{0}$ twice gives the acceleration-level constraint $\mathbf{C}_q\,\mathbf{a} = \boldsymbol{\gamma}$ . Combining with Newton’s law:

$\begin{bmatrix} \mathbf{M} & \mathbf{C}_q^T \\ \mathbf{C}_q & \mathbf{0} \end{bmatrix} \begin{bmatrix} \mathbf{a} \\ \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{Q}_\text{total} \\ \boldsymbol{\gamma} \end{bmatrix}$

This is the saddle-point system that the simulator solves at every time step. It’s a single linear system in the unknowns $\mathbf{a}$ and $\boldsymbol{\lambda}$ . Once solved, we have:

$\mathbf{a}$ — the generalised accelerations, ready to integrate (forward dynamics).
$\boldsymbol{\lambda}$ — the Lagrange multipliers = constraint reaction forces (interesting in their own right, especially for inverse dynamics).

The name “saddle-point” comes from the associated optimisation interpretation: the solution is a minimum of the system’s kinetic energy functional in $\mathbf{a}$ and a maximum of the Lagrangian in $\boldsymbol{\lambda}$ — a saddle point of the augmented functional. For us it’s just a linear solve.

8. Bilateral vs unilateral contact

A bilateral constraint (revolute, prismatic, cam/leva contact) enforces $\mathbf{C}(\mathbf{q}, t) = \mathbf{0}$ exactly at all times. The associated Lagrange multiplier can have any sign — positive or negative — and the constraint force can act in either direction.

A unilateral contact (penalty-based ground contact, tyre-on-road) does not produce a constraint equation. Instead it appears as a force in $\mathbf{Q}_\text{total}$ . The force only exists when the bodies interpenetrate; it always pushes them apart. No multiplier is required.

The trade-off:

Bilateral constraints are exactly satisfied (up to numerical precision). Fast, clean, but require invertibility of the constraint Jacobian and may force spurious attractive contact if the real physics would have lost contact.
Penalty unilateral contact is approximately satisfied via very stiff forces ( $K_\text{hertz}$ large). Robust, physical (contact can open and close naturally), but makes the ODE stiff and imposes small integration timesteps.

Our simulator supports both and uses each where appropriate: bilateral for joint connections that should never separate, penalty for wheel-on-ground and similar contact dynamics.

9. Energy

Three forms of energy live in the system:

$T = \tfrac{1}{2}\,\mathbf{v}^T \mathbf{M}(\mathbf{q})\,\mathbf{v} \qquad \text{(kinetic energy)}$

$V_g = \sum_i m_i\,g_0\,y_{\text{CG},i} \qquad \text{(gravitational potential)}$

$V_s = \sum \tfrac{1}{2}\,k\,(L - L_0)^2 \qquad \text{(elastic spring potential)}$

Total mechanical energy is $E = T + V_g + V_s$ . For a system with only bilateral constraints, conservative forces, and no dampers, $E$ is conserved along the trajectory; this is the single most sensitive correctness test of a dynamic simulator.

Penalty contact and dampers dissipate energy — monotonically decreasing $E$ is the correctness test for those.

10. Forward vs inverse dynamics

The same saddle-point system supports two complementary questions:

Forward dynamics: given $\mathbf{Q}_\text{total}(\mathbf{q}, \mathbf{v}, t)$ and initial $(\mathbf{q}_0, \mathbf{v}_0)$ , find the trajectory $\mathbf{q}(t), \mathbf{v}(t)$ . Integrate the block system forward in time — an initial-value DAE problem.
Inverse dynamics: prescribe the motion $\mathbf{q}(t), \mathbf{v}(t), \mathbf{a}(t)$ (typically via user constraints that make $n_\text{DOF} = 0$ ), and read off the Lagrange multipliers $\boldsymbol{\lambda}(t)$ . No integration is needed — one linear solve per time step is enough. The multipliers directly report bearing reactions, driving torque, guide normal forces.

Both are just different projections of the same underlying saddle-point system — the difference is only whether you advance $(\mathbf{q}, \mathbf{v})$ or whether $(\mathbf{q}, \mathbf{v})$ are prescribed.

11. FAQ — common points of confusion

Three questions that regularly come up when an engineer first meets this framework. Each maps directly back to a section above, but the phrasing is closer to the way the question actually gets asked in practice.

Q1. Why doesn’t my simulated engine conserve energy?

Check Section 9. Energy conservation is a strict test, but only in the right conditions — you need (i) only bilateral constraints (joints, not penalty contacts), (ii) only conservative forces (gravity and springs, not dampers), and (iii) no driving torque doing work into the system. Common failure modes:

You have a damper. Any $c\,dL/dt$ term in a spring-damper, or the $C_\text{damp}\,v_\text{pen}\,\delta$ term in a penalty contact, dissipates energy on purpose. The monotonic decrease of $E$ is the correctness test in that case, not its constancy.
You have penalty contact. Even with $C_\text{damp} = 0$ , the $K_\text{hertz}$ term is conservative in theory (it’s a spring) but in practice the stiff dynamics mean integration error dominates unless tolerances are very tight. Tightening rtol/atol is the fix.
You have drift. For long simulations of index-1 DAE formulations, constraint violations at the position level can accumulate even when the acceleration-level constraint is exactly enforced. Symptom: energy slowly grows or shrinks over many periods. Fixes are Baumgarte stabilisation (feedback correction), projection (re-solve $\mathbf{C}(\mathbf{q}) = \mathbf{0}$ periodically), or just tighter ODE tolerances.
You have a driving user constraint. A user constraint like $\theta_\text{crank} - \omega t = 0$ is an external input — its Lagrange multiplier is the driving torque, and $\lambda\,\dot{\theta}$ is the power that the “motor” pumps into the system. Energy isn’t conserved; it grows at the rate the motor supplies work.

Q2. Why is the mass matrix block-diagonal?

Because we define one coordinate triplet $(x_i, y_i, \theta_i)$ per body (Section 3), and the kinetic energy of body A has no coordinate overlap with body B. Each body’s kinetic-energy contribution is $\tfrac{1}{2}\,\mathbf{v}_i^T\,\mathbf{M}_{\text{body},i}\,\mathbf{v}_i$ , involving only its own three coordinates. When you stack those into the full Lagrangian, the resulting global $\mathbf{M}(\mathbf{q})$ has one $3\times 3$ block per body and zeros everywhere else.

The physical mechanical coupling between bodies comes through the constraint Jacobian $\mathbf{C}_q$ , not through $\mathbf{M}$ . That’s exactly why the saddle-point system couples $\mathbf{M}$ and $\mathbf{C}_q^T$ — to introduce the between-body interaction that the mass matrix alone cannot express.

In a formulation that uses different variables — e.g. relative joint coordinates for a tree-structured robot — the effective mass matrix is not block-diagonal, because the dependency chain propagates inertias through the joints. We chose the simpler representation (absolute body coordinates + constraints) specifically because the block-diagonal mass matrix is cheap to assemble and the constraint mechanics is well-studied.

Q3. Is the driving torque a “force” or a “constraint”?

This one trips everyone up. In our framework it’s a constraint, and the Lagrange multiplier associated with that constraint is the torque you have to apply:

Inverse dynamics (the slider-crank chapters): we prescribe $\theta_\text{crank}(t) = \omega t$ as a user constraint ( $C_\text{user} = \theta_\text{crank} - \omega t = 0$ ). The mechanism’s saddle-point solve returns a Lagrange multiplier $\lambda_\text{user}$ for that equation, and $\tau_\text{drive} = -\lambda_\text{user}$ is the torque a motor would have to supply to produce the prescribed rotation. Given motion → find force.
Forward dynamics (where you’d actually simulate a motor, not yet built in this project): the driving torque becomes an applied force, part of $\mathbf{Q}_\text{total}$ . You’d add $Q_\text{user}[\theta_\text{crank}] = \tau_\text{motor}(t, \omega)$ and let the solver integrate $(\mathbf{q}, \mathbf{v})$ forward — $\theta_\text{crank}(t)$ emerges from the dynamics rather than being prescribed.

These two formulations are duals of each other; the same mathematical object (the $\lambda$ of a user constraint = the $Q$ of an applied torque) plays opposite roles. Inverse asks “what torque realises this motion?”; forward asks “what motion does this torque produce?”. The saddle-point system handles both — it’s the interpretation that changes.

12. Summary — the state of physics in this series

Everything in the slider-crank, multi-cylinder, boxer, and rocking-couples chapters reduces to:

Bodies with $(m,\; I_\text{cg},\; \mathbf{r}_G)$ per rigid body.
Constraints $\mathbf{C}(\mathbf{q}, t) = \mathbf{0}$ defined per joint.
Forces summed into $\mathbf{Q}_\text{total}$ .
The saddle-point system derived from Lagrangian mechanics:

Solve it. Integrate $(\mathbf{q}, \mathbf{v})$ forward, or extract $\boldsymbol{\lambda}$ directly. Everything else — slider-crank secondary imbalance, boxer cancellation, rocking couples, vibration spectra — is a consequence.

See the companion chapter The Computational Machinery for how each piece of this physics is assembled in Python, which constraint types are pre-built, and which example scripts exercise which physical features.