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

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.

Efficient Constrained Dynamics Algorithms based on an Equivalent LQR Formulation using Gauss' Principle of Least Constraint

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

Safe-Planner: A Single-Outcome Replanner for Computing Strong Cyclic Policies in Fully Observable Non-Deterministic Domains

Replanners are efficient methods for solving non-deterministic planning problems. Despite showing good scalability, existing replanners often fail to solve problems involving a large number of misleading plans, i.e., weak plans that do not lead to strong solutions, however, due to their minimal lengths, are likely to be found at every replanning iteration. The poor performance of replanners in such problems is due to their all-outcome determinization. That is, when compiling from non-deterministic to classical, they include all compiled classical operators in a single deterministic domain which leads replanners to continually generate misleading plans. We introduce an offline replanner, called Safe-Planner (SP), that relies on a single-outcome determinization to compile a non-deterministic domain to a set of classical domains, and ordering heuristics for ranking the obtained classical domains. The proposed single-outcome determinization and the heuristics allow for alternating between different classical domains. We show experimentally that this approach can allow SP to avoid generating misleading plans but to generate weak plans that directly lead to strong solutions. The experiments show that SP outperforms state-of-the-art non-deterministic solvers by solving a broader range of problems. We also validate the practical utility of SP in real-world non-deterministic robotic tasks.