arXiv subjects

Patrick M. Wensing

Publications and source records attributed to Patrick M. Wensing.

At least 19 recordsLinked to original sources

arXiv2024

URDF+: An Enhanced URDF for Robots with Kinematic Loops

Designs incorporating kinematic loops are becoming increasingly prevalent in the robotics community. Despite the existence of dynamics algorithms to deal with the effects of such loops, many modern simulators rely on dynamics libraries that require robots to be represented as kinematic trees. This requirement is reflected in the de facto standard format for describing robots, the Universal Robot Description Format (URDF), which does not support kinematic loops resulting in closed chains. This paper introduces an enhanced URDF, termed URDF+, which addresses this key shortcoming of URDF while retaining the intuitive design philosophy and low barrier to entry that the robotics community values. The URDF+ keeps the elements used by URDF to describe open chains and incorporates new elements to encode loop joints. We also offer an accompanying parser that processes the system models coming from URDF+ so that they can be used with recursive rigid-body dynamics algorithms for closed-chain systems that group bodies into local, decoupled loops. This parsing process is fully automated, ensuring optimal grouping of constrained bodies without requiring manual specification from the user. We aim to advance the robotics community towards this elegant solution by developing efficient and easy-to-use software tools.

cs.RO↗

arXiv2024

Cafe-Mpc: A Cascaded-Fidelity Model Predictive Control Framework with Tuning-Free Whole-Body Control

This work introduces an optimization-based locomotion control framework for on-the-fly synthesis of complex dynamic maneuvers. At the core of the proposed framework is a cascaded-fidelity model predictive controller (Cafe-Mpc). Cafe-Mpc strategically relaxes the planning problem along the prediction horizon (i.e., with descending model fidelity, increasingly coarse time steps, and relaxed constraints) for computational and performance gains. This problem is numerically solved with an efficient customized multiple-shooting iLQR (MS-iLQR) solver that is tailored for hybrid systems. The action-value function from Cafe-Mpc is then used as the basis for a new value-function-based whole-body control (VWBC) technique that avoids additional tuning for the WBC. In this respect, the proposed framework unifies whole-body MPC and more conventional whole-body quadratic programming (QP), which have been treated as separate components in previous works. We study the effects of the cascaded relaxations in Cafe-Mpc on the tracking performance and required computation time. We also show that the Cafe-Mpc, if configured appropriately, advances the performance of whole-body MPC without necessarily increasing computational cost. Further, we show the superior performance of the proposed VWBC over the Riccati feedback controller in terms of constraint handling. The proposed framework enables accomplishing for the first time gymnastic-style running barrel rolls on the MIT Mini Cheetah. Video: https://youtu.be/YiNqrgj9mb8.

cs.RO↗

arXiv2023

Coriolis Factorizations and their Connections to Riemannian Geometry

Many energy-based control strategies for mechanical systems require the choice of a Coriolis factorization satisfying a skew-symmetry property. This paper (a) explores if and when a control designer has flexibility in this choice, (b) develops a canonical choice related to the Christoffel symbols, and (c) describes how to efficiently perform control computations with it for constrained mechanical systems. We link the choice of a Coriolis factorization to the notion of an affine connection on the configuration manifold and show how properties of the connection relate with the associated factorization. In particular, the factorization based on the Christoffel symbols is linked with a torsion-free property that can limit the twisting of system trajectories during passivity-based control. We then develop a way to induce Coriolis factorizations for constrained mechanisms from unconstrained ones, which provides a pathway to use the theory for efficient control computations with high-dimensional systems such as humanoids and quadruped robots with open- and closed-chain mechanisms. A collection of algorithms is provided (and made available open source) to support the recursive computation of passivity-based control laws, adaptation laws, and regressor matrices in future applications.

eess.SY↗

arXiv2023

A Propagation Perspective on Recursive Forward Dynamics for Systems with Kinematic Loops

