A floating-base robot (e.g., a legged robot) is modeled by generalized coordinates \( \mathbf{q} \in \mathbb{R}^{n+6} \), where 6 coordinates describe the free base pose in SE(3) and \( n \) coordinates describe internal joints. In the absence of contact, the equations of motion can be written in the compact form

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

where \( \mathbf{M}(\mathbf{q}) \) is the joint-space inertia matrix, \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \) collects Coriolis, centrifugal, and gravity terms, \( \boldsymbol{\tau} \) are actuator efforts, \( \mathbf{S} \) is the actuation selection matrix, and \( \mathbf{f}_{ext} \) are non-contact generalized forces.

When the robot touches the environment, the geometry of contact introduces additional constraints on positions, velocities, and accelerations of contact points. Corresponding contact forces act along the admissible directions and enter the dynamics as generalized forces through the contact Jacobian. A high-level modeling pipeline is:

flowchart TD
  A["Robot geometry + environment"] --> B["Define contact points and gap/relative pose functions"]
  B --> C["Classify constraints: bilateral / unilateral, holonomic / nonholonomic"]
  C --> D["Differentiate to obtain contact Jacobians J_c(q)"]
  D --> E["Assemble constrained dynamics (KKT system)"]
  E --> F["Solve for q_ddot and contact forces lambda"]
        

In this lesson we treat contact as ideal, rigid constraints (no compliance), and we do not design any controller. Instead, we study how to embed contact consistently into the equations of motion so that simulated trajectories satisfy non-penetration and kinematic compatibility.

Consider a set of \( m_b \) bilateral, holonomic contact constraints. Each constraint is represented by a smooth function \( \boldsymbol{\phi}_b : \mathbb{R}^{n+6} \to \mathbb{R}^{m_b} \) such that

\[ \boldsymbol{\phi}_b(\mathbf{q}) = \mathbf{0} \]

characterizes perfect contact. For example, for a point on the foot with position \( \mathbf{p}_c(\mathbf{q}) \in \mathbb{R}^3 \) constrained to coincide with a fixed point \( \mathbf{p}_s \in \mathbb{R}^3 \) on the ground, we can take \( \boldsymbol{\phi}_b(\mathbf{q}) = \mathbf{p}_c(\mathbf{q}) - \mathbf{p}_s \).

Differentiating the constraint with respect to time gives the velocity-level constraint. Let \( t \mapsto \mathbf{q}(t) \) be a trajectory with \( \boldsymbol{\phi}_b(\mathbf{q}(t)) \equiv \mathbf{0} \). Then

\[ \frac{\mathrm{d}}{\mathrm{d}t}\boldsymbol{\phi}_b(\mathbf{q}(t)) = \frac{\partial \boldsymbol{\phi}_b}{\partial \mathbf{q}}(\mathbf{q}(t)) \dot{\mathbf{q}}(t) = \mathbf{J}_b(\mathbf{q}) \dot{\mathbf{q}} = \mathbf{0}, \]

where \( \mathbf{J}_b(\mathbf{q}) \in \mathbb{R}^{m_b \times (n+6)} \) is the bilateral contact Jacobian.

Differentiating once more yields the acceleration-level constraint

