Consider an \( n \)-DOF manipulator with generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \). In earlier lessons you have seen that a wrench \( \mathbf{w} \in \mathbb{R}^6 \) applied at a frame whose spatial Jacobian is \( \mathbf{J}( \mathbf{q} ) \in \mathbb{R}^{6 \times n} \) induces the joint torque vector

\[ \boldsymbol{\tau}_{\text{wrench}}(\mathbf{q},\mathbf{w}) = \mathbf{J}(\mathbf{q})^{\top} \mathbf{w}. \]

Let \( V(\mathbf{q}) \) be the total potential energy of the manipulator (typically dominated by gravity). The associated generalized force from potential energy is \( -\nabla_{\mathbf{q}} V(\mathbf{q}) \). If \( \boldsymbol{\tau}_a \) denotes the joint torques commanded by the actuators and \( \{\mathbf{w}_i\} \) are external wrenches acting at known frames with Jacobians \( \{\mathbf{J}_i(\mathbf{q})\} \), the unconstrained static equilibrium condition is

\[ \boldsymbol{\tau}_a - \nabla_{\mathbf{q}} V(\mathbf{q}) - \sum_i \mathbf{J}_i(\mathbf{q})^{\top} \mathbf{w}_i = \mathbf{0}. \]

This expresses that the sum of all generalized forces (actuation, gravity, and mapped wrenches) vanishes; equivalently, the virtual work of all forces is zero for any virtual displacement \( \delta \mathbf{q} \).

In many practical tasks, the manipulator is not free: the end-effector is pressed against a surface, a loop closure imposes geometric constraints, or some joints are mechanically linked. These situations are modeled as constraints on \( \mathbf{q} \), and they introduce additional reaction forces or constraint wrenches into the equilibrium equations.

Let us consider holonomic constraints expressed as smooth functions \( \boldsymbol{\phi}(\mathbf{q}) \in \mathbb{R}^k \) with \( \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0} \) at admissible configurations. Examples:

• Loop closure: the distance between two frames is fixed.
• Environmental contact: the end-effector position lies on a surface.
• Joint coupling: a linear relation such as \( q_3 - q_2 = \text{const} \).

The Jacobian of the constraints is \( \mathbf{J}_{\phi}(\mathbf{q}) = \dfrac{\partial \boldsymbol{\phi}}{\partial \mathbf{q}} \in \mathbb{R}^{k \times n} \). A virtual displacement \( \delta \mathbf{q} \) is admissible iff it satisfies the linearized constraints

\[ \mathbf{J}_{\phi}(\mathbf{q})\,\delta \mathbf{q} = \mathbf{0}. \]

For static equilibrium, the principle of virtual work in the presence of the constraints states that for every admissible \( \delta \mathbf{q} \)

\[ \delta W = \Big( \boldsymbol{\tau}_a - \nabla_{\mathbf{q}} V(\mathbf{q}) - \sum_i \mathbf{J}_i(\mathbf{q})^{\top} \mathbf{w}_i \Big)^{\top} \delta \mathbf{q} = 0. \]

Denote the total generalized force (excluding reactions) by \( \mathbf{f}_g(\mathbf{q}) \):

\[ \mathbf{f}_g(\mathbf{q}) = \boldsymbol{\tau}_a - \nabla_{\mathbf{q}} V(\mathbf{q}) - \sum_i \mathbf{J}_i(\mathbf{q})^{\top} \mathbf{w}_i. \]

Then the virtual-work statement is \( \mathbf{f}_g(\mathbf{q})^{\top} \delta \mathbf{q} = 0 \) for all \( \delta \mathbf{q} \) such that \( \mathbf{J}_{\phi}(\mathbf{q})\,\delta \mathbf{q} = \mathbf{0} \).

A classical linear-algebra fact is that the set of vectors orthogonal to the nullspace of \( \mathbf{J}_{\phi}(\mathbf{q}) \) is exactly the column space of \( \mathbf{J}_{\phi}(\mathbf{q})^{\top} \). Hence

\[ \mathbf{f}_g(\mathbf{q}) \in \operatorname{Range}\big( \mathbf{J}_{\phi}(\mathbf{q})^{\top} \big). \]

Therefore, there exists a vector of Lagrange multipliers \( \boldsymbol{\lambda} \in \mathbb{R}^k \) such that

