Recall from previous lessons that an unconstrained rigid manipulator with generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \) has dynamics

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]

where \( \mathbf{M}(\mathbf{q}) \) is the inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects Coriolis and centrifugal terms, \( \mathbf{g}(\mathbf{q}) \) is gravity, and \( \boldsymbol{\tau} \) are actuator torques.

Suppose we introduce holonomic constraints \( \boldsymbol{\phi}(\mathbf{q},t) = \mathbf{0} \) of dimension \( m \) (e.g., closed kinematic loops, end-effector lying on a surface). Using Lagrange multipliers \( \boldsymbol{\lambda} \in \mathbb{R}^m \), the constrained EoM become

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}(\mathbf{q})^{\mathsf{T}}\boldsymbol{\lambda}, \]

\[ \boldsymbol{\phi}(\mathbf{q},t) = \mathbf{0}, \quad \mathbf{J}(\mathbf{q}) := \frac{\partial \boldsymbol{\phi}}{\partial \mathbf{q}}(\mathbf{q},t) \in \mathbb{R}^{m \times n}. \]

Introduce the generalized velocity \( \mathbf{v} = \dot{\mathbf{q}} \). A standard state choice is \( \mathbf{y} = \begin{bmatrix}\mathbf{q}^{\mathsf{T}} & \mathbf{v}^{\mathsf{T}}\end{bmatrix}^{\mathsf{T}} \). The constrained dynamics can then be written as a semi-explicit DAE:

\[ \dot{\mathbf{q}} = \mathbf{v}, \quad \mathbf{M}(\mathbf{q})\dot{\mathbf{v}} = \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\mathbf{v})\mathbf{v} - \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{\mathsf{T}}\boldsymbol{\lambda}, \]

\[ \boldsymbol{\phi}(\mathbf{q},t) = \mathbf{0}. \]

This is a DAE because we have algebraic equations \( \boldsymbol{\phi}(\mathbf{q},t) = \mathbf{0} \) coupling the state but not its time derivative explicitly. For numerical simulation, we must manipulate these equations so that a time-stepper can advance \( \mathbf{q},\mathbf{v} \) while simultaneously enforcing the constraints.

Differentiating the constraint once gives the velocity-level constraint:

\[ \dot{\boldsymbol{\phi}}(\mathbf{q},t) = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}} + \frac{\partial \boldsymbol{\phi}}{\partial t}(\mathbf{q},t) = \mathbf{J}(\mathbf{q})\mathbf{v} + \boldsymbol{\phi}_t(\mathbf{q},t) = \mathbf{0}. \]

Differentiating again yields the acceleration-level form:

\[ \ddot{\boldsymbol{\phi}}(\mathbf{q},t) = \mathbf{J}(\mathbf{q})\ddot{\mathbf{q}} + \dot{\mathbf{J}}(\mathbf{q},\mathbf{v})\mathbf{v} + \boldsymbol{\phi}_{tt}(\mathbf{q},t) = \mathbf{0}, \]

where \( \dot{\mathbf{J}}(\mathbf{q},\mathbf{v}) \) is the directional derivative of \( \mathbf{J} \) along \( \mathbf{v} \), and \( \boldsymbol{\phi}_{tt} \) accounts for explicit time dependence in the constraints.

Substituting the dynamics \( \ddot{\mathbf{q}} = \dot{\mathbf{v}} \) from the Lagrange multiplier equation gives a linear system in the unknowns \( \dot{\mathbf{v}},\boldsymbol{\lambda} \):

\[ \mathbf{M}(\mathbf{q})\dot{\mathbf{v}} = \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\mathbf{v})\mathbf{v} - \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{\mathsf{T}}\boldsymbol{\lambda}, \]

\[ \mathbf{J}(\mathbf{q})\dot{\mathbf{v}} = -\dot{\mathbf{J}}(\mathbf{q},\mathbf{v})\mathbf{v} - \boldsymbol{\phi}_{tt}(\mathbf{q},t). \]

Collecting these, we obtain the fundamental constraint block system solved at each time step:

\[ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & \mathbf{J}(\mathbf{q})^{\mathsf{T}} \\ \mathbf{J}(\mathbf{q}) & \mathbf{0} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}} \\[4pt] \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\mathbf{v})\mathbf{v} - \mathbf{g}(\mathbf{q}) \\ -\dot{\mathbf{J}}(\mathbf{q},\mathbf{v})\mathbf{v} - \boldsymbol{\phi}_{tt}(\mathbf{q},t) \end{bmatrix}. \]

This representation is often called an index-1 DAE formulation, because once the algebraic constraints have been differentiated to the acceleration level, the system behaves numerically like a stiff ODE in the variables \( \mathbf{q},\mathbf{v} \) with algebraic unknowns \( \boldsymbol{\lambda} \) solved from the block system.

Consider a uniform time grid \( t_k = t_0 + k h \) with step size \( h \). A simple explicit time-stepping scheme for constrained dynamics proceeds as follows at each step:

1. Given \( \mathbf{q}_k,\mathbf{v}_k \), evaluate \( \mathbf{M}_k = \mathbf{M}(\mathbf{q}_k) \), \( \mathbf{J}_k = \mathbf{J}(\mathbf{q}_k) \), \( \dot{\mathbf{J}}_k \), \( \boldsymbol{\phi}_k = \boldsymbol{\phi}(\mathbf{q}_k,t_k) \).
2. Assemble the block matrix and right-hand side using the index-1 formulation (possibly with stabilization, see next section) and solve for \( \dot{\mathbf{v}}_k \) and \( \boldsymbol{\lambda}_k \).
3. Update velocities by a second-order method, e.g. velocity-Verlet/symplectic Euler: \( \mathbf{v}_{k+1} = \mathbf{v}_k + h \dot{\mathbf{v}}_k \).
4. Update configurations: \( \mathbf{q}_{k+1} = \mathbf{q}_k + h \mathbf{v}_{k+1} \).
5. Optionally project \( \mathbf{q}_{k+1},\mathbf{v}_{k+1} \) back to the constraint manifold to remove accumulated numerical drift.

The following diagram summarizes the simulation loop (one constrained time step):

flowchart TD
  A["q_k, v_k"] --> B["Eval M(q_k), J(q_k), Jdot, phi(q_k,t_k)"]
  B --> C["Form block matrix [M, J^T; J, 0]"]
  C --> D["Solve for vdot_k and lambda_k"]
  D --> E["Update v_{k+1} = v_k + h vdot_k"]
  E --> F["Update q_{k+1} = q_k + h v_{k+1}"]
  F --> G["Optional: project onto phi(q,t)=0, J(q)v=0"]
  G --> H["q_{k+1}, v_{k+1} for next step"]
        

More advanced integrators (Newmark, generalized-α, implicit midpoint) follow the same pattern: at each step, a nonlinear system involving the constraint block structure is solved, typically by Newton iterations. The linearization in each Newton step again exposes block matrices with \( \mathbf{M} \) and \( \mathbf{J} \).

Finite precision and discretization cause constraint drift: even if \( \boldsymbol{\phi}(\mathbf{q}_0,t_0) = \mathbf{0} \) and \( \dot{\boldsymbol{\phi}}(\mathbf{q}_0,t_0) = \mathbf{0} \), numerical integration produces nonzero values as time advances. Two standard countermeasures are Baumgarte stabilization and geometric projection.

4.1. Baumgarte Stabilization at the Acceleration Level

Baumgarte proposed to replace the ideal constraint acceleration \( \ddot{\boldsymbol{\phi}} = \mathbf{0} \) by a damped oscillator:

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

with positive parameters \( \alpha,\beta \). Substituting the expressions for \( \dot{\boldsymbol{\phi}} \) and \( \ddot{\boldsymbol{\phi}} \) yields

\[ \mathbf{J}(\mathbf{q})\ddot{\mathbf{q}} = -\dot{\mathbf{J}}(\mathbf{q},\mathbf{v})\mathbf{v} - \boldsymbol{\phi}_{tt}(\mathbf{q},t) - 2\alpha\left( \mathbf{J}(\mathbf{q})\mathbf{v} + \boldsymbol{\phi}_t(\mathbf{q},t) \right) - \beta^2 \boldsymbol{\phi}(\mathbf{q},t). \]