\[ \frac{\mathrm{d}}{\mathrm{d}t}\big(\mathbf{J}_b(\mathbf{q}) \dot{\mathbf{q}}\big) = \mathbf{J}_b(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{J}}_b(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \mathbf{0}, \]

where \( \dot{\mathbf{J}}_b(\mathbf{q},\dot{\mathbf{q}}) \) is obtained by differentiating the Jacobian along the motion.

Contact forces associated with bilateral constraints arise naturally as Lagrange multipliers \( \boldsymbol{\lambda}_b \in \mathbb{R}^{m_b} \). They enter the dynamics as generalized forces

\[ \mathbf{f}_c = \mathbf{J}_b(\mathbf{q})^\top \boldsymbol{\lambda}_b. \]

Physically, \( \boldsymbol{\lambda}_b \) are the forces in constraint space that enforce kinematic compatibility at the contact points.

Foot–ground contacts are typically unilateral: the foot cannot pull the ground. This is modeled using a gap function \( g_n(\mathbf{q}) \in \mathbb{R} \), measuring signed distance along the contact normal. A common convention is \( g_n(\mathbf{q}) \geq 0 \), with \( g_n(\mathbf{q}) = 0 \) at contact and \( g_n(\mathbf{q}) > 0 \) when separated.

Let \( \lambda_n \) denote the normal component of the contact force (expressed in constraint space). The idealized rigid, frictionless, unilateral contact model uses complementarity conditions:

\[ g_n(\mathbf{q}) \geq 0, \quad \lambda_n \geq 0, \quad g_n(\mathbf{q})\,\lambda_n = 0. \]

These conditions encode three regimes:

• Open contact: if \( g_n(\mathbf{q}) > 0 \), then the force must vanish, \( \lambda_n = 0 \).
• Closed contact: if \( g_n(\mathbf{q}) = 0 \), non-penetration allows a compressive reaction \( \lambda_n \geq 0 \).
• No adhesion: tensile forces (\( \lambda_n < 0 \)) are forbidden.

With friction, the tangential contact forces \( \boldsymbol{\lambda}_t \) are constrained to lie inside a (possibly conic) admissible set, e.g. the Coulomb friction cone

\[ \|\boldsymbol{\lambda}_t\|_2 \leq \mu\, \lambda_n, \]

where \( \mu \) is the friction coefficient and \( \|\cdot\|_2 \) is the Euclidean norm. In this lesson we focus mainly on the normal direction; tangential dynamics can be incorporated by enlarging the constraint space.

For a contact point with position \( \mathbf{p}_c(\mathbf{q}) \in \mathbb{R}^3 \), the linear velocity is

\[ \dot{\mathbf{p}}_c(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{J}_p(\mathbf{q}) \dot{\mathbf{q}}, \quad \mathbf{J}_p(\mathbf{q}) \triangleq \frac{\partial \mathbf{p}_c}{\partial \mathbf{q}}(\mathbf{q}). \]

For a rigid contact that constrains both translation and rotation of a frame attached to the robot, we often work with a spatial contact Jacobian \( \mathbf{J}_c(\mathbf{q}) \in \mathbb{R}^{k \times (n+6)} \) such that the twist of the contact frame satisfies

\[ \boldsymbol{\nu}_c = \mathbf{J}_c(\mathbf{q}) \dot{\mathbf{q}}, \]

where \( \boldsymbol{\nu}_c \) stacks linear and angular velocities. The rows of \( \mathbf{J}_c \) corresponding to constrained velocity directions must vanish for a sticking contact, leading again to velocity-level constraints \( \mathbf{J}_c(\mathbf{q}) \dot{\mathbf{q}} = \mathbf{0} \).

It is often convenient to decompose \( \mathbf{J}_c \) into normal and tangential parts, e.g. in 3D

\[ \mathbf{J}_c(\mathbf{q}) = \begin{bmatrix} \mathbf{J}_n(\mathbf{q}) \\[0.3em] \mathbf{J}_t(\mathbf{q}) \end{bmatrix}, \quad \boldsymbol{\lambda} = \begin{bmatrix} \lambda_n \\[0.3em] \boldsymbol{\lambda}_t \end{bmatrix}, \quad \mathbf{f}_c = \mathbf{J}_c^\top \boldsymbol{\lambda} = \mathbf{J}_n^\top \lambda_n + \mathbf{J}_t^\top \boldsymbol{\lambda}_t. \]

This decomposition is purely a change of basis in constraint space, but it aligns naturally with the physical interpretation of normal and tangential contact forces.

Let \( \mathbf{J}_c(\mathbf{q}) \) denote the Jacobian of all active contact constraints (including both bilateral and unilateral contacts that are currently closed). The constrained dynamics can be written as

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

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{a}_c, \]

where \( \mathbf{a}_c \) encodes any prescribed contact acceleration (for rigid sticking contact typically \( \mathbf{a}_c = \mathbf{0} \)).

Combining these two equations gives a saddle-point (KKT) linear system in \( \ddot{\mathbf{q}} \) and \( \boldsymbol{\lambda} \):

\[ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & -\mathbf{J}_c(\mathbf{q})^\top \\[0.3em] \mathbf{J}_c(\mathbf{q}) & \mathbf{0} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\[0.3em] \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{S}^\top \boldsymbol{\tau} + \mathbf{f}_{ext} - \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \\[0.3em] \mathbf{a}_c - \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \end{bmatrix}. \]

Proof sketch. Substitute the contact force expression \( \mathbf{J}_c^\top \boldsymbol{\lambda} \) into the unconstrained dynamics, and rearrange so that the unknowns are grouped into \( [\ddot{\mathbf{q}}^\top \; \boldsymbol{\lambda}^\top]^\top \). The second block row is exactly the acceleration-level constraint.

The KKT matrix is symmetric but indefinite. Solving this system yields accelerations consistent with rigid contact and the corresponding contact forces. For unilateral constraints, the complementarity and inequality conditions must additionally be enforced (often via optimization or time-stepping schemes, which are beyond the scope of this lesson).

When contact is created or broken (e.g., a leg striking the ground), forces may be impulsive, producing discontinuities in velocity but not in configuration. Let \( \dot{\mathbf{q}}^- \) and \( \dot{\mathbf{q}}^+ \) denote pre- and post-impact velocities. Ideal rigid impacts are modeled by an impulse \( \boldsymbol{\Lambda} \) applied over a negligible time interval.

The jump in generalized momentum satisfies

\[ \mathbf{M}(\mathbf{q})\big(\dot{\mathbf{q}}^+ - \dot{\mathbf{q}}^-\big) = \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\Lambda}, \]