\[ \mathbf{f}_g(\mathbf{q}) = \mathbf{J}_{\phi}(\mathbf{q})^{\top} \boldsymbol{\lambda}. \]

Substituting back, the constrained static equilibrium equations become

\[ \boldsymbol{\tau}_a - \nabla_{\mathbf{q}} V(\mathbf{q}) - \sum_i \mathbf{J}_i(\mathbf{q})^{\top} \mathbf{w}_i = \mathbf{J}_{\phi}(\mathbf{q})^{\top} \boldsymbol{\lambda}, \quad \boldsymbol{\phi}(\mathbf{q}) = \mathbf{0}. \]

The term \( \mathbf{J}_{\phi}(\mathbf{q})^{\top} \boldsymbol{\lambda} \) is the generalized reaction force that enforces the constraints. Each component of \( \boldsymbol{\lambda} \) has the dimension of force (or moment) and represents the intensity of the corresponding constraint reaction.

flowchart TD
  C["Constraint functions phi(q) = 0"] --> JPHI["Compute J_phi(q) = d phi / d q"]
  JPHI --> DELQ["Admissible delta_q satisfy J_phi(q) * delta_q = 0"]
  DELQ --> VW["Virtual work delta_W = (tau_a - grad V - sum J_i^T w_i)^T delta_q"]
  VW --> ORTH["Require delta_W = 0 for all admissible delta_q"]
  ORTH --> RANGE["Conclude total generalized force is in Range(J_phi^T)"]
  RANGE --> LAMBDA["Introduce lambda with tau_a - grad V - sum J_i^T w_i = J_phi^T lambda"]
        

A very common special case of holonomic constraints in robotics is rigid contact between the end-effector and the environment. Suppose the end-effector Cartesian position \( \mathbf{x}(\mathbf{q}) \in \mathbb{R}^3 \) must remain on a smooth surface given implicitly by \( \psi(\mathbf{x}) = 0 \). The composite constraint in joint space is

\[ \phi(\mathbf{q}) = \psi\big(\mathbf{x}(\mathbf{q})\big) = 0. \]

Its Jacobian is

\[ \mathbf{J}_{\phi}(\mathbf{q}) = \frac{\partial \phi}{\partial \mathbf{q}} = \frac{\partial \psi}{\partial \mathbf{x}} \frac{\partial \mathbf{x}}{\partial \mathbf{q}} = \nabla_{\mathbf{x}} \psi(\mathbf{x})^{\top} \mathbf{J}_x(\mathbf{q}), \]

where \( \mathbf{J}_x(\mathbf{q}) \) is the translational part of the manipulator Jacobian.

In rigid, frictionless contact, the reaction force at the contact point is collinear with the surface normal. Let \( \mathbf{n}(\mathbf{x}) \) be the outward unit normal at the contact point. The contact force is then \( \lambda \,\mathbf{n}(\mathbf{x}) \) for some scalar \( \lambda \) (which is typically nonnegative in a compression-only model). The corresponding joint-space reaction torque is

\[ \boldsymbol{\tau}_{\text{contact}} = \mathbf{J}_x(\mathbf{q})^{\top} \big( \lambda\,\mathbf{n}(\mathbf{x}) \big). \]

Comparing this with the general formula \( \mathbf{J}_{\phi}(\mathbf{q})^{\top} \boldsymbol{\lambda} \), we see that Lagrange multipliers are essentially contact forces expressed in the constraint directions.

For multiple contact points, or multi-finger grasps, we stack all contact forces in a vector \( \boldsymbol{\lambda} \) and define a contact Jacobian \( \mathbf{J}_c(\mathbf{q}) \) whose rows select the constrained directions. The total reaction torque becomes

\[ \boldsymbol{\tau}_{\text{reaction}} = \mathbf{J}_c(\mathbf{q})^{\top} \boldsymbol{\lambda}. \]

Static equilibrium with contact then reads

\[ \boldsymbol{\tau}_a - \nabla_{\mathbf{q}} V(\mathbf{q}) - \mathbf{J}(\mathbf{q})^{\top} \mathbf{w}_{\text{ext}} = \mathbf{J}_c(\mathbf{q})^{\top} \boldsymbol{\lambda}, \quad \text{together with the geometric contact constraints}. \]

