A closed-chain mechanism has at least one kinematic loop: starting from a point on the mechanism and following connected rigid bodies and joints, one can return to the starting point without leaving the structure. The configuration coordinates of such systems are constrained by holonomic equations of the form

\[ \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0}, \quad \boldsymbol{\phi} : \mathbb{R}^n \rightarrow \mathbb{R}^m,\quad m \le n, \]

where \( \mathbf{q} \) collects generalized coordinates, and \( \boldsymbol{\phi} \) encodes loop-closure conditions. The constraint Jacobian is

\[ \mathbf{J}_c(\mathbf{q}) = \frac{\partial\boldsymbol{\phi}(\mathbf{q})}{\partial \mathbf{q}} \in \mathbb{R}^{m \times n}, \]

and for consistent closed-chain motion we require the velocity and acceleration constraints

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

Together with the unconstrained robot dynamics

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

the constrained dynamics with Lagrange multipliers becomes

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

with unknown constraint forces encoded by \( \boldsymbol{\lambda} \). At each time instant we solve a coupled linear system for \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \).

flowchart TD
  A["Choose mechanism (4-bar, parallel wrist)"] --> B["Select generalized coordinates q"]
  B --> C["Derive loop-closure equations phi(q) = 0"]
  C --> D["Compute constraint Jacobian J_c(q)"]
  D --> E["Form constrained dynamics: M(q) qdd + h(q,qdot) + J_c^T lambda = tau"]
  E --> F["Solve block system for qdd and lambda each step"]
  F --> G["Integrate qdot, q while enforcing constraints"]
        

Consider a classical planar four-bar linkage with link lengths \( a,b,c,d > 0 \). The ground link connects fixed pivots \( A \) and \( D \) at distance \( d \). The actuated crank has length \( a \) and angle \( \theta_2 \) about \( A \), the coupler has length \( b \) and angle \( \theta_3 \), and the rocker has length \( c \) and angle \( \theta_4 \) about \( D \).

Placing \( A \) at the origin and \( D \) at \( (d,0) \), the position of joint \( B \) (end of crank) is

\[ \mathbf{p}_B = \begin{bmatrix} a\cos\theta_2 \\ a\sin\theta_2 \end{bmatrix}, \]

and the position of joint \( C \) can be computed in two ways: through the coupler and through the rocker:

\[ \mathbf{p}_C^{(1)} = \mathbf{p}_B + \begin{bmatrix} b\cos\theta_3 \\ b\sin\theta_3 \end{bmatrix}, \qquad \mathbf{p}_C^{(2)} = \begin{bmatrix} d \\ 0 \end{bmatrix} + \begin{bmatrix} c\cos\theta_4 \\ c\sin\theta_4 \end{bmatrix}. \]

Loop closure requires \( \mathbf{p}_C^{(1)} = \mathbf{p}_C^{(2)} \), yielding two scalar holonomic constraints:

\[ \boldsymbol{\phi}(\mathbf{q}) = \begin{bmatrix} a\cos\theta_2 + b\cos\theta_3 - d - c\cos\theta_4 \\ a\sin\theta_2 + b\sin\theta_3 - c\sin\theta_4 \end{bmatrix} = \mathbf{0}, \quad \mathbf{q} = \begin{bmatrix} \theta_2 \\ \theta_3 \\ \theta_4 \end{bmatrix}. \]

The constraint Jacobian is

\[ \mathbf{J}_c(\mathbf{q}) = \frac{\partial\boldsymbol{\phi}}{\partial\mathbf{q}} = \begin{bmatrix} -a\sin\theta_2 & -b\sin\theta_3 & c\sin\theta_4 \\ a\cos\theta_2 & b\cos\theta_3 & -c\cos\theta_4 \end{bmatrix}. \]