while the post-impact velocity must obey the velocity-level contact constraint

\[ \mathbf{J}_c(\mathbf{q}) \dot{\mathbf{q}}^+ = \mathbf{0} \quad \text{(sticking impact)}. \]

Combining these gives another KKT system:

\[ \begin{bmatrix} \mathbf{M}(\mathbf{q}) & -\mathbf{J}_c(\mathbf{q})^\top \\[0.3em] \mathbf{J}_c(\mathbf{q}) & \mathbf{0} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{q}}^+ \\[0.3em] \boldsymbol{\Lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}}^- \\[0.3em] \mathbf{0} \end{bmatrix}. \]

This structure mirrors the continuous-time constrained dynamics, with accelerations replaced by post-impact velocities and multipliers by impulses. Different restitution models (e.g., Newton or Poisson) adjust the right-hand side to permit partial bouncing.

We now implement a simple KKT-based contact solver in Python for the continuous-time constrained dynamics. We assume that previous lessons (and libraries) provide \( \mathbf{M}(\mathbf{q}) \) and \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \).

import numpy as np

def solve_contact_dynamics(M, h, Jc, tau, f_ext, qd, ac_des=None):
    """
    Solve
        [ M  -Jc.T ] [ qddot ] = [ S.T tau + f_ext - h ]
        [ Jc   0   ] [ lambda ]   [ ac_des - Jc_dot qd ]
    for qddot and lambda, given M, h, Jc at the current state.

    Parameters
    ----------
    M : (n,n) array_like
        Joint-space inertia matrix M(q).
    h : (n,) array_like
        Bias term h(q, qd).
    Jc : (m,n) array_like
        Contact Jacobian for active constraints.
    tau : (n_act,) or (n,) array_like
        Generalized effort; here we assume full actuation for simplicity.
    f_ext : (n,) array_like
        Generalized non-contact external forces.
    qd : (n,) array_like
        Generalized velocities.
    ac_des : (m,) array_like or None
        Desired constraint acceleration; if None, zero is used (rigid contact).

    Returns
    -------
    qddot : (n,) ndarray
        Joint accelerations satisfying the constraints.
    lam : (m,) ndarray
        Contact forces in constraint space.
    """
    M = np.asarray(M)
    h = np.asarray(h).reshape(-1)
    Jc = np.asarray(Jc)
    qd = np.asarray(qd).reshape(-1)
    f_ext = np.asarray(f_ext).reshape(-1)
    n = M.shape[0]
    m = Jc.shape[0]

    if ac_des is None:
        ac_des = np.zeros(m)
    else:
        ac_des = np.asarray(ac_des).reshape(-1)

    # For teaching purposes, we approximate Jc_dot qd numerically using a small finite difference.
    # In practice, robotics libraries (e.g., Pinocchio) can compute Jc_dot exactly.
    eps = 1e-6
    # Dummy approximation: assume Jc_dot qd = 0 here (quasi-static / low-speed assumption)
    Jc_dot_qd = np.zeros(m)

    # Assemble KKT matrix and right-hand side
    KKT = np.block([
        [M, -Jc.T],
        [Jc, np.zeros((m, m))]
    ])

    rhs = np.concatenate([
        tau + f_ext - h,
        ac_des - Jc_dot_qd
    ])

    sol = np.linalg.solve(KKT, rhs)
    qddot = sol[:n]
    lam = sol[n:]

    return qddot, lam