Here \( \mathbf{J}(\mathbf{q}) \) is an aggregate Jacobian for external wrenches (payloads, applied forces) and \( \mathbf{w}_{\text{ext}} \) is the stacked wrench vector.

In practice, we may know some of the quantities in the equilibrium equations and wish to solve for the remaining ones. Typical scenarios:

• Given a desired contact force \( \boldsymbol{\lambda} \) (e.g. normal force on a surface), compute the joint torques \( \boldsymbol{\tau}_a \) required to realize it.
• Given joint torques and external loads, compute the internal reaction forces \( \boldsymbol{\lambda} \).
• In redundant-contact settings (more contact forces than necessary for equilibrium), characterize internal forces that do not affect the net wrench.

Collect the equilibrium condition in the compact form

\[ \boldsymbol{\tau}_a = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{\top} \mathbf{w}_{\text{ext}} + \mathbf{J}_c(\mathbf{q})^{\top} \boldsymbol{\lambda}, \]

where \( \mathbf{g}(\mathbf{q}) = \nabla_{\mathbf{q}} V(\mathbf{q}) \) is the gravity torque vector (which you will later recover as part of the full dynamics).

If the desired contact forces \( \boldsymbol{\lambda} \) are known (for instance from a grasp planning computation), static joint torques are given directly by the above expression.

Conversely, if \( \boldsymbol{\tau}_a \) is known and \( \boldsymbol{\lambda} \) is unknown, we obtain a linear system

\[ \mathbf{J}_c(\mathbf{q})^{\top} \boldsymbol{\lambda} = \boldsymbol{\tau}_a - \mathbf{g}(\mathbf{q}) - \mathbf{J}(\mathbf{q})^{\top} \mathbf{w}_{\text{ext}}. \]

If \( \mathbf{J}_c(\mathbf{q})^{\top} \) has full column rank, this linear system has a unique solution. In redundant contact (more reaction components than joint DOFs), the system is underdetermined and admits infinitely many solutions

\[ \boldsymbol{\lambda} = \boldsymbol{\lambda}_0 + \mathbf{N}\,\boldsymbol{\eta}, \]

where \( \boldsymbol{\lambda}_0 \) is a particular solution, \( \mathbf{N} \) is a basis for the nullspace of \( \mathbf{J}_c(\mathbf{q})^{\top} \), and \( \boldsymbol{\eta} \) parameterizes internal forces that do not change joint torques.

A common choice is the minimum-norm solution, obtained via the Moore–Penrose pseudoinverse \( \big(\mathbf{J}_c(\mathbf{q})^{\top}\big)^{\dagger} \).

flowchart TD
  IN["Known: q, g(q), J(q), J_c(q), w_ext"] --> CHOICE["Choose which variables are known: \ntau_a or lambda"]
  CHOICE --> CASE1["If lambda is specified: \ncompute tau_a = g(q) + \nJ^T w_ext + J_c^T lambda"]
  CHOICE --> CASE2["If tau_a is specified: \nsolve J_c^T lambda = \ntau_a - g(q) - J^T w_ext"]
  CASE2 --> LS["Use linear solve or \npseudoinverse to get lambda"]
  LS --> INT["Add nullspace components to explore \ninternal forces if needed"]
        

We now implement a simple constrained static equilibrium computation for a planar 2R arm. The arm has link lengths \( \ell_1, \ell_2 \) and joint angles \( q_1, q_2 \). We consider:

• gravity torques from link masses \( m_1, m_2 \),
• an external wrench at the end-effector \( \mathbf{w}_{\text{ext}} = [F_x, F_y, 0, 0, 0, 0]^{\top} \),
• a scalar constraint that the end-effector lies on a vertical line \( x = x_0 \) (e.g. a frictionless wall).

The horizontal position is \( x(\mathbf{q}) = \ell_1 \cos q_1 + \ell_2 \cos(q_1+q_2) \), so the holonomic constraint is \( \phi(\mathbf{q}) = x(\mathbf{q}) - x_0 = 0 \). Its Jacobian is

\[ \mathbf{J}_{\phi}(\mathbf{q}) = \begin{bmatrix} -\ell_1 \sin q_1 - \ell_2 \sin(q_1+q_2) & -\ell_2 \sin(q_1+q_2) \end{bmatrix}. \]

