Patrick M. Wensing

Patrick M. Wensing, Jean-Jacques Slotine

This paper describes the use of spatially-sparse inputs to influence global changes in the behavior of Dynamic Movement Primitives (DMPs). The dynamics of DMPs are analyzed through the framework of contraction theory as networked hierarchies of contracting or transversely contracting systems. Within this framework, sparsely-inhibited rhythmic DMPs (SI-RDMPs) are introduced to both inhibit or enable rhythmic primitives through spatially-sparse modification of the DMP dynamics. SI-RDMPs are demonstrated in experiments to manage start-stop transitions for walking experiments with the MIT Cheetah. New analytical results on the coupling of oscillators with diverse natural frequencies are also discussed.

Gianluigi Grandesso, Elisa Alboni, Gastone P. Rosati Papini et al.

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.

Shubham Singh, Ryan P. Russell, Patrick M. Wensing

An essential need for many model-based robot control algorithms is the ability to quickly and accurately compute partial derivatives of the equations of motion. State of the art approaches to this problem often use analytical methods based on the chain rule applied to existing dynamics algorithms. Although these methods are an improvement over finite differences in terms of accuracy, they are not always the most efficient. In this paper, we contribute new closed-form expressions for the first-order partial derivatives of inverse dynamics, leading to a recursive algorithm. The algorithm is benchmarked against chain-rule approaches in Fortran and against an existing algorithm from the Pinocchio library in C++. Tests consider computing the partial derivatives of inverse and forward dynamics for robots ranging from kinematic chains to humanoids and quadrupeds. Compared to the previous open-source Pinocchio implementation, our new analytical results uncover a key computational restructuring that enables efficiency gains. Speedups of up to 1.4x are reported for calculating the partial derivatives of inverse dynamics for the 50-dof Talos humanoid.

Hua Chen, Zejun Hong, Shunpeng Yang et al.

This paper studies capturability and push recovery for quadrupedal locomotion. Despite the rich literature on capturability analysis and push recovery control for legged robots, existing tools are developed mainly for bipeds or humanoids. Distinct quadrupedal features such as point contacts and multiple swinging legs prevent direct application of these methods. To address this gap, we propose a switched systems model for quadruped dynamics, and instantiate the abstract viability concept for quadrupedal locomotion with a time-based gait. Capturability is characterized through a novel specification of dynamically balanced states that addresses the time-varying nature of quadrupedal locomotion and balance. A linear inverted pendulum (LIP) model is adopted to demonstrate the theory and show how the newly developed quadrupedal capturability can be used in motion planning for quadrupedal push recovery. We formulate and solve an explicit model predictive control (EMPC) problem whose optimal solution fully characterizes quadrupedal capturability with the LIP. Given this analysis, an optimization-based planning scheme is devised for determining footsteps and center of mass references during push recovery. To validate the effectiveness of the overall framework, we conduct numerous simulation and hardware experiments. Simulation results illustrate the necessity of considering dynamic balance for quadrupedal capturability, and the significant improvement in disturbance rejection with the proposed strategy. Experimental validations on a replica of the Mini Cheetah quadruped demonstrate an up to 100% improvement as compared with state-of-the-art.

Leo Kozachkov, Patrick M. Wensing, Jean-Jacques Slotine

We prove that Riemannian contraction in a supervised learning setting implies generalization. Specifically, we show that if an optimizer is contracting in some Riemannian metric with rate $λ> 0$, it is uniformly algorithmically stable with rate $\mathcal{O}(1/λn)$, where $n$ is the number of labelled examples in the training set. The results hold for stochastic and deterministic optimization, in both continuous and discrete-time, for convex and non-convex loss surfaces. The associated generalization bounds reduce to well-known results in the particular case of gradient descent over convex or strongly convex loss surfaces. They can be shown to be optimal in certain linear settings, such as kernel ridge regression under gradient flow.

Vince Kurtz, Patrick M. Wensing, Hai Lin

Task-space Passivity-Based Control (PBC) for manipulation has numerous appealing properties, including robustness to modeling error and safety for human-robot interaction. Existing methods perform poorly in singular configurations, however, such as when all the robot's joints are fully extended. Additionally, standard methods for constrained task-space PBC guarantee passivity only when constraints are not active. We propose a convex-optimization-based control scheme that provides guarantees of singularity avoidance, passivity, and feasibility. This work paves the way for PBC with passivity guarantees under other types of constraints as well, including joint limits and contact/friction constraints. The proposed methods are validated in simulation experiments on a 7 degree-of-freedom manipulator.

Vince Kurtz, He Li, Patrick M. Wensing et al.

Seemingly in defiance of basic physics, cats consistently land on their feet after falling. In this paper, we design a controller that lands the Mini Cheetah quadruped robot on its feet as well. Specifically, we explore how trajectory optimization and machine learning can work together to enable highly dynamic bioinspired behaviors. We find that a reflex approach, in which a neural network learns entire state trajectories, outperforms a policy approach, in which a neural network learns a mapping from states to control inputs. We validate our proposed controller in both simulation and hardware experiments, and are able to land the robot on its feet from falls with initial pitch angles between -90 and 90 degrees.

Junwei Liu, Hua Chen, Patrick M. Wensing et al.

Balancing is a fundamental need for legged robots due to their unstable floating-base nature. Balance control has been thoroughly studied for simple models such as the linear inverted pendulum thanks to the concept of the instantaneous capture point (ICP), yet the constant center of mass height assumption limits the application. This paper explores balancing of the variable-height inverted pendulum (VHIP) model by introducing the \emph{instantaneous capture input} (ICI), an extension of the ICP based on its key properties. Namely, the ICI can be computed as a function of the state, and when this function is used as the control policy, the ICI is rendered stationary and the system will eventually come to a stop. This characterization induces an analytical region of capturable states for the VHIP, which can be used to conceptually guide where to step. To further address state and control constraints during recovery, we present and theoretically analyze an explicit ICI-based controller with online optimal feedback gains. Simulations demonstrate the validity of our controller for capturability maintenance compared to an approach based on the divergent component of motion.

John N. Nganga, Patrick M. Wensing

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.

Hua Chen, Bingheng Wang, Zejun Hong et al.

This paper studies jumping for wheeled-bipedal robots, a motion that takes full advantage of the benefits from the hybrid wheeled and legged design features. A comprehensive hierarchical scheme for motion planning and control of jumping with wheeled-bipedal robots is developed. Underactuation of the wheeled-bipedal dynamics is the main difficulty to be addressed, especially in the planning problem. To tackle this issue, a novel wheeled-spring-loaded inverted pendulum (W-SLIP) model is proposed to characterize the essential dynamics of wheeled-bipedal robots during jumping. Relying on a differential-flatness-like property of the W-SLIP model, a tractable quadratic programming based solution is devised for planning jumping motions for wheeled-bipedal robots. Combined with a kinematic planning scheme accounting for the flight phase motion, a complete planning scheme for the W-SLIP model is developed. To enable accurate tracking of the planned trajectories, a linear quadratic regulator based wheel controller and a task-space whole-body controller for the other joints are blended through disturbance observers. The overall planning and control scheme is validated using V-REP simulations of a prototype wheeled-bipedal robot.

He Li, Robert J. Frei, Patrick M. Wensing

This letter presents a new predictive control architecture for high-dimensional robotic systems. As opposed to a conventional Model Predictive Control (MPC) approach to locomotion that formulates a hierarchical sequence of optimization problems, the proposed work formulates a single optimization problem posed over a hierarchy of models, and is thus named Model Hierarchy Predictive Control (MHPC). MHPC is formulated as a multi-phase receding-horizon Trajectory Optimization (TO) problem, and can be implemented using any general multi-phase TO solver. MHPC is benchmarked in simulation on a quadruped, a biped, and a quadrotor, demonstrating control performance on par or exceeding whole-body MPC while maintaining a lower computational cost in each case. A preliminary gap jumping experiment is conducted on the MIT Mini Cheetah with the control policy generated offline, demonstrating the physical validity of the generated trajectories and motivating online MHPC in future work.

Sebastian Echeandia, Patrick M. Wensing

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.

Vince Kurtz, Patrick M. Wensing, Hai Lin

Reduced-order template models are widely used to control high degree-of-freedom legged robots, but existing methods for template-based whole-body control rely heavily on heuristics and often suffer from robustness issues. In this letter, we propose a template-based whole-body control method grounded in the formal framework of approximate simulation. Our central contribution is to demonstrate how the Hamiltonian structure of rigid-body dynamics can be exploited to establish approximate simulation for a high-dimensional nonlinear system. The resulting controller is passive, more robust to push disturbances, uneven terrain, and modeling errors than standard QP-based methods, and naturally enables high center of mass walking. Our theoretical results are supported by simulation experiments with a 30 degree-of-freedom Valkyrie humanoid model.

He Li, Patrick M. Wensing

This paper presents a Differential Dynamic Programming (DDP) framework for trajectory optimization (TO) of hybrid systems with state-based switching. The proposed Hybrid Systems DDP (HS-DDP) approach is considered for application to whole-body motion planning with legged robots. Specifically, HS-DDP incorporates three algorithmic advances: an impact-aware DDP step addressing the impact event in legged locomotion, an Augmented Lagrangian (AL) method dealing with the switching constraint, and a Switching Time Optimization (STO) algorithm that optimizes switching times by leveraging the structure of DDP. Further, a Relaxed Barrier (ReB) method is used to manage inequality constraints and is integrated into HS-DDP for locomotion planning. The performance of the developed algorithms is benchmarked on a simulation model of the MIT Mini Cheetah executing a bounding gait. We demonstrate the effectiveness of AL and ReB for handling switching constraints, friction constraints, and torque limits. By comparing to previous solutions, we show that the STO algorithm achieves 2.3 times more reduction of total switching times, demonstrating the efficiency of our method.

Hua Chen, Patrick M. Wensing, Wei Zhang

This paper considers the optimal control problem of an extended spring-loaded inverted pendulum (SLIP) model with two additional actuators for active leg length and hip torque modulation. These additional features arise naturally in practice, allowing for consideration of swing leg kinematics during flight and active control over stance dynamics. On the other hand, nonlinearity and the hybrid nature of the overall SLIP dynamics introduce challenges in the analysis and control of the model. In this paper, we first show that the stance dynamics of the considered SLIP model are differentially flat, which has a strong implication regarding controllability of the stance dynamics. Leveraging this powerful property, a tractable optimal control strategy is developed. This strategy enables online solution while also treating the hybrid nature of the SLIP dynamics. Together with the optimal control strategy, the extended SLIP model grants active disturbance rejection capability at any point during the gait. Performance of the proposed control strategy is demonstrated via numerical tests and shows significant advantage over existing methods.

Octavio Villarreal, Victor Barasuol, Patrick M. Wensing et al.

We present a novel control strategy for dynamic legged locomotion in complex scenarios, that considers information about the morphology of the terrain in contexts when only on-board mapping and computation are available. The strategy is built on top of two main elements: first a contact sequence task that provides safe foothold locations based on a convolutional neural network to perform fast and continuous evaluation of the terrain in search of safe foothold locations; then a model predictive controller that considers the foothold locations given by the contact sequence task to optimize target ground reaction forces. We assess the performance of our strategy through simulations of the hydraulically actuated quadruped robot HyQReal traversing rough terrain under realistic on-board sensing and computing conditions.

Vince Kurtz, Rafael Rodrigues da Silva, Patrick M. Wensing et al.

Reduced-order template models like the Linear Inverted Pendulum (LIP) and Spring-Loaded Inverted Pendulum (SLIP) are widely used tools for controlling high-dimensional humanoid robots. However, connections between templates and whole-body models have lacked formal underpinnings, preventing formal guarantees when it comes to integrated controller design. We take a small step towards addressing this gap by considering the notion of approximate simulation. Derived from simulation relations for discrete transition systems in formal methods, approximate similarity means that the outputs of two systems can remain $ε$-close. In this paper, we consider the case of controlling a balancer via planning with the LIP model. We show that the balancer approximately simulates the LIP and derive linear constraints that are sufficient conditions for maintaining ground contact. This allows for rapid planning and replanning with the template model by solving a quadratic program that enforces contact constraints in the full model. We demonstrate the efficacy of this planning and control paradigm in a simulated push recovery scenario for a planar 4-link balancer.

Patrick M. Wensing, Günter Niemeyer, Jean-Jacques E. Slotine

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.

Patrick M. Wensing, Jean-Jacques E. Slotine

This paper studies collaboration through the cloud in the context of cooperative adaptive control for robot manipulators. We first consider the case of multiple robots manipulating a common object through synchronous centralized update laws to identify unknown inertial parameters. Through this development, we introduce a notion of Collective Sufficient Richness, wherein parameter convergence can be enabled through teamwork in the group. The introduction of this property and the analysis of stable adaptive controllers that benefit from it constitute the main new contributions of this work. Building on this original example, we then consider decentralized update laws, time-varying network topologies, and the influence of communication delays on this process. Perhaps surprisingly, these nonidealized networked conditions inherit the same benefits of convergence being determined through collective effects for the group. Simple simulations of a planar manipulator identifying an unknown load are provided to illustrate the central idea and benefits of Collective Sufficient Richness.

Patrick M. Wensing, Sangbae Kim, Jean-Jacques Slotine

With the increased application of model-based whole-body control in legged robots, there has been a resurgence of research interest into methods for accurate system identification. An important class of methods focuses on the inertial parameters of rigid-body systems. These parameters consist of the mass, first mass moment (related to center of mass location), and rotational inertia matrix of each link. The main contribution of this paper is to formulate physical-consistency constraints on these parameters as Linear Matrix Inequalities (LMIs). The use of these constraints in identification can accelerate convergence and increase robustness to noisy data. It is critically observed that the proposed LMIs are expressed in terms of the covariance of the mass distribution, rather than its rotational moments of inertia. With this perspective, connections to the classical problem of moments in mathematics are shown to yield new bounding-volume constraints on the mass distribution of each link. While previous work ensured physical plausibility or used convex optimization in identification, the LMIs here uniquely enable both advantages. Constraints are applied to identification of a leg for the MIT Cheetah 3 robot. Detailed properties of transmission components are identified alongside link inertias, with parameter optimization carried out to global optimality through semidefinite programming.