PV-OSIMr: A Lowest Order Complexity Algorithm for Computing the Delassus Matrix

Authors: Ajay Suresha Sathya, Wilm Decré, Jan Swevers · IEEE Robotics and Automation Letters (RA-L) 2024

Licences

Open Access
Paper: CC BY-SA 4.0

Everything below draws on the full paper under its CC BY-SA 4.0 licence; its figure is credited to the authors.

TL;DR

Reorders the PV-OSIM computations so propagated constraints never exceed six dimensions, giving O(n + m²) Delassus-matrix computation, which the authors show is asymptotically optimal.

Why it matters

Robots with many contacts, such as humanoids with feet and multi-fingered hands, need this matrix at high rates for simulation and control, so fewer operations means faster solvers. Because building an m × m matrix and visiting all n joints is unavoidable, the authors argue O(n + m²) is the optimal order. The algorithm was implemented with CasADi code generation and, per the paper, also in C++ in the Pinocchio library.

Problem

The Delassus matrix, or inverse operational space inertia matrix, links constraint forces to constraint accelerations and is needed for constrained and contact dynamics simulation and operational space control, where it is often the costliest step in MPC and RL. Computing it naively costs O(n³ + m²n). The best known algorithm, EFPA, reaches O(n + md + m²), while the authors' earlier PV-OSIM is O(n + m²d) and scales poorly when many constraints must be propagated through many joints.

Proposed solution

PV-OSIMr modifies PV-OSIM's backward sweep so that, instead of propagating constraint-space matrices that grow with the number of constraints, it propagates fixed 6×6 extended force propagators and spatial inverse inertias between consecutive branching links (the set N). A limited forward sweep over N computes the inverse inertias, a limited backward sweep computes each end-effector's constraint-space propagator, and each block of the Delassus matrix is assembled in constant time. This gives O(n + m²) complexity.

Key insights

When a link supports more than six constraint dimensions, propagating 6D spatial quantities and recovering constraint-space terms later is cheaper than propagating constraint-space terms directly.
Expensive quantities only need computing at closest common ancestors of constrained links, so the extra sweeps run over a reduced set of links.
Lower asymptotic complexity does not guarantee fewer operations on small robots: EFPA was costlier than PV-OSIM and PV-OSIMr for nearly all tested mechanisms because of its three-sweep structure and per-link frame transforms.
LTL-OSIM, despite its higher complexity, beat EFPA on a single Shadow hand because the hand's branching creates sparsity it exploits.

Main results

Complexity: PV-OSIMr O(n + m²) against PV-OSIM O(n + m²d), EFPA O(n + md + m²), KJR O(n + m²d), LTL-OSIM O(nd² + m²d + md²) and naive O(n³ + m²n).
Atlas standing with four 3D point contacts per foot (12D per foot): PV-OSIM needed 19.9% more operations than PV-OSIMr; with one 6D weld per foot the two algorithms were identical.
Atlas with Shadow hands (3D constraint per fingertip, 15D per hand): Section IV-D reports up to about 34% fewer operations than PV-OSIM, the next most efficient algorithm, while the Conclusions state up to 24% improvement on the Atlas examples.
On a branched test mechanism with 6D welds at each branch tip, PV-OSIMr matched PV-OSIM for a single branch and scaled more gently from two branches upward; EFPA eventually beat PV-OSIM for long mechanisms with ten branches per side.
In one case PV-OSIM needed about 1.5% fewer operations: Atlas with 6D welds on both hands and both feet, where constraints pass through only three joints before the floating base.
All benchmarks are operation counts from the CasADi implementation, not wall-clock timings.

Limitations

PV-OSIM can be cheaper when more than six constraint dimensions pass through only a few (three or fewer) single-DoF joints, because its rank-1 updates beat PV-OSIMr's two rank-6 computations.
When constraints meet only at the floating base, or the mechanism is a single chain, PV-OSIMr gives no gain over PV-OSIM.
Results are reported as operation counts rather than measured run times.
The algorithm is presented for kinematic trees; the C++ Pinocchio version was to be released only after acceptance.

Field impact

The paper won the RA-L 2025 Best Paper Award and has 4 citations. The authors released their CasADi implementation as source code on GitHub and state that a C++ version was implemented in the Pinocchio library for release on acceptance.

Abstract

We present PV-OSIMr, an efficient algorithm for computing the Delassus matrix (also known as the inverse operational space inertia matrix) for a kinematic tree, with the lowest order computational complexity known in literature. PV-OSIMr is derived by optimizing the Popov-Vereshchagin (PV) solver computations using the compositionality of the force and motion propagators. It has a computational complexity of O(n + m²) compared to O(n + m²d) of the original PV-OSIM algorithm and O(n+md+m²) of the extended force propagator algorithm (EFPA), where n is the number of joints, m is the number of constraints and d is the depth of the kinematic tree. Since Delassus matrix computation requires constructing an m × m sized matrix and must consider all the n joints at least once, the asymptotic computational complexity of PV-OSIMr is optimal. We further benchmark our algorithm and find it to be often more efficient than the PV-OSIM and EFPA in practice.

Open Source Robotics