Contact Dynamics · Numerical Simulation

MATLAB Reimplementation of CANAL

Cascaded Newton-based Augmented Lagrangian for rigid-body contact simulation

Contact Dynamics Numerical Optimization Augmented Lagrangian MATLAB

Demo

Example rigid-contact simulation from my MATLAB reimplementation of the CANAL solver.

Overview

As part of my research experience in the Interactive and Networked Robotics Laboratory at Seoul National University, I studied numerical methods for rigid-body contact dynamics and implemented a MATLAB version of the Cascaded Newton-based Augmented Lagrangian (CANAL) contact solver.

The project focused on understanding the numerical structure of complementarity-based rigid-contact simulation by reproducing the core solver pipeline and validating it on simplified rigid-body contact scenarios.

Notation

\( \hat{v} \) representative generalized velocity for the current simulation step
\( \lambda \) constraint impulse; for contact, it contains normal and tangential impulses
\( J \) constraint Jacobian mapping generalized velocity to constraint-space velocity
\( i \) index of the \(i\)-th constraint or contact
\( k \) simulation time-step index
\( t_k \) integration time step
\( \lambda_{i,n} \) normal contact impulse at contact \(i\)
\( \lambda_{i,t} \) tangential friction impulse at contact \(i\)
\( \mu_i \) Coulomb friction coefficient at contact \(i\)
\( \delta_i \) auxiliary variable associated with slip and the friction-cone boundary
\( e_i \) constraint error / stabilization bias used in the velocity-level constraint
\( A,\; b \) matrix and vector collecting the discretized rigid-body dynamics
\( z \) slack variable representing the constraint-space velocity interface
\( u \) augmented-Lagrangian multiplier
\( \beta \) augmented-Lagrangian penalty parameter

Key Formulation

Signorini–Coulomb Contact NCP

\[ 0 \leq \lambda_{i,n} \;\perp\; J_{i,n}\hat{v} + e_{i,n} \geq 0 \] \[ 0 \leq \delta_i \;\perp\; \mu_i \lambda_{i,n} - \left\|\lambda_{i,t}\right\| \geq 0 \] \[ \delta_i \lambda_{i,t} + \mu_i \lambda_{i,n} J_{i,t}\hat{v} = 0 \]

Rigid frictional contact is modeled as a nonlinear complementarity problem (NCP). The normal condition enforces unilateral contact, while the tangential conditions capture sticking and Coulomb frictional sliding.

Multi-Contact Dynamics

\[ \text{Solve} \qquad A\hat{v} = b + J^{T}\lambda \] \[ \text{subject to} \qquad (J\hat{v},\lambda) \in S_c \]

Each simulation step couples the discretized rigid-body dynamics with the nonlinear contact constraints represented by the set \(S_c\).

Slack-Variable Formulation

\[ A\hat{v} = b + J^{T}\lambda, \qquad (z,\lambda)\in S_c, \qquad J\hat{v}=z \]

CANAL introduces the slack variable \(z\) to separate the dynamics from the contact interface. This enables the contact problem to be treated within an augmented-Lagrangian framework.

Augmented-Lagrangian Update

\[ u \leftarrow u + \beta \left( J\hat{v}-z \right) \]

The outer augmented-Lagrangian iteration updates the multiplier \(u\) according to the mismatch between the physical constraint velocity \(J\hat{v}\) and the slack variable \(z\).

Cascaded Newton Inner Solve

\[ r(\hat{v}) = A\hat{v} - b - \sum_i J_i^{T}\lambda_i \]

Within each augmented-Lagrangian iteration, CANAL solves a convexified surrogate contact problem using a Newton-based procedure. The cascaded construction avoids directly applying a standard semismooth Newton solver to the original non-convex contact residual.

What I Implemented

Contact Dynamics Formulation

Implemented discretized rigid-body dynamics together with unilateral and frictional contact constraints in MATLAB.

Contact Projection

Implemented contact impulse updates and friction-cone operations required to distinguish open, sticking, and sliding contact states.

Augmented-Lagrangian Iteration

Implemented the outer primal-dual iteration that gradually enforces consistency between system dynamics and the contact constraints.

Cascaded Newton Solver

Reproduced the cascaded Newton structure used to solve the inner surrogate problem, including Newton updates and line-search-based step selection.

Simulation Pipeline

Built the simulation loop from contact detection and constraint construction through solver execution and rigid-body state integration.

Numerical Validation

Tested the implementation on simplified rigid-contact scenarios to examine convergence, contact forces, and resulting object motion.

Solver Structure

Rigid-body state
→
Contact NCP
→
Surrogate problem
→
Cascaded Newton solve
→
AL update
→
State integration

Outcome

This project gave me hands-on experience translating a mathematical contact model into a working physics simulator. In particular, I gained experience with nonlinear complementarity problems, frictional contact modeling, augmented-Lagrangian optimization, Newton methods, and numerical debugging of rigid-body simulation.

The implementation was a technical reproduction and learning project based on the CANAL formulation rather than a new contact-solver contribution.

Reference

J. Lee, M. Lee, S. Park, J. Yun, and D. Lee, “Variations of Augmented Lagrangian for Robotic Multi-Contact Simulation.”

Reference Paper →