Since \( n = 3 \) and \( m = 2 \), the mobility is \( n - m = 1 \): the 4-bar has a single degree of freedom, typically chosen as the crank angle \( \theta_2 \). The passive angles \( \theta_3,\theta_4 \) are algebraic functions of \( \theta_2 \) determined by the above constraints.

Treat the 4-bar as a 3-DOF open-chain planar manipulator with coordinates \( \mathbf{q}=[\theta_2,\theta_3,\theta_4]^{\top} \), subject to the two constraints \( \boldsymbol{\phi}(\mathbf{q})=\mathbf{0} \). Using the Lagrange–Euler derivation from previous chapters, the unconstrained dynamics is

\[ \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} \in \mathbb{R}^{3\times 3} \) is the inertia matrix, \( \mathbf{C}\dot{\mathbf{q}} \) collects Coriolis/centrifugal terms, and \( \mathbf{g} \) is gravity. In compact notation \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} \).

The constrained dynamics with Lagrange multipliers becomes

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

together with the acceleration-level constraint

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

Collecting unknowns \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \), we obtain the KKT system (Karush–Kuhn–Tucker-like saddle-point system)

\[ \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}} \\[4pt] \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \\[4pt] -\dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} \end{bmatrix}. \]

For a 4-bar, \( \mathbf{M} \in \mathbb{R}^{3\times 3} \) and \( \mathbf{J}_c \in \mathbb{R}^{2\times 3} \), so the block system is \( 5\times 5 \). The constraint forces in the joints can be recovered from the multiplier vector \( \boldsymbol{\lambda} \).

flowchart TD
  S["Given q, qdot, tau"] --> A["Assemble M(q), h(q,qdot), J_c(q), Jdot_c(q,qdot)"]
  A --> B["Solve block linear system for qdd and lambda"]
  B --> C["Update state: qdot_new, q_new via integration"]
  C --> D["Optionally project back to constraint manifold phi(q)=0"]
        

An alternative is to eliminate the passive coordinates explicitly. Choose the independent coordinate \( q_r = \theta_2 \) and denote the dependent coordinates \( \mathbf{q}_d = [\theta_3,\theta_4]^{\top} \). Locally (away from singular configurations) the implicit function theorem guarantees

\[ \mathbf{q}_d = \mathbf{f}(q_r), \quad \dot{\mathbf{q}}_d = \mathbf{J}_d(q_r)\,\dot{q}_r, \quad \ddot{\mathbf{q}}_d = \mathbf{J}_d(q_r)\,\ddot{q}_r + \dot{\mathbf{J}}_d(q_r,\dot{q}_r)\,\dot{q}_r, \]

for some Jacobian \( \mathbf{J}_d = \frac{\partial \mathbf{f}}{\partial q_r} \). Let the full coordinate vector be

\[ \mathbf{q} = \begin{bmatrix} q_r \\ \mathbf{q}_d \end{bmatrix}, \quad \dot{\mathbf{q}} = \mathbf{S}(q_r)\,\dot{q}_r, \quad \ddot{\mathbf{q}} = \mathbf{S}(q_r)\,\ddot{q}_r + \dot{\mathbf{S}}(q_r,\dot{q}_r)\,\dot{q}_r, \]

where the selection matrix \( \mathbf{S}(q_r) \in \mathbb{R}^{3\times 1} \) is

\[ \mathbf{S}(q_r) = \begin{bmatrix} 1 \\ \mathbf{J}_d(q_r) \end{bmatrix}. \]

Substituting into the unconstrained dynamics and premultiplying by \( \mathbf{S}(q_r)^{\top} \) yields a scalar reduced-coordinate equation

\[ M_r(q_r)\,\ddot{q}_r + h_r(q_r,\dot{q}_r) = \tau_r, \]

where

\[ M_r(q_r) = \mathbf{S}(q_r)^{\top}\mathbf{M}(\mathbf{q})\,\mathbf{S}(q_r), \qquad h_r(q_r,\dot{q}_r) = \mathbf{S}(q_r)^{\top} \bigl(\mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) - \mathbf{M}(\mathbf{q})\,\dot{\mathbf{S}}(q_r,\dot{q}_r)\,\dot{q}_r\bigr), \]

