Consider a parallel robot or any mechanism with loops (e.g., a four-bar, 5-bar, or Stewart platform). Let \( \mathbf{q} \in \mathbb{R}^n \) denote all joint coordinates (actuated and passive). For a serial robot without loops, the dynamics are written as

\[ \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 symmetric positive-definite inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects Coriolis and centrifugal terms, and \( \mathbf{g}(\mathbf{q}) \) is the gravity vector.

For a closed-chain mechanism, holonomic constraints relate the joint coordinates:

\[ \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0}, \quad \boldsymbol{\phi} : \mathbb{R}^n \rightarrow \mathbb{R}^m, \quad m = \text{number of independent constraints}. \]

The constraints generate internal constraint forces that cannot be captured by joint torques alone. Introducing Lagrange multipliers \( \boldsymbol{\lambda} \in \mathbb{R}^m \), the dynamic equations become

\[ \mathbf{M}(\mathbf{q})\,\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{S}_a^{\top}\boldsymbol{\tau}_a + \mathbf{J}_c(\mathbf{q})^{\top}\boldsymbol{\lambda}, \]

where \( \mathbf{S}_a \) selects the actuated joints, \( \boldsymbol{\tau}_a \) are the actuator torques, and \( \mathbf{J}_c(\mathbf{q}) = \frac{\partial \boldsymbol{\phi}}{\partial \mathbf{q}} \) is the constraint Jacobian. The term \( \mathbf{J}_c^{\top}\boldsymbol{\lambda} \) represents the generalized constraint forces that enforce loop closure.

flowchart TD
  Q["Closed-chain mechanism geometry"] --> PHI["Define holonomic constraints phi(q)=0"]
  PHI --> JC["Compute constraint Jacobian J_c(q)"]
  JC --> DYN["Serial-like dynamics: M(q) qddot + C(q,qdot) qdot + g(q)"]
  DYN --> LAG["Augment with Lagrange multipliers lambda"]
  LAG --> RED["Either solve full DAE or derive reduced coordinates"]
  RED --> SIM["Integrate dynamics for simulation / analysis"]
        

From the holonomic constraints \( \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0} \), we obtain first and second time derivatives:

\[ \mathbf{J}_c(\mathbf{q})\,\dot{\mathbf{q}} = \mathbf{0}, \quad \mathbf{J}_c(\mathbf{q})\,\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} = \mathbf{0}. \]

Stacking the dynamics with the acceleration-level constraint gives the linear system for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \):

\[ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & -\mathbf{J}_c(\mathbf{q})^{\top} \\ \mathbf{J}_c(\mathbf{q}) & \mathbf{0} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\[2mm] \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{S}_a^{\top}\boldsymbol{\tau}_a - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \\[2mm] -\,\dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} \end{bmatrix}. \]

Together with the configuration-level constraint \( \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0} \), this forms an index-3 DAE in the standard Lagrange formulation. Using only the acceleration-level constraint (and carefully chosen initial conditions) yields an index-1 system in terms of \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \), which is numerically better conditioned.

Assuming the block matrix is nonsingular (no constraint singularity), we can solve at each time step for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \). The multipliers \( \boldsymbol{\lambda} \) then reveal internal forces in closed loops, which are important for structural design but do not affect the motion directly when expressed in minimal coordinates.

Let us partition joint coordinates into actuated and passive sets:

\[ \mathbf{q} = \begin{bmatrix} \mathbf{q}_a \\ \mathbf{q}_p \end{bmatrix}, \quad \boldsymbol{\tau} = \begin{bmatrix} \boldsymbol{\tau}_a \\ \boldsymbol{\tau}_p \end{bmatrix}, \quad \boldsymbol{\tau}_p = \mathbf{0}. \]

The inertia matrix and other terms are partitioned conformably:

\[ \mathbf{M} = \begin{bmatrix} \mathbf{M}_{aa} & \mathbf{M}_{ap} \\ \mathbf{M}_{pa} & \mathbf{M}_{pp} \end{bmatrix}, \quad \mathbf{J}_c = \begin{bmatrix} \mathbf{J}_{c,a} & \mathbf{J}_{c,p} \end{bmatrix}. \]

The dynamics and acceleration-level constraints can then be written as