Let \( \lambda \) be the scalar constraint multiplier. We choose a desired normal reaction (e.g. from grasp or contact planning) and compute the required joint torques as

\[ \boldsymbol{\tau}_a = \mathbf{g}(\mathbf{q}) + \mathbf{J}(\mathbf{q})^{\top} \mathbf{w}_{\text{ext}} + \mathbf{J}_{\phi}(\mathbf{q})^{\top}\lambda. \]

5.1 Python (NumPy, optional Robotics Toolbox)

import numpy as np

def planar2R_kinematics(q, l1, l2):
    q1, q2 = q
    x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
    y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
    return np.array([x, y])

def planar2R_jacobian(q, l1, l2):
    q1, q2 = q
    s1 = np.sin(q1)
    c1 = np.cos(q1)
    s12 = np.sin(q1 + q2)
    c12 = np.cos(q1 + q2)
    J = np.zeros((2, 2))
    J[0, 0] = -l1 * s1 - l2 * s12
    J[0, 1] = -l2 * s12
    J[1, 0] =  l1 * c1 + l2 * c12
    J[1, 1] =  l2 * c12
    return J

def planar2R_gravity_torque(q, l1, l2, m1, m2, g=9.81):
    """
    Gravity-only static torques from potential energy.
    Link COMs at l1/2 and l2/2 along each link.
    """
    q1, q2 = q
    # y-coordinates of COMs
    y1 = (l1 / 2.0) * np.sin(q1)
    y2 = l1 * np.sin(q1) + (l2 / 2.0) * np.sin(q1 + q2)
    V = m1 * g * y1 + m2 * g * y2
    # symbolic gradient via finite differences (for clarity)
    eps = 1e-6
    grad = np.zeros(2)
    for i in range(2):
        dq = np.zeros(2)
        dq[i] = eps
        V_plus = planar2R_potential(q + dq, l1, l2, m1, m2, g)
        V_minus = planar2R_potential(q - dq, l1, l2, m1, m2, g)
        grad[i] = (V_plus - V_minus) / (2.0 * eps)
    return grad

def planar2R_potential(q, l1, l2, m1, m2, g):
    q1, q2 = q
    y1 = (l1 / 2.0) * np.sin(q1)
    y2 = l1 * np.sin(q1) + (l2 / 2.0) * np.sin(q1 + q2)
    return m1 * g * y1 + m2 * g * y2

def constrained_static_torque(q, l1, l2, m1, m2,
                              w_ext, lamb):
    """
    q      : [q1, q2]
    w_ext  : 6D spatial wrench at end-effector (Fx, Fy, 0, 0, 0, 0)
    lamb   : scalar constraint multiplier (normal reaction)
    """
    q = np.asarray(q, dtype=float)
    w_ext = np.asarray(w_ext, dtype=float).reshape(6,)
    # gravity
    g_tau = planar2R_gravity_torque(q, l1, l2, m1, m2)
    # Jacobian for planar forces (2x2)
    Jxy = planar2R_jacobian(q, l1, l2)
    # embed planar forces into 6D: [Fx, Fy, 0, 0, 0, 0]
    J = np.zeros((6, 2))
    J[0:2, :] = Jxy
    # external torque
    tau_ext = J.T @ w_ext
    # constraint Jacobian for phi(q) = x(q) - x0
    # only horizontal component
    J_phi = np.array([[-l1 * np.sin(q[0]) - l2 * np.sin(q[0] + q[1]),
                       -l2 * np.sin(q[0] + q[1])]])
    tau_con = J_phi.T * lamb
    tau_a = g_tau + tau_ext + tau_con
    return tau_a

if __name__ == "__main__":
    l1, l2 = 1.0, 0.7
    m1, m2 = 2.0, 1.0
    q = np.deg2rad([40.0, 30.0])
    w_ext = np.array([0.0, -20.0, 0.0, 0.0, 0.0, 0.0])  # downward 20 N
    lamb = 50.0  # desired normal reaction at wall
    tau = constrained_static_torque(q, l1, l2, m1, m2, w_ext, lamb)
    print("Required joint torques:", tau)

# Note: In a full robotics stack you could instead use
# the 'roboticstoolbox-python' library (Corke) to build
# the 2R manipulator, compute Jacobians and static torques,
# and then add constraint reactions as additional generalized forces.
      