# Example: simple 2-DOF planar model with one holonomic contact constraint
if __name__ == "__main__":
    n = 2
    M = np.array([[2.0, 0.1],
                  [0.1, 1.0]])
    h = np.array([0.0, 0.0])
    Jc = np.array([[1.0, 1.0]])  # scalar constraint: q1 + q2 = constant
    tau = np.array([0.0, 0.0])
    f_ext = np.array([0.0, 0.0])
    qd = np.array([0.0, 0.0])

    qddot, lam = solve_contact_dynamics(M, h, Jc, tau, f_ext, qd)
    print("qddot =", qddot)
    print("lambda =", lam)
      

In a realistic floating-base setting, one would obtain M, h, and Jc from a robotics dynamics library such as pinocchio or a Python port of RBDL, using a URDF description of the robot. The KKT solution pattern remains identical.

In C++, libraries such as RBDL and Pinocchio (C++ core) provide routines for computing \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \), and frame Jacobians. Below is a minimal example using Eigen matrices and a schematic call to RBDL to obtain model-dependent quantities.

#include <iostream>
#include <Eigen/Dense>
// #include <rbdl/rbdl.h>  // if using RBDL

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

struct ContactSystem {
    MatrixXd M;  // (n x n)
    VectorXd h;  // (n)
    MatrixXd Jc; // (m x n)
};

ContactSystem buildContactSystem(const VectorXd& q,
                                 const VectorXd& qd) {
    // In a real implementation, call RBDL or Pinocchio here:
    //   RigidBodyDynamics::Model model;
    //   RigidBodyDynamics::CompositeRigidBodyAlgorithm(model, q, M, update_kinematics);
    //   RigidBodyDynamics::InverseDynamics(model, q, qd, VectorXd::Zero(n), h);
    //   RigidBodyDynamics::CalcPointJacobian6D(model, q, body_id, contact_point, Jc);
    // For illustration we use small constant matrices.
    std::size_t n = 2;
    std::size_t m = 1;
    ContactSystem sys;
    sys.M = MatrixXd::Zero(n, n);
    sys.M(0,0) = 2.0;
    sys.M(1,1) = 1.0;
    sys.M(0,1) = 0.1;
    sys.M(1,0) = 0.1;

    sys.h = VectorXd::Zero(n);
    sys.Jc = MatrixXd::Zero(m, n);
    sys.Jc(0,0) = 1.0;
    sys.Jc(0,1) = 1.0;
    return sys;
}

