We consider a planar (sagittal-plane) model of a legged robot during single support. The model consists of:

• A floating base (torso) with planar pose.
• A planar 2-DOF leg (hip and knee) attached at the base.
• A point foot in rigid contact with level ground.

The generalized coordinates collect base pose and joint angles:

\[ \mathbf{q} = \begin{bmatrix} x_b \\ y_b \\ \theta_b \\ q_1 \\ q_2 \end{bmatrix} \in \mathbb{R}^5, \quad \dot{\mathbf{q}} = \begin{bmatrix} \dot{x}_b \\ \dot{y}_b \\ \dot{\theta}_b \\ \dot{q}_1 \\ \dot{q}_2 \end{bmatrix}. \]

Here \( (x_b,y_b) \) is the base reference point (e.g., base center of mass), \( \theta_b \) is the base orientation, and \( q_1,q_2 \) are hip and knee joint coordinates (relative to the base and thigh, respectively).

During stance, we enforce that the foot position \( \mathbf{p}_f(\mathbf{q}) \in \mathbb{R}^2 \) is fixed at a world point \( \mathbf{p}_f^\ast \). This yields two holonomic constraints and reduces the effective configuration dimension from 5 to 3 DOF while the contact is active.

flowchart TD
  W["World frame"] --> B["Floating base (x_b, y_b, theta_b)"]
  B --> H["Hip joint q1"]
  H --> K["Knee joint q2"]
  K --> F["Point foot (stance contact)"]
        

We denote link masses by \( m_b, m_1, m_2 \), planar inertias by \( I_b, I_1, I_2 \), and link lengths and center-of-mass offsets by \( l_1, l_2, c_1, c_2 \). Gravity acts in negative \( y \)-direction with magnitude \( g \).

For a free-floating multibody system (no constraints), the dynamics can be written in generalized coordinates as:

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

where:

• \( \mathbf{M}(\mathbf{q}) \in \mathbb{R}^{5\times 5} \) is the symmetric positive-definite mass matrix.
• \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \) collects Coriolis/centrifugal and gravity terms.
• \( \boldsymbol{\tau} \in \mathbb{R}^2 \) contains the joint torques at hip and knee.
• \( \mathbf{S} \in \mathbb{R}^{2\times 5} \) is the selection matrix mapping generalized forces to actuated joints (here only joint coordinates are actuated).

For our 5-DOF system with unactuated base coordinates and actuated joints, we choose

\[ \mathbf{S} = \begin{bmatrix} 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}, \quad \mathbf{S}^T \boldsymbol{\tau} = \begin{bmatrix} 0 \\ 0 \\ 0 \\ \tau_1 \\ \tau_2 \end{bmatrix}. \]

The mass matrix can be expressed through link Jacobians and spatial inertias. Let \( \mathbf{J}_i(\mathbf{q}) \) be the planar twist Jacobian mapping generalized velocities to the body-twist of link \( i \in \{b,1,2\} \), and let \( \mathbf{M}_i \) be its spatial inertia matrix (in planar case, a 3×3 inertia). Then

\[ \mathbf{M}(\mathbf{q}) = \sum_{i \in \{b,1,2\}} \mathbf{J}_i(\mathbf{q})^T \mathbf{M}_i \mathbf{J}_i(\mathbf{q}), \]

which guarantees symmetry and positive definiteness since for any nonzero \( \mathbf{v} \),

\[ \mathbf{v}^T \mathbf{M}(\mathbf{q}) \mathbf{v} = \sum_i \left\| \mathbf{M}_i^{\frac{1}{2}} \mathbf{J}_i(\mathbf{q}) \mathbf{v} \right\|^2 > 0. \]

The term \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \) can be obtained from the Lagrangian \( \mathcal{L} = K - P \) via Euler–Lagrange equations or from the Christoffel-symbol-based Coriolis matrix, both of which were derived in earlier chapters.

We now compute the stance foot position as a function of generalized coordinates. Let \( \mathbf{p}_b = [x_b, y_b]^T \) be the base reference point, and define planar rotation

\[ \mathbf{R}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}. \]

The thigh and shank orientations are \( \theta_1 = \theta_b + q_1 \) and \( \theta_2 = \theta_b + q_1 + q_2 \). Assuming the hip is co-located with the base reference point, the foot position is