\[ \begin{aligned} \mathbf{M}_{aa}\ddot{\mathbf{q}}_a + \mathbf{M}_{ap}\ddot{\mathbf{q}}_p + \mathbf{h}_a &= \boldsymbol{\tau}_a + \mathbf{J}_{c,a}^{\top}\boldsymbol{\lambda}, \\ \mathbf{M}_{pa}\ddot{\mathbf{q}}_a + \mathbf{M}_{pp}\ddot{\mathbf{q}}_p + \mathbf{h}_p &= \mathbf{J}_{c,p}^{\top}\boldsymbol{\lambda}, \\ \mathbf{J}_{c,a}\ddot{\mathbf{q}}_a + \mathbf{J}_{c,p}\ddot{\mathbf{q}}_p + \boldsymbol{\gamma} &= \mathbf{0}, \end{aligned} \]

where \( \mathbf{h} = \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) \) and \( \boldsymbol{\gamma} = \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \). If \( \mathbf{J}_{c,p} \) is invertible, we can solve for \( \ddot{\mathbf{q}}_p \) and \( \boldsymbol{\lambda} \) in terms of \( \ddot{\mathbf{q}}_a \), then eliminate them and obtain an equation purely in \( \ddot{\mathbf{q}}_a \). This is one way to arrive at closed-form dynamics at the actuator level.

Alternatively, we can introduce a null-space basis of the constraint Jacobian. Let \( \mathbf{N}(\mathbf{q}) \in \mathbb{R}^{n \times r} \) with \( r = n - m \) satisfy

\[ \mathbf{J}_c(\mathbf{q})\,\mathbf{N}(\mathbf{q}) = \mathbf{0}, \quad \dot{\mathbf{q}} = \mathbf{N}(\mathbf{q})\,\mathbf{v}. \]

Then \( \mathbf{v} \in \mathbb{R}^r \) parametrizes velocities consistent with the constraints. Projecting the dynamics with \( \mathbf{N}(\mathbf{q})^{\top} \) eliminates \( \boldsymbol{\lambda} \):

\[ \mathbf{N}^{\top}\mathbf{M}\mathbf{N}\,\dot{\mathbf{v}} + \mathbf{N}^{\top}\bigl(\mathbf{C}\dot{\mathbf{q}} + \mathbf{g}\bigr) = \mathbf{N}^{\top}\mathbf{S}_a^{\top}\boldsymbol{\tau}_a. \]

This is the essence of projection-based closed-chain dynamics: evolve only independent coordinates and avoid solving explicitly for \( \boldsymbol{\lambda} \).

A more geometric approach uses minimal coordinates \( \mathbf{q}_r \in \mathbb{R}^{r} \), \( r = n-m \), that parametrize the configuration manifold of the closed-chain mechanism. Assume there is a smooth mapping

\[ \mathbf{q} = \mathbf{f}(\mathbf{q}_r), \quad \mathbf{f} : \mathbb{R}^{r} \rightarrow \mathbb{R}^{n}, \]

such that \( \boldsymbol{\phi}(\mathbf{f}(\mathbf{q}_r)) = \mathbf{0} \) for all \( \mathbf{q}_r \). Differentiating,

\[ \dot{\mathbf{q}} = \frac{\partial \mathbf{f}}{\partial \mathbf{q}_r}\,\dot{\mathbf{q}}_r = \mathbf{\Phi}(\mathbf{q}_r)\,\dot{\mathbf{q}}_r, \]

where \( \mathbf{\Phi}(\mathbf{q}_r) \in \mathbb{R}^{n \times r} \) has full column rank and spans the constraint-consistent velocities. The kinetic energy becomes

\[ T = \tfrac{1}{2}\dot{\mathbf{q}}^{\top}\mathbf{M}(\mathbf{q})\dot{\mathbf{q}} = \tfrac{1}{2}\dot{\mathbf{q}}_r^{\top}\, \underbrace{\bigl( \mathbf{\Phi}^{\top}\mathbf{M}(\mathbf{f}(\mathbf{q}_r))\mathbf{\Phi} \bigr)}_{\mathbf{M}_r(\mathbf{q}_r)} \,\dot{\mathbf{q}}_r, \]

