Constrained Articulated Body Dynamics Algorithms

All authors are with INRIA Willow.

Abstract

Rigid-body dynamics algorithms have played an essential role in robotics development. By finely exploiting the underlying robot structure, they allow the computation of the robot kinematics, dynamics, and related physical quantities with low complexity, enabling their integration into chipsets with limited resources or their evaluation at very high frequency for demanding applications (e.g., model predictive control, large-scale simulation, reinforcement learning, etc.). While most of these algorithms operate on constraint-free settings, only a few have been proposed so far to adequately account for constrained dynamical systems while depicting low algorithmic complexity. In this article, we introduce a series of new algorithms with reduced (and lowest) complexity for the forward simulation of constrained dynamical systems. Notably, we revisit the so-called articulated body algorithm (ABA) and the Popov–Vereshchagin algorithm (PV) in the light of proximal-point optimization and introduce two new algorithms, called constrained ABA and proxPV. These two new algorithms depict linear complexities while being robust to singular cases (e.g., redundant constraints, singular constraints, etc.). We establish the connection with existing literature formulations, especially the relaxed formulation at the heart of the MuJoCo and Drake simulators. We also propose an efficient and new algorithm to compute the damped Delassus inverse matrix with the lowest known computational complexity. All these algorithms have been implemented inside the open-source framework Pinocchio and depict, on a wide range of robotic systems ranging from robot manipulators to complex humanoid robots, state-of-the-art performances compared to alternative solutions of the literature.

Overview

Computational time for UR5 robot.
The proximal formulation of the equality-constrained dynamics problem results in four different algorithms, namely proxLTL, proxLTLs, proxPV, and constrainedABA depending on the usage of maximal or minimal coordinates during derivation and the order of elimination of the primal and dual variables, as shown in the overview figure above.

Selected Benchmarks

Computational timings of different algorithms.
Operation count Talos.

Related Links

This work heavily relies on the Pinocchio library.

BibTeX

@ARTICLE{sathya_constrainedABA_2024,
      author={Sathya, Ajay Suresha and Carpentier, Justin},
      journal={IEEE Transactions on Robotics}, 
      title={Constrained Articulated Body Dynamics Algorithms}, 
      year={2025},    
      volume={41},
      number={},
      pages={430-449},
      keywords={Heuristic algorithms;Robots;Robot kinematics;Symmetric matrices;Vectors;Prediction algorithms;Kinematics;Computational efficiency;Computational complexity;Symbols;Direct/inverse dynamics formulation;dynamics;humanoid robots;optimization and optimal control},
      doi={10.1109/TRO.2024.3502515}}