24 November 2023 in IEEE Transactions on Robotics
We derive a family of efficient constrained dynamics algorithms by formulating an equivalent linear quadratic regulator (LQR) problem using Gauss' principle of least constraint and solving it using dynamic programming. Our approach builds upon the pioneering (but largely unknown) $O(n + m^{2}\;d + m^{3})$ solver by Popov and Vereshchagin (PV), where $n$ , $m$ , and $d$ are the number of joints, number of constraints, and the kinematic tree depth, respectively. We provide an expository derivation for the original PV solver and extend it to floating-base kinematic trees with constraints allowed on any link. We make new connections between the LQR's dual Hessian and the inverse operational space inertia matrix (OSIM), permitting efficient OSIM computation, which we further accelerate using matrix inversion lemma. By generalizing the elimination ordering and accounting for MuJoCo -type soft constraints, we derive two original $O(n + m)$ complexity solvers. Our numerical results indicate that significant simulation speed-up can be achieved for high dimensional robots like quadrupeds and humanoids using our algorithms as they scale better than the widely used $O(nd^{2} + m^{2}\;d + d^{2}\;m)$ LTL algorithm of Featherstone. The derivation through the LQR-constrained dynamics connection can make our algorithm accessible to a wider audience and enable cross fertilization of software and research results between the fields.
19 November 2024 in IEEE Transactions on Robotics
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.
2 March 2021 in IEEE Robotics and Automation Letters
Extensive work has been done on efficiently resolving hierarchical robot task specifications that minimize the l -2 norm of linear constraint violations, but not for l -1 norm, in which there has recently been growing interest for sparse control. Both approaches require solving a cascade of quadratic programs (QP) or linear programs (LP). In this letter, we introduce alternate and more efficient approaches to hierarchical l -1 norm minimization by formulating it as a single LP that can be solved by any off-the-shelf solver. The first approach is a recursive method that transforms the lexicographic LP (LLP) into a single objective problem using Lagrangian duality. The second approach, which forms the main focus of this letter, is a weighted method based on the exact penalty method, that is equivalent to the original LLP for a well chosen set of weights. We propose methods to compute and adapt these weights. The algorithms were applied on an interesting dual arm robot task. We discuss and benchmark the computational efficiency of these methods. Simplicity of the weighted method makes it a promising approach for tackling challenging prioritized robot control problems involving a control horizon or nonlinear constraints. Within this letter, we take the first step towards that goal by demonstrating the efficacy of the weighted method on a simpler instantaneous robot control problem with linear constraints.
12 January 2026 in IEEE Transactions on Robotics
Efficient rigid-body dynamics algorithms are instrumental in enabling high-frequency dynamics evaluation for resource-intensive applications (e.g., model predictive control, large-scale simulation, reinforcement learning), potentially on resource-constrained hardware. Existing recursive algorithms with low computational complexity are mostly restricted to kinematic trees with external contact constraints or are sensitive to singular cases (e.g., linearly dependent constraints and kinematic singularities), severely impacting their practical usage in existing simulators. This article introduces two original, low-complexity recursive algorithms: the loop-constrained articulated body algorithm (LCABA) and proxBBO, both based on a proximal dynamics formulation for forward simulation of closed-loop mechanisms. These algorithms are derived from first principles using non-serial dynamic programming, exhibit linear complexity in practical scenarios, and are numerically robust in the face of singular cases. They extend the existing constrained articulated body algorithm (constrainedABA) to handle internal loops and the pioneering BBO algorithm from the 1980 s to singular cases. Both algorithms have been implemented by leveraging the open-source Pinocchio library, benchmarked in detail, and demonstrate state-of-the-art performance for various robot topologies, including over $6\times$ speed-ups compared to existing non-recursive algorithms for high degree-of-freedom systems with internal loops, such as recent humanoid robots.
27 September 2024 in IEEE Robotics and Automation Letters
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 recently proposed PV-OSIM algorithm using the compositionality of the force and motion propagators. It has a computational complexity of $O(n+m^{2})$ compared to $ O(n + m^{2}d)$ of the 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 the Delassus matrix is an $m \times m$ sized matrix and its computation must consider all the $n$ joints, PV-OSIMr's asymptotic computational complexity is optimal. We further benchmark our algorithm and find it to be often more efficient than the PV-OSIM and EFPA in practice.
1 September 2022 in Journal of Korea Robotics Society