so the reduced inertia matrix is \( \mathbf{M}_r(\mathbf{q}_r) = \mathbf{\Phi}^{\top}\mathbf{M}(\mathbf{f}(\mathbf{q}_r))\mathbf{\Phi} \). Since \( \mathbf{M}(\mathbf{q}) \) is positive definite and \( \mathbf{\Phi} \) has full column rank,

\[ \forall \mathbf{v} \neq \mathbf{0}: \quad \mathbf{v}^{\top}\mathbf{M}_r(\mathbf{q}_r)\mathbf{v} = (\mathbf{\Phi}\mathbf{v})^{\top}\mathbf{M}(\mathbf{q}) (\mathbf{\Phi}\mathbf{v}) > 0, \]

so \( \mathbf{M}_r(\mathbf{q}_r) \) is positive definite. Writing the Lagrangian in reduced coordinates \( L_r(\mathbf{q}_r,\dot{\mathbf{q}}_r) = T(\mathbf{q}_r,\dot{\mathbf{q}}_r) - V(\mathbf{f}(\mathbf{q}_r)) \), the Euler–Lagrange equations yield

\[ \mathbf{M}_r(\mathbf{q}_r)\,\ddot{\mathbf{q}}_r + \mathbf{C}_r(\mathbf{q}_r,\dot{\mathbf{q}}_r)\,\dot{\mathbf{q}}_r + \mathbf{g}_r(\mathbf{q}_r) = \mathbf{\Phi}(\mathbf{q}_r)^{\top}\mathbf{S}_a^{\top}\boldsymbol{\tau}_a, \]

where \( \mathbf{C}_r \) and \( \mathbf{g}_r \) are the reduced Coriolis and gravity terms. This equation is formally identical to open-chain dynamics, but the reduced inertia and forces encode the effect of the closed-chain geometry. Energy conservation and passivity properties carry over if the mapping \( \mathbf{f} \) is chosen smoothly.

Consider a planar four-bar linkage with link lengths \( \ell_1,\ell_2,\ell_3,\ell_4 \). Let \( \theta_i \) denote the absolute angle of link \( i \) with respect to ground. Treat \( \mathbf{q} = [\theta_1,\theta_2,\theta_3,\theta_4]^{\top} \) as generalized coordinates. The loop-closure vector equation \( \mathbf{r}_1 + \mathbf{r}_2 = \mathbf{r}_4 + \mathbf{r}_3 \) gives two scalar constraints (x and y components):

\[ \begin{aligned} \phi_1(\mathbf{q}) &= \ell_1 \cos\theta_1 + \ell_2 \cos\theta_2 - \ell_3 \cos\theta_3 - \ell_4 \cos\theta_4 = 0, \\ \phi_2(\mathbf{q}) &= \ell_1 \sin\theta_1 + \ell_2 \sin\theta_2 - \ell_3 \sin\theta_3 - \ell_4 \sin\theta_4 = 0. \end{aligned} \]

The constraint Jacobian is

\[ \mathbf{J}_c(\mathbf{q}) = \begin{bmatrix} -\ell_1\sin\theta_1 & -\ell_2\sin\theta_2 & \ell_3\sin\theta_3 & \ell_4\sin\theta_4 \\ \ell_1\cos\theta_1 & \ell_2\cos\theta_2 & -\ell_3\cos\theta_3 & -\ell_4\cos\theta_4 \end{bmatrix}. \]

Suppose \( \theta_1 \) is actuated and the other joints are passive. Denote \( \mathbf{q}_a = [\theta_1] \), \( \mathbf{q}_p = [\theta_2,\theta_3,\theta_4]^{\top} \). The inertia matrix \( \mathbf{M}(\mathbf{q}) \) can be constructed by summing link contributions (as in serial manipulators), but the closed chain is enforced through \( \mathbf{J}_c(\mathbf{q}) \) as above. The full dynamics take the form

\[ \mathbf{M}(\mathbf{q})\,\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{b}\,\boldsymbol{\tau}_1 + \mathbf{J}_c(\mathbf{q})^{\top}\boldsymbol{\lambda}, \]