and \( \tau_r \) is the torque in the actuated joint. This reduced formulation removes the multipliers but requires solving the nonlinear constraints for \( \mathbf{q}_d \) at each evaluation. Numerically, both formulations can be used; the choice depends on implementation convenience and conditioning.

A parallel wrist (spherical parallel manipulator) is a 3-degree-of-freedom mechanism whose end-effector is constrained to pure rotation about a fixed point. Many architectures exist (e.g., 3-RRR, 3-RUS, Agile Eye). A common modeling pattern is:

• The platform orientation is parameterized by a minimal 3D representation, such as Z–Y–X Euler angles \( \mathbf{q}_p = [\phi,\theta,\psi]^{\top} \).
• Each leg has one or more actuated joints with coordinates \( \mathbf{q}_{a,i} \) and additional passive joints \( \mathbf{q}_{d,i} \).
• Loop-closure constraints enforce coincidence of platform anchor points represented in base coordinates.

Denote the full generalized coordinate vector

\[ \mathbf{q} = \begin{bmatrix} \mathbf{q}_p \\ \mathbf{q}_a \\ \mathbf{q}_d \end{bmatrix} \in \mathbb{R}^n, \]

where \( \mathbf{q}_a \) collects all actuated joint angles in the legs and \( \mathbf{q}_d \) collects the passive ones. For each leg \( i \), a loop-closure vector constraint has the form

\[ \boldsymbol{\phi}_i(\mathbf{q}_p,\mathbf{q}_{a,i},\mathbf{q}_{d,i}) = \mathbf{0}, \]

expressing that the position of the connection point on the platform, rotated by \( \mathbf{q}_p \), equals the position reachable by the leg. Stacking all leg constraints,

\[ \boldsymbol{\phi}(\mathbf{q}) = \begin{bmatrix} \boldsymbol{\phi}_1 \\ \boldsymbol{\phi}_2 \\ \boldsymbol{\phi}_3 \end{bmatrix} = \mathbf{0}, \quad \mathbf{J}_c(\mathbf{q}) = \frac{\partial\boldsymbol{\phi}}{\partial\mathbf{q}} = \begin{bmatrix} \mathbf{J}_{p} & \mathbf{J}_{a} & \mathbf{J}_{d} \end{bmatrix}. \]

Typically, the number of independent actuated variables \( n_a \) is 3, matching the platform orientation DOF. The remaining coordinates are algebraically constrained, giving a closed-chain system similar in structure to the 4-bar, but embedded in 3D.

Partition the generalized coordinates for a parallel wrist into independent (actuated + platform) coordinates \( \mathbf{q}_I \) and dependent coordinates \( \mathbf{q}_D \):

\[ \mathbf{q} = \begin{bmatrix} \mathbf{q}_I \\ \mathbf{q}_D \end{bmatrix}, \qquad \boldsymbol{\tau} = \begin{bmatrix} \boldsymbol{\tau}_I \\ \mathbf{0} \end{bmatrix}, \]

where no actuator torque acts directly on the dependent coordinates. The full unconstrained dynamics is

\[ \begin{bmatrix} \mathbf{M}_{II} & \mathbf{M}_{ID} \\ \mathbf{M}_{DI} & \mathbf{M}_{DD} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}}_I \\ \ddot{\mathbf{q}}_D \end{bmatrix} + \begin{bmatrix} \mathbf{h}_I \\ \mathbf{h}_D \end{bmatrix} + \begin{bmatrix} \mathbf{J}_{c,I}^{\top} \\ \mathbf{J}_{c,D}^{\top} \end{bmatrix} \boldsymbol{\lambda} = \begin{bmatrix} \boldsymbol{\tau}_I \\ \mathbf{0} \end{bmatrix}. \]

