Closed-chain robots and robots with contact or loop-closure constraints are naturally described by \( n \) generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \) and \( m \) holonomic constraints \( \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0} \). In previous lessons, we wrote the constrained dynamics in Lagrange-multiplier form:

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \mathbf{u} + \mathbf{J}_\phi(\mathbf{q})^\top \boldsymbol{\lambda}, \quad \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0}, \]

where \( \mathbf{M}(\mathbf{q}) \) is the inertia matrix, \( \mathbf{h} \) collects Coriolis, centrifugal and gravity terms, \( \mathbf{u} \) are generalized inputs, \( \mathbf{J}_\phi(\mathbf{q}) = \partial \boldsymbol{\phi}/\partial \mathbf{q} \) is the constraint Jacobian, and \( \boldsymbol{\lambda} \) are Lagrange multipliers enforcing the constraints.

In exact arithmetic, the solution stays on the constraint manifold \( \boldsymbol{\phi}(\mathbf{q}(t)) = \mathbf{0} \) for all time. In a numerical integrator, rounding and discretization cause constraint drift:

\[ \boldsymbol{\phi}(\mathbf{q}(t_k)) \neq \mathbf{0}, \quad \mathbf{J}_\phi(\mathbf{q}(t_k)) \dot{\mathbf{q}}(t_k) \neq \mathbf{0}. \]

Constraint drift leads to non-physical motion (loop length changes, contact penetration, etc.). Baumgarte stabilization modifies the differential side of the constraints to drive violations back to zero in a controlled way while still using standard ODE integrators.

For a holonomic constraint \( \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0} \), the velocity and acceleration consistency conditions are obtained by differentiation.

Velocity level. Differentiate once with respect to time:

\[ \frac{\mathrm{d}}{\mathrm{d}t} \boldsymbol{\phi}(\mathbf{q}(t)) = \mathbf{J}_\phi(\mathbf{q}) \dot{\mathbf{q}} = \mathbf{0}, \]

where \( \mathbf{J}_\phi(\mathbf{q}) \in \mathbb{R}^{m \times n} \) is the constraint Jacobian.

Acceleration level. Differentiate once more:

\[ \frac{\mathrm{d}}{\mathrm{d}t}\big( \mathbf{J}_\phi(\mathbf{q}) \dot{\mathbf{q}} \big) = \mathbf{J}_\phi(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}}) \dot{\mathbf{q}} = \mathbf{0}, \]

where \( \dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}}) \) is the material time derivative of the Jacobian along the motion.

In practice, one solves for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \) at each instant from the coupled system

\[ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & -\mathbf{J}_\phi(\mathbf{q})^\top \\ \mathbf{J}_\phi(\mathbf{q}) & \mathbf{0} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\ \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{S}^\top \mathbf{u} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \\ -\dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \end{bmatrix}. \]

Numerically integrating this system should, in theory, maintain \( \boldsymbol{\phi} = \mathbf{0} \), but discretization errors accumulate, especially for long time horizons or stiff dynamics. Baumgarte's idea is to shape the constraint violation dynamics directly.

Let us define the constraint error: \( \mathbf{e}(t) := \boldsymbol{\phi}(\mathbf{q}(t)) \). Ideally, \( \mathbf{e}(t) \equiv \mathbf{0} \). For a numerical solution with drift, we want to enforce a stable error dynamics of the form

\[ \ddot{\mathbf{e}} + 2\alpha \dot{\mathbf{e}} + \beta^2 \mathbf{e} = \mathbf{0}, \quad \alpha > 0, \; \beta > 0, \]

which is a second-order linear system with exponentially decaying solutions if all eigenvalues lie in the open left half-plane.

We express \( \dot{\mathbf{e}} \) and \( \ddot{\mathbf{e}} \) in terms of \( \mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}} \):

\[ \dot{\mathbf{e}} = \mathbf{J}_\phi(\mathbf{q}) \dot{\mathbf{q}}, \quad \ddot{\mathbf{e}} = \mathbf{J}_\phi(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}. \]

Substituting into the desired error dynamics yields

