The Computational Machinery: 2D MBSD in Python

Top-level map: Field Guide

Theory chapter: The Physics of 2D Multibody Systems — the mathematical framework this chapter implements.

This is the other half of the physics story: how each concept from the theory chapter is actually assembled in code, which constraint and force primitives are available, which example scripts exercise which features, and where to find each piece in the repository.

This is a reference guide, not a tutorial — jump to the relevant section when you need to know “what’s available” or “where does X live”.

1. How a mechanism is represented

The core data structure is MBody in 2D-Kinematics/mbody.py, which holds five lists (plus gravity, time, and driving angular velocity):

class MBody:
    bodies                 : List[Body]
    rev_joints             : List[RevJoint]
    prism_joints           : List[PrismJoint]
    punto_linea_joints     : List[PuntoLineaJoint]
    leva_joints            : List[LevaJoint]
    gear_joints            : List[GearJoint]
    user_constraints       : List[UserConstraint]
    g                      : np.ndarray(2)          # gravity vector
    w                      : float                  # user-defined angular velocity
    t                      : float                  # current simulation time

Each Body(name, mass, inertia, rG) is a rigid body. Body 0 is always ground (mass = 0, inertia = 0) and is implicitly pinned at the origin by the first three constraint equations.

The generalised-coordinate vector $\mathbf{q}$ is packed as:

$\mathbf{q} = [\, x_0,\, y_0,\, \theta_0,\, x_1,\, y_1,\, \theta_1,\, \ldots,\, x_{n-1},\, y_{n-1},\, \theta_{n-1},\, (\text{extra coords: 2 per LevaJoint})\,]$

Total size: ncoord = 3*n_bodies + 2*n_leva_joints.

2. Available constraints (all pre-built)

Name	Class	Eqs added	Extra coords	File
Revolute (pin)	`RevJoint(i, j, ri, rj)`	2	0	`mbody.py`
Prismatic (slider)	`PrismJoint(i, j, ri, rj, hi)`	2	0	`mbody.py`
Point-on-line	`PuntoLineaJoint(i, j, ri, rj, hi)`	1	0	`mbody.py`
CAM (Leva)	`LevaJoint(i, j, geom_i, geom_j, ri, rj)`	3	2	`mbody.py`
Gear	`GearJoint(i, j, tau)`	1	0	`mbody.py`
User-defined	`UserConstraint(constraint_fn, jacobian_fn, dt_jacobian_fn, dt_constraint_fn, dtdt_constraint_fn, count)`	`count`	0	`mbody.py`

Plus three implicit equations that pin the ground body ( $x_0 = y_0 = \theta_0 = 0$ ).

Quick reference per joint

RevJoint(i, j, ri, rj) — two points ri on body i and rj on body j are forced to coincide globally. Simulates a pin.
PrismJoint(i, j, ri, rj, hi) — bodies have the same angle and the point ri stays on a line through rj oriented perpendicular to hi. Simulates a slider.
PuntoLineaJoint(i, j, ri, rj, hi) — point rj (on body j) lies on a line through ri on body i in direction perpendicular to hi. A single-equation point-on-line constraint.
LevaJoint(i, j, geom_i, geom_j, ri, rj) — surfaces geom_i and geom_j (subclasses of Surface with .position(s) and .tangent(s)) stay in tangent contact. Adds two extra coordinates $(s_i, s_j)$ for the surface parameters at the contact points.
GearJoint(i, j, tau) — enforces $\theta_i - \tau \cdot \theta_j = 0$ (a single scalar equation). Fixed transmission ratio.
UserConstraint(...) — arbitrary scalar equations; you supply five callbacks: the constraint function, its position Jacobian, its time derivative, its $\partial/\partial t$ , and its $\partial^2/\partial t^2$ . Used for driving motions ( $\theta_\text{crank} = \omega \cdot t$ ), locking a coordinate (q[8] = 0), coupling (pin-slot), etc.