We revisit the concept of constraint embedding as a means for dealing with kinematic loop constraints during dynamics computations for rigid-body systems. Specifically, we consider the local loop constraints emerging from common actuation sub-mechanisms in modern robotics systems (e.g., geared motors, differential drives, and four-bar mechanisms). However, rather than develop the concept of constraint embedding from the perspective of graphical analysis, we present a novel analysis of constraint embedding that generalizes the traditional concepts of joint models and motion/force subspaces between individual rigid bodies to generalized joint models and motion/force subspaces between groups of rigid bodies subject to loop constraints. The generalized concepts are used in a self-contained, articulated-body-based derivation of the constraint-embedding-based recursive algorithm for forward dynamics. The derivation represents the first assembly method to demonstrate the recursivity of articulated inertia computation in the presence of loop constraints. We demonstrate the broad applicability of the generalized joint concepts by showing how they also lead to the constraint-embedding-based recursive algorithm for inverse dynamics. Lastly, we benchmark our open-source implementation in C++ for the forward dynamics algorithm against a state-of-the-art, non-recursive algorithm. Our benchmarking validates that constraint embedding outperforms the non-recursive alternative in the case of local kinematic loops.

cs.RO↗

arXiv2023

A Unified Perspective on Multiple Shooting In Differential Dynamic Programming

Differential Dynamic Programming (DDP) is an efficient computational tool for solving nonlinear optimal control problems. It was originally designed as a single shooting method and thus is sensitive to the initial guess supplied. This work considers the extension of DDP to multiple shooting (MS), improving its robustness to initial guesses. A novel derivation is proposed that accounts for the defect between shooting segments during the DDP backward pass, while still maintaining quadratic convergence locally. The derivation enables unifying multiple previous MS algorithms, and opens the door to many smaller algorithmic improvements. A penalty method is introduced to strategically control the step size, further improving the convergence performance. An adaptive merit function and a more reliable acceptance condition are employed for globalization. The effects of these improvements are benchmarked for trajectory optimization with a quadrotor, an acrobot, and a manipulator. MS-DDP is also demonstrated for use in Model Predictive Control (MPC) for dynamic jumping with a quadruped robot, showing its benefits over a single shooting approach.

cs.RO↗

arXiv2023

Hybrid Volitional Control of a Robotic Transtibial Prosthesis using a Phase Variable Impedance Controller

For robotic transtibial prosthesis control, the global kinematics of the tibia can be used to monitor the progression of the gait cycle and command smooth and continuous actuation. In this work, these global tibia kinematics are used to define a phase variable impedance controller (PVIC), which is then implemented as the nonvolitional base controller within a hybrid volitional control framework (PVI-HVC). The gait progression estimation and biomechanic performance of one able-bodied individual walking on a robotic ankle prosthesis via a bypass adapter are compared for three control schemes: a passive benchmark controller, PVIC, and PVI-HVC. The different actuation of each controller had a direct effect on the global tibia kinematics, but the average deviation between the estimated and ground truth gait percentage were 1.6%, 1.8%, and 2.1%, respectively, for each controller. Both PVIC and PVI-HVC produced good agreement with able-bodied kinematic and kinetic references. As designed, PVI-HVC results were similar to those of PVIC when the user used low volitional intent, but yielded higher peak plantarflexion, peak torque, and peak power when the user commanded high volitional input in late stance. This additional torque and power also allowed the user to volitionally and continuously achieve activities beyond level walking, such as ascending ramps, avoiding obstacles, standing on tip-toes, and tapping the foot. In this way, PVI-HVC offers the kinetic and kinematic performance of the PVIC during level ground walking, along with the freedom to volitionally pursue alternative activities.

cs.RO↗

arXiv2023

A Geometric Characterization of Observability in Inertial Parameter Identification

