From de0b948c49a60088a50de327bf42bff845d814d5 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 7 Apr 2026 17:24:51 +0200 Subject: [PATCH 01/12] python/algo/solvers: expose all solvers constructors --- bindings/python/algorithm/solvers/expose-admm-solver.cpp | 7 ++++++- bindings/python/algorithm/solvers/expose-pgs-solver.cpp | 7 ++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/bindings/python/algorithm/solvers/expose-admm-solver.cpp b/bindings/python/algorithm/solvers/expose-admm-solver.cpp index 6eff45ae60..16f66aa0f9 100644 --- a/bindings/python/algorithm/solvers/expose-admm-solver.cpp +++ b/bindings/python/algorithm/solvers/expose-admm-solver.cpp @@ -339,8 +339,13 @@ namespace pinocchio bp::class_ cl( "ADMMConstraintSolver", "Alternating Direction Method of Multipliers (ADMM) solver for contact dynamics.", + bp::init<>(bp::arg("self"), "Default constructor.")); + cl.def( bp::init( - bp::args("self", "problem_size"), "Constructor with problem dimension.")); + bp::args("self", "problem_size"), + "Constructor with problem dimension. Allows to pre-allocate data if problem_size is " + "known in advance. The solver will automatically resize its workspace and the result in " + "any case.")); cl // Base solver diff --git a/bindings/python/algorithm/solvers/expose-pgs-solver.cpp b/bindings/python/algorithm/solvers/expose-pgs-solver.cpp index fb11c6cef6..d95fb590c6 100644 --- a/bindings/python/algorithm/solvers/expose-pgs-solver.cpp +++ b/bindings/python/algorithm/solvers/expose-pgs-solver.cpp @@ -206,8 +206,13 @@ namespace pinocchio // Expose the solver itself bp::class_ cl( "PGSConstraintSolver", "Projected Gauss-Seidel (PGS) solver for contact dynamics.", + bp::init<>(bp::arg("self"), "Default constructor.")); + cl.def( bp::init( - bp::args("self", "problem_size"), "Constructor with problem dimension.")); + bp::args("self", "problem_size"), + "Constructor with problem dimension. Allows to pre-allocate data if problem_size is " + "known in advance. The solver will automatically resize its workspace and the result in " + "any case.")); cl // Base solver From 57e0e60031b78fbfb0a28b80bdb935be69bc7bd5 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 7 Apr 2026 17:26:31 +0200 Subject: [PATCH 02/12] examples: add admm constraint solver example --- examples/admm-constraint-solver.py | 216 +++++++++++++++++++++++++++++ 1 file changed, 216 insertions(+) create mode 100644 examples/admm-constraint-solver.py diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py new file mode 100644 index 0000000000..9094f50728 --- /dev/null +++ b/examples/admm-constraint-solver.py @@ -0,0 +1,216 @@ +""" +Stack of two cubes solved with the ADMM constraint solver. + +This example demonstrates how to: + - Build a kinematic model with two free-floating cubes + - Build a geometry model with the cube shapes and a ground plane + - Manually define 8 PointContactConstraintModel constraints: + 4 for the floor-cube 1 interaction (bottom corners of cube 1) + 4 for the cube 1-cube 2 interaction (corners at the shared face) + - Build the Delassus operator via the Cholesky decomposition + - Compute the constraint drift vector g = J * v_free + - Solve the constrained problem with the ADMM solver + - Print convergence statistics +""" + +import sys + +import numpy as np +import pinocchio as pin + +# ─── 1. Kinematic / dynamic model ──────────────────────────────────────────── + +model = pin.Model() +# Default gravity is already (0, 0, -9.81, 0, 0, 0); shown here for clarity. +model.gravity = pin.Motion(np.array([0.0, 0.0, -9.81, 0.0, 0.0, 0.0])) + +box_size = 1.0 # edge length of each cube [m] +box_half = box_size / 2.0 +box_mass = 1.0 # mass of each cube [kg] + +box_inertia = pin.Inertia.FromBox(box_mass, box_size, box_size, box_size) + +# Cube 1 - free-flyer joint branching from the universe (joint 0) +joint1_id = model.addJoint( + 0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "box1_joint" +) +model.appendBodyToJoint(joint1_id, box_inertia, pin.SE3.Identity()) + +# Cube 2 - free-flyer joint, also branching from the universe +joint2_id = model.addJoint( + 0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "box2_joint" +) +model.appendBodyToJoint(joint2_id, box_inertia, pin.SE3.Identity()) + +# ─── 2. Geometry model ──────────────────────────────────────────────────────── + +geom_model = pin.GeometryModel() + +try: + import coal + + cube_shape = coal.Box(box_size, box_size, box_size) + + # Cube 1 - geometry in joint1's local frame (centred at the joint origin) + geom_box1 = pin.GeometryObject("box1", joint1_id, 0, pin.SE3.Identity(), cube_shape) + geom_box1.meshColor = np.array([0.2, 0.6, 0.2, 1.0]) + geom_model.addGeometryObject(geom_box1) + + # Cube 2 - geometry in joint2's local frame + geom_box2 = pin.GeometryObject("box2", joint2_id, 0, pin.SE3.Identity(), cube_shape) + geom_box2.meshColor = np.array([0.2, 0.2, 0.8, 1.0]) + geom_model.addGeometryObject(geom_box2) + + # Floor - half-space { p : p_z >= 0 } attached to the universe joint (id 0) + floor_shape = coal.Halfspace(np.array([0.0, 0.0, 1.0]), 0.0) + geom_floor = pin.GeometryObject("floor", 0, 0, pin.SE3.Identity(), floor_shape) + geom_floor.meshColor = np.array([0.5, 0.5, 0.5, 0.5]) + geom_model.addGeometryObject(geom_floor) + +except ImportError: + print("coal not found - geometry model will be empty (solver still runs).") + +# ─── 3. Initial configuration ───────────────────────────────────────────────── + +# Free-flyer q = [tx, ty, tz, qx, qy, qz, qw] +# Cube 1 centre at z = box_half (bottom face touching z = 0) +# Cube 2 centre at z = 3*box_half (bottom face touching the top of cube 1) +q_box1 = np.array([0.0, 0.0, box_half, 0.0, 0.0, 0.0, 1.0]) +q_box2 = np.array([0.0, 0.0, 3.0 * box_half, 0.0, 0.0, 0.0, 1.0]) +q0 = np.concatenate([q_box1, q_box2]) + +v0 = np.zeros(model.nv) # zero initial velocity +tau0 = np.zeros(model.nv) # no external torques +dt = 1e-3 # time-step [s] + +# ─── 4. Build the 8 contact constraints ────────────────────────────────────── + +friction_coeff = 0.8 + + +def make_corner_constraints(model, jid1, jid2, z1, z2): + """ + Return 4 PointContactConstraintModel objects for the corners of a planar + contact interface between two bodies. + + Parameters + ---------- + jid1, jid2 : joint indices of the two contacting bodies + z1 : z-coordinate of the contact plane in jid1's local frame + (e.g. +box_half for the top face, 0.0 for the world floor) + z2 : z-coordinate of the contact plane in jid2's local frame + (e.g. -box_half for the bottom face) + """ + corners_xy = [ + np.array([+box_half, +box_half]), + np.array([-box_half, +box_half]), + np.array([-box_half, -box_half]), + np.array([+box_half, -box_half]), + ] + cms = [] + for xy in corners_xy: + p1 = np.array([xy[0], xy[1], z1]) + p2 = np.array([xy[0], xy[1], z2]) + placement1 = pin.SE3(np.eye(3), p1) + placement2 = pin.SE3(np.eye(3), p2) + cm = pin.PointContactConstraintModel(model, jid1, placement1, jid2, placement2) + cm.set = pin.CoulombFrictionCone(friction_coeff) + cms.append(cm) + return cms + + +constraint_models = pin.StdVec_ConstraintModel() + +# 4 constraints: floor (universe, jid=0) ↔ cube 1 bottom face +# - in universe frame: contact points are on the floor plane z = 0 +# - in box1 local frame: contact points are at the bottom corners z = -box_half +for cm in make_corner_constraints(model, 0, joint1_id, 0.0, -box_half): + constraint_models.append(pin.ConstraintModel(cm)) + +# 4 constraints: cube 1 top face ↔ cube 2 bottom face +# - in box1 local frame: top corners at z = +box_half +# - in box2 local frame: bottom corners at z = -box_half +for cm in make_corner_constraints(model, joint1_id, joint2_id, +box_half, -box_half): + constraint_models.append(pin.ConstraintModel(cm)) + +total_residual_size = sum(cm.residualSize() for cm in constraint_models) +print(f"Number of constraints: {len(constraint_models)}") +print(f"Total constraint residual size: {total_residual_size}") + +# ─── 5. Delassus operator and drift vector ──────────────────────────────────── + +data = model.createData() +fext = [pin.Force.Zero() for _ in range(model.njoints)] + +# CRBA is required before building the Cholesky decomposition of the Delassus +# matrix G = J M⁻¹ Jᵀ. +pin.crba(model, data, q0, pin.Convention.WORLD) + +# Free acceleration: velocity the system would reach in one time-step without contacts. +v_free = v0 + dt * pin.aba(model, data, q0, v0, tau0, fext) + +# Initialise constraint data and evaluate constraint Jacobians at q0. +constraint_datas = pin.StdVec_ConstraintData() +for cmodel in constraint_models: + cdata = cmodel.createData() + cmodel.calc(model, data, cdata) + constraint_datas.append(cdata) + +# Cholesky decomposition of the Delassus matrix. +chol = pin.ConstraintCholeskyDecomposition( + model, data, constraint_models, constraint_datas +) +chol.compute(model, data, constraint_models, constraint_datas, 1e-10) + +# DelassusCholeskyExpression wraps the Cholesky factors for efficient solves. +delassus_expr = chol.getDelassusCholeskyExpression() + +# Constraint Jacobian and drift g = J v_free. +Jc = pin.getConstraintsJacobian(model, data, constraint_models, constraint_datas) +g = Jc @ v_free + +print(f"Delassus matrix size: {delassus_expr.matrix().shape}") +print(f"Drift vector ‖g‖: {np.linalg.norm(g):.4e}") + +# ─── 6. ADMM solver ─────────────────────────────────────────────────────────── + +solver = pin.ADMMConstraintSolver() + +settings = pin.ADMMSolverSettings() +settings.max_iterations = 1000 +settings.absolute_feasibility_tol = 1e-10 +settings.relative_feasibility_tol = 1e-12 +settings.absolute_complementarity_tol = 1e-10 +settings.relative_complementarity_tol = 1e-12 +settings.admm_update_rule = pin.ADMMUpdateRule.SPECTRAL +settings.mu_prox = 1e-6 +settings.stat_record = ( + True # per-iteration statistics. Turn off for faster solves if not needed. +) +settings.solve_ncp = True + +result = pin.ADMMSolverResult() +has_converged = solver.solve( + delassus_expr, g, constraint_models, constraint_datas, settings, result +) + +# ─── 7. Results ─────────────────────────────────────────────────────────────── + +print() +print("── ADMM solver results ──────────────────────────────────────────────") +print(f" Converged: {result.converged}") +print(f" Iterations: {result.iterations}") +print(f" Primal feasibility: {result.primal_feasibility:.4e}") +print(f" Dual feasibility: {result.dual_feasibility:.4e}") +print(f" Complementarity: {result.complementarity:.4e}") +print(f" Final rho: {result.rho:.4e}") +print("─────────────────────────────────────────────────────────────────────") + +impulses = result.retrieveConstraintImpulses() +velocities = result.retrieveConstraintVelocities() +print(f"\nConstraint impulses ‖λ‖: {np.linalg.norm(impulses):.4e}") +print(f"Constraint velocities ‖v‖: {np.linalg.norm(velocities):.4e}") + +if not has_converged: + print("\nWarning: solver did not converge within the iteration budget.") + sys.exit(1) From 85bf434b42dcb78974dfc9d5aee17afc36f133b1 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 7 Apr 2026 17:28:17 +0200 Subject: [PATCH 03/12] changelog/examples: update with admm-constraint-solver.py example --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 532871d383..b725c3225a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add `internal::matrix_product` in `math.hpp` - Add `internal::matrix_inversion` in `math.hpp` - Add `internal::matrix_inversion_code_generated` in `math.hpp` +- Add `examples/admm-constraint-solver.py`: how to use the constraints API to model a stack of cubes, how to use the Delassus operator, how to use the ADMM solver to solve the constraint problem. ### Changed - Clean delassus API: DelassusOperatorBase define the main delassus API and each method calls `derived().[name-of-method]Impl` From e092378e93c5b198e7d38b883f94f5fc18a50638 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 9 Apr 2026 11:00:03 +0200 Subject: [PATCH 04/12] cmake: Run admm-constraint-solver as a test --- examples/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index a784333088..547d699d53 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -122,6 +122,7 @@ if(BUILD_PYTHON_INTERFACE) model-graph model-configuration-converter ellipsoid-joint-kinematics + admm-constraint-solver ) if(BUILD_WITH_URDF_SUPPORT) From bf79d9df82613cd7d9e661a0ec0e2ba0249a2c01 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 9 Apr 2026 11:02:23 +0200 Subject: [PATCH 05/12] example: Don't call deprecated API --- examples/admm-constraint-solver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index 9094f50728..d451baf503 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -163,7 +163,7 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): chol.compute(model, data, constraint_models, constraint_datas, 1e-10) # DelassusCholeskyExpression wraps the Cholesky factors for efficient solves. -delassus_expr = chol.getDelassusCholeskyExpression() +delassus_expr = chol.getDelassusOperatorCholeskyExpression() # Constraint Jacobian and drift g = J v_free. Jc = pin.getConstraintsJacobian(model, data, constraint_models, constraint_datas) From 8020307a8fcd8f4435b5c18eb7657634d8e62cf3 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 9 Apr 2026 11:04:39 +0200 Subject: [PATCH 06/12] example: geom_model is not used --- examples/admm-constraint-solver.py | 38 ++++-------------------------- 1 file changed, 5 insertions(+), 33 deletions(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index d451baf503..798b092b58 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -42,35 +42,7 @@ ) model.appendBodyToJoint(joint2_id, box_inertia, pin.SE3.Identity()) -# ─── 2. Geometry model ──────────────────────────────────────────────────────── - -geom_model = pin.GeometryModel() - -try: - import coal - - cube_shape = coal.Box(box_size, box_size, box_size) - - # Cube 1 - geometry in joint1's local frame (centred at the joint origin) - geom_box1 = pin.GeometryObject("box1", joint1_id, 0, pin.SE3.Identity(), cube_shape) - geom_box1.meshColor = np.array([0.2, 0.6, 0.2, 1.0]) - geom_model.addGeometryObject(geom_box1) - - # Cube 2 - geometry in joint2's local frame - geom_box2 = pin.GeometryObject("box2", joint2_id, 0, pin.SE3.Identity(), cube_shape) - geom_box2.meshColor = np.array([0.2, 0.2, 0.8, 1.0]) - geom_model.addGeometryObject(geom_box2) - - # Floor - half-space { p : p_z >= 0 } attached to the universe joint (id 0) - floor_shape = coal.Halfspace(np.array([0.0, 0.0, 1.0]), 0.0) - geom_floor = pin.GeometryObject("floor", 0, 0, pin.SE3.Identity(), floor_shape) - geom_floor.meshColor = np.array([0.5, 0.5, 0.5, 0.5]) - geom_model.addGeometryObject(geom_floor) - -except ImportError: - print("coal not found - geometry model will be empty (solver still runs).") - -# ─── 3. Initial configuration ───────────────────────────────────────────────── +# ─── 2. Initial configuration ───────────────────────────────────────────────── # Free-flyer q = [tx, ty, tz, qx, qy, qz, qw] # Cube 1 centre at z = box_half (bottom face touching z = 0) @@ -83,7 +55,7 @@ tau0 = np.zeros(model.nv) # no external torques dt = 1e-3 # time-step [s] -# ─── 4. Build the 8 contact constraints ────────────────────────────────────── +# ─── 3. Build the 8 contact constraints ────────────────────────────────────── friction_coeff = 0.8 @@ -137,7 +109,7 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): print(f"Number of constraints: {len(constraint_models)}") print(f"Total constraint residual size: {total_residual_size}") -# ─── 5. Delassus operator and drift vector ──────────────────────────────────── +# ─── 4. Delassus operator and drift vector ──────────────────────────────────── data = model.createData() fext = [pin.Force.Zero() for _ in range(model.njoints)] @@ -172,7 +144,7 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): print(f"Delassus matrix size: {delassus_expr.matrix().shape}") print(f"Drift vector ‖g‖: {np.linalg.norm(g):.4e}") -# ─── 6. ADMM solver ─────────────────────────────────────────────────────────── +# ─── 5. ADMM solver ─────────────────────────────────────────────────────────── solver = pin.ADMMConstraintSolver() @@ -194,7 +166,7 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): delassus_expr, g, constraint_models, constraint_datas, settings, result ) -# ─── 7. Results ─────────────────────────────────────────────────────────────── +# ─── 6. Results ─────────────────────────────────────────────────────────────── print() print("── ADMM solver results ──────────────────────────────────────────────") From 527cf3d2d834268ed2b7ce5e5a180894d058c21d Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 9 Apr 2026 11:43:50 +0200 Subject: [PATCH 07/12] examples/admm: update test to handle multiple simulation steps --- examples/admm-constraint-solver.py | 153 ++++++++++++++++++----------- 1 file changed, 98 insertions(+), 55 deletions(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index 798b092b58..fc43027c40 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -52,7 +52,7 @@ q0 = np.concatenate([q_box1, q_box2]) v0 = np.zeros(model.nv) # zero initial velocity -tau0 = np.zeros(model.nv) # no external torques +zero_torque = np.zeros(model.nv) # no external torques dt = 1e-3 # time-step [s] # ─── 3. Build the 8 contact constraints ────────────────────────────────────── @@ -60,14 +60,14 @@ friction_coeff = 0.8 -def make_corner_constraints(model, jid1, jid2, z1, z2): +def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): """ Return 4 PointContactConstraintModel objects for the corners of a planar contact interface between two bodies. Parameters ---------- - jid1, jid2 : joint indices of the two contacting bodies + joint1_id, joint2_id : joint indices of the two contacting bodies z1 : z-coordinate of the contact plane in jid1's local frame (e.g. +box_half for the top face, 0.0 for the world floor) z2 : z-coordinate of the contact plane in jid2's local frame @@ -83,9 +83,11 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): for xy in corners_xy: p1 = np.array([xy[0], xy[1], z1]) p2 = np.array([xy[0], xy[1], z2]) - placement1 = pin.SE3(np.eye(3), p1) - placement2 = pin.SE3(np.eye(3), p2) - cm = pin.PointContactConstraintModel(model, jid1, placement1, jid2, placement2) + joint1_placement = pin.SE3(np.eye(3), p1) + joint2_placement = pin.SE3(np.eye(3), p2) + cm = pin.PointContactConstraintModel( + model, joint1_id, joint1_placement, joint2_id, joint2_placement + ) cm.set = pin.CoulombFrictionCone(friction_coeff) cms.append(cm) return cms @@ -109,45 +111,24 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): print(f"Number of constraints: {len(constraint_models)}") print(f"Total constraint residual size: {total_residual_size}") -# ─── 4. Delassus operator and drift vector ──────────────────────────────────── +# ─── 4. Setup and run the simulation loop ──────────────────────────────────── data = model.createData() fext = [pin.Force.Zero() for _ in range(model.njoints)] -# CRBA is required before building the Cholesky decomposition of the Delassus -# matrix G = J M⁻¹ Jᵀ. -pin.crba(model, data, q0, pin.Convention.WORLD) - -# Free acceleration: velocity the system would reach in one time-step without contacts. -v_free = v0 + dt * pin.aba(model, data, q0, v0, tau0, fext) - -# Initialise constraint data and evaluate constraint Jacobians at q0. +# Initialise constraint data constraint_datas = pin.StdVec_ConstraintData() for cmodel in constraint_models: cdata = cmodel.createData() - cmodel.calc(model, data, cdata) constraint_datas.append(cdata) -# Cholesky decomposition of the Delassus matrix. +# Initialize constraint cholesky chol = pin.ConstraintCholeskyDecomposition( model, data, constraint_models, constraint_datas ) -chol.compute(model, data, constraint_models, constraint_datas, 1e-10) - -# DelassusCholeskyExpression wraps the Cholesky factors for efficient solves. -delassus_expr = chol.getDelassusOperatorCholeskyExpression() - -# Constraint Jacobian and drift g = J v_free. -Jc = pin.getConstraintsJacobian(model, data, constraint_models, constraint_datas) -g = Jc @ v_free - -print(f"Delassus matrix size: {delassus_expr.matrix().shape}") -print(f"Drift vector ‖g‖: {np.linalg.norm(g):.4e}") - -# ─── 5. ADMM solver ─────────────────────────────────────────────────────────── +# Initialize constraint solver, its settings and result solver = pin.ADMMConstraintSolver() - settings = pin.ADMMSolverSettings() settings.max_iterations = 1000 settings.absolute_feasibility_tol = 1e-10 @@ -160,29 +141,91 @@ def make_corner_constraints(model, jid1, jid2, z1, z2): True # per-iteration statistics. Turn off for faster solves if not needed. ) settings.solve_ncp = True - result = pin.ADMMSolverResult() -has_converged = solver.solve( - delassus_expr, g, constraint_models, constraint_datas, settings, result -) -# ─── 6. Results ─────────────────────────────────────────────────────────────── - -print() -print("── ADMM solver results ──────────────────────────────────────────────") -print(f" Converged: {result.converged}") -print(f" Iterations: {result.iterations}") -print(f" Primal feasibility: {result.primal_feasibility:.4e}") -print(f" Dual feasibility: {result.dual_feasibility:.4e}") -print(f" Complementarity: {result.complementarity:.4e}") -print(f" Final rho: {result.rho:.4e}") -print("─────────────────────────────────────────────────────────────────────") - -impulses = result.retrieveConstraintImpulses() -velocities = result.retrieveConstraintVelocities() -print(f"\nConstraint impulses ‖λ‖: {np.linalg.norm(impulses):.4e}") -print(f"Constraint velocities ‖v‖: {np.linalg.norm(velocities):.4e}") - -if not has_converged: - print("\nWarning: solver did not converge within the iteration budget.") - sys.exit(1) +# Simulate for a few time-steps, solving the constraint problem at each step. +horizon = 10 +q = q0.copy() +v = v0.copy() +for t in range(horizon): + # CRBA is required before building the Cholesky decomposition of the Delassus + # matrix G = J M⁻¹ Jᵀ. + # Note that other delassus operators may not necessarily require CRBA. + pin.crba(model, data, q, pin.Convention.WORLD) + + # Free acceleration: velocity the system would reach in one + # time-step without contacts. + v_free = v + dt * pin.aba(model, data, q, v, zero_torque, fext) + + # The constraint models may need to be updated if the position of + # contact points changed. + # Note: we don't do it in this example to keep things simple + # (q does not change much since the cubes are stable), + # but it would look something like: + # for cmodel in constraint_models: + # cmodel.joint1_placement = ... # relative placement of the + # contact point in joint1's local frame. + # cmodel.joint2_placement = ... # relative placement of the + # contact point in joint2's local frame. + + # Run calc on constraint models + for cmodel, cdata in zip(constraint_models, constraint_datas): + cmodel.calc(model, data, cdata) + + # Cholesky decomposition of the Delassus matrix. + chol.compute(model, data, constraint_models, constraint_datas, 1e-10) + + # DelassusCholeskyExpression wraps the Cholesky factors for efficient solves. + delassus_expr = chol.getDelassusOperatorCholeskyExpression() + + # Constraint Jacobian and drift g = J v_free. + Jc = pin.getConstraintsJacobian(model, data, constraint_models, constraint_datas) + g = Jc @ v_free + + print(f"Delassus matrix size: {delassus_expr.matrix().shape}") + print(f"Drift vector ‖g‖: {np.linalg.norm(g):.4e}") + + # Solve the constraint problem with the ADMM solver. + has_converged = solver.solve( + delassus_expr, g, constraint_models, constraint_datas, settings, result + ) + + print() + print(f"time step: {t}") + print("── ADMM solver results ──────────────────────────────────────────────") + print(f" Converged: {result.converged}") + print(f" Iterations: {result.iterations}") + print(f" Primal feasibility: {result.primal_feasibility:.4e}") + print(f" Dual feasibility: {result.dual_feasibility:.4e}") + print(f" Complementarity: {result.complementarity:.4e}") + print(f" Final rho: {result.rho:.4e}") + print("─────────────────────────────────────────────────────────────────────") + + constraint_impulses = result.retrieveConstraintImpulses() + constraint_velocities = result.retrieveConstraintVelocities() + print(f"\nConstraint impulses ‖λ‖: {np.linalg.norm(constraint_impulses):.4e}") + print(f"Constraint velocities ‖v‖: {np.linalg.norm(constraint_velocities):.4e}") + + if not has_converged: + print("\nWarning: solver did not converge within the iteration budget.") + sys.exit(1) + + # Update configuration and velocity for the next time step by + # applying the constraint impulses. + # Note: we don't do it in this example to keep things simple, + # but it would look something like: + constraint_forces = ( + 1.0 / dt + ) * constraint_impulses # convert impulses to forces/torques + tau_constraints = Jc.T @ constraint_forces # map to generalized torques + v_new = v + dt * pin.aba(model, data, q, v, zero_torque + tau_constraints, fext) + q_new = pin.integrate(model, q, v_new * dt) + + # The cubes should be stable so the configuration should not change much, + # and the velocity should be close to zero: + assert np.linalg.norm(v_new) < 1e-8 + assert np.linalg.norm(q_new - q) < 1e-8 + + # Update q and v for the next iteration. + q = q_new.copy() + v = v_new.copy() From f878482091fe8abeaaf14384a85f094481aa884a Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 9 Apr 2026 13:44:46 +0200 Subject: [PATCH 08/12] examples/admm: update comment and fix setFriction --- examples/admm-constraint-solver.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index fc43027c40..606932db17 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -88,7 +88,7 @@ def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): cm = pin.PointContactConstraintModel( model, joint1_id, joint1_placement, joint2_id, joint2_placement ) - cm.set = pin.CoulombFrictionCone(friction_coeff) + cm.setFriction(friction_coeff) cms.append(cm) return cms @@ -157,11 +157,11 @@ def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): # time-step without contacts. v_free = v + dt * pin.aba(model, data, q, v, zero_torque, fext) - # The constraint models may need to be updated if the position of - # contact points changed. - # Note: we don't do it in this example to keep things simple - # (q does not change much since the cubes are stable), - # but it would look something like: + # In theory, the constraint models' placements need to be updated at each time step + # of the simulation to reflect the current configuration q. + # We don't do it in this specific example since the cubes are stable and the contact + # positions don't change. + # In a more general case, you would need to do something like: # for cmodel in constraint_models: # cmodel.joint1_placement = ... # relative placement of the # contact point in joint1's local frame. From 5019189f86b25ecfc6d7569341ce41ca35470137 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 9 Apr 2026 13:48:16 +0200 Subject: [PATCH 09/12] examples/admm: remove useless comment --- examples/admm-constraint-solver.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index 606932db17..29db7960e3 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -212,8 +212,6 @@ def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): # Update configuration and velocity for the next time step by # applying the constraint impulses. - # Note: we don't do it in this example to keep things simple, - # but it would look something like: constraint_forces = ( 1.0 / dt ) * constraint_impulses # convert impulses to forces/torques From 64af983cdd4c4e2319b0f66c657b2995e8d43be6 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 9 Apr 2026 13:49:29 +0200 Subject: [PATCH 10/12] example: Use more idiomatic python --- examples/admm-constraint-solver.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index 29db7960e3..0b54d8abf9 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -93,7 +93,7 @@ def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): return cms -constraint_models = pin.StdVec_ConstraintModel() +constraint_models = [] # 4 constraints: floor (universe, jid=0) ↔ cube 1 bottom face # - in universe frame: contact points are on the floor plane z = 0 @@ -117,7 +117,7 @@ def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): fext = [pin.Force.Zero() for _ in range(model.njoints)] # Initialise constraint data -constraint_datas = pin.StdVec_ConstraintData() +constraint_datas = [] for cmodel in constraint_models: cdata = cmodel.createData() constraint_datas.append(cdata) From 7f371e0de57d7e9b13c46c14db84e2f10c74dd39 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 9 Apr 2026 14:14:58 +0200 Subject: [PATCH 11/12] example: Fix a comment --- examples/admm-constraint-solver.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index 0b54d8abf9..85948e4788 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -3,7 +3,6 @@ This example demonstrates how to: - Build a kinematic model with two free-floating cubes - - Build a geometry model with the cube shapes and a ground plane - Manually define 8 PointContactConstraintModel constraints: 4 for the floor-cube 1 interaction (bottom corners of cube 1) 4 for the cube 1-cube 2 interaction (corners at the shared face) From d614faf77651a64bd77312bd98da656c171c86dd Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 9 Apr 2026 14:16:01 +0200 Subject: [PATCH 12/12] examples/admm: update simulation loop --- examples/admm-constraint-solver.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/examples/admm-constraint-solver.py b/examples/admm-constraint-solver.py index 85948e4788..ebe5f99193 100644 --- a/examples/admm-constraint-solver.py +++ b/examples/admm-constraint-solver.py @@ -147,6 +147,12 @@ def make_corner_constraints(model, joint1_id, joint2_id, z1, z2): q = q0.copy() v = v0.copy() for t in range(horizon): + # Data needs to be informed of the current state of the system for + # downstream computations. + data.q_in = q + data.v_in = v + data.tau_in = zero_torque + # CRBA is required before building the Cholesky decomposition of the Delassus # matrix G = J M⁻¹ Jᵀ. # Note that other delassus operators may not necessarily require CRBA.