void solveContactDynamics(const ContactSystem& sys,
                          const VectorXd& tau,
                          const VectorXd& f_ext,
                          const VectorXd& qd,
                          VectorXd& qddot_out,
                          VectorXd& lambda_out) {
    std::size_t n = sys.M.rows();
    std::size_t m = sys.Jc.rows();

    MatrixXd KKT(n + m, n + m);
    KKT.setZero();
    KKT.block(0, 0, n, n) = sys.M;
    KKT.block(0, n, n, m) = -sys.Jc.transpose();
    KKT.block(n, 0, m, n) = sys.Jc;

    VectorXd rhs(n + m);
    rhs.head(n) = tau + f_ext - sys.h;
    rhs.tail(m).setZero();  // ac_des - Jc_dot * qd, here assumed zero

    VectorXd sol = KKT.fullPivLu().solve(rhs);
    qddot_out = sol.head(n);
    lambda_out = sol.tail(m);
}

int main() {
    VectorXd q(2), qd(2), tau(2), f_ext(2);
    q.setZero();
    qd.setZero();
    tau.setZero();
    f_ext.setZero();

    ContactSystem sys = buildContactSystem(q, qd);
    VectorXd qddot(2), lambda(1);
    solveContactDynamics(sys, tau, f_ext, qd, qddot, lambda);

    std::cout << "qddot = " << qddot.transpose() << std::endl;
    std::cout << "lambda = " << lambda.transpose() << std::endl;
    return 0;
}
      

The mapping from contact forces in constraint space (lambda) to joint torques is Jc.transpose() * lambda, consistent with the theoretical development.

In Java, the EJML library provides efficient dense linear algebra suitable for implementing contact solvers. Below is a basic KKT solver using EJML for a small system. Here we assume that robot-specific functions provide M, h, and Jc.

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

public class ContactDynamics {

    public static class ContactSystem {
        public DMatrixRMaj M;   // (n x n)
        public DMatrixRMaj h;   // (n x 1)
        public DMatrixRMaj Jc;  // (m x n)
    }

    public static void solveContactDynamics(ContactSystem sys,
                                            DMatrixRMaj tau,
                                            DMatrixRMaj fExt,
                                            DMatrixRMaj qd,
                                            DMatrixRMaj qddotOut,
                                            DMatrixRMaj lambdaOut) {
        int n = sys.M.numRows;
        int m = sys.Jc.numRows;

        DMatrixRMaj KKT = new DMatrixRMaj(n + m, n + m);
        CommonOps_DDRM.fill(KKT, 0.0);

        // Top-left block: M
        CommonOps_DDRM.insert(sys.M, KKT, 0, 0);

        // Top-right block: -Jc.T
        DMatrixRMaj JcT = new DMatrixRMaj(sys.Jc.numCols, sys.Jc.numRows);
        CommonOps_DDRM.transpose(sys.Jc, JcT);
        CommonOps_DDRM.scale(-1.0, JcT);
        CommonOps_DDRM.insert(JcT, KKT, 0, n);

        // Bottom-left block: Jc
        CommonOps_DDRM.insert(sys.Jc, KKT, n, 0);

        // RHS
        DMatrixRMaj rhs = new DMatrixRMaj(n + m, 1);
        DMatrixRMaj temp = new DMatrixRMaj(n, 1);

        // tau + fExt - h
        CommonOps_DDRM.add(tau, fExt, temp);
        CommonOps_DDRM.subtractEquals(temp, sys.h);
        CommonOps_DDRM.insert(temp, rhs, 0, 0);

        // Constraint acceleration term (assumed zero)
        for (int i = 0; i < m; ++i) {
            rhs.set(n + i, 0, 0.0);
        }

        // Solve KKT * sol = rhs
        DMatrixRMaj sol = new DMatrixRMaj(n + m, 1);
        CommonOps_DDRM.solve(KKT, rhs, sol);

        for (int i = 0; i < n; ++i) {
            qddotOut.set(i, 0, sol.get(i, 0));
        }
        for (int i = 0; i < m; ++i) {
            lambdaOut.set(i, 0, sol.get(n + i, 0));
        }
    }
}
      

A Java-based robotics stack can combine such solvers with kinematics engines implemented in Java (or bindings to C++ libraries) to simulate floating-base robots with contact.