This paper presents an algorithm to geometrically characterize inertial parameter identifiability for an articulated robot. The geometric approach tests identifiability across the infinite space of configurations using only a finite set of conditions and without approximation. It can be applied to general open-chain kinematic trees ranging from industrial manipulators to legged robots, and it is the first solution for this broad set of systems that is provably correct. The high-level operation of the algorithm is based on a key observation: Undetectable changes in inertial parameters can be represented as sequences of inertial transfers across the joints. Drawing on the exponential parameterization of rigid-body kinematics, undetectable inertial transfers are analyzed in terms of observability from linear systems theory. This analysis can be applied recursively, and lends an overall complexity of $O(N)$ to characterize parameter identifiability for a system of $N$ bodies. Matlab source code for the new algorithm is provided.

cs.RO↗

arXiv2023

Details of Second-Order Partial Derivatives of Rigid-Body Inverse Dynamics

The details of second-order partial derivatives of rigid-body Inverse/Forward dynamics are provided. Several properties and identities using Spatial Vector Algebra are listed, along with their detailed derivations. The expressions build upon previous work by the author on first-order partial derivatives of inverse dynamics. The first/second-order derivatives are also extended for systems with external forces. Finally, the KKT Forward dynamics and Impact dynamics derivatives are derived.

cs.RO↗

arXiv2023

Multi-Shooting Differential Dynamic Programming for Hybrid Systems using Analytical Derivatives

Differential Dynamic Programming (DDP) is a popular technique used to generate motion for dynamic-legged robots in the recent past. However, in most cases, only the first-order partial derivatives of the underlying dynamics are used, resulting in the iLQR approach. Neglecting the second-order terms often slows down the convergence rate compared to full DDP. Multi-Shooting is another popular technique to improve robustness, especially if the dynamics are highly non-linear. In this work, we consider Multi-Shooting DDP for trajectory optimization of a bounding gait for a simplified quadruped model. As the main contribution, we develop Second-Order analytical partial derivatives of the rigid-body contact dynamics, extending our previous results for fixed/floating base models with multi-DoF joints. Finally, we show the benefits of a novel Quasi-Newton method for approximating second-order derivatives of the dynamics, leading to order-of-magnitude speedups in the convergence compared to the full DDP method.

cs.RO↗

arXiv2023

CACTO: Continuous Actor-Critic with Trajectory Optimization -- Towards global optimality

This paper presents a novel algorithm for the continuous control of dynamical systems that combines Trajectory Optimization (TO) and Reinforcement Learning (RL) in a single framework. The motivations behind this algorithm are the two main limitations of TO and RL when applied to continuous nonlinear systems to minimize a non-convex cost function. Specifically, TO can get stuck in poor local minima when the search is not initialized close to a "good" minimum. On the other hand, when dealing with continuous state and control spaces, the RL training process may be excessively long and strongly dependent on the exploration strategy. Thus, our algorithm learns a "good" control policy via TO-guided RL policy search that, when used as initial guess provider for TO, makes the trajectory optimization process less prone to converge to poor local optima. Our method is validated on several reaching problems featuring non-convex obstacle avoidance with different dynamical systems, including a car model with 6D state, and a 3-joint planar manipulator. Our results show the great capabilities of CACTO in escaping local minima, while being more computationally efficient than the Deep Deterministic Policy Gradient (DDPG) and Proximal Policy Optimization (PPO) RL algorithms.

cs.RO↗

arXiv2023

On Second-Order Derivatives of Rigid-Body Dynamics: Theory & Implementation

Model-based control for robots has increasingly been dependent on optimization-based methods like Differential Dynamic Programming and iterative LQR (iLQR). These methods can form the basis of Model-Predictive Control (MPC), which is commonly used for controlling legged robots. Computing the partial derivatives of the dynamics is often the most expensive part of these algorithms, regardless of whether analytical methods, Finite Difference, Automatic Differentiation (AD), or Chain-Rule accumulation is used. Since the second-order derivatives of dynamics result in tensor computations, they are often ignored, leading to the use of iLQR, instead of the full second-order DDP method. In this paper, we present analytical methods to compute the second-order derivatives of inverse and forward dynamics for open-chain rigid-body systems with multi-DoF joints and fixed/floating bases. An extensive comparison of accuracy and run-time performance with AD and other methods is provided, including the consideration of code-generation techniques in C/C++ to speed up the computations. For the 36 DoF ATLAS humanoid, the second-order Inverse, and the Forward dynamics derivatives take approx 200 mu s, and approx 2.1 ms respectively, resulting in a 3x speedup over the AD approach.

