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

TL;DR

The Delassus matrix (inverse OSIM) is a bottleneck in constrained rigid-body dynamics for robotics and graphics. The paper introduces PV-OSIMr, a lowest-order algorithm with complexity $O(n + m^2)$ derived from PV-OSIM via extended force propagator compositionality, enabling efficient computation on kinematic trees. Through CasADi-based implementation and extensive benchmarks (including Atlas with various constraint setups), PV-OSIMr consistently outperforms PV-OSIM, EFPA, and LTL-OSIM in practical scenarios, with up to roughly 34% fewer operations than PV-OSIM in some cases. The work argues asymptotic optimality and provides integration with Pinocchio/CasADi ecosystems, supporting scalable constrained dynamics for MPC and RL tasks.

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^2 ) compared to O(n + m^2d) of the original PV-OSIM algorithm and O(n+md+m^2 ) 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 x 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.

Figures (6)

• Figure 1: Schematic diagram of a kinematic tree, nodes are the links, edges are the joints. The green nodes are the fictitious links representing end-effectors.
• Figure 2: Graphical illustration of the different computation sweeps in \ref{['alg:PV_OSIMr']}.
• Figure 3: Benchmarking the number of operations of the different OSIM algorithms.
• Figure 4: Comparing the scaling of the recursive algorithms on more mechanisms.
• Figure 5: Comparing the number of computation operations of the OSIM algorithms for the Atlas robot.