MATLAB offers direct matrix operations for implementing contact dynamics. The following function solves the continuous-time KKT system for given \( \mathbf{M}, \mathbf{h}, \mathbf{J}_c, \boldsymbol{\tau}, \mathbf{f}_{ext} \). This function can be wrapped in a MATLAB Function block inside Simulink.

function [qddot, lambda] = contact_dynamics(M, h, Jc, tau, f_ext, qd, ac_des)
%CONTACT_DYNAMICS Solve constrained dynamics via KKT system.
%   M*qddot + h = tau + f_ext + Jc.'*lambda
%   Jc*qddot + Jc_dot*qd = ac_des
%
% Inputs:
%   M      (n x n)     inertia matrix
%   h      (n x 1)     bias term
%   Jc     (m x n)     contact Jacobian
%   tau    (n x 1)     generalized effort
%   f_ext  (n x 1)     external forces
%   qd     (n x 1)     velocities
%   ac_des (m x 1)     desired constraint acceleration (0 for rigid contact)
%
% Outputs:
%   qddot  (n x 1)     accelerations
%   lambda (m x 1)     contact forces

if nargin < 7 || isempty(ac_des)
    ac_des = zeros(size(Jc,1),1);
end

n = size(M,1);
m = size(Jc,1);

% Approximate Jc_dot*qd as zero for low-speed motions
Jc_dot_qd = zeros(m,1);

KKT = [M, -Jc.';
       Jc, zeros(m,m)];

rhs = [tau + f_ext - h;
       ac_des - Jc_dot_qd];

sol = KKT \ rhs;
qddot  = sol(1:n);
lambda = sol(n+1:end);
end
      

In Simulink, one can:

1. Create a MATLAB Function block with signature [qddot, lambda] = fcn(M,h,Jc,tau,f_ext,qd).
2. Inside, call contact_dynamics as above.
3. Feed qddot into an integrator chain to obtain qd and q, and re-compute M, h, and Jc at each time step.

Simscape Multibody provides built-in rigid contact elements, but the above formulation makes the constraint structure explicit, which is useful for analysis.

Wolfram Mathematica can be used to derive contact-constrained equations of motion symbolically. Consider a simple planar model with configuration \( q(t) \), Lagrangian \( L(q,\dot{q}) \), and a single holonomic contact constraint \( \phi(q) = 0 \). The Euler–Lagrange equations with a Lagrange multiplier \( \lambda(t) \) are

\[ \frac{\mathrm{d}}{\mathrm{d}t} \frac{\partial L}{\partial \dot{q}} - \frac{\partial L}{\partial q} = \lambda\,\frac{\partial \phi}{\partial q}, \quad \phi(q) = 0. \]

The following Mathematica code sets up and solves this system symbolically:

(* Define symbols *)
Clear[q, L, phi, lambda];
q[t_] := q[t];

(* Example: point mass with Lagrangian L = 1/2 m qdot^2 - V(q) *)
m = Symbol["m"];
V[q_] := 0; (* flat potential for simplicity *)
Lagr[qdot_] := 1/2 m qdot^2 - V[q[t]];

(* Holonomic constraint: phi(q) == 0 *)
phi[q_] := q[t];

(* Compute Euler-Lagrange equation with multiplier lambda[t] *)
qdot = D[q[t], t];
eqEL = D[D[Lagr[qdot], qdot], t] - D[Lagr[qdot], q[t]]
       == lambda[t] D[phi[q], q[t]];

eqConstraint = phi[q] == 0;

(* Express in terms of q''[t] and lambda[t] *)
sol = Solve[{eqEL, eqConstraint}, {D[q[t], {t, 2}], lambda[t]}];

FullSimplify[sol]
      

For multi-DOF floating-base robots, one would vectorize q, define a matrix-valued constraint phi[q_List], and solve for the vector of accelerations and multipliers. The structure of the resulting equations aligns with the KKT form discussed earlier.