where \( \mathbf{b} = [1,0,0,0]^{\top} \) selects the actuated joint. Solving the coupled system for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \) yields the accelerations and internal constraint forces. Alternatively, using the loop closure to eliminate, say, \( \theta_3 \) and \( \theta_4 \) gives a minimal coordinate representation in terms of \( \theta_1,\theta_2 \), from which \( \mathbf{M}_r(\mathbf{q}_r) \) can be derived.

In simulation, we must integrate the closed-chain dynamics while maintaining \( \boldsymbol{\phi}(\mathbf{q}) \approx \mathbf{0} \) and \( \mathbf{J}_c(\mathbf{q})\dot{\mathbf{q}} \approx \mathbf{0} \). Two main routes are common:

• Direct DAE integration. Use a DAE solver (index-1 or index-2) on the system in Section 2, with \( \mathbf{q},\dot{\mathbf{q}} \) as differential variables and \( \boldsymbol{\lambda} \) algebraic.
• Reduced ODE integration. Parametrize the motion in minimal coordinates \( \mathbf{q}_r \) and integrate the reduced dynamics \( \mathbf{M}_r(\mathbf{q}_r)\ddot{\mathbf{q}}_r + \dots = \dots \).

In the DAE approach, numerical drift can cause constraint violations. Constraint stabilization techniques (e.g. Baumgarte-type methods) augment the acceleration-level constraint with proportional and derivative feedback of the position and velocity error, effectively adding stabilizing terms in the multiplier equation. In the reduced approach, the constraints are satisfied by construction, but we must be able to compute \( \mathbf{\Phi}(\mathbf{q}_r) \) and \( \mathbf{M}_r(\mathbf{q}_r) \) efficiently.

flowchart TD
  S0["Given q, qdot at time t"] --> S1["Form M(q), C(q,qdot), g(q)"]
  S1 --> S2["Compute J_c(q), J_cdot(q,qdot)"]
  S2 --> S3["Solve linear system for qddot and lambda"]
  S3 --> S4["Integrate qdot and q to t + dt"]
  S4 --> S5["Project back to constraint manifold if needed"]
  S5 --> S6["Repeat for next time step"]
        

Python, combined with numpy and scipy.integrate, is convenient for prototyping closed-chain dynamics. Below is a skeleton for integrating a generic constrained mechanism using the acceleration-level formulation. The example assumes that helper functions for M(q), C(q, qdot), g(q), Jc(q), and Jc_dot(q, qdot) are supplied (these can be generated symbolically via sympy or robotics libraries such as pinocchio or roboticstoolbox).

import numpy as np
from numpy.linalg import solve
from scipy.integrate import solve_ivp

def M(q):
    # Inertia matrix M(q) (n x n)
    raise NotImplementedError

def C_term(q, qdot):
    # Coriolis/centrifugal term C(q, qdot) qdot, dimension n
    raise NotImplementedError

def g_term(q):
    # Gravity term g(q), dimension n
    raise NotImplementedError

def Jc(q):
    # Constraint Jacobian J_c(q) (m x n)
    raise NotImplementedError

def Jc_dot(q, qdot):
    # Time derivative of J_c(q) times qdot, i.e. J_c_dot(q, qdot) qdot (m)
    raise NotImplementedError

def S_a():
    # Selection matrix for actuated joints (n_a x n)
    raise NotImplementedError

def tau_a(t, q, qdot):
    # Actuator torque vector (n_a), possibly time or state dependent
    return np.zeros(S_a().shape[0])

def closed_chain_rhs(t, x):
    # State x = [q; qdot]
    # Returns xdot = [qdot; qddot]
    n = x.size // 2
    q = x[:n]
    qdot = x[n:]

    Mq = M(q)                     # (n x n)
    h = C_term(q, qdot) + g_term(q)  # (n)
    J = Jc(q)                     # (m x n)
    Jdot_qdot = Jc_dot(q, qdot)   # (m)

    Sa = S_a()                    # (n_a x n)
    tau = tau_a(t, q, qdot)       # (n_a)

    # Build saddle-point system
    m = J.shape[0]
    K = np.zeros((n + m, n + m))
    rhs = np.zeros(n + m)

    K[:n, :n] = Mq
    K[:n, n:] = -J.T
    K[n:, :n] = J
    # lower-right block is already zeros

    rhs[:n] = Sa.T @ tau - h
    rhs[n:] = -Jdot_qdot

    sol = solve(K, rhs)
    qddot = sol[:n]
    # lambdas = sol[n:]  # constraint forces if needed

    xdot = np.zeros_like(x)
    xdot[:n] = qdot
    xdot[n:] = qddot
    return xdot