Velocity-level constraints are

\[ \mathbf{J}_{c,I}\,\dot{\mathbf{q}}_I + \mathbf{J}_{c,D}\,\dot{\mathbf{q}}_D = \mathbf{0}, \]

and, differentiating once more,

\[ \mathbf{J}_{c,I}\,\ddot{\mathbf{q}}_I + \mathbf{J}_{c,D}\,\ddot{\mathbf{q}}_D + \dot{\mathbf{J}}_c\,\dot{\mathbf{q}} = \mathbf{0}. \]

Assuming \( \mathbf{J}_{c,D} \) is nonsingular, the acceleration constraint gives

\[ \ddot{\mathbf{q}}_D = -\mathbf{J}_{c,D}^{-1}\mathbf{J}_{c,I}\,\ddot{\mathbf{q}}_I - \mathbf{J}_{c,D}^{-1}\dot{\mathbf{J}}_c\,\dot{\mathbf{q}}. \]

Substituting into the dynamics eliminates \( \ddot{\mathbf{q}}_D \) and yields a reduced set of equations in \( \ddot{\mathbf{q}}_I \) and the multipliers \( \boldsymbol{\lambda} \). This procedure generalizes the 4-bar reduced-coordinate approach to a multi-loop, 3D parallel wrist. In practice, the computation relies heavily on the rotation matrix relationships learned in earlier chapters and on efficient numerical linear algebra.

We now implement a simple numerical step for the 4-bar constrained dynamics using Python, numpy for linear algebra, and a basic time-stepping loop. For brevity, we model the 4-bar links as point masses at their centers with planar rotation about z.

import numpy as np

# Four-bar geometric parameters
a, b, c, d = 0.2, 0.4, 0.3, 0.5  # [m]

# Link inertias about joint axes (simple approx: I = m * l^2 / 12)
m2, m3, m4 = 1.0, 1.0, 1.0
I2 = m2 * a**2 / 12.0
I3 = m3 * b**2 / 12.0
I4 = m4 * c**2 / 12.0
g  = 9.81

def constraints(q):
    """Loop-closure constraints phi(q) in R^2."""
    th2, th3, th4 = q
    phi1 = a * np.cos(th2) + b * np.cos(th3) - d - c * np.cos(th4)
    phi2 = a * np.sin(th2) + b * np.sin(th3) - c * np.sin(th4)
    return np.array([phi1, phi2])

def Jc(q):
    """Constraint Jacobian J_c(q) in R^{2x3}."""
    th2, th3, th4 = q
    return np.array([
        [-a * np.sin(th2), -b * np.sin(th3),  c * np.sin(th4)],
        [ a * np.cos(th2),  b * np.cos(th3), -c * np.cos(th4)]
    ])

def M(q):
    """Inertia matrix M(q) for planar 4-bar (very simplified)."""
    # Here we neglect coupling between joints for illustration and
    # use a diagonal inertia matrix.
    return np.diag([I2, I3, I4])

def h(q, qdot):
    """Coriolis + gravity vector h(q, qdot)."""
    th2, th3, th4 = q
    # Simple gravity acting at midpoints of links, projected to joint torques.
    # This is not exact but suffices to illustrate constraint handling.
    tau2_g = -m2 * g * (a / 2.0) * np.cos(th2)
    tau3_g = -m3 * g * (b / 2.0) * np.cos(th3)
    tau4_g = -m4 * g * (c / 2.0) * np.cos(th4)
    return np.array([tau2_g, tau3_g, tau4_g])