5.2 C++ (Eigen + basic robotics computations)

The C++ snippet below uses the Eigen library for linear algebra. In a ROS-based stack, one could also use KDL (Kinematics and Dynamics Library) to obtain Jacobians and then add constraint reactions in the same way.

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

using Eigen::Vector2d;
using Eigen::VectorXd;
using Eigen::Matrix2d;
using Eigen::MatrixXd;

static const double g_acc = 9.81;

Vector2d planar2RGravityTorque(const Vector2d& q,
                               double l1, double l2,
                               double m1, double m2)
{
    double q1 = q(0);
    double q2 = q(1);

    auto potential = [&](const Vector2d& qv) {
        double q1v = qv(0);
        double q2v = qv(1);
        double y1 = (l1 / 2.0) * std::sin(q1v);
        double y2 = l1 * std::sin(q1v) + (l2 / 2.0) * std::sin(q1v + q2v);
        return m1 * g_acc * y1 + m2 * g_acc * y2;
    };

    double eps = 1e-6;
    Vector2d grad;
    for (int i = 0; i < 2; ++i) {
        Vector2d qp = q;
        Vector2d qm = q;
        qp(i) += eps;
        qm(i) -= eps;
        double Vp = potential(qp);
        double Vm = potential(qm);
        grad(i) = (Vp - Vm) / (2.0 * eps);
    }
    return grad;
}

Matrix2d planar2RJacobian(const Vector2d& q,
                          double l1, double l2)
{
    double q1 = q(0);
    double q2 = q(1);
    double s1 = std::sin(q1);
    double c1 = std::cos(q1);
    double s12 = std::sin(q1 + q2);
    double c12 = std::cos(q1 + q2);

    Matrix2d J;
    J(0,0) = -l1 * s1 - l2 * s12;
    J(0,1) = -l2 * s12;
    J(1,0) =  l1 * c1 + l2 * c12;
    J(1,1) =  l2 * c12;
    return J;
}

Vector2d constrainedStaticTorque(const Vector2d& q,
                                 double l1, double l2,
                                 double m1, double m2,
                                 const VectorXd& wrench6,
                                 double lambda)
{
    Vector2d g_tau = planar2RGravityTorque(q, l1, l2, m1, m2);
    Matrix2d Jxy = planar2RJacobian(q, l1, l2);

    MatrixXd J(6, 2);
    J.setZero();
    J.block<2,2>(0,0) = Jxy;

    Vector2d tau_ext = J.transpose() * wrench6;

    Eigen::RowVector2d Jphi;
    double q1 = q(0);
    double q2 = q(1);
    Jphi(0) = -l1 * std::sin(q1) - l2 * std::sin(q1 + q2);
    Jphi(1) = -l2 * std::sin(q1 + q2);

    Vector2d tau_con = Jphi.transpose() * lambda;

    return g_tau + tau_ext + tau_con;
}

int main()
{
    double l1 = 1.0;
    double l2 = 0.7;
    double m1 = 2.0;
    double m2 = 1.0;

    Vector2d q;
    q(0) = 40.0 * M_PI / 180.0;
    q(1) = 30.0 * M_PI / 180.0;

    VectorXd wrench6(6);
    wrench6.setZero();
    wrench6(1) = -20.0; // Fy

    double lambda = 50.0; // normal reaction at wall
    Vector2d tau = constrainedStaticTorque(q, l1, l2, m1, m2, wrench6, lambda);
    std::cout << "Joint torques: " << tau.transpose() << std::endl;
    return 0;
}
      

5.3 Java (simple linear algebra, EJML optional)

Java has several numerical libraries (e.g. EJML) useful in robotic computation stacks. Below, for compactness, we use only primitive arrays and manual operations for the 2D case.

public class Planar2RStatics {

    static double gAcc = 9.81;

    static double potential(double[] q, double l1, double l2,
                            double m1, double m2) {
        double q1 = q[0];
        double q2 = q[1];
        double y1 = (l1 / 2.0) * Math.sin(q1);
        double y2 = l1 * Math.sin(q1) + (l2 / 2.0) * Math.sin(q1 + q2);
        return m1 * gAcc * y1 + m2 * gAcc * y2;
    }