Constraint assembly logic in 2D-Kinematics/constraints.py builds the full $C(\mathbf{q}, t)$ vector; Jacobians in 2D-Kinematics/jacobians.py; time derivatives in 2D-Kinematics/derivatives.py.

3. Force primitives

All force computations live in 2D-Dynamics/forces.py. The available generators:

Name	Function	Class (where applicable)	What it adds
Mass matrix	`mass_matrix(mb, q)`	—	$\mathbf{M}(\mathbf{q})$ , block-diagonal with Steiner-shifted inertia
Gravity	`gravity_forces(mb, q, g=None)`	—	$m_i \cdot g$ force on each body, plus torque from offset CG
Spring-damper	`spring_damper_forces(mb, q, v, springs)`	`Spring(i, j, k, c, l0, ri, rj)`	linear spring between two body-fixed points, optional damping
Ground contact (simple)	`ground_contact_forces(mb, q, v, contacts)`	`GroundContact(body, contact_radius, y_ground, K_hertz, C_damp, mu, v_lim, y_ground_fn)`	Hertz penalty contact between a circular wheel and a $y(x)$ terrain
Surface-to-surface contact	`surface_contact_forces(mb, q, v, surface_contacts)`	`SurfaceContact(body_i, body_j, geom_i, geom_j, K_hertz, C_damp, mu, v_lim, ...)`	Full surface-to-surface penalty contact with nonlinear tangent-contact point finding (matches MATLAB `MaxDistancia_2D`)
Centrifugal (pseudo)	`centrifugal_forces(mb, q, omega, alpha)`	—	Pseudo-forces in a rotating reference frame (rarely used)
User forces	`Q_user_fn(t, q, v) -> ndarray`	—	Arbitrary applied generalised forces passed as a callback

Energy helpers (also in forces.py):

kinetic_energy(M, v)
potential_energy(mb, q, g=None, springs=None)
total_energy(mb, q, v, M, g=None, springs=None)

4. The saddle-point solver

The heart of the dynamics: solve_acceleration_with_lagrange(mb, q, v, t, M, Q_total) in 2D-Kinematics/solver.py.

Given $\mathbf{M}$ , $\mathbf{Q}_\text{total}$ , the current $(\mathbf{q}, \mathbf{v})$ , and implicitly the constraint Jacobian $\mathbf{C}_q$ and time derivatives, it assembles the block system

\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} \\ -(\dot{\mathbf{C}}_q \cdot \mathbf{v} + C_{tt}) \end{bmatrix}

and calls np.linalg.solve. Returns $(\mathbf{a}, \boldsymbol{\lambda})$ .

This is called from every time step of forward or inverse dynamics.

4.1 Position / velocity / acceleration kinematic solvers

When the mechanism is fully constrained ( $n_\text{DOF} = 0$ ), the same solver.py module provides:

newton_raphson(mb, q0, t, tol=1e-10, max_iter=50) — Newton–Raphson on $C(\mathbf{q}, t) = 0$ starting from q0, returns $\mathbf{q}$ satisfying $\|C\| < \text{tol}$ .
solve_position(mb, q0, t) — wrapper around Newton–Raphson.
solve_velocity(mb, q, t) — one linear solve $\mathbf{C}_q \cdot \mathbf{v} = -C_t$ .
solve_acceleration(mb, q, v, t) — one linear solve $\mathbf{C}_q \cdot \mathbf{a} = \boldsymbol{\gamma}$ , purely kinematic (no mass matrix, no forces).

For underconstrained systems (free DOFs), all three solvers fall back to lstsq for the minimum-norm solution.

5. Dynamics entry points

All in 2D-Dynamics/dynamics_solver.py.

Forward dynamics — `solve_dynamics_scipy(...)`

State is $\mathbf{y} = [\mathbf{q}, \mathbf{v}]$ of size $2 \cdot \text{ncoord}$ . The function wraps scipy.integrate.solve_ivp with an RHS that:

Optionally projects $\mathbf{q}$ back onto the constraint manifold via solve_position.
Assembles $\mathbf{Q}_\text{total}$ via assemble_system().
Solves the saddle-point system for $(\mathbf{a}, \boldsymbol{\lambda})$ .
Returns $d\mathbf{y}/dt = [\mathbf{v}, \mathbf{a}]$ to scipy.

Tight default tolerances (rtol=1e-9, atol=1e-11) to get crisp energy-conservation properties on rigid-body systems. For penalty-contact problems (stiff ODE), looser tolerances are used and projection can be enabled for LevaJoints.

Inverse dynamics — `solve_inverse_dynamics(...)`

Requires $n_\text{DOF} = 0$ (user constraints drive all remaining freedoms). At each time in tspan:

Position solve (Newton–Raphson).
Velocity solve (one linear solve).
Saddle-point solve → returns both $\mathbf{a}$ and $\boldsymbol{\lambda}$ .

No time integration. Returns arrays q_all, v_all, a_all, lambda_all, Q_total_all over the time grid. The Lagrange multiplier time histories $\boldsymbol{\lambda}(t)$ are the “bearing reactions / driving torque / guide normals” we extract in every inverse-dynamics chapter.

6. Example scripts — by category

All live in 2D-Dynamics/examples/ (or 2D-mbsd/ for the kinematics-only examples).

Kinematic-only (driven at constant angular velocity)

slider_crank.py — the canonical reciprocating mechanism
cam_follower_angular.py — first use of the LevaJoint contact
four_bar.py + variants — classic 4-bar linkage
geneva_drive.py — intermittent-motion cam
pantograph.py — parallelogram linkage
scotch_yoke.py — sinusoidal reciprocating motion

Forward dynamics

dynamic_pendulum.py — Phase-1 validation; machine-precision energy conservation
dynamic_double_pendulum.py — 2-DOF coupled oscillation
dynamic_mass_spring_damper.py — canonical damped oscillator; analytical match to machine precision
dynamic_slider_crank_gravity.py — gravity-driven, undriven slider-crank
dynamic_terrain_wheel.py — simplified wheel-on-ground with elastic (penalty) contact
dynamic_terrain_wheel_rolling.py — circular wheel rolling on wavy terrain with friction
dynamic_terrain_wheel_polygonal.py — polygonal wheel + full surface-surface contact (MATLAB fidelity)
dynamic_cam_follower.py — CAM-constraint dynamics

Inverse dynamics and vibration analysis

inverse_slider_crank.py — saddle-point solve every step; driving torque + bearing reactions over one crank cycle
vibration_slider_crank.py — FFT of inverse-dynamics signals; primary + secondary imbalance
vibration_slider_crank_no_gravity.py — gravity-off comparison reveals which harmonics are gravity-driven vs inertia-driven

Multi-cylinder engine analysis

In 2D-Dynamics/examples/multi-cylinder-nograv/:

_common.py — shared helpers: run_single_cylinder, superpose_time, phasor_sum, phasor_moment_sum, fft_amplitude, harmonic_bin
i4_analysis.py — inline-4 phasor analysis
boxer4_analysis.py — flat/boxer-4 with sign-flipped opposed pairs
rocking_couples.py — moment sums across I4/boxer/straight-3 layouts

7. Physics concept → code map