\[ \mathbf{p}_f(\mathbf{q}) = \mathbf{p}_b + \mathbf{R}(\theta_1) \begin{bmatrix} l_1 \\ 0 \end{bmatrix} + \mathbf{R}(\theta_2) \begin{bmatrix} l_2 \\ 0 \end{bmatrix}. \]

The stance contact with a fixed world point \( \mathbf{p}_f^\ast \) yields the holonomic constraint

\[ \boldsymbol{\phi}(\mathbf{q}) = \mathbf{p}_f(\mathbf{q}) - \mathbf{p}_f^\ast = \mathbf{0}, \quad \boldsymbol{\phi}:\mathbb{R}^5 \rightarrow \mathbb{R}^2. \]

Differentiating w.r.t. time, we obtain the velocity and acceleration level constraints using the contact Jacobian \( \mathbf{J}_c(\mathbf{q}) \):

\[ \mathbf{J}_c(\mathbf{q}) = \frac{\partial \mathbf{p}_f(\mathbf{q})}{\partial \mathbf{q}} \in \mathbb{R}^{2\times 5}, \]

\[ \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}. \]

The 2 constraints reduce the instantaneous motion space to a 3-dimensional subspace of \( \mathbb{R}^5 \), consistent with intuition: 3 coordinates (e.g., one base translation, base orientation, and one leg joint) can move independently while maintaining foot contact.

When the stance foot is rigidly constrained, the equations of motion acquire additional generalized forces from the contact reaction. Let \( \boldsymbol{\lambda} \in \mathbb{R}^2 \) denote the Cartesian contact wrench at the foot (normal and tangential planar reaction forces). The constrained dynamics are

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

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

We can assemble these into a linear system in the unknowns \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \):

\[ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & -\mathbf{J}_c(\mathbf{q})^T \\ \mathbf{J}_c(\mathbf{q}) & \mathbf{0} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\ \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{S}^T\boldsymbol{\tau} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \\ - \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \end{bmatrix}. \]

Assuming a non-singular contact (i.e., \( \mathbf{J}_c(\mathbf{q}) \) has full row rank), the block matrix is invertible, and we can explicitly eliminate \( \boldsymbol{\lambda} \). From the first block row,

\[ \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} \left( \mathbf{S}^T \boldsymbol{\tau} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{J}_c(\mathbf{q})^T \boldsymbol{\lambda} \right). \]

Substituting into the constraint-acceleration equation gives

\[ \mathbf{J}_c \mathbf{M}^{-1} \left( \mathbf{S}^T \boldsymbol{\tau} - \mathbf{h} + \mathbf{J}_c^T \boldsymbol{\lambda} \right) + \dot{\mathbf{J}}_c \dot{\mathbf{q}} = \mathbf{0}, \]

\[ \left( \mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^T \right)\boldsymbol{\lambda} = -\mathbf{J}_c \mathbf{M}^{-1} \left( \mathbf{S}^T \boldsymbol{\tau} - \mathbf{h} \right) - \dot{\mathbf{J}}_c \dot{\mathbf{q}}. \]

The contact inertia \( \boldsymbol{\Lambda}_c(\mathbf{q}) = \mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^T \) is symmetric and positive definite. Solving for the contact force:

\[ \boldsymbol{\lambda} = \boldsymbol{\Lambda}_c(\mathbf{q})^{-1} \left[ -\mathbf{J}_c \mathbf{M}^{-1} \left( \mathbf{S}^T \boldsymbol{\tau} - \mathbf{h} \right) - \dot{\mathbf{J}}_c \dot{\mathbf{q}} \right], \]

and then substitute back to obtain \( \ddot{\mathbf{q}} \). This procedure generalizes to higher-dimensional floating-base legged robots with multiple contacts.

For a time-stepping simulation of the stance phase (with fixed contact), at each integration step we need to:

1. Evaluate \( \mathbf{M}(\mathbf{q}) \).
2. Compute \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \) (Coriolis and gravity).
3. Compute contact Jacobian \( \mathbf{J}_c(\mathbf{q}) \) and \( \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}}) \).
4. Form and solve the constrained dynamics for \( \ddot{\mathbf{q}} \).
5. Integrate \( \dot{\mathbf{q}} \) and \( \mathbf{q} \).

The following flowchart summarizes the computation for one time step given current state and joint torques.

flowchart TD
  S["State (q, qdot), torques tau"] --> M["Compute M(q), h(q, qdot)"]
  M --> J["Compute J_c(q), Jdot_c(q, qdot)"]
  J --> L["Solve for lambda via Lambda_c = J_c M^{-1} J_c^T"]
  L --> QDD["Compute qddot from constrained EoM"]
  QDD --> INT["Integrate to update (q, qdot)"]
  INT --> S_NEXT["Next step state"]
        

In practice, one must also monitor contact-consistency conditions (e.g., normal force non-negativity) and switch to a flight-phase model if contact is lost. Here we focus purely on the stance-phase modeling.

Below is a compact Python skeleton illustrating how to construct \( \mathbf{M}, \mathbf{h}, \mathbf{J}_c, \dot{\mathbf{J}}_c \dot{\mathbf{q}} \) for the planar floating-base leg using symbolic expressions and then evaluate them numerically. For brevity, we show only the key steps; the symbolic engine (SymPy) handles the detailed algebra.

import sympy as sp
import numpy as np

# --- Symbolic model definition ---

# Generalized coordinates and velocities
x_b, y_b, th_b, q1, q2 = sp.symbols("x_b y_b th_b q1 q2", real=True)
dx_b, dy_b, dth_b, dq1, dq2 = sp.symbols("dx_b dy_b dth_b dq1 dq2", real=True)

q = sp.Matrix([x_b, y_b, th_b, q1, q2])
dq = sp.Matrix([dx_b, dy_b, dth_b, dq1, dq2])

# Parameters
m_b, m1, m2 = sp.symbols("m_b m1 m2", positive=True)
I_b, I1, I2 = sp.symbols("I_b I1 I2", positive=True)
l1, l2, c1, c2 = sp.symbols("l1 l2 c1 c2", positive=True)
g = sp.symbols("g", positive=True)

def R(theta):
    return sp.Matrix([[sp.cos(theta), -sp.sin(theta)],
                      [sp.sin(theta),  sp.cos(theta)]])

# Orientations
th1 = th_b + q1
th2 = th_b + q1 + q2

# Positions
p_b = sp.Matrix([x_b, y_b])
p_c1 = p_b + R(th1) * sp.Matrix([c1, 0])
p_knee = p_b + R(th1) * sp.Matrix([l1, 0])
p_c2 = p_knee + R(th2) * sp.Matrix([c2, 0])
p_f = p_knee + R(th2) * sp.Matrix([l2, 0])  # foot position

# Jacobians for COMs and foot
J_b   = p_b.jacobian(q)
J_c1  = p_c1.jacobian(q)
J_c2  = p_c2.jacobian(q)
J_cf  = p_f.jacobian(q)   # contact Jacobian (position)

# Velocities from Jacobians
v_b  = J_b  * dq
v_c1 = J_c1 * dq
v_c2 = J_c2 * dq

# Angular velocities (planar)
w_b  = dth_b
w_1  = dth_b + dq1
w_2  = dth_b + dq1 + dq2

# Kinetic and potential energy
K = (sp.Rational(1, 2) * m_b * v_b.dot(v_b)
     + sp.Rational(1, 2) * m1 * v_c1.dot(v_c1)
     + sp.Rational(1, 2) * m2 * v_c2.dot(v_c2)
     + sp.Rational(1, 2) * I_b * w_b**2
     + sp.Rational(1, 2) * I1 * w_1**2
     + sp.Rational(1, 2) * I2 * w_2**2)

P = m_b * g * y_b + m1 * g * p_c1[1] + m2 * g * p_c2[1]
L = K - P

# Mass matrix via quadratic form in dq
M_sym = sp.hessian(K, dq)

# Gravity term from potential
g_sym = sp.Matrix([sp.diff(P, qi) for qi in q])

# Euler-Lagrange equations to obtain full dynamics M ddq + h = S^T tau
ddq = sp.symbols("ddx_b ddy_b ddth_b ddq1 ddq2")
ddq = sp.Matrix(ddq)

# Here we build E(q, dq, ddq) = 0 and identify h as terms independent of ddq
E = sp.Matrix([
    sp.diff(sp.diff(L, dq_i), 't') - sp.diff(L, q_i)
    for dq_i, q_i in zip(dq, q)
])
# In practice, replace 't' derivatives using chain rule and match coefficients
# of ddq to M_sym, and the remaining terms to h_sym.

# Contact Jacobian and its time derivative (used as J_c and Jdot_c dq)
Jc_sym = J_cf
Jc_dot_dq_sym = sp.Matrix([sp.diff(Jc_sym[i, j], q_k) * dq[k]
                           for i in range(Jc_sym.rows)
                           for j in range(Jc_sym.cols)
                           for k, q_k in enumerate(q)])
# Reshape into (2 x 5) matrix for Jdot_c dq
Jc_dot_dq_sym = sp.Matrix(2, 5, Jc_dot_dq_sym)

# Lambdify numeric functions (M, g, Jc, Jc_dot_dq)
M_fun = sp.lambdify((q, m_b, m1, m2, I_b, I1, I2, l1, l2, c1, c2), M_sym, "numpy")
g_fun = sp.lambdify((q, m_b, m1, m2, l1, l2, c1, c2, g), g_sym, "numpy")
Jc_fun = sp.lambdify((q, l1, l2), Jc_sym, "numpy")
Jc_dot_dq_fun = sp.lambdify((q, dq, l1, l2), Jc_dot_dq_sym, "numpy")

# --- Numeric constrained dynamics (stance phase) ---

def constrained_dynamics(q_val, dq_val, tau_val, params):
    """
    q_val: shape (5,)
    dq_val: shape (5,)
    tau_val: shape (2,)
    params: dict with masses, inertias, lengths, c offsets, gravity
    returns: ddq (5,), lambda_contact (2,)
    """
    m_bv, m1v, m2v = params["m_b"], params["m1"], params["m2"]
    I_bv, I1v, I2v = params["I_b"], params["I1"], params["I2"]
    l1v, l2v = params["l1"], params["l2"]
    c1v, c2v = params["c1"], params["c2"]
    gv = params["g"]

    q_np = np.asarray(q_val, dtype=float).reshape(5, 1)
    dq_np = np.asarray(dq_val, dtype=float).reshape(5, 1)
    tau_np = np.asarray(tau_val, dtype=float).reshape(2, 1)

    M = np.array(M_fun(q_np, m_bv, m1v, m2v, I_bv, I1v, I2v, l1v, l2v, c1v, c2v), dtype=float)
    g_vec = np.array(g_fun(q_np, m_bv, m1v, m2v, l1v, l2v, c1v, c2v, gv), dtype=float)

    # Placeholder: you would also compute Coriolis/Centrifugal from the full Lagrangian
    h = g_vec  # for slow motions near rest, Coriolis approx. negligible

    Jc = np.array(Jc_fun(q_np, l1v, l2v), dtype=float)
    Jc_dot_dq = np.array(Jc_dot_dq_fun(q_np, dq_np, l1v, l2v), dtype=float)

    # Selection matrix S (2 x 5)
    S = np.array([[0, 0, 0, 1, 0],
                  [0, 0, 0, 0, 1]], dtype=float)

    # Build and solve the linear system
    Z = np.zeros((2, 2))
    A = np.block([[M, -Jc.T],
                  [Jc, Z]])
    rhs = np.vstack([S.T @ tau_np - h,
                     -Jc_dot_dq])

    sol = np.linalg.solve(A, rhs)
    ddq = sol[:5, :].reshape(5)
    lam = sol[5:, :].reshape(2)
    return ddq, lam
      

This code emphasizes the symbolic-to-numeric pathway: from geometric definitions of positions to automatic derivation of \( \mathbf{M}, \mathbf{g}, \mathbf{J}_c \), and finally to constrained dynamics. For high-DOF 3D robots, one would typically use dedicated robotics libraries (e.g., Pinocchio in Python/C++) rather than hand-deriving expressions.

In C++, we typically use Eigen for linear algebra and a robotics dynamics library (e.g., Pinocchio or RBDL) for model evaluation. Below is a structure-first implementation: we assume functions computing \( \mathbf{M}, \mathbf{h}, \mathbf{J}_c, \dot{\mathbf{J}}_c\dot{\mathbf{q}} \), and we focus on assembling and solving the constrained dynamics system.

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

struct LegParams {
  double m_b, m1, m2;
  double I_b, I1, I2;
  double l1, l2, c1, c2;
  double g;
};

using Vec5 = Eigen::Matrix<double, 5, 1>;
using Vec2 = Eigen::Matrix<double, 2, 1>;
using Mat5 = Eigen::Matrix<double, 5, 5>;
using Mat2 = Eigen::Matrix<double, 2, 2>;
using Mat25 = Eigen::Matrix<double, 2, 5>;
using Mat52 = Eigen::Matrix<double, 5, 2>;

// These functions should be implemented from the planar model or via a library.
Mat5 massMatrix(const Vec5& q, const LegParams& p);
Vec5 biasTerm(const Vec5& q, const Vec5& dq, const LegParams& p); // h(q, dq)
Mat25 contactJacobian(const Vec5& q, const LegParams& p);
Mat25 contactJdotV(const Vec5& q, const Vec5& dq, const LegParams& p); // Jdot_c dq

struct DynamicsResult {
  Vec5 ddq;
  Vec2 lambda;
};

DynamicsResult constrainedDynamics(const Vec5& q,
                                   const Vec5& dq,
                                   const Vec2& tau,
                                   const LegParams& p)
{
  Mat5 M = massMatrix(q, p);
  Vec5 h = biasTerm(q, dq, p);
  Mat25 Jc = contactJacobian(q, p);
  Mat25 Jc_dot_v = contactJdotV(q, dq, p);

  // Selection matrix S (2 x 5)
  Eigen::Matrix<double, 2, 5> S;
  S.setZero();
  S(0, 3) = 1.0; // hip
  S(1, 4) = 1.0; // knee

  // Assemble block system
  Mat2 Z = Mat2::Zero();
  Eigen::Matrix<double, 7, 7> A;
  A.setZero();
  // Top-left: M
  A.block<5,5>(0, 0) = M;
  // Top-right: -Jc^T
  A.block<5,2>(0, 5) = -Jc.transpose();
  // Bottom-left: Jc
  A.block<2,5>(5, 0) = Jc;
  // Bottom-right: zeros

  Eigen::Matrix<double, 7, 1> rhs;
  rhs.setZero();
  rhs.segment<5>(0) = S.transpose() * tau - h;
  rhs.segment<2>(5) = -Jc_dot_v * dq;  // Jdot_c dq

  Eigen::Matrix<double, 7, 1> sol = A.fullPivLu().solve(rhs);

  DynamicsResult res;
  res.ddq = sol.segment<5>(0);
  res.lambda = sol.segment<2>(5);
  return res;
}

int main() {
  LegParams p{/* m_b */ 10.0, /* m1 */ 3.0, /* m2 */ 2.0,
              /* I_b */ 1.0,  /* I1 */ 0.2, /* I2 */ 0.1,
              /* l1 */ 0.5,   /* l2 */ 0.5,
              /* c1 */ 0.25,  /* c2 */ 0.25,
              /* g  */ 9.81};

  Vec5 q;   q << 0.0, 0.9, 0.0, 0.4, -0.8;
  Vec5 dq;  dq.setZero();
  Vec2 tau; tau << 0.0, 0.0;

  DynamicsResult res = constrainedDynamics(q, dq, tau, p);
  std::cout << "ddq = " << res.ddq.transpose() << std::endl;
  std::cout << "lambda = " << res.lambda.transpose() << std::endl;
  return 0;
}
      

In a full implementation, massMatrix, biasTerm, and contactJacobian can be obtained from a C++ robotics library using a URDF description of the floating-base legged robot. The constrained solve structure remains the same.

Java lacks a de facto standard robotics library, but the same structure carries over using a numerical linear algebra package such as EJML. Below is a template that mirrors the C++ structure.

import org.ejml.simple.SimpleMatrix;

public class FloatingBaseLeg {

    public static class LegParams {
        public double m_b, m1, m2;
        public double I_b, I1, I2;
        public double l1, l2, c1, c2;
        public double g;
    }

    // M(q)
    static SimpleMatrix massMatrix(SimpleMatrix q, LegParams p) {
        // TODO: implement from planar model or call native robotics library
        return SimpleMatrix.identity(5);
    }

    // h(q, dq)
    static SimpleMatrix biasTerm(SimpleMatrix q, SimpleMatrix dq, LegParams p) {
        // TODO: implement gravity and Coriolis from model
        return new SimpleMatrix(5, 1);
    }

    // J_c(q)
    static SimpleMatrix contactJacobian(SimpleMatrix q, LegParams p) {
        // TODO: analytical J_c for planar leg
        return new SimpleMatrix(2, 5);
    }

    // Jdot_c(q, dq) * dq
    static SimpleMatrix contactJdotV(SimpleMatrix q, SimpleMatrix dq, LegParams p) {
        // TODO: implement analytic expression or finite differences
        return new SimpleMatrix(2, 1);
    }

    public static class DynamicsResult {
        public SimpleMatrix ddq;
        public SimpleMatrix lambda;
    }

    public static DynamicsResult constrainedDynamics(SimpleMatrix q,
                                                     SimpleMatrix dq,
                                                     SimpleMatrix tau,
                                                     LegParams p) {
        SimpleMatrix M = massMatrix(q, p);
        SimpleMatrix h = biasTerm(q, dq, p);
        SimpleMatrix Jc = contactJacobian(q, p);
        SimpleMatrix JcDotV = contactJdotV(q, dq, p);

        // Selection matrix S (2 x 5)
        double[][] Sdata = {
                {0, 0, 0, 1, 0},
                {0, 0, 0, 0, 1}
        };
        SimpleMatrix S = new SimpleMatrix(Sdata);

        // Build block matrix A (7 x 7)
        SimpleMatrix A = new SimpleMatrix(7, 7);
        A.insertIntoThis(0, 0, M);
        A.insertIntoThis(0, 5, Jc.transpose().scale(-1.0));
        A.insertIntoThis(5, 0, Jc);

        // Right-hand side
        SimpleMatrix rhs = new SimpleMatrix(7, 1);
        rhs.insertIntoThis(0, 0, S.transpose().mult(tau).minus(h));
        rhs.insertIntoThis(5, 0, JcDotV.negative());

        SimpleMatrix sol = A.solve(rhs);

        DynamicsResult res = new DynamicsResult();
        res.ddq = sol.extractMatrix(0, 5, 0, 1);
        res.lambda = sol.extractMatrix(5, 7, 0, 1);
        return res;
    }

    public static void main(String[] args) {
        LegParams p = new LegParams();
        p.m_b = 10.0; p.m1 = 3.0; p.m2 = 2.0;
        p.I_b = 1.0;  p.I1 = 0.2; p.I2 = 0.1;
        p.l1 = 0.5;   p.l2 = 0.5;
        p.c1 = 0.25;  p.c2 = 0.25;
        p.g  = 9.81;

        SimpleMatrix q  = new SimpleMatrix(new double[][] { {0.0},{0.9},{0.0},{0.4},{-0.8} });
        SimpleMatrix dq = new SimpleMatrix(5,1);
        SimpleMatrix tau = new SimpleMatrix(new double[][] { {0.0},{0.0} });

        DynamicsResult res = constrainedDynamics(q, dq, tau, p);
        System.out.println("ddq = " + res.ddq.transpose());
        System.out.println("lambda = " + res.lambda.transpose());
    }
}
      

As in C++, the Java code makes the constrained dynamics structure explicit. The remaining task for the reader is to implement the model-specific functions using planar kinematics and dynamics that were derived in previous sections.

In MATLAB, we can both script the equations of motion and create a Simulink/Simscape Multibody model of the floating-base leg. The script below focuses on the constrained solve; the functions M_leg, h_leg, and Jc_leg should be implemented from the planar derivation.

function [ddq, lambda] = floating_leg_dynamics(q, dq, tau, p)
% q: 5x1, dq: 5x1, tau: 2x1, p: struct of parameters

M  = M_leg(q, p);      % 5x5
h  = h_leg(q, dq, p);  % 5x1
Jc = Jc_leg(q, p);     % 2x5
Jc_dot_dq = Jcdot_leg(q, dq, p); % 2x1

S = [0 0 0 1 0;
     0 0 0 0 1];

A = [M, -Jc';
     Jc, zeros(2,2)];

rhs = [S' * tau - h;
       -Jc_dot_dq];

sol = A \ rhs;
ddq = sol(1:5);
lambda = sol(6:7);
end
      

A typical Simulink/Simscape Multibody workflow for the same robot is:

1. Construct a planar multibody model with a floating base and 2-link leg.
2. Constrain the foot with a 2-DOF planar joint to a fixed ground body (for stance).
3. Drive hip and knee revolute joints with torque sources; log joint and base states.
4. Use Simscape sensors to obtain reaction forces at the foot joint, corresponding to \( \boldsymbol{\lambda} \).

Simscape thus serves as an independent validation of the symbolic modeling and numerical integration implemented in MATLAB scripts.

Mathematica excels at symbolic derivation of multibody dynamics. The following snippet sets up the Lagrangian for the planar floating-base leg and derives equations of motion using Euler–Lagrange equations, then rewrites them into the standard \( \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T\boldsymbol{\lambda} \) form for stance.

ClearAll["Global`*"];

(* Generalized coordinates as functions of time *)
q[t_] := {xB[t], yB[t], thB[t], q1[t], q2[t]};
dq[t_] := D[q[t], t];

(* Parameters *)
params = {mB, m1, m2, IB, I1, I2, l1, l2, c1, c2, g};

(* Rotation matrix for planar case *)
R[th_] := { {Cos[th], -Sin[th]}, {Sin[th], Cos[th]} };

(* Orientations *)
th1[t_] := thB[t] + q1[t];
th2[t_] := thB[t] + q1[t] + q2[t];

(* Positions *)
pB[t_] := {xB[t], yB[t]};
pC1[t_] := pB[t] + R[th1[t]].{c1, 0};
pKnee[t_] := pB[t] + R[th1[t]].{l1, 0};
pC2[t_] := pKnee[t] + R[th2[t]].{c2, 0};
pF[t_] := pKnee[t] + R[th2[t]].{l2, 0};

(* Velocities *)
vB[t_] := D[pB[t], t];
vC1[t_] := D[pC1[t], t];
vC2[t_] := D[pC2[t], t];

wB[t_] := D[thB[t], t];
w1[t_] := D[th1[t], t];
w2[t_] := D[th2[t], t];

(* Energies *)
K[t_] :=
  1/2 mB vB[t].vB[t] + 1/2 m1 vC1[t].vC1[t] + 1/2 m2 vC2[t].vC2[t] +
  1/2 IB wB[t]^2 + 1/2 I1 w1[t]^2 + 1/2 I2 w2[t]^2;

P[t_] := mB g yB[t] + m1 g pC1[t][[2]] + m2 g pC2[t][[2]];

L[t_] := K[t] - P[t];

(* Lagrange equations: d/dt (dL/ddq) - dL/dq = Q *)
coords = {xB[t], yB[t], thB[t], q1[t], q2[t]};
dcoords = D[coords, t];
ddcoords = D[dcoords, t];

(* Generalized torques: base coordinates unactuated, joints actuated *)
Qgen = {0, 0, 0, tau1[t], tau2[t]};

eom = Table[
  D[D[L[t], dcoords[[i]]], t] - D[L[t], coords[[i]]] == Qgen[[i]],
  {i, 1, Length[coords]}
];

(* Contact constraints: pF[t] == pFStar (fixed point) *)
pFStar = {xFStar, yFStar};
phi[t_] := pF[t] - pFStar;
constraints = {phi[t][[1]] == 0, phi[t][[2]] == 0};

(* Differentiate constraints twice to get acceleration-level constraints *)
constraintsVel = D[constraints, t];
constraintsAcc = D[constraintsVel, t];

(* Solve symbolically for accelerations and contact forces lambda1, lambda2 *)
lambda1[t]; lambda2[t];
lambdaVec = {lambda1[t], lambda2[t]};

(* Rewrite constraintsAcc in terms of ddcoords and lambda, then solve linear system *)
(* In practice, use Mathematica's Solve or LinearSolve after expressing eom and
   constraintsAcc in matrix form. The result can be rearranged to
   M(q) ddq + h(q, dq) == S^T tau + Jc^T lambda. *)
      

Mathematica can further be used to export C or Python code for \( \mathbf{M}, \mathbf{h}, \mathbf{J}_c \), which can then be plugged into the numeric solvers shown earlier.

In this lesson we instantiated floating-base multibody modeling on a simple planar legged robot, combining base coordinates, joint-space dynamics, and contact constraints into a unified framework. We derived the stance-phase equations of motion with contact forces, expressed them in block-matrix form, and showed how to solve for accelerations and contact reactions using the contact inertia \( \boldsymbol{\Lambda}_c = \mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^T \). Finally, we demonstrated multi-language implementations (Python, C++, Java, MATLAB/Simulink, Mathematica) that all realize the same structural computation, preparing the ground for more complex whole-body legged robot models.