Ajay Suresha Sathya

The Delassus matrix, closely related to the operational-space inertia matrix, is a fundamental quantity in robotics with applications in simulation, system identification, and control. Traditional approaches compute and store this matrix explicitly, either in sparse or dense form. In this work, we depart from this convention by treating the Delassus matrix as a matrix-free operator. We derive efficient algorithms with low computational complexity that multiply the Delassus matrix or its damped inverse by an input vector or matrix. Unlike approaches based on explicit matrix construction, our method achieves a linear memory footprint, making it scalable to problems with thousands of constraints and suitable for execution on resource-limited hardware. We implement these matrix-free operations on top of the open-source Pinocchio library and evaluate their performance against state-of-the-art methods that rely on explicit matrix computation. Our benchmarks demonstrate substantial speedups, ranging from approximately $2\times$ to $4\times$ for single-robot scenarios, up to $10\times$ when joint friction is enabled, and over $400\times$ in the Pyramid+Robots contact-rich multi-body scene.

The Delassus matrix, closely related to the operational-space inertia matrix, is a fundamental quantity in robotics with applications in simulation, system identification, and control. Traditional approaches compute and store this matrix explicitly, either in sparse or dense form. In this work, we depart from this convention by treating the Delassus matrix as a matrix-free operator. We derive efficient algorithms with low computational complexity that multiply the Delassus matrix or its damped inverse by an input vector or matrix. Unlike approaches based on explicit matrix construction, our method achieves a linear memory footprint, making it scalable to problems with thousands of constraints and suitable for execution on resourcelimited hardware. We implement these matrix-free operations on top of the open-source Pinocchio library and evaluate their performance against state-of-the-art methods that rely on explicit matrix computation. Our benchmarks demonstrate substantial speedups, ranging from 2x to over 400x, in contact-rich scenarios.

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.

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.

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.

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.

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^2d + 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^2d + d^2m) 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

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.