Optimization and stabilization of trajectories for constrained dynamical systems

Abstract

Contact constraints, such as those between a foot and the ground or a hand and an object, are inherent in many robotic tasks. These constraints define a manifold of feasible states; while well understood mathematically, they pose numerical challenges to many algorithms for planning and controlling whole-body dynamic motions. In this paper, we present an approach to the synthesis and stabilization of complex trajectories for both fully-actuated and underactuated robots subject to contact constraints. We introduce a trajectory optimization algorithm (DIRCON) that extends the direct collocation method, naturally incorporating manifold constraints to produce a nominal trajectory with third-order integration accuracy-a critical feature for achieving reliable tracking control. We adapt the classical time-varying linear quadratic regulator to produce a local cost-to-go in the manifold tangent plane. Finally, we descend the cost-to-go using a quadratic program that incorporates unilateral friction and torque constraints. This approach is demonstrated on three complex walking and climbing locomotion examples in simulation.

Hybrid direct collocation and control in the constraint-consistent subspace for dynamic legged robot locomotion

“…We show that applying direct collocation to this model provides motion plans that are consistent with the kinematic constraints along the trajectory and not only at the discretization points of the collocation algorithm. Our method achieves this without adding artificial decision variables as in [8]. With a similar motivation, early work on optimal biped walking using minimal coordinates models was explored by Hardt et al [22].…”

“…In this paper we provide a method to solve the problem in (2) recognizing this condition. With the same purpose, Posa et al [8] recently presented DIRCON, a modified direct collocation algorithm for optimizing in the manifold of feasible states. Instead of modifying the algorithm, we use the constraint-consistent direct dynamics of a legged robot using the projection method derived by Aghili [13].…”

“…The main advantage of using the projected dynamics is that the resulting plans are constraint-consistent with the contact constraints, and therefore easy to execute on the real robot. Posa et al [8] address the problem of properly stabilizing a legged robot as a constrained system. They propose to use the optimal trajectories from the planner on a low-level Quadratic Programming feedback controller using the Linear Quadratic Regulator solutions as cost functions.…”

“…Accelerations are constraint-consistent if and only if they satisfy (8). F c is related to the forces at the contact points λ ∈ R k ,…”

“…With this motivation, many approaches based on Trajectory Optimization (TO) have been proposed [6], [7], [8], [9], [10], [11], [12] demonstrating that optimization is a valid framework to derive motion plans that are not driven by quasi-static stability criteria. However, legged robots are floating-base, high dimensional systems, underactuated and permanently influenced by contact forces.…”