The only change in the block system is the right-hand side of the constraint row, which now includes stabilization terms:

\[ \mathbf{J}(\mathbf{q})\dot{\mathbf{v}} = -\dot{\mathbf{J}}(\mathbf{q},\mathbf{v})\mathbf{v} - \boldsymbol{\phi}_{tt}(\mathbf{q},t) - 2\alpha\left( \mathbf{J}(\mathbf{q})\mathbf{v} + \boldsymbol{\phi}_t(\mathbf{q},t) \right) - \beta^2 \boldsymbol{\phi}(\mathbf{q},t). \]

For a single scalar constraint, the homogeneous error dynamics reduces to the linear second-order ODE

\[ \ddot{\phi}(t) + 2\alpha \dot{\phi}(t) + \beta^2 \phi(t) = 0, \]

whose characteristic polynomial has the form \( s^2 + 2\alpha s + \beta^2 \). Choosing positive parameters gives exponentially decaying constraint violations in the continuous-time limit. In discrete time, \( \alpha,\beta \) must also respect the time step size to avoid numerical instabilities.

4.2. Geometric Projection

An alternative strategy is projection:

1. Take an unconstrained trial step \( \tilde{\mathbf{q}}_{k+1},\tilde{\mathbf{v}}_{k+1} \) ignoring constraints.
2. Find corrections \( \Delta\mathbf{q},\Delta\mathbf{v} \) that solve the linearized constraint equations \( \boldsymbol{\phi}(\tilde{\mathbf{q}}_{k+1} + \Delta\mathbf{q},t_{k+1}) = \mathbf{0} \) and \( \mathbf{J}(\tilde{\mathbf{q}}_{k+1} + \Delta\mathbf{q}) (\tilde{\mathbf{v}}_{k+1} + \Delta\mathbf{v}) = \mathbf{0} \).
3. Update \( \mathbf{q}_{k+1} = \tilde{\mathbf{q}}_{k+1} + \Delta\mathbf{q} \), \( \mathbf{v}_{k+1} = \tilde{\mathbf{v}}_{k+1} + \Delta\mathbf{v} \).

Projection methods can be combined with Baumgarte stabilization and structure-preserving integrators to achieve long-term constraint satisfaction and good energy behavior in closed kinematic chains and parallel robots.

To illustrate constrained simulation numerically without writing a full robot model, we consider a point mass in the plane constrained to move on a circle of radius \( R \):

\[ \mathbf{q} = \begin{bmatrix} x \\ y \end{bmatrix}, \quad \boldsymbol{\phi}(\mathbf{q}) = x^2 + y^2 - R^2 = 0. \]

The mass is subject to gravity in the negative \( y \) direction. With \( m \) the mass, \( \mathbf{M} = m \mathbf{I}_2 \), \( \mathbf{g} = \begin{bmatrix} 0 \\ m g \end{bmatrix} \), and no actuators (\( \boldsymbol{\tau} = \mathbf{0} \)), we have

\[ \mathbf{J}(\mathbf{q}) = \begin{bmatrix} 2x & 2y \end{bmatrix}, \quad \dot{\mathbf{J}}(\mathbf{q},\mathbf{v}) = \begin{bmatrix} 2\dot{x} & 2\dot{y} \end{bmatrix}. \]

The constrained block system with Baumgarte stabilization for this example reads

\[ \begin{bmatrix} m\mathbf{I}_2 & \mathbf{J}(\mathbf{q})^{\mathsf{T}} \\ \mathbf{J}(\mathbf{q}) & 0 \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}} \\ \lambda \end{bmatrix} = \begin{bmatrix} -\mathbf{g} \\ -\dot{\mathbf{J}}(\mathbf{q},\mathbf{v})\mathbf{v} - 2\alpha \mathbf{J}(\mathbf{q})\mathbf{v} - \beta^2 \boldsymbol{\phi}(\mathbf{q}) \end{bmatrix}, \]

with scalar multiplier \( \lambda \). This toy system has the same algebraic structure as a closed-chain robot: the block matrix couples accelerations and multipliers via constraint Jacobians and is solved at every time step.