Physics concept	Where it lives in code
Generalised coordinates $\mathbf{q}$	Packed vector in `MBody` (ordering in `constraints.py`)
Body mass matrix	`mass_matrix()` in `forces.py`
Constraint equations $C(\mathbf{q},t)$	`constraints(mb, q, t)` in `constraints.py`
Constraint Jacobian $\mathbf{C}_q$	`jacobian(mb, q, t)` in `jacobians.py`
$d\mathbf{C}_q/dt$	`dt_jacobian(mb, q, v, t)` in `derivatives.py`
$\partial^2 C/\partial t^2$	`dtdt_constraints(mb, q, v, t)` in `derivatives.py`
Gravity force	`gravity_forces()`
Spring / damper	`spring_damper_forces()` + `Spring` class
Penalty contact (ground)	`ground_contact_forces()` + `GroundContact`
Penalty contact (surface-surface)	`surface_contact_forces()` + `SurfaceContact`
$\mathbf{Q}_\text{total}$ assembly	`assemble_system()` in `dynamics_solver.py`
Saddle-point solve	`solve_acceleration_with_lagrange()` in `solver.py`
Forward dynamics	`solve_dynamics_scipy()`
Inverse dynamics	`solve_inverse_dynamics()`
Position solve	`newton_raphson()` / `solve_position()`
Velocity solve	`solve_velocity()`
Kinetic energy	`kinetic_energy(M, v)`
Potential energy	`potential_energy(mb, q, g=None, springs=None)`
Phasor multi-cylinder superposition	`phasor_sum()` + `superpose_time()` in `multi-cylinder-nograv/_common.py`
Rocking-couple moment sum	`phasor_moment_sum()` same file

8. Test coverage

All live in 2D-Test/.

test_kinematics.py — position/velocity/acceleration solvers
test_constraints.py — each joint type (revolute, prismatic, point-on-line, CAM, gear, user) gets its own integration
test_physical_behavior.py — end-to-end correctness on known mechanisms
test_dynamics.py — 13 tests covering mass matrix properties, gravity, damped oscillator, ground contact, energy conservation, pendulum period, etc.

All 4 modules + 13 dynamics tests green as of the latest run.

9. FAQ — implementation gotchas

Complements the conceptual FAQ in the physics chapter. These are implementation-level questions that come up when reading or extending the code.

Q: Why do I need the γ (gamma) term in the saddle-point RHS (§4)?

The $\boldsymbol{\gamma} = -(\dot{\mathbf{C}}_q \cdot \mathbf{v} + \partial^2 C/\partial t^2)$ term captures the inertia of the constraints themselves — the bookkeeping that comes from having differentiated $C(\mathbf{q}, t) = 0$ twice in time. Without it the acceleration solve would only be valid for a frozen instant; the moment any body moves, the Jacobian $\mathbf{C}_q$ changes, and the constraint equation that the solver is actually enforcing ( $\mathbf{C}_q \cdot \mathbf{a} = \boldsymbol{\gamma}$ ) would no longer reduce $C(\mathbf{q}, t)$ to zero.

Two distinct contributions live inside $\boldsymbol{\gamma}$ , with different physical meanings:

$\dot{\mathbf{C}}_q \cdot \mathbf{v}$ — the “convective” term. $\mathbf{C}_q$ is a function of the configuration $\mathbf{q}$ , so as the bodies move it evolves: new orientation angles rotate body-fixed vectors into new world directions, and so on. This term captures the fact that “which direction the constraint pushes” changes with position. Shows up in every joint whose Jacobian isn’t literally constant — which is basically every joint with a rotation in it. Computed analytically per joint in 2D-Kinematics/derivatives.py::dt_jacobian().
$\partial^2 C/\partial t^2$ — the “explicit time” term. Non-zero only for user-supplied constraints that depend explicitly on $t$ (driving motions like $\theta_\text{crank} - \omega \cdot t = 0$ ). For every joint constraint in constraints.py this is zero because joints don’t move on their own; only user constraints inject time-dependence. Computed per user constraint in the user-supplied dtdt_constraint_fn callback, plus the joint-internal calls in derivatives.py::dtdt_constraints().

Concrete failure mode if you forgot $\boldsymbol{\gamma}$ : run any simulation with moving revolute joints and the system will immediately violate its own constraints. A pin that should connect two bodies will start letting them drift apart at a rate proportional to $\dot{\mathbf{C}}_q \cdot \mathbf{v}$ , because the integrator is being told “just keep the Jacobian-projected acceleration zero” rather than “keep the actual constraint satisfied”. Energy-conservation tests would fail almost immediately. This is why the solver always calls dt_jacobian and dtdt_constraints before assembling the saddle-point RHS.