def Jc_dot(q, qdot):
    """Time derivative of J_c(q)."""
    th2, th3, th4 = q
    th2d, th3d, th4d = qdot
    # Differentiate Jc analytically
    J = Jc(q)
    Jdot = np.zeros_like(J)
    # d/dt [-a sin(th2)] = -a cos(th2) * th2d, etc.
    Jdot[0, 0] = -a * np.cos(th2) * th2d
    Jdot[0, 1] = -b * np.cos(th3) * th3d
    Jdot[0, 2] =  c * np.cos(th4) * th4d
    Jdot[1, 0] = -a * np.sin(th2) * th2d
    Jdot[1, 1] = -b * np.sin(th3) * th3d
    Jdot[1, 2] = -c * np.sin(th4) * th4d
    return Jdot

def constrained_dynamics_step(q, qdot, tau, dt):
    """
    Single time step of index-1 DAE integration:
    solve block system for qdd, lambda, then integrate.
    """
    Mc = M(q)
    hc = h(q, qdot)
    J = Jc(q)
    Jdot = Jc_dot(q, qdot)

    # Build KKT system
    Z = np.zeros((2, 2))
    K_top = np.hstack((Mc, J.T))
    K_bot = np.hstack((J,  Z))
    K = np.vstack((K_top, K_bot))

    rhs_top = tau - hc
    rhs_bot = -Jdot @ qdot
    rhs = np.concatenate((rhs_top, rhs_bot))

    sol = np.linalg.solve(K, rhs)
    qdd = sol[:3]
    lam = sol[3:]

    # Explicit Euler step (for simplicity)
    qdot_new = qdot + dt * qdd
    q_new = q + dt * qdot_new

    # Optional: project back to constraint manifold (simple Newton step)
    for _ in range(2):
        phi = constraints(q_new)
        Jn = Jc(q_new)
        dq = np.linalg.lstsq(Jn.T @ Jn, -Jn.T @ phi, rcond=None)[0]
        q_new = q_new + dq

    return q_new, qdot_new, qdd, lam

if __name__ == "__main__":
    # Initial consistent configuration (crank at 10 degrees)
    q = np.array([np.deg2rad(10.0), np.deg2rad(60.0), np.deg2rad(30.0)])
    qdot = np.zeros(3)
    tau = np.array([1.0, 0.0, 0.0])  # drive only the crank joint
    dt = 1e-3

    for k in range(1000):
        q, qdot, qdd, lam = constrained_dynamics_step(q, qdot, tau, dt)
        if k % 100 == 0:
            print(f"step {k}: th2 = {np.rad2deg(q[0]):.2f} deg, lambda = {lam}")
      

This example focuses on how to enforce kinematic constraints during dynamics integration. A full implementation would use more accurate inertial models and a higher-order integration scheme.

C++ is widely used in real-time robotic systems. Here we show a generic routine that takes an inertia matrix, bias vector, constraint Jacobian, and right-hand sides to solve the KKT system at each step. We use the Eigen library for linear algebra.

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

using Eigen::MatrixXd;
using Eigen::VectorXd;

// Solve KKT system:
// [ M  J^T ] [ qdd ]   = [ tau - h ]
// [ J   0  ] [ lam ]     [   -gamma ]
// where gamma = Jdot * qdot for holonomic constraints.
struct KKTResult {
    VectorXd qdd;
    VectorXd lambda;
};

KKTResult solveKKT(const MatrixXd& M,
                   const MatrixXd& J,
                   const VectorXd& tau_minus_h,
                   const VectorXd& gamma)
{
    int n = static_cast<int>(M.rows());
    int m = static_cast<int>(J.rows());

    MatrixXd K(n + m, n + m);
    K.setZero();
    K.block(0, 0, n, n) = M;
    K.block(0, n, n, m) = J.transpose();
    K.block(n, 0, m, n) = J;

    VectorXd rhs(n + m);
    rhs.segment(0, n) = tau_minus_h;
    rhs.segment(n, m) = -gamma;

    VectorXd sol = K.fullPivLu().solve(rhs);

    KKTResult res;
    res.qdd    = sol.segment(0, n);
    res.lambda = sol.segment(n, m);
    return res;
}