Problem 1 (Bilateral Contact for a Floating Base):
Consider a 2D floating-base system with configuration \( \mathbf{q} = [x\; y\; \theta\; q_1]^\top \), where \( (x,y) \) is the base position, \( \theta \) its orientation, and \( q_1 \) a joint angle. A point on the end-effector has coordinates \( \mathbf{p}_c(\mathbf{q}) = (x + \ell\cos(\theta + q_1),\; y + \ell\sin(\theta + q_1))^\top \). The end-effector is required to be fixed at a world point \( \mathbf{p}_s = (x_s, y_s)^\top \).
(a) Derive the holonomic contact constraint \( \boldsymbol{\phi}_b(\mathbf{q}) \) and its Jacobian \( \mathbf{J}_b(\mathbf{q}) \).
(b) Show explicitly that \( \mathbf{J}_b(\mathbf{q}) \dot{\mathbf{q}} = \mathbf{0} \) enforces zero contact point velocity.

Solution:
(a) The contact constraint enforces \( \mathbf{p}_c(\mathbf{q}) = \mathbf{p}_s \), so

\[ \boldsymbol{\phi}_b(\mathbf{q}) = \begin{bmatrix} x + \ell\cos(\theta + q_1) - x_s \\ y + \ell\sin(\theta + q_1) - y_s \end{bmatrix} \in \mathbb{R}^2. \]

The Jacobian is

\[ \mathbf{J}_b(\mathbf{q}) = \frac{\partial \boldsymbol{\phi}_b}{\partial \mathbf{q}} = \begin{bmatrix} 1 & 0 & -\ell\sin(\theta + q_1) & -\ell\sin(\theta + q_1) \\ 0 & 1 & \;\;\ell\cos(\theta + q_1) & \;\;\ell\cos(\theta + q_1) \end{bmatrix}. \]

(b) The end-effector velocity is

\[ \dot{\mathbf{p}}_c = \begin{bmatrix} \dot{x} - \ell\sin(\theta + q_1)(\dot{\theta} + \dot{q}_1) \\ \dot{y} + \ell\cos(\theta + q_1)(\dot{\theta} + \dot{q}_1) \end{bmatrix}. \]

Multiplying the Jacobian by \( \dot{\mathbf{q}} \) gives

\[ \mathbf{J}_b(\mathbf{q}) \dot{\mathbf{q}} = \begin{bmatrix} \dot{x} - \ell\sin(\theta + q_1)(\dot{\theta} + \dot{q}_1) \\ \dot{y} + \ell\cos(\theta + q_1)(\dot{\theta} + \dot{q}_1) \end{bmatrix} = \dot{\mathbf{p}}_c. \]

Thus \( \mathbf{J}_b(\mathbf{q}) \dot{\mathbf{q}} = \mathbf{0} \) is equivalent to \( \dot{\mathbf{p}}_c = \mathbf{0} \), i.e. zero contact point velocity.

Problem 2 (KKT System for a Single Bilateral Constraint):
Let \( \mathbf{M} \in \mathbb{R}^{n \times n} \) be positive definite, and let \( \mathbf{J} \in \mathbb{R}^{1 \times n} \) represent a single bilateral constraint. Show that the KKT system

\[ \begin{bmatrix} \mathbf{M} & -\mathbf{J}^\top \\ \mathbf{J} & 0 \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\[0.3em] \lambda \end{bmatrix} = \begin{bmatrix} \mathbf{b} \\[0.3em] c \end{bmatrix} \]

has a unique solution if \( \mathbf{J} \) is nonzero. Derive the explicit expression for \( \lambda \).

Solution:
From the first block row,

\[ \mathbf{M}\ddot{\mathbf{q}} - \mathbf{J}^\top \lambda = \mathbf{b} \quad \Rightarrow \quad \ddot{\mathbf{q}} = \mathbf{M}^{-1}(\mathbf{b} + \mathbf{J}^\top \lambda). \]

Substituting into the second block row gives

\[ \mathbf{J}\ddot{\mathbf{q}} = c \quad \Rightarrow \quad \mathbf{J}\mathbf{M}^{-1}(\mathbf{b} + \mathbf{J}^\top \lambda) = c. \]

Since \( \mathbf{J} \) is a row vector, \( \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^\top \in \mathbb{R} \) is a scalar. Rearranging,