    static double[] gravityTorque(double[] q, double l1, double l2,
                                  double m1, double m2) {
        double eps = 1e-6;
        double[] grad = new double[2];
        for (int i = 0; i < 2; ++i) {
            double[] qp = q.clone();
            double[] qm = q.clone();
            qp[i] += eps;
            qm[i] -= eps;
            double Vp = potential(qp, l1, l2, m1, m2);
            double Vm = potential(qm, l1, l2, m1, m2);
            grad[i] = (Vp - Vm) / (2.0 * eps);
        }
        return grad;
    }

    static double[][] jacobian2R(double[] q, double l1, double l2) {
        double q1 = q[0];
        double q2 = q[1];
        double s1 = Math.sin(q1);
        double c1 = Math.cos(q1);
        double s12 = Math.sin(q1 + q2);
        double c12 = Math.cos(q1 + q2);

        double[][] J = new double[2][2];
        J[0][0] = -l1 * s1 - l2 * s12;
        J[0][1] = -l2 * s12;
        J[1][0] =  l1 * c1 + l2 * c12;
        J[1][1] =  l2 * c12;
        return J;
    }

    static double[] constrainedStaticTorque(double[] q,
                                            double l1, double l2,
                                            double m1, double m2,
                                            double[] wrench6,
                                            double lambda) {
        double[] gTau = gravityTorque(q, l1, l2, m1, m2);
        double[][] Jxy = jacobian2R(q, l1, l2);

        // J is 6x2, only first two rows nonzero
        double[] tauExt = new double[2];
        tauExt[0] = Jxy[0][0] * wrench6[0] + Jxy[1][0] * wrench6[1];
        tauExt[1] = Jxy[0][1] * wrench6[0] + Jxy[1][1] * wrench6[1];

        double q1 = q[0];
        double q2 = q[1];
        double jphi0 = -l1 * Math.sin(q1) - l2 * Math.sin(q1 + q2);
        double jphi1 = -l2 * Math.sin(q1 + q2);

        double[] tauCon = new double[2];
        tauCon[0] = jphi0 * lambda;
        tauCon[1] = jphi1 * lambda;

        double[] tau = new double[2];
        for (int i = 0; i < 2; ++i) {
            tau[i] = gTau[i] + tauExt[i] + tauCon[i];
        }
        return tau;
    }

    public static void main(String[] args) {
        double l1 = 1.0, l2 = 0.7;
        double m1 = 2.0, m2 = 1.0;
        double[] q = {
            Math.toRadians(40.0),
            Math.toRadians(30.0)
        };
        double[] wrench6 = new double[6];
        wrench6[0] = 0.0;   // Fx
        wrench6[1] = -20.0; // Fy
        double lambda = 50.0;

        double[] tau = constrainedStaticTorque(q, l1, l2, m1, m2, wrench6, lambda);
        System.out.println("tau1 = " + tau[0] + ", tau2 = " + tau[1]);
    }
}
      

5.4 MATLAB / Simulink (Robotics System Toolbox)

In MATLAB, the Robotics System Toolbox provides a rigidBodyTree representation of manipulators and automatic computation of Jacobians and static torques (as a special case of inverse dynamics). We illustrate a 2R arm with a contact constraint handled by an explicit Lagrange multiplier.

% Define 2R planar manipulator model
robot = rigidBodyTree("DataFormat","column","MaxNumBodies",3);

L1 = 1.0; L2 = 0.7;
m1 = 2.0; m2 = 1.0;

body1 = rigidBody("link1");
jnt1  = rigidBodyJoint("joint1","revolute");
setFixedTransform(jnt1,trvec2tform([0 0 0]));
jnt1.JointAxis = [0 0 1];
body1.Joint = jnt1;
addBody(robot,body1,"base");

body2 = rigidBody("link2");
jnt2  = rigidBodyJoint("joint2","revolute");
setFixedTransform(jnt2,trvec2tform([L1 0 0]));
jnt2.JointAxis = [0 0 1];
body2.Joint = jnt2;
addBody(robot,body2,"link1");

ee = rigidBody("ee");
setFixedTransform(ee.Joint,trvec2tform([L2 0 0]));
addBody(robot,ee,"link2");

robot.Gravity = [0 -9.81 0];

q = deg2rad([40; 30]);