\[ \mathbf{J}_\phi(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + 2\alpha \mathbf{J}_\phi(\mathbf{q}) \dot{\mathbf{q}} + \beta^2 \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0}. \]

This is the Baumgarte-stabilized acceleration-level constraint. Comparing to the original (unstabilized) acceleration condition \( \mathbf{J}_\phi \ddot{\mathbf{q}} + \dot{\mathbf{J}}_\phi \dot{\mathbf{q}} = \mathbf{0} \), the extra terms \( 2\alpha \mathbf{J}_\phi \dot{\mathbf{q}} + \beta^2 \boldsymbol{\phi} \) act as proportional-derivative feedback on the constraint violation and its rate.

The constrained dynamics with Baumgarte stabilization becomes

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \mathbf{u} + \mathbf{J}_\phi(\mathbf{q})^\top \boldsymbol{\lambda}, \]

\[ \mathbf{J}_\phi(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + 2\alpha \mathbf{J}_\phi(\mathbf{q}) \dot{\mathbf{q}} + \beta^2 \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0}. \]

Solving for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \) now enforces both the dynamics and the stabilized constraint.

For a single scalar constraint \( \phi(q) \), linearized around a consistent trajectory, the error approximately satisfies

\[ \ddot{e} + 2\alpha \dot{e} + \beta^2 e \approx 0, \]

which has characteristic polynomial \( s^2 + 2\alpha s + \beta^2 = 0 \) and eigenvalues \( s = -\alpha \pm \sqrt{\alpha^2 - \beta^2} \). For \( \alpha > 0 \), \( \beta > 0 \), the real part of \( s \) is always negative, so small constraint violations decay exponentially (possibly with oscillations).

Combining the dynamic and stabilized constraint equations, we obtain the linear system for unknowns \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \):