cs.RO↗

arXiv2022

Beyond Convexity -- Contraction and Global Convergence of Gradient Descent

This paper considers the analysis of continuous time gradient-based optimization algorithms through the lens of nonlinear contraction theory. It demonstrates that in the case of a time-invariant objective, most elementary results on gradient descent based on convexity can be replaced by much more general results based on contraction. In particular, gradient descent converges to a unique equilibrium if its dynamics are contracting in any metric, with convexity of the cost corresponding to the special case of contraction in the identity metric. More broadly, contraction analysis provides new insights for the case of geodesically-convex optimization, wherein non-convex problems in Euclidean space can be transformed to convex ones posed over a Riemannian manifold. In this case, natural gradient descent converges to a unique equilibrium if it is contracting in any metric, with geodesic convexity of the cost corresponding to contraction in the natural metric. New results using semi-contraction provide additional insights into the topology of the set of optimizers in the case when multiple optima exist. Furthermore, they show how semi-contraction may be combined with specific additional information to reach broad conclusions about a dynamical system. The contraction perspective also easily extends to time-varying optimization settings and allows one to recursively build large optimization structures out of simpler elements. Extensions to natural primal-dual optimization and game-theoretic contexts further illustrate the potential reach of these new perspectives.

math.OC↗

arXiv2022

Optimization-Based Control for Dynamic Legged Robots

In a world designed for legs, quadrupeds, bipeds, and humanoids have the opportunity to impact emerging robotics applications from logistics, to agriculture, to home assistance. The goal of this survey is to cover the recent progress toward these applications that has been driven by model-based optimization for the real-time generation and control of movement. The majority of the research community has converged on the idea of generating locomotion control laws by solving an optimal control problem (OCP) in either a model-based or data-driven manner. However, solving the most general of these problems online remains intractable due to complexities from intermittent unidirectional contacts with the environment, and from the many degrees of freedom of legged robots. This survey covers methods that have been pursued to make these OCPs computationally tractable, with specific focus on how environmental contacts are treated, how the model can be simplified, and how these choices affect the numerical solution methods employed. The survey focuses on model-based optimization, covering its recent use in a stand alone fashion, and suggesting avenues for combination with learning-based formulations to further accelerate progress in this growing field.

cs.RO↗

arXiv2022

Zero-Shot Retargeting of Learned Quadruped Locomotion Policies Using Hybrid Kinodynamic Model Predictive Control

Reinforcement Learning (RL) has witnessed great strides for quadruped locomotion, with continued progress in the reliable sim-to-real transfer of policies. However, it remains a challenge to reuse a policy on another robot, which could save time for retraining. In this work, we present a framework for zero-shot policy retargeting wherein diverse motor skills can be transferred between robots of different shapes and sizes. The new framework centers on a planning-and-control pipeline that systematically integrates RL and Model Predictive Control (MPC). The planning stage employs RL to generate a dynamically plausible trajectory as well as the contact schedule, avoiding the combinatorial complexity of contact sequence optimization. This information is then used to seed the MPC to stabilize and robustify the policy roll-out via a new Hybrid Kinodynamic (HKD) model that implicitly optimizes the foothold locations. Hardware results show an ability to transfer policies from both the A1 and Laikago robots to the MIT Mini Cheetah robot without requiring any policy re-tuning.

cs.RO↗

arXiv2022

Versatile Real-Time Motion Synthesis via Kino-Dynamic MPC with Hybrid-Systems DDP