Q: Why does `solve_dynamics_scipy()` keep $\boldsymbol{\lambda}$ hidden by default, but `solve_inverse_dynamics()` returns it?

solve_dynamics_scipy()’s primary output is the trajectory $(\mathbf{q}, \mathbf{v})$ over time — the motion. The Lagrange multipliers $\boldsymbol{\lambda}(t)$ exist at every RHS evaluation (they’re computed inside solve_acceleration_with_lagrange) but scipy’s ODE interface only cares about the state derivatives, so $\boldsymbol{\lambda}$ is discarded after each call. If you want the bearing reactions from a forward-dynamics run, you have to post-process: re-run solve_acceleration_with_lagrange at each saved $(\mathbf{q}(t), \mathbf{v}(t))$ pair and collect $\boldsymbol{\lambda}(t)$ manually.

solve_inverse_dynamics(), by contrast, is designed around $\boldsymbol{\lambda}$ — the whole point is to compute constraint reaction forces for a given prescribed motion. So it returns lambda_all as a first-class output.

Q: My LevaJoint dynamics simulation runs painfully slowly. Why?

The LevaJoint constraint (surface-to-surface cam contact) isn’t expressed in closed form — the two extra surface-parameter coordinates $(s_i, s_j)$ per joint need to be found by a nonlinear optimisation at every position-solve call. That’s scipy.optimize.fsolve inside _find_closest_surface_params() (for force-based SurfaceContact) or inside the leva constraint function (for bilateral LevaJoint). Each RHS evaluation in scipy.integrate.solve_ivp therefore runs a small nonlinear solve per joint. Typical slowdown: ~10× compared to a mechanism with only revolute/prismatic joints.

Workarounds are documented in the Phase-3 chapters of DYNAMICS_VALIDATION_RESULTS.md; the short version is “use looser tolerances and fewer output points unless you really need the bilateral-contact fidelity”.

Q: What should I do if my simulation errors with `LinAlgError: Singular matrix`?

Three usual suspects, in order of likelihood:

Over-constrained system. You’ve added a constraint that is redundant with or contradicts an existing one. The constraint Jacobian then has rank less than n_restr, so the block matrix in the saddle-point system is singular. Fix: check that your n_restr and n_coord make sense (see nrestr / ncoord properties of MBody) and that you haven’t accidentally declared the same joint twice.
Degenerate configuration. The constraint Jacobian is full-rank generically but drops rank at specific configurations (e.g. a four-bar locked at a dead-centre). Symptom: the simulation works almost everywhere but fails at particular crank angles. Fix: perturb the initial condition, or add a small damping/Baumgarte term to regularise.
Initial condition off the constraint manifold. If $\mathbf{q}_0$ does not satisfy $C(\mathbf{q}_0, 0) = 0$ and position projection is disabled, the first linear solve may encounter a badly-conditioned system. Fix: run solve_position(mb, q0_guess, 0) to project onto the manifold before handing it to the dynamics.

Q: Does the simulator handle “pure rolling without slip” as a hard constraint, or only as a friction-penalty approximation?

Penalty-friction only, by design. Rolling without slip is mathematically a non-holonomic constraint — $v_\text{wheel,x} + R \cdot \omega_\text{wheel} = 0$ at the velocity level, not derivable as the time-derivative of any position equation. Our simulator has no non-holonomic constraint machinery: all joint classes in mbody.py, the assembly in constraints.py, and the saddle-point solver itself are built around holonomic $C(\mathbf{q}, t) = 0$ expressions and their time derivatives.

What we do have is the regularised Coulomb friction $F_t = -\mu \cdot F_n \cdot \tanh(v_\text{slip} / v_\text{lim})$ attached to GroundContact and SurfaceContact. This is a force, not a constraint — it lives in $\mathbf{Q}_\text{total}$ . When $\mu \cdot F_n$ is large enough to overcome whatever would cause slip, the friction term drives $v_\text{slip} \to 0$ and “rolling” emerges naturally from the dynamics. In the terrain-wheel example, slip settles to ~22 mm/s (0.4% of forward velocity) after a ~15 ms spin-up transient — numerically close to pure rolling, but not mathematically exact.