int main()
{
    // Example sizes for a 4-bar: n = 3, m = 2
    MatrixXd M = MatrixXd::Identity(3, 3);
    MatrixXd J(2, 3);
    J << 1.0, 0.0, 0.0,
          0.0, 1.0, 1.0;

    VectorXd tau_minus_h(3);
    tau_minus_h << 1.0, 0.0, 0.0;

    VectorXd gamma(2);
    gamma.setZero();

    KKTResult res = solveKKT(M, J, tau_minus_h, gamma);

    std::cout << "qdd = " << res.qdd.transpose() << std::endl;
    std::cout << "lambda = " << res.lambda.transpose() << std::endl;

    return 0;
}
      

For a specific mechanism, the functions that assemble \( \mathbf{M} \), \( \mathbf{h} \), \( \mathbf{J}_c \), and \( \gamma \) (constraint acceleration right-hand side) are generated from the underlying geometry, using either hand-derived expressions or symbolic tools.

Java is less common for low-level robotics dynamics but is useful for simulation and teaching. Below is a simple class that evaluates the 4-bar constraints and their Jacobian at a given configuration. A numerical time-stepping scheme can then use a linear solver (e.g., Apache Commons Math) to implement the KKT step.

public class FourBarConstraints {

    private final double a, b, c, d;

    public FourBarConstraints(double a, double b, double c, double d) {
        this.a = a;
        this.b = b;
        this.c = c;
        this.d = d;
    }

    // q = [th2, th3, th4]
    public double[] phi(double[] q) {
        double th2 = q[0];
        double th3 = q[1];
        double th4 = q[2];

        double phi1 = a * Math.cos(th2) + b * Math.cos(th3) - d - c * Math.cos(th4);
        double phi2 = a * Math.sin(th2) + b * Math.sin(th3) - c * Math.sin(th4);
        return new double[]{phi1, phi2};
    }

    // Jc(q) in row-major order, size 2x3
    public double[][] Jc(double[] q) {
        double th2 = q[0];
        double th3 = q[1];
        double th4 = q[2];

        double[][] J = new double[2][3];
        J[0][0] = -a * Math.sin(th2);
        J[0][1] = -b * Math.sin(th3);
        J[0][2] =  c * Math.sin(th4);

        J[1][0] =  a * Math.cos(th2);
        J[1][1] =  b * Math.cos(th3);
        J[1][2] = -c * Math.cos(th4);
        return J;
    }

    public static void main(String[] args) {
        FourBarConstraints fb = new FourBarConstraints(0.2, 0.4, 0.3, 0.5);
        double[] q = {Math.toRadians(10.0), Math.toRadians(60.0), Math.toRadians(30.0)};
        double[] phi = fb.phi(q);
        System.out.printf("phi1 = %.6f, phi2 = %.6f%n", phi[0], phi[1]);

        double[][] J = fb.Jc(q);
        System.out.printf("Jc = [%f %f %f; %f %f %f]%n",
                J[0][0], J[0][1], J[0][2],
                J[1][0], J[1][1], J[1][2]);
    }
}
      

This code focuses on the constraint geometry; dynamics would additionally require a mass matrix builder and a linear solver for the KKT system, analogous to the C++ and Python implementations.

MATLAB and Simulink are classical environments for closed-chain modeling. Below is a MATLAB function that evaluates \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \), and \( \mathbf{J}_c(\mathbf{q}) \) for use in either an ODE/DAE solver or a Simulink S-Function block.

function [M,h,Jc,Jcdot] = fourbar_dynamics_matrices(q, qdot, params)
% FOURBAR_DYNAMICS_MATRICES  Planar 4-bar building blocks.
%   q    : [th2; th3; th4]
%   qdot : [th2dot; th3dot; th4dot]
%   params: struct with fields a,b,c,d,m2,m3,m4,g

a = params.a; b = params.b; c = params.c; d = params.d;
m2 = params.m2; m3 = params.m3; m4 = params.m4; g = params.g;