Specialized motions such as jumping are often achieved on quadruped robots by solving a trajectory optimization problem once and executing the trajectory using a tracking controller. This approach is in parallel with Model Predictive Control (MPC) strategies that commonly control regular gaits via online re-planning. In this work, we present a nonlinear MPC (NMPC) technique that unlocks on-the-fly re-planning of specialized motion skills and regular locomotion within a unified framework. The NMPC reasons about a hybrid kinodynamic model, and is solved using a variant of a constrained Differential Dynamic Programming (DDP) solver. The proposed NMPC enables the robot to perform a variety of agile skills like jumping, bounding, and trotting, and the rapid transition between these skills. We evaluated the proposed algorithm with three challenging motion sequences that combine multiple agile skills, on two quadruped platforms, Unitree A1, and MIT Mini Cheetah, showing its effectiveness and generality.

cs.RO↗

arXiv2022

Analytical Second-Order Partial Derivatives of Rigid-Body Inverse Dynamics

Optimization-based robot control strategies often rely on first-order dynamics approximation methods, as in iLQR. Using second-order approximations of the dynamics is expensive due to the costly second-order partial derivatives of the dynamics with respect to the state and control. Current approaches for calculating these derivatives typically use automatic differentiation (AD) and chain-rule accumulation or finite-difference. In this paper, for the first time, we present analytical expressions for the second-order partial derivatives of inverse dynamics for open-chain rigid-body systems with floating base and multi-DoF joints. A new extension of spatial vector algebra is proposed that enables the analysis. A recursive algorithm with complexity of $\mathcal{O}(Nd^2)$ is also provided where $N$ is the number of bodies and $d$ is the depth of the kinematic tree. A comparison with AD in CasADi shows speedups of 1.5-3$\times$ for serial kinematic trees with $N> 5$, and a C++ implementation shows runtimes of $\approx$51$μs$ for a quadruped.

cs.RO↗

arXiv2022

Accelerating Second-Order Differential Dynamic Programming for Rigid-Body Systems

This letter presents a method to reduce the computational demands of including second-order dynamics sensitivity information into the Differential Dynamic Programming (DDP) trajectory optimization algorithm. An approach to DDP is developed where all the necessary derivatives are computed with the same complexity as in the iterative Linear Quadratic Regulator (iLQR). Compared to linearized models used in iLQR, DDP more accurately represents the dynamics locally, but it is not often used since the second-order derivatives of the dynamics are tensorial and expensive to compute. This work shows how to avoid the need for computing the derivative tensor by instead leveraging reverse-mode accumulation of derivative information to compute a key vector-tensor product directly. We also show how the structure of the dynamics can be used to further accelerate these computations in rigid-body systems. Benchmarks of this approach for trajectory optimization with multi-link manipulators show that the benefits of DDP can often be included without sacrificing evaluation time, and can be done in fewer iterations than iLQR.

cs.RO↗

arXiv2022

Numerical Methods to Compute the Coriolis Matrix and Christoffel Symbols for Rigid-Body Systems

This article presents methods to efficiently compute the Coriolis matrix and underlying Christoffel symbols (of the first kind) for tree-structure rigid-body systems. The algorithms can be executed purely numerically, without requiring partial derivatives as in unscalable symbolic techniques. The computations share a recursive structure in common with classical methods such as the Composite-Rigid-Body Algorithm and are of the lowest possible order: $O(Nd)$ for the Coriolis matrix and $O(Nd^2)$ for the Christoffel symbols, where $N$ is the number of bodies and $d$ is the depth of the kinematic tree. Implementation in C/C++ shows computation times on the order of 10-20 $μ$s for the Coriolis matrix and 40-120 $μ$s for the Christoffel symbols on systems with 20 degrees of freedom. The results demonstrate feasibility for the adoption of these algorithms within high-rate ($>$1kHz) loops for model-based control applications.

cs.RO↗