\[ \mathbf{J}\mathbf{M}^{-1}\mathbf{b} + \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^\top \lambda = c \quad \Rightarrow \quad \lambda = \frac{c - \mathbf{J}\mathbf{M}^{-1}\mathbf{b}} {\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^\top}. \]

Because \( \mathbf{M} \) is positive definite and \( \mathbf{J} \neq \mathbf{0} \), the denominator is strictly positive, so \( \lambda \) is uniquely defined. Substituting back yields a unique \( \ddot{\mathbf{q}} \) as well.

Problem 3 (Unilateral Contact States):
A point mass moves vertically above a rigid ground at \( y = 0 \), with configuration \( q = y \). The gap function is \( g_n(q) = y \). The normal contact force is denoted \( \lambda_n \). Describe, in terms of \( y \), \( \dot{y} \), and \( \lambda_n \), the conditions that characterize (i) flying phase, (ii) sticking contact phase, and (iii) liftoff.

Solution (including state-transition diagram):
The regimes can be summarized as:

flowchart TD
  A["Start"] --> B["y > 0 (above ground)"]
  B --> C["Flying: lambda_n = 0"]
  C --> D["Integrate unconstrained dynamics"]
  D --> E["Check y = 0 and ydot <= 0?"]
  E -->|no| C
  E -->|yes| F["Impact event; switch to contact"]
  F --> G["Sticking contact: y = 0, ydot = 0, lambda_n >= 0"]
  G --> H["Compute lambda_n from KKT system"]
  H --> I["Check lambda_n > 0?"]
  I -->|yes| G
  I -->|no| J["Liftoff: lambda_n = 0, switch to flying"]
        

(i) Flying phase: we have \( y > 0 \), so the unilateral constraint is inactive and complementarity enforces \( \lambda_n = 0 \). Dynamics are unconstrained.
(ii) Sticking contact phase: the mass is at the ground with \( y = 0 \) and \( \dot{y} = 0 \), and the KKT system yields a compressive reaction \( \lambda_n \geq 0 \) that prevents penetration.
(iii) Liftoff: when the KKT solution produces \( \lambda_n \leq 0 \) at \( y = 0 \), the unilateral model cannot sustain a tensile force. We set \( \lambda_n = 0 \) and switch to the flying phase dynamics with \( y \geq 0 \).

Problem 4 (Impact Velocity for a Simple Hopper):
A 1D hopper of mass \( m \) moves vertically and impacts the ground at \( y = 0 \). Just before impact the velocity is \( \dot{y}^- < 0 \). Assume a perfectly plastic impact (no bounce) and a unilateral constraint \( y \geq 0 \). Using the impulse-momentum equation and constraint \( \dot{y}^+ = 0 \), compute the normal impulse \( \Lambda_n \) and justify that complementarity is satisfied.

Solution:
The impulse-momentum equation in 1D is

\[ m(\dot{y}^+ - \dot{y}^-) = \Lambda_n. \]

For a plastic impact we enforce \( \dot{y}^+ = 0 \), so

\[ -m\dot{y}^- = \Lambda_n. \]

Since \( \dot{y}^- < 0 \), we have \( \Lambda_n = -m\dot{y}^- > 0 \), i.e. the impulse is compressive. During impact, the configuration does not change, so \( g_n(q) = y = 0 \). Complementarity is satisfied in the limit sense: \( g_n = 0 \) and \( \Lambda_n \geq 0 \), consistent with an inelastic sticking impact in the rigid unilateral contact model.

In this lesson we modeled contact for floating-base and multi-body robots as kinematic and dynamic constraints. We introduced holonomic bilateral constraints and unilateral constraints with complementarity, defined contact Jacobians, and derived constrained equations of motion in KKT form for both continuous-time dynamics and impulsive impacts. We then implemented KKT-based solvers in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, emphasizing how contact forces enter as Lagrange multipliers. These modeling tools are foundational for simulating legged locomotion and manipulation in subsequent modeling and control studies, even though control laws are not yet considered here.