Why the penalty approach is actually the right choice here, not just a shortcut:

Scenario	Hard rolling constraint	Penalty friction (ours)
Tyre on dry road, well-bonded	Exact	Excellent approximation (slip ≈ 0)
Tyre on ice / wet road	Must switch to friction model manually	Handles seamlessly — $\mu \cdot F_n$ just gets smaller, slip grows
Burnout / wheelspin under high torque	Constraint is violated → simulation fails or needs a complementarity solver	Friction saturates at $\mu \cdot F_n$ and the wheel correctly spins faster than $-v_x/R$
Tyre slowly sliding down a slope	Impossible — the constraint forbids it	Natural; static friction holds until tangential force exceeds $\mu \cdot F_n$ , then slides
Transition between rolling and sliding	Hard switch; usually needs LCP (linear complementarity) machinery	Smooth — controlled by the $\tanh$ regularisation

Real tyres do slip, and modern tyre models (Pacejka “Magic Formula”, brush model, etc.) are all force-based — they compute friction as a function of slip ratio with a more elaborate saturation curve than $\tanh$ . Our approach is the simplest member of that family; swapping $\tanh(v_\text{slip}/v_\text{lim})$ for a Magic-Formula curve would be a drop-in replacement with the rest of the framework unchanged.

When you’d actually need hard rolling. Three genuine use cases:

Robot-kinematics control synthesis. Designing a path-follower for a differential-drive robot, where its dynamics are defined to roll without slip. You want exactness so the inverse-kinematics solve doesn’t have a residual slip to compensate for.
Classical textbook problems. A disc rolling down an incline with $v = R \cdot \omega$ imposed exactly. You want the analytical answer, not a simulated-with-regularisation answer.
Tight-tolerance kinematic-accuracy applications. Gear meshes genuinely should never slip — mechanically interlocked. We already handle this case with the holonomic GearJoint ( $\theta_i - \tau \cdot \theta_j = 0$ as a position-level equation), which is the right formulation because gear teeth physically can’t slip (up to tooth breakage).

For vehicles, engines, walking mechanisms, anything where real physics includes the possibility of slip, the penalty approach is both simpler to implement and more faithful to reality.

What it would take to add hard non-holonomic constraints. If the need arises:

A new constraint class in mbody.py analogous to UserConstraint, but expressing a velocity-level equation of the form $D(\mathbf{q}, t) \cdot \mathbf{v} = 0$ (takes $\mathbf{q}, \mathbf{v}, t$ , returns a scalar).
A second Jacobian-assembly pass that builds the velocity-level Jacobian matrix $D(\mathbf{q}, t)$ — separate from the holonomic-constraint Jacobian $\mathbf{C}_q$ .
Extend the saddle-point block by adding one more row-column pair per non-holonomic constraint, with its own Lagrange multipliers $\boldsymbol{\mu}_\text{nh}$ alongside the existing $\boldsymbol{\lambda}$ . Structurally the block system generalises cleanly; you just grow both axes.
Integration is unchanged — it’s still a DAE, just with an extra algebraic-equation type.

Probably 100 lines of code, spread across mbody.py, constraints.py (or a new nh_constraints.py), and the saddle-point assembly in solver.py. It hasn’t been added because no current use case needs it — but the extension is well-defined if it ever does.

10. Where to go next

The Physics of 2D Multibody Systems — companion theoretical chapter.
Concepts Primer — signal-processing and DAE vocabulary used throughout.
The Slider-Crank, Three Ways — first ebook chapter that exercises the whole pipeline.
Multi-Cylinder I4, Boxer-4, Rocking Couples — progressively advanced engine-analysis chapters reusing the same computational machinery.
2D-mbsd/matab-vs-py.md — migration audit from the original MATLAB Simulon codebase: what’s ported, what’s out of scope, what’s genuinely not carried over.