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.

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.

Direct methods for trajectory optimization are widely used for planning locally optimal trajectories of robotic systems. Most state-of-the-art techniques treat the discontinuous dynamics of contact 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 through contact that eliminates the requirement for an a priori mode ordering. Motivated by the formulation of multi-contact dynamics as a Linear Complementarity Problem (LCP) for forward simulation, the proposed algorithm leverages Sequential Quadratic Programming (SQP) to naturally resolve contact constraint forces while simultaneously optimizing a trajectory and satisfying nonlinear complementarity constraints. The method scales well to high dimensional systems with large numbers of possible modes. We demonstrate the approach using three increasingly complex systems: rotating a pinned object with a finger, planar walking with the Spring Flamingo robot, and high speed bipedal running on the FastRunner platform.

Maintaining balance is fundamental to legged robots. The most commonly used mechanisms for balance control are taking a step, regulating the center of pressure ('ankle strategies'), and to a lesser extent, changing centroidal angular momentum (e.g., 'hip strategies'). In this paper, we disregard these three mechanisms, instead focusing on a fourth: varying center of mass height. We study a 2D variableheight center of mass model, and analyze how center of mass height variation can be used to achieve balance, in the sense of convergence to a fixed point of the dynamics. In this analysis, we pay special attention to the constraint of unilateral contact forces. We first derive a necessary condition that must be satisfied to be able to achieve balance. We then present two control laws, and derive their regions of attraction in closed form. We show that one of the control laws achieves balance from any state satisfying the necessary condition for balance. Finally, we briefly discuss the relative importance of CoM height variation and other balance mechanisms.

Abstract-Many critical tasks in robotics, such as locomotion or manipulation, involve collisions between a rigid body and the environment or between multiple bodies. Methods based on sums-of-squares (SOS) for numerical computation of Lyapunov certificates are a powerful tool for analyzing the stability of continuous nonlinear systems, and can additionally be used to automatically synthesize stabilizing feedback controllers. Here, we present a method for applying sums-of-squares verification to rigid bodies with Coulomb friction undergoing discontinuous, inelastic impact events. The proposed algorithm explicitly generates Lyapunov certificates for stability, positive invariance, and safety over admissible (non-penetrating) states and contact forces. We leverage the complementarity formulation of contact, which naturally generates the semialgebraic constraints that define this admissible region. The approach is demonstrated on multiple robotics examples, including simple models of a walking robot, a perching aircraft, and control design of a balancing robot.