Python, together with numpy and scipy, is widely used for robotics prototyping. Robotics-focused toolboxes such as roboticstoolbox-python provide high-level dynamics models, but here we implement the constrained integrator for the circle example directly using NumPy.

import numpy as np

# Physical and numerical parameters
m = 1.0
g = 9.81
R = 1.0
alpha = 10.0    # Baumgarte parameters
beta = 20.0
h = 1e-3        # time step
n_steps = 10000

def phi(q):
    # Constraint: x^2 + y^2 - R^2 = 0
    x, y = q
    return x*x + y*y - R*R

def J(q):
    # Jacobian row (shape 1x2)
    x, y = q
    return np.array([[2.0 * x, 2.0 * y]])

def Jdot(q, v):
    # Time derivative of Jacobian times v uses Jdot as 1x2
    vx, vy = v
    return np.array([[2.0 * vx, 2.0 * vy]])

def step(q, v, h):
    """
    Perform one constrained time step using a symplectic-like update:
      v_{k+1} = v_k + h * vdot_k
      q_{k+1} = q_k + h * v_{k+1}
    """
    # Mass matrix and gravity
    M = m * np.eye(2)
    g_vec = np.array([0.0, m * g])

    J_q = J(q)           # 1x2
    Jdot_q = Jdot(q, v)  # 1x2
    phi_q = phi(q)       # scalar

    # Build 3x3 block matrix A and right-hand side rhs
    A = np.zeros((3, 3))
    # Top-left block: M
    A[0:2, 0:2] = M
    # Top-right block: J^T
    A[0:2, 2] = J_q.T[:, 0]
    # Bottom-left block: J
    A[2, 0:2] = J_q[0, :]

    # Right-hand side
    rhs = np.zeros(3)
    # Dynamics: M vdot = -g + J^T lambda
    rhs[0:2] = -g_vec
    # Constraint acceleration with Baumgarte stabilization
    Jv = J_q.dot(v)[0]                 # scalar
    Jdotv = Jdot_q.dot(v)[0]          # scalar
    rhs[2] = -Jdotv - 2.0 * alpha * Jv - (beta ** 2) * phi_q

    sol = np.linalg.solve(A, rhs)
    vdot = sol[0:2]
    # lambda = sol[2]  # not used further here

    # Symplectic-like update
    v_new = v + h * vdot
    q_new = q + h * v_new

    return q_new, v_new

# Initial conditions: start on circle with small tangential velocity
q = np.array([R, 0.0])
v = np.array([0.0, 0.5])

trajectory_q = []
for k in range(n_steps):
    trajectory_q.append(q.copy())
    q, v = step(q, v, h)

trajectory_q = np.array(trajectory_q)
print("Final constraint value:", phi(q))
      

In practice, one would wrap step into a class representing a general constrained system, with methods to compute \( \mathbf{M},\mathbf{J},\dot{\mathbf{J}} \) for a given robot model (possibly obtained from a robotics library) and reuse the same integration loop.

In C++, high-performance robotics libraries such as RBDL, pinocchio, and Drake rely heavily on the Eigen linear algebra library. The following snippet shows the same constrained integrator skeleton implemented with Eigen types.

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

using Eigen::Matrix2d;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

struct ConstrainedMassOnCircle {
    double m{1.0};
    double g{9.81};
    double R{1.0};
    double alpha{10.0};
    double beta{20.0};

    double phi(const Vector2d& q) const {
        double x = q(0), y = q(1);
        return x * x + y * y - R * R;
    }

    Eigen::RowVector2d J(const Vector2d& q) const {
        double x = q(0), y = q(1);
        Eigen::RowVector2d Jq;
        Jq << 2.0 * x, 2.0 * y;
        return Jq;
    }

    Eigen::RowVector2d Jdot(const Vector2d& v) const {
        double vx = v(0), vy = v(1);
        Eigen::RowVector2d Jd;
        Jd << 2.0 * vx, 2.0 * vy;
        return Jd;
    }

    void step(Vector2d& q, Vector2d& v, double h) const {
        Matrix2d M = m * Matrix2d::Identity();
        Vector2d g_vec(0.0, m * g);

        Eigen::RowVector2d Jq = J(q);
        Eigen::RowVector2d Jd = Jdot(v);

        double phi_q = phi(q);
        double Jv = Jq * v;
        double Jdv = Jd * v;

        Matrix3d A = Matrix3d::Zero();
        // M block
        A.block<2,2>(0,0) = M;
        // J^T block
        A.block<2,1>(0,2) = Jq.transpose();
        // J block
        A.block<1,2>(2,0) = Jq;

        Vector3d rhs = Vector3d::Zero();
        rhs.segment<2>(0) = -g_vec;
        rhs(2) = -Jdv - 2.0 * alpha * Jv - beta * beta * phi_q;

        Vector3d sol = A.fullPivLu().solve(rhs);
        Vector2d vdot = sol.segment<2>(0);
        // double lambda = sol(2);

        v += h * vdot;
        q += h * v;
    }
};

int main() {
    ConstrainedMassOnCircle sys;
    double h = 1e-3;
    int n_steps = 10000;

    Vector2d q(sys.R, 0.0);
    Vector2d v(0.0, 0.5);

    for (int k = 0; k < n_steps; ++k) {
        sys.step(q, v, h);
    }
    std::cout << "Final phi(q): " << sys.phi(q) << std::endl;
    return 0;
}
      

Replacing the toy model with a full manipulator model only changes the functions computing \( \mathbf{M}(\mathbf{q}),\mathbf{J}(\mathbf{q}),\dot{\mathbf{J}}(\mathbf{q},\mathbf{v}) \) and the right-hand side forces. The constraint solve pattern remains identical.

For Java-based robotics stacks, the EJML (Efficient Java Matrix Library) package is a common choice for linear algebra. The following code uses EJML's SimpleMatrix class to implement the constrained step.

import org.ejml.simple.SimpleMatrix;

public class ConstrainedMassOnCircle {

    double m = 1.0;
    double g = 9.81;
    double R = 1.0;
    double alpha = 10.0;
    double beta = 20.0;

    double phi(SimpleMatrix q) {
        double x = q.get(0);
        double y = q.get(1);
        return x * x + y * y - R * R;
    }

    SimpleMatrix J(SimpleMatrix q) {
        double x = q.get(0);
        double y = q.get(1);
        // 1x2 row
        return new SimpleMatrix(1, 2, true, new double[]{2.0 * x, 2.0 * y});
    }

    SimpleMatrix Jdot(SimpleMatrix v) {
        double vx = v.get(0);
        double vy = v.get(1);
        return new SimpleMatrix(1, 2, true, new double[]{2.0 * vx, 2.0 * vy});
    }

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

        SimpleMatrix Jq = J(q);      // 1x2
        SimpleMatrix Jd = Jdot(v);   // 1x2

        double phiVal = phi(q);
        double Jv = Jq.mult(v).get(0);
        double Jdv = Jd.mult(v).get(0);

        // Build 3x3 block matrix A
        SimpleMatrix A = new SimpleMatrix(3, 3);
        // Top-left 2x2
        A.insertIntoThis(0, 0, M);
        // Top-right 2x1 = J^T
        A.set(0, 2, Jq.get(0));
        A.set(1, 2, Jq.get(1));
        // Bottom-left 1x2
        A.set(2, 0, Jq.get(0));
        A.set(2, 1, Jq.get(1));

        // rhs vector
        SimpleMatrix rhs = new SimpleMatrix(3, 1);
        rhs.set(0, 0, -gVec.get(0));
        rhs.set(1, 0, -gVec.get(1));
        rhs.set(2, 0, -Jdv - 2.0 * alpha * Jv - beta * beta * phiVal);

        SimpleMatrix sol = A.solve(rhs);
        SimpleMatrix vdot = sol.rows(0, 2);
        // double lambda = sol.get(2);

        v = v.plus(vdot.scale(h));
        q = q.plus(v.scale(h));

        // Update references (caller holds q, v)
        q.set(0, 0, q.get(0));
        q.set(1, 0, q.get(1));
        v.set(0, 0, v.get(0));
        v.set(1, 0, v.get(1));
    }

    public static void main(String[] args) {
        ConstrainedMassOnCircle sys = new ConstrainedMassOnCircle();
        double h = 1e-3;
        int nSteps = 10000;

        SimpleMatrix q = new SimpleMatrix(2, 1, true, new double[]{sys.R, 0.0});
        SimpleMatrix v = new SimpleMatrix(2, 1, true, new double[]{0.0, 0.5});

        for (int k = 0; k < nSteps; ++k) {
            sys.step(q, v, h);
        }
        System.out.println("Final phi(q) = " + sys.phi(q));
    }
}
      

As in C++, replacing the toy model by a full robot only affects the model-specific routines phi, J, and the generalized forces, not the basic constraint block solve logic.

MATLAB is a standard environment for numerical dynamics. Its robotics toolbox and Simulink provide high-level blocks for multibody systems, but the core constrained integrator can be implemented directly as follows:

m = 1.0;
g = 9.81;
R = 1.0;
alpha = 10.0;
beta = 20.0;

h = 1e-3;
n_steps = 10000;

q = [R; 0.0];    % position [x;y]
v = [0.0; 0.5];  % velocity [vx;vy]

phi = @(q) q(1)^2 + q(2)^2 - R^2;
J   = @(q) [2*q(1), 2*q(2)];
Jdot = @(v) [2*v(1), 2*v(2)];

for k = 1:n_steps
    M = m * eye(2);
    g_vec = [0.0; m*g];

    Jq = J(q);
    Jd = Jdot(v);
    phi_q = phi(q);
    Jv = Jq * v;
    Jdv = Jd * v;

    % Build 3x3 block matrix
    A = [M, Jq.';
         Jq, 0];

    rhs = zeros(3,1);
    rhs(1:2) = -g_vec;
    rhs(3) = -Jdv - 2*alpha*Jv - beta^2*phi_q;

    sol = A \ rhs;
    vdot = sol(1:2);
    % lambda = sol(3);

    v = v + h * vdot;
    q = q + h * v;
end

fprintf('Final constraint value phi(q) = %.6e\n', phi(q));
      

In Simulink, the same equations can be implemented via a custom S-Function or a subsystem that (i) computes \( \mathbf{M},\mathbf{J},\dot{\mathbf{J}} \), (ii) forms and solves the block system at each sample time, and (iii) feeds the resulting accelerations into integrator blocks for \( \mathbf{v} \) and \( \mathbf{q} \). Simscape Multibody automates much of this for complex closed-chain mechanisms.

Wolfram Mathematica's NDSolve can directly handle DAEs arising from constrained mechanical systems. For the circle example (without Baumgarte stabilization), one can specify the constraints explicitly:

m = 1.0;
g = 9.81;
R = 1.0;

(* Unknown functions: x(t), y(t), lambda(t) *)
eqs = {
   x''[t] == (2 lambda[t] x[t])/m,
   y''[t] == -g + (2 lambda[t] y[t])/m,
   x[t]^2 + y[t]^2 == R^2,
   x[t] x'[t] + y[t] y'[t] == 0
   };

(* Initial conditions on circle with tangential velocity *)
ics = {
   x[0] == R,    y[0] == 0,
   x'[0] == 0,   y'[0] == 0.5
   };

sol = NDSolve[Join[eqs, ics], {x, y, lambda}, {t, 0, 10}];

ParametricPlot[
  Evaluate[{x[t], y[t]} /. sol],
  {t, 0, 10}
]
      

For complex multibody robots, Mathematica's symbolic capabilities can derive \( \mathbf{M}(\mathbf{q}) \), constraint equations, and even their Jacobians, which are then passed to NDSolve as a DAE system. This provides a powerful environment for verifying hand-derived constrained dynamics models.

In this lesson we formulated the dynamics of constrained manipulators as DAEs, highlighted the index-1 acceleration-level block structure, and showed how numerical integrators solve a coupled system for accelerations and Lagrange multipliers at each time step. Constraint drift motivated Baumgarte stabilization and projection methods, which are essential for long-term closed-chain simulations. Finally, we implemented a simple example system in Python, C++, Java, MATLAB, and Wolfram Mathematica, illustrating how the same block structure underlies constrained integration in a variety of robotics software stacks. These techniques form the numerical backbone for simulating parallel robots, closed kinematic chains, and multi-contact systems in advanced modeling and control.