% End-effector external wrench (Fx, Fy, Fz, Mx, My, Mz)
wrenchExt = [0; -20; 0; 0; 0; 0];

% Spatial Jacobian at end-effector
J = geometricJacobian(robot,q,"ee");   % 6 x n

% Gravity torques (special case of inverse dynamics)
tau_g = inverseDynamics(robot,q,zeros(2,1),zeros(2,1));

% Contact constraint: x(q) = x0 at end-effector (vertical wall)
T_ee = getTransform(robot,q,"ee");
x = T_ee(1,4);     % horizontal position
x0 = x;            % choose current position as constrained one
% Compute derivative of phi(q) = x(q) - x0 with respect to q
Jphi = J(1,:);     % first row corresponds to Fx direction

lambda = 50.0;     % desired normal reaction at wall

tau_contact = Jphi.' * lambda;
tau_ext = J.' * wrenchExt;

tau_a = tau_g + tau_ext + tau_contact;

disp("Static equilibrium torques with constraint:");
disp(tau_a);

% Simulink note:
% A Simulink model can use a Rigid Body Tree block (Simscape Multibody)
% and Joint blocks with zero velocity and acceleration inputs.
% Feeding the same configuration q and external wrench into a
% "Joint Torques" output gives the same tau_a in steady state.
      

5.5 Wolfram Mathematica (symbolic constrained equilibrium)

Finally, we can use Mathematica to derive constrained static equilibrium symbolically via Lagrange multipliers:

(* Define symbols *)
ClearAll[q1, q2, l1, l2, m1, m2, g, Fx, Fy, x0, lambda, tau1, tau2];

(* Kinematics *)
x[q1_, q2_] := l1 Cos[q1] + l2 Cos[q1 + q2];
y[q1_, q2_] := l1 Sin[q1] + l2 Sin[q1 + q2];

(* Potential energy from gravity (planar, gravity in -y) *)
V[q1_, q2_] :=
  m1 g (l1/2 Sin[q1]) +
  m2 g (l1 Sin[q1] + l2/2 Sin[q1 + q2]);

(* External wrench at end-effector *)
wx = Fx; wy = Fy;

(* Planar Jacobian for forces *)
J11 = D[x[q1, q2], q1]; J12 = D[x[q1, q2], q2];
J21 = D[y[q1, q2], q1]; J22 = D[y[q1, q2], q2];

tauExt1 = J11 wx + J21 wy;
tauExt2 = J12 wx + J22 wy;

(* Constraint phi(q) = x(q) - x0 = 0, multiplier lambda *)
phi[q1_, q2_] := x[q1, q2] - x0;
Jphi1 = D[phi[q1, q2], q1];
Jphi2 = D[phi[q1, q2], q2];

(* Equilibrium equations: tau_a - dV/dq + J^T w + Jphi^T lambda = 0 *)
eq1 = tau1 - D[V[q1, q2], q1] + tauExt1 + Jphi1 lambda == 0;
eq2 = tau2 - D[V[q1, q2], q2] + tauExt2 + Jphi2 lambda == 0;

(* Solve for actuator torques tau1, tau2 given lambda and q *)
solTau = Solve[{eq1, eq2}, {tau1, tau2}]

(* Example substitution to evaluate numerically *)
solNumeric = solTau /. {l1 -> 1.0, l2 -> 0.7,
                         m1 -> 2.0, m2 -> 1.0,
                         g -> 9.81,
                         Fx -> 0.0, Fy -> -20.0,
                         lambda -> 50.0,
                         q1 -> 40 Degree, q2 -> 30 Degree};
N[solNumeric]
      

This symbolic perspective is particularly powerful when studying the structure of constrained equations for more complex closed-chain mechanisms and for verifying numerical implementations.

In this lesson we extended manipulator statics to include holonomic constraints such as loop closures and rigid environmental contacts. Using the virtual work principle, we established that equilibrium in the presence of constraints implies the existence of Lagrange multipliers whose images under the constraint Jacobian transpose are the generalized reaction forces. We connected these multipliers to contact wrenches via the manipulator Jacobian, discussed the linear algebra of solving constrained equilibrium (including internal-force modes), and implemented small but representative examples in Python, C++, Java, MATLAB/Simulink, and Mathematica. These ideas form the static counterpart of the constrained dynamics formulations you will encounter in later chapters.