# Example integration
def simulate_closed_chain(x0, t_span, dt=1e-3):
    t_eval = np.arange(t_span[0], t_span[1] + dt, dt)
    sol = solve_ivp(closed_chain_rhs, t_span, x0, t_eval=t_eval, method="RK45")
    return sol.t, sol.y
      

In a more advanced implementation, one would use a DAE solver (e.g. IDA via scikits.odes) and enforce position-level constraints explicitly in the solver. Libraries like pinocchio, RBDL (with Python bindings), or roboticstoolbox-python provide efficient routines for \( \mathbf{M} \), \( \mathbf{C} \), and \( \mathbf{g} \).

In C++, libraries like Eigen (for linear algebra) and RBDL or pinocchio (for rigid-body dynamics) are widely used. A typical pattern is to obtain \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) from a rigid-body library and implement the constraint saddle-point system explicitly:

#include <Eigen/Dense>

// Assume you have functions that wrap an RBDL or Pinocchio model:
void computeDynamics(const Eigen::VectorXd& q,
                     const Eigen::VectorXd& qdot,
                     Eigen::MatrixXd& M,
                     Eigen::VectorXd& h); // h = C(q,qdot) qdot + g(q)

void computeConstraints(const Eigen::VectorXd& q,
                        const Eigen::VectorXd& qdot,
                        Eigen::MatrixXd& Jc,
                        Eigen::VectorXd& Jc_dot_qdot);

Eigen::MatrixXd Sa(); // selection matrix for actuated joints

Eigen::VectorXd tau_a(double t,
                      const Eigen::VectorXd& q,
                      const Eigen::VectorXd& qdot);

Eigen::VectorXd rhs_closed_chain(double t,
                                 const Eigen::VectorXd& x)
{
    int n = static_cast<int>(x.size()) / 2;
    Eigen::VectorXd q   = x.head(n);
    Eigen::VectorXd qdot = x.tail(n);

    Eigen::MatrixXd M(n, n);
    Eigen::VectorXd h(n);
    computeDynamics(q, qdot, M, h);

    Eigen::MatrixXd Jc;
    Eigen::VectorXd Jc_dot_qdot;
    computeConstraints(q, qdot, Jc, Jc_dot_qdot);

    Eigen::MatrixXd S = Sa();
    Eigen::VectorXd tau = tau_a(t, q, qdot);

    int m = static_cast<int>(Jc.rows());

    Eigen::MatrixXd K(n + m, n + m);
    K.setZero();
    K.block(0, 0, n, n)       = M;
    K.block(0, n, n, m)       = -Jc.transpose();
    K.block(n, 0, m, n)       = Jc;
    // lower-right m x m block stays zero

    Eigen::VectorXd b(n + m);
    b.head(n)   = S.transpose() * tau - h;
    b.tail(m)   = -Jc_dot_qdot;

    Eigen::VectorXd sol = K.fullPivLu().solve(b);
    Eigen::VectorXd qddot = sol.head(n);
    // Eigen::VectorXd lambda = sol.tail(m);

    Eigen::VectorXd xdot(x.size());
    xdot.head(n) = qdot;
    xdot.tail(n) = qddot;
    return xdot;
}
      

This derivative function can then be integrated with your preferred ODE integrator. For stiff constrained dynamics, implicit or symplectic integrators are often preferable.

Java is less common for low-level robot dynamics but can be used with numerical libraries such as EJML. Below is a sketch of a closed-chain acceleration computation using EJML dense matrices:

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

public class ClosedChainDynamics {

    // User-defined: return M(q), h(q,qdot) = C(q,qdot) qdot + g(q)
    static void computeDynamics(double[] q, double[] qdot,
                                DMatrixRMaj M, DMatrixRMaj h) {
        // Fill M and h appropriately
    }

    // User-defined: Jc(q) and Jc_dot(q,qdot) qdot
    static void computeConstraints(double[] q, double[] qdot,
                                   DMatrixRMaj Jc, DMatrixRMaj JcDotQdot) {
        // Fill Jc and JcDotQdot
    }

    static DMatrixRMaj Sa() {
        // Build selection matrix for actuated joints
        return null;
    }

    static DMatrixRMaj tauA(double t, double[] q, double[] qdot) {
        // Return actuator torques as column vector
        return null;
    }

    public static double[] rhs(double t, double[] x) {
        int n = x.length / 2;
        double[] q = new double[n];
        double[] qdot = new double[n];
        System.arraycopy(x, 0,     q,    0, n);
        System.arraycopy(x, n,     qdot, 0, n);

        // Build matrices
        DMatrixRMaj M = new DMatrixRMaj(n, n);
        DMatrixRMaj h = new DMatrixRMaj(n, 1);
        computeDynamics(q, qdot, M, h);

        DMatrixRMaj Jc = new DMatrixRMaj(0, 0);       // will be resized
        DMatrixRMaj JcDotQdot = new DMatrixRMaj(0, 1);
        computeConstraints(q, qdot, Jc, JcDotQdot);

        int m = Jc.getNumRows();
        DMatrixRMaj S = Sa();
        DMatrixRMaj tau = tauA(t, q, qdot);

        // Build saddle-point matrix K and rhs b
        DMatrixRMaj K = new DMatrixRMaj(n + m, n + m);
        CommonOps_DDRM.fill(K, 0.0);
        CommonOps_DDRM.insert(M, K, 0, 0);

        DMatrixRMaj JcT = new DMatrixRMaj(n, m);
        CommonOps_DDRM.transpose(Jc, JcT);
        CommonOps_DDRM.insert(JcT, K, 0, n);
        CommonOps_DDRM.insert(Jc,  K, n, 0);

        DMatrixRMaj b = new DMatrixRMaj(n + m, 1);
        DMatrixRMaj tmp = new DMatrixRMaj(n, 1);

        // b_head = S^T tau - h
        DMatrixRMaj ST = new DMatrixRMaj(n, S.getNumRows());
        CommonOps_DDRM.transpose(S, ST);
        CommonOps_DDRM.mult(ST, tau, tmp);
        CommonOps_DDRM.subtract(tmp, h, tmp);
        CommonOps_DDRM.insert(tmp, b, 0, 0);

        // b_tail = -Jc_dot(q,qdot) qdot
        DMatrixRMaj minusJcDotQdot = new DMatrixRMaj(m, 1);
        CommonOps_DDRM.changeSign(JcDotQdot, minusJcDotQdot);
        CommonOps_DDRM.insert(minusJcDotQdot, b, n, 0);

        // Solve K [qddot; lambda] = b
        DMatrixRMaj sol = b.copy();
        CommonOps_DDRM.solve(K, b, sol);

        double[] xdot = new double[2 * n];
        // xdot_head = qdot
        System.arraycopy(qdot, 0, xdot, 0, n);
        // xdot_tail = qddot
        for (int i = 0; i < n; ++i) {
            xdot[n + i] = sol.get(i, 0);
        }
        return xdot;
    }
}
      

This method can be wrapped into an ODE integrator (or a semi-implicit scheme) to simulate the motion. For educational purposes, a 4-bar or 5-bar mechanism is a good test case.

MATLAB is widely used for robot dynamics modeling, and Simulink makes it easy to simulate DAEs. Below is a function implementing the right-hand side of a closed-chain DAE using the acceleration-level formulation. It can be used with stiff ODE/DAE solvers such as ode15s or inside a Simulink block (e.g., MATLAB Function block).

function dx = closed_chain_dynamics(t, x, param)
% State x = [q; qdot]

n = numel(x) / 2;
q = x(1:n);
qdot = x(n+1:end);

[Mq, h]       = compute_dynamics(q, qdot, param);   % h = C(q,qdot) qdot + g(q)
[Jc, Jcdotqd] = compute_constraints(q, qdot, param);
Sa            = selection_matrix(param);           % n_a x n
tau           = actuator_torques(t, q, qdot, param);

m = size(Jc, 1);

K  = zeros(n + m, n + m);
rhs = zeros(n + m, 1);

K(1:n, 1:n)       = Mq;
K(1:n, n+1:end)   = -Jc.';
K(n+1:end, 1:n)   = Jc;

rhs(1:n)          = Sa.' * tau - h;
rhs(n+1:end)      = -Jcdotqd;

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

dx        = zeros(size(x));
dx(1:n)   = qdot;
dx(n+1:end) = qddot;
end
      

In Simulink, one can connect this dynamic block with position and velocity feedback that projects the state back onto the constraint manifold. Alternatively, minimal-coordinate models can be implemented as pure ODEs using the reduced inertia \( \mathbf{M}_r(\mathbf{q}_r) \).

Wolfram Mathematica is well suited for symbolic derivation and numerical solution of constrained dynamics. Below is a schematic implementation for a planar four-bar, where phi[q1, q2, q3, q4] == 0 encodes the two holonomic constraints, and lambda[t] are the Lagrange multipliers:

(* Symbolic setup *)
Clear[q1, q2, q3, q4, t];
q1 = q1[t]; q2 = q2[t]; q3 = q3[t]; q4 = q4[t];

(* Example: placeholder kinetic and potential energies *)
T = 1/2 m1 l1^2 D[q1, t]^2 + 1/2 m2 l2^2 D[q2, t]^2 +
    1/2 m3 l3^2 D[q3, t]^2 + 1/2 m4 l4^2 D[q4, t]^2;

V = m1 g l1 Cos[q1] + m2 g l2 Cos[q2] +
    m3 g l3 Cos[q3] + m4 g l4 Cos[q4];

L = T - V;

(* Constraints phi1 == 0, phi2 == 0 *)
phi1 = l1 Cos[q1] + l2 Cos[q2] - l3 Cos[q3] - l4 Cos[q4];
phi2 = l1 Sin[q1] + l2 Sin[q2] - l3 Sin[q3] - l4 Sin[q4];

lambda1 = lambda1[t];
lambda2 = lambda2[t];

(* Lagrange equations with multipliers *)
eqs = Table[
   D[D[L, D[qi, t]], t] - D[L, qi] +
     If[i <= 2,
       D[phi1, qi] lambda1 + D[phi2, qi] lambda2,
       0
     ] == tau[i][t],
   {i, 1, 4} /. {q1 -> q1, q2 -> q2, q3 -> q3, q4 -> q4} /. 
     {qi -> {q1, q2, q3, q4}[[i]]}
];

(* Add constraint equations at position level *)
constr = {phi1 == 0, phi2 == 0};

(* Numerical solution with NDSolve: specify parameters, torques, initial conditions *)
sol = NDSolve[
   Join[eqs /. tau[1][t] -> tau1fun[t] /. tau[2][t] -> 0 /. tau[3][t] -> 0 /. tau[4][t] -> 0,
        constr,
        (* Initial conditions consistent with constraints *)
        {q1[0] == q10, q2[0] == q20, q3[0] == q30, q4[0] == q40,
         q1'[0] == dq10, q2'[0] == dq20, q3'[0] == dq30, q4'[0] == dq40}],
   {q1, q2, q3, q4, lambda1, lambda2},
   {t, 0, tEnd}
];
      

Mathematica can also be used symbolically to obtain closed-form expressions for \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{J}_c(\mathbf{q}) \), and reduced inertia matrices, which can then be exported to other languages.

In this lesson we extended the standard robot dynamics formulation to mechanisms with closed kinematic loops, such as parallel robots and four-bar linkages. Holonomic loop-closure constraints introduce Lagrange multipliers and lead to saddle-point systems coupling accelerations and constraint forces. We showed how these systems can be interpreted as DAEs, how null-space projection and reduced-coordinate mappings recover minimal-coordinate dynamics, and how energy structure is preserved through the reduced inertia matrix \( \mathbf{M}_r(\mathbf{q}_r) \).

Finally, we outlined implementation strategies in Python, C++, Java, MATLAB/Simulink, and Mathematica that leverage rigid-body dynamics libraries and linear-algebra routines to simulate parallel robot dynamics. In the next lesson, these tools will be applied to a concrete parallel manipulator, the Stewart platform.