A direct method for trajectory optimization of rigid bodies through contact

Abstract

Direct methods for trajectory optimization are widely used for planning locally optimal trajectories of robotic systems. Many critical tasks, such as locomotion and manipulation, often involve impacting the ground or objects in the environment. Most state-of-the-art techniques treat the discontinuous dynamics that result from impacts as discrete modes and restrict the search for a complete path to a specified sequence through these modes. Here we present a novel method for trajectory planning of rigid body systems that contact their environment through inelastic impacts and Coulomb friction. This method eliminates the requirement for a priori mode ordering. Motivated by the formulation of multi-contact dynamics as a Linear Complementarity Problem (LCP) for forward simulation, the proposed algorithm poses the optimization problem as a Mathematical Program with Complementarity Constraints (MPCC). We leverage Sequential Quadratic Programming (SQP) to naturally resolve contact constraint forces while simultaneously optimizing a trajectory that satisfies the complementarity constraints. The method scales well to high dimensional systems with large numbers of possible modes. We demonstrate the approach on four increasingly complex systems: rotating a pinned object with a finger, simple grasping and manipulation, planar walking with the Spring Flamingo robot, and high speed bipedal running on the FastRunner platform.

Trajectory Optimization With Implicit Hard Contacts

“…On the other hand, methods that attempt to holistically solve the problem by directly incorporating complementarity constraints or by mixed integer programming have shown to be computationally heavy and typically require constraint smoothing for convergence [2], [3], [14]- [21]. As a consequence, these approaches are not suitable for real-time applications.…”

Trajectory and foothold optimization using low-dimensional models for rough terrain locomotion

“…Terrain adaptation and automatic gait discovery can be approached using general trajectory optimization methods, similar to [1][2][3] [13]. Nonetheless, these optimization methods tend to be plagued by local minima, limiting their applicability to rough terrain locomotion.…”

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

“…Early work on the use of a Linear Complementary Problem for solving trajectories of discrete contact systems was formulated in [19]. Posa et al [7] apply direct transcription and an implicit hard contact model to a locomotion problem using the contact forces as decision variables. Similarly, Dai et al [9] used direct transcription over a reduced model of the dynamics together with kinematic constraints.…”

“…However, additional nonlinear constraints are introduced to ensure unilateral contact forces. An in-depth comparison against [7], [8] is necessary to determine the differences in terms of computation times.…”

“…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.…”