\[ \underbrace{ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & -\mathbf{J}_\phi(\mathbf{q})^\top \\ \mathbf{J}_\phi(\mathbf{q}) & \mathbf{0} \end{bmatrix}}_{\mathbf{K}(\mathbf{q})} \begin{bmatrix} \ddot{\mathbf{q}} \\ \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{S}^\top \mathbf{u} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \\ -\dot{\mathbf{J}}_\phi(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - 2\alpha \mathbf{J}_\phi(\mathbf{q}) \dot{\mathbf{q}} - \beta^2 \boldsymbol{\phi}(\mathbf{q}) \end{bmatrix}. \]

At each integration step, given \( \mathbf{q}, \dot{\mathbf{q}} \), we assemble \( \mathbf{K}(\mathbf{q}) \), compute the right-hand side, solve the linear system, and then integrate \( \ddot{\mathbf{q}} \) to update velocities and positions.

Parameter guidelines.

• Choose \( \alpha \) and \( \beta \) such that the characteristic polynomial has a desired damping ratio \( \zeta \) and natural frequency \( \omega_n \), by identifying \( 2\alpha = 2\zeta \omega_n \) and \( \beta^2 = \omega_n^2 \).
• Larger \( \omega_n \) (thus larger \( \beta \)) yields faster convergence but increases stiffness of the ODE, requiring smaller integration steps.
• Too small \( \alpha \) leads to oscillatory constraint errors; too large makes the system numerically stiff.
• In coupled multi-constraint systems, the global stability picture is more involved, but the scalar analysis gives intuition for tuning.

The following diagram summarizes a single integration step using Baumgarte stabilization for a robot with holonomic constraints.

flowchart TD
  S["Given q, qdot at time t"] --> C1["Compute phi(q), J_phi(q), Jdot_phi(q, qdot)"]
  C1 --> D1["Assemble M(q), h(q, qdot), S, u(t)"]
  D1 --> SYS["Form linear system K * [qddot; lambda] = rhs"]
  SYS --> SOLVE["Solve for qddot and lambda"]
  SOLVE --> INT["Integrate qdot, q with ODE step"]
  INT --> CHECK["Check magnitude of phi(q_new)"]
  CHECK -->|small| NEXT["Proceed to next time step"]
  CHECK -->|large| TUNE["Adjust alpha, beta or step size"]
        

This structure is common to most rigid-body simulation engines that support constrained dynamics with stabilization techniques.

As a minimal example, consider a point mass of mass \( m \) in the plane with coordinates \( \mathbf{q} = (x,y)^\top \) constrained to move on a circle of radius \( L \) around the origin:

\[ \phi(\mathbf{q}) = x^2 + y^2 - L^2 = 0. \]

We model gravity via \( \mathbf{h} = (0,\, mg)^\top \) and no control inputs. The matrices are \( \mathbf{M} = m \mathbf{I}_2 \), \( \mathbf{J}_\phi = [\,2x \;\; 2y\,] \).

import numpy as np
from scipy.integrate import solve_ivp

m = 1.0
g = 9.81
L = 1.0
alpha = 5.0   # Baumgarte parameter
beta  = 10.0  # Baumgarte parameter

def dynamics(t, y):
    # State y = [x, y, vx, vy]
    q = y[0:2]
    qdot = y[2:4]

    x, y_pos = q
    vx, vy = qdot

    # Inertia and bias
    M = m * np.eye(2)
    h = np.array([0.0, m * g])

    # Constraint phi(q) = x^2 + y^2 - L^2
    phi = x**2 + y_pos**2 - L**2

    # J_phi and Jdot_phi
    J = np.array([[2.0 * x, 2.0 * y_pos]])   # 1 x 2
    Jdot = np.array([[2.0 * vx, 2.0 * vy]])  # 1 x 2

    # Assemble linear system for [qddot; lambda]
    # K is 3 x 3: [ M   -J^T ]
    #             [ J    0   ]
    K = np.zeros((3, 3))
    K[0:2, 0:2] = M
    K[0:2, 2:3] = -J.T
    K[2:3, 0:2] = J
    # K[2,2] = 0 already

    # Right-hand side
    rhs_dyn = -h
    rhs_con = - (Jdot @ qdot
                 + 2.0 * alpha * (J @ qdot)
                 + beta**2 * phi)
    rhs = np.concatenate((rhs_dyn, rhs_con))

    # Solve for qddot and lambda
    sol = np.linalg.solve(K, rhs)
    qddot = sol[0:2]
    # lam = sol[2]

    return np.concatenate((qdot, qddot))

# Example initial condition slightly off the circle
y0 = np.array([L * 1.01, 0.0, 0.0, 0.0])
t_span = (0.0, 5.0)
t_eval = np.linspace(t_span[0], t_span[1], 1001)

sol = solve_ivp(dynamics, t_span, y0, t_eval=t_eval, rtol=1e-6, atol=1e-9)

# Compute constraint violation over time
r2 = sol.y[0, :]**2 + sol.y[1, :]**2
phi_vals = r2 - L**2
print("Max |phi|:", np.max(np.abs(phi_vals)))
      

By adjusting \( \alpha \) and \( \beta \), one can observe how the constraint violation \( \phi(\mathbf{q}(t)) \) is driven to zero and how the numerical stiffness changes.

In C++, libraries such as Eigen for linear algebra and dynamics libraries like RBDL or Pinocchio can be used to assemble \( \mathbf{M}, \mathbf{h}, \mathbf{J}_\phi \). Below is a minimal sketch corresponding to the planar constrained mass example.

#include <iostream>
#include <Eigen/Dense>

struct State {
    double x;
    double y;
    double vx;
    double vy;
};

struct Params {
    double m;
    double g;
    double L;
    double alpha;
    double beta;
};

Eigen::Vector4d baumgarteRHS(const State& s, const Params& p) {
    double x  = s.x;
    double y  = s.y;
    double vx = s.vx;
    double vy = s.vy;

    // Inertia
    Eigen::Matrix2d M = p.m * Eigen::Matrix2d::Identity();
    Eigen::Vector2d h;
    h << 0.0, p.m * p.g;

    // Constraint
    double phi = x * x + y * y - p.L * p.L;

    Eigen::RowVector2d J;
    J << 2.0 * x, 2.0 * y;

    Eigen::RowVector2d Jdot;
    Jdot << 2.0 * vx, 2.0 * vy;

    Eigen::Vector2d qdot;
    qdot << vx, vy;

    // Build K (3x3)
    Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
    K.block<2,2>(0,0) = M;
    K.block<2,1>(0,2) = -J.transpose();
    K.block<1,2>(2,0) = J;

    // Right-hand side
    Eigen::Vector3d rhs;
    rhs.segment<2>(0) = -h;
    double rhs_con = -(Jdot * qdot
                       + 2.0 * p.alpha * (J * qdot)
                       + p.beta * p.beta * phi);
    rhs(2) = rhs_con;

    // Solve K * [qddot; lambda] = rhs
    Eigen::Vector3d sol = K.fullPivLu().solve(rhs);
    Eigen::Vector2d qddot = sol.segment<2>(0);

    Eigen::Vector4d ydot;
    ydot << vx, vy, qddot(0), qddot(1);
    return ydot;
}

// Integrate baumgarteRHS with your preferred ODE integrator (e.g., RK4).
      

In a full robot implementation, M, h, and constraint Jacobians would be computed via a multi-body dynamics library; the Baumgarte terms are then added in the constraint right-hand side exactly as shown.

In Java, one can use EJML for dense linear algebra. Below is a sketch of the Baumgarte step for the same planar mass example.

import org.ejml.simple.SimpleMatrix;

public class BaumgarteMass2D {

    static class Params {
        double m = 1.0;
        double g = 9.81;
        double L = 1.0;
        double alpha = 5.0;
        double beta = 10.0;
    }

    static class State {
        double x, y, vx, vy;
    }

    public static double[] deriv(double t, double[] y, Params p) {
        double x  = y[0];
        double yPos = y[1];
        double vx = y[2];
        double vy = y[3];

        SimpleMatrix M = SimpleMatrix.identity(2).scale(p.m);
        SimpleMatrix h = new SimpleMatrix(2, 1, true, new double[]{0.0, p.m * p.g});

        double phi = x * x + yPos * yPos - p.L * p.L;

        SimpleMatrix J = new SimpleMatrix(1, 2, true, new double[]{2.0 * x, 2.0 * yPos});
        SimpleMatrix Jdot = new SimpleMatrix(1, 2, true, new double[]{2.0 * vx, 2.0 * vy});
        SimpleMatrix qdot = new SimpleMatrix(2, 1, true, new double[]{vx, vy});

        // Build K (3x3)
        SimpleMatrix K = new SimpleMatrix(3, 3);
        K.insertIntoThis(0, 0, M);
        K.insertIntoThis(0, 2, J.transpose().scale(-1.0));
        K.insertIntoThis(2, 0, J);

        // RHS
        SimpleMatrix rhs = new SimpleMatrix(3, 1);
        rhs.insertIntoThis(0, 0, h.scale(-1.0));
        double rhsCon = -(Jdot.mult(qdot).get(0, 0)
                          + 2.0 * p.alpha * J.mult(qdot).get(0, 0)
                          + p.beta * p.beta * phi);
        rhs.set(2, 0, rhsCon);

        // Solve
        SimpleMatrix sol = K.solve(rhs);
        double ax = sol.get(0, 0);
        double ay = sol.get(1, 0);

        return new double[]{vx, vy, ax, ay};
    }

    // Use any ODE integrator (e.g., fixed-step RK4) calling deriv(t, y, p).
}
      

For more complex robots, EJML can be combined with symbolic or code-generated expressions for \( \mathbf{M}, \mathbf{h}, \mathbf{J}_\phi \), or integrated with Java-based robotics frameworks that expose these quantities.

MATLAB, together with the Robotics System Toolbox and Simscape Multibody, is well suited to implement Baumgarte stabilization. For custom dynamics, we can write an ODE function with stabilized constraints and integrate with ode45 or ode15s.

function dy = baumgarte_mass2d(t, y, params)
    % y = [x; y; vx; vy]
    x   = y(1);
    y_p = y(2);
    vx  = y(3);
    vy  = y(4);

    m     = params.m;
    g     = params.g;
    L     = params.L;
    alpha = params.alpha;
    beta  = params.beta;

    M = m * eye(2);
    h = [0.0; m * g];

    phi = x^2 + y_p^2 - L^2;

    J    = [2 * x, 2 * y_p];      % 1x2
    Jdot = [2 * vx, 2 * vy];      % 1x2
    qdot = [vx; vy];

    % Build K and rhs
    K = zeros(3,3);
    K(1:2,1:2) = M;
    K(1:2,3)   = -J.';
    K(3,1:2)   = J;

    rhs_dyn = -h;
    rhs_con = - (Jdot * qdot ...
                 + 2 * alpha * (J * qdot) ...
                 + beta^2 * phi);
    rhs = [rhs_dyn; rhs_con];

    sol = K \ rhs;
    qddot = sol(1:2);

    dy = [vx; vy; qddot];
end

% Example usage:
params.m = 1.0;
params.g = 9.81;
params.L = 1.0;
params.alpha = 5.0;
params.beta  = 10.0;

y0 = [params.L * 1.01; 0; 0; 0];
[t, Y] = ode45(@(t,y) baumgarte_mass2d(t,y,params), [0 5], y0);

phi_vals = Y(:,1).^2 + Y(:,2).^2 - params.L^2;
max_phi = max(abs(phi_vals))
      

In Simulink, one typically implements the Baumgarte-stabilized constraints using a MATLAB Function block that outputs the accelerations \( \ddot{\mathbf{q}} \) given \( \mathbf{q}, \dot{\mathbf{q}} \), then integrates them using standard Integrator blocks. Simscape Multibody offers built-in constraint stabilization mechanisms that are conceptually similar.

Mathematica is particularly convenient for symbolic derivation of constrained dynamics, followed by numerical integration with NDSolve. For the planar constrained mass:

m  = 1.0;
g  = 9.81;
L  = 1.0;
alpha = 5.0;
beta  = 10.0;

x  = x[t];
y  = y[t];

vx = D[x, t];
vy = D[y, t];

phi = x^2 + y^2 - L^2;
J   = {2 x, 2 y};

(* qddot and lambda as unknown functions *)
ax = D[x, {t,2}];
ay = D[y, {t,2}];
lambda = lambda[t];

eqDynX = m ax == - 2 x lambda;
eqDynY = m ay == m g - 2 y lambda;

(* Baumgarte stabilized constraint *)
phiDot  = D[phi, t];
phiDDot = D[phiDot, t];

eqCon = phiDDot + 2 alpha phiDot + beta^2 phi == 0;

(* Solve for ax, ay, lambda in terms of x, y, vx, vy *)
sol = Solve[{eqDynX, eqDynY, eqCon}, {ax, ay, lambda}];

(* Substitute back and set up system for NDSolve *)
sys = {D[x, {t,2}] == ax /. sol[[1]],
       D[y, {t,2}] == ay /. sol[[1]]};

ic = {x[0] == 1.01 L, y[0] == 0,
      x'[0] == 0,      y'[0] == 0};

solN = NDSolve[Join[sys, ic], {x, y}, {t, 0, 5}];

phiVals   = (x[t]^2 + y[t]^2 - L^2) /. solN[[1]];
MaxConstraintViolation = MaxValue[{Abs[phiVals], 0 <= t <= 5}, t]
      

For multi-DOF robots, Mathematica can be used to symbolically compute \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \), and \( \boldsymbol{\phi}(\mathbf{q}) \), and then automatically generate efficient numerical code with Baumgarte stabilization.

In this lesson we formulated Baumgarte stabilization for holonomic constraints in robotic dynamics. Starting from Lagrange-multiplier DAEs, we introduced stabilized acceleration-level constraints that yield a second-order error dynamics \( \ddot{e} + 2\alpha \dot{e} + \beta^2 e = 0 \). We expressed the resulting augmented linear system for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \), discussed stability and parameter choices, and illustrated an algorithmic simulation loop. Implementation sketches in Python, C++, Java, MATLAB/Simulink, and Mathematica demonstrated how Baumgarte stabilization can be integrated into practical simulation code for constrained robotic systems.

The next lesson will build on this by studying numerical simulation strategies for constrained dynamics more broadly and comparing Baumgarte stabilization to alternative approaches such as projection methods and index-reduction techniques.