th2 = q(1); th3 = q(2); th4 = q(3);
th2d = qdot(1); th3d = qdot(2); th4d = qdot(3);

% Inertias (simple approximation)
I2 = m2 * a^2 / 12;
I3 = m3 * b^2 / 12;
I4 = m4 * c^2 / 12;

M = diag([I2 I3 I4]);

% Gravity torques
tau2_g = -m2 * g * (a/2) * cos(th2);
tau3_g = -m3 * g * (b/2) * cos(th3);
tau4_g = -m4 * g * (c/2) * cos(th4);
h = [tau2_g; tau3_g; tau4_g];

% Constraint Jacobian
Jc = [ -a*sin(th2), -b*sin(th3),  c*sin(th4);
        a*cos(th2),  b*cos(th3), -c*cos(th4) ];

% Time derivative Jcdot
Jcdot = zeros(2,3);
Jcdot(1,1) = -a*cos(th2)*th2d;
Jcdot(1,2) = -b*cos(th3)*th3d;
Jcdot(1,3) =  c*cos(th4)*th4d;
Jcdot(2,1) = -a*sin(th2)*th2d;
Jcdot(2,2) = -b*sin(th3)*th3d;
Jcdot(2,3) = -c*sin(th4)*th4d;
      

In Simulink, one can implement the KKT solve in a MATLAB Function block that calls fourbar_dynamics_matrices, assembles the block system, and outputs \( \ddot{\mathbf{q}} \) and the constraint forces. This provides a graphical environment for coupling the 4-bar dynamics with controllers designed in later courses.

Wolfram Mathematica is powerful for symbolic derivation of closed-chain dynamics. The snippet below defines the 4-bar constraints symbolically and computes the Jacobian and its time derivative using the chain rule.

(* Four-bar parameters *)
ClearAll["Global`*"];
a = Symbol["a"]; b = Symbol["b"]; c = Symbol["c"]; d = Symbol["d"];

(* Generalized coordinates and derivatives *)
th2[t_] := Symbol["th2"][t];
th3[t_] := Symbol["th3"][t];
th4[t_] := Symbol["th4"][t];

q[t_] := {th2[t], th3[t], th4[t]};

phi[t_] := {
  a*Cos[th2[t]] + b*Cos[th3[t]] - d - c*Cos[th4[t]],
  a*Sin[th2[t]] + b*Sin[th3[t]] - c*Sin[th4[t]]
};

(* Constraint Jacobian Jc(q) *)
Jc[t_] := D[phi[t], {q[t]}];

(* Time derivative Jcdot(q,qdot) using total derivative *)
Jcdot[t_] := D[Jc[t], t];

(* Evaluate symbolic expressions at a configuration *)
th2Val = 10 Degree; th3Val = 60 Degree; th4Val = 30 Degree;

JcNumeric = Jc[t] /. {
   th2[t] -> th2Val, th3[t] -> th3Val, th4[t] -> th4Val
};
JcNumeric // MatrixForm
      

With further effort, one can derive the full mass matrix, Coriolis terms, and gravity vector symbolically and export the resulting expressions to C, C++, or Python for high-performance simulation of the closed-chain dynamics.

In this lesson we instantiated the general theory of constrained and closed-chain dynamics on two important mechanism classes: the planar 4-bar linkage and parallel wrist (spherical parallel) robots. For the 4-bar, we derived explicit holonomic constraints, their Jacobian, and the KKT formulation that couples joint accelerations with constraint forces. We contrasted the Lagrange multiplier approach with a reduced-coordinate formulation based on eliminating passive joints.

For parallel wrists, we formalized the loop-closure equations at the leg level and showed how partitioning the generalized coordinates into independent and dependent subsets leads to a reduced dynamic system and clarifies the role of constraint Jacobian blocks in detecting singularities. Implementation snippets in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrated how these equations can be realized numerically, paving the way for simulation and, in later chapters, for model-based control of parallel robots.