Simultaneous Contact, Gait, and Motion Planning for Robust Multilegged Locomotion via Mixed-Integer Convex Optimization

Traditional motion planning approaches for multilegged locomotion divide the problem into several stages, such as contact search and trajectory generation. However, reasoning about contacts and motions simultaneously is crucial for the generation of complex whole-body behaviors. Currently, coupling theses problems has required either the assumption of a fixed gait sequence and flat terrain condition, or nonconvex optimization with intractable computation time. In this letter, we propose a mixed-integer convex formulation to plan simultaneously contact locations, gait transitions, and motion, in a computationally efficient fashion. In contrast to previous works, our approach is not limited to flat terrain nor to a prespecified gait sequence. Instead, we incorporate the friction cone stability margin, approximate the robot's torque limits, and plan the gait using mixed-integer convex constraints. We experimentally validated our approach on the HyQ robot by traversing different challenging terrains, where nonconvexity and flat terrain assumptions might lead to suboptimal or unstable plans. Our method increases the motion robustness while keeping a low computation time.

Related papers:

title: Footstep planning on uneven terrain with mixed-integer convex optimization
abstract: We present a new method for planning footstep placements for a robot walking on uneven terrain with obstacles, using a mixed-integer quadratically-constrained quadratic program (MIQCQP). Our approach is unique in that it handles obstacle avoidance, kinematic reachability, and rotation of footstep placements, which typically have required non-convex constraints, in a single mixed-integer optimization that can be efficiently solved to its global optimum. Reachability is enforced through a convex inner approximation of the reachable space for the robot's feet. Rotation of the footsteps is handled by a piecewise linear approximation of sine and cosine, designed to ensure that the approximation never overestimates the robot's reachability. Obstacle avoidance is ensured by decomposing the environment into convex regions of obstacle-free configuration space and assigning each footstep to one such safe region. We demonstrate this technique in simple 2D and 3D environments and with real environments sensed by a humanoid robot. We also discuss computational performance of the algorithm, which is currently capable of planning short sequences of a few steps in under one second or longer sequences of 10-30 footsteps in tens of seconds to minutes on common laptop computer hardware. Our implementation is available within the Drake MATLAB toolbox .

title: Planning and execution of dynamic whole-body locomotion for a hydraulic quadruped on challenging terrain
abstract: We present a framework for dynamic quadrupedal locomotion over challenging terrain, where the choice of appropriate footholds is crucial for the success of the behaviour. We build a model of the environment on-line and on-board using an efficient occupancy grid representation. We use Any-time-Repairing A* (ARA*) to search over a tree of possible actions, choose a rough body path and select the locally-best footholds accordingly. We run a n-step lookahead optimization of the body trajectory using a dynamic stability metric, the Zero Moment Point (ZMP), that generates natural dynamic whole-body motions. A combination of floating-base inverse dynamics and virtual model control accurately executes the desired motions on an actively compliant system. Experimental trials show that this framework allows us to traverse terrains at nearly 6 times the speed of our previous work, evaluated over the same set of trials.

title: A mixed-integer convex optimization framework for robust multilegged robot locomotion planning over challenging terrain
abstract: This paper introduces an optimization-based framework for robust multilegged walking motion planning. Previous approaches use fixed gait sequences, and rely on Zero Moment Point (ZMP) to guarantee dynamic stability. While this combination works well on flat ground, it does not generalize to uneven terrain requiring aggressive gait or gait transition. To overcome such difficulties, in this paper, we present an optimization framework, that can plan both the contact location and gait sequence simultaneously in a mixed-integer convex optimization program. Moreover, we rely on the Contact Wrench Cone (CWC) stability criterion, which generalizes the ZMP criterion to uneven terrain with friction cone constraints, and we plan the walking motion together with the angular momentum through a convex optimization program. Our approach is successfully tested on a LittleDog quadruped over simulated scenarios. We show that on the flat ground, our planner generates a periodic gait, same as Central Pattern Generator + ZMP planner; while on uneven terrain, our planner can successfully generate a motion containing different gaits, with a center-of-mass motion that respects the friction cone constraints, which are violated by ZMP planners. This improvement clearly demonstrates the advantage of our approach over traditional planning strategies.

title: An Efficient Acyclic Contact Planner for Multiped Robots
abstract: We present a contact planner for complex legged locomotion tasks: standing up, climbing stairs using a handrail, crossing rubble, and getting out of a car. The need for such a planner was shown at the DARPA Robotics Challenge, where such behaviors could not be demonstrated (except for egress). Current planners suffer from their prohibitive algorithmic complexity because they deploy a tree of robot configurations projected in contact with the environment. We tackle this issue by introducing a reduction property: the reachability condition. This condition defines a geometric approximation of the contact manifold, which is of low dimension, presents a Cartesian topology, and can be efficiently sampled and explored. The hard contact planning problem can then be decomposed into two subproblems: first, we plan a path for the root without considering the whole-body configuration, using a sampling-based algorithm; then, we generate a discrete sequence of whole-body configurations in static equilibrium along this path, using a deterministic contact-selection algorithm. The reduction breaks the algorithm complexity encountered in previous works, resulting in the first interactive implementation of a contact planner (open source). While no contact planner has yet been proposed with theoretical completeness, we empirically show the interest of our framework: in a few seconds, with high success rates, we generate complex contact plans for various scenarios and two robots: HRP-2 and HyQ. These plans are validated in dynamic simulations or on the real HRP-2 robot.

title: Contribution to the Synthesis of Biped Gait
abstract: The autonomous motion of a mathematical model of a biped, performing locomotion in a specific gait, is described. The equations of motion and a method for solving the stability of the system are presented. It is shown how regions of controllability can be obtained as a function of the controllable element and the coefficient of friction.

title: A universal stability criterion of the foot contact of legged robots - adios ZMP
abstract: This paper proposes a universal stability criterion of the foot contact of legged robots. The proposed method checks if the sum of the gravity and the inertia wrench applied to the COG of the robot, which is proposed to be the stability criterion, is inside the polyhedral convex cone of the contact wrench between the feet of a robot and its environment. The criterion can be used to determine the strong stability of the foot contact when a robot walks on an arbitrary terrain and/or when the hands of the robot are in contact with it under the sufficient friction assumption. The determination is equivalent to check if the ZMP is inside the support polygon of the feet when the robot walks on a horizontal plane with sufficient friction. The criterion can also be used to determine if the foot contact is sufficiently weakly stable when the friction follows a physical law. Therefore, the proposed criterion can be used to judge what the ZMP can, and it can be used in more universal cases

title: On the stability of walking systems
abstract: We reconsider here the stability criteria usually proposed for the analysis of walking systems, exhibiting their limits and their ambiguity. We propose then some new criteria based on a thorough analysis of the dynamics of walking systems and precise deﬁnitions concerning their stability. Numerical methods are presented then to deal with these new criteria.

title: Trajectory and foothold optimization using low-dimensional models for rough terrain locomotion
abstract: We present a trajectory optimization framework for legged locomotion on rough terrain. We jointly optimize the center of mass motion and the foothold locations, while considering terrain conditions. We use a terrain costmap to quantify the desirability of a foothold location. We increase the gait's adaptability to the terrain by optimizing the step phase duration and modulating the trunk attitude, resulting in motions with guaranteed stability. We show that the combination of parametric models, stochastic-based exploration and receding horizon planning allows us to handle the many local minima associated with different terrain conditions and walking patterns. This combination delivers robust motion plans without the need for warm-starting. Moreover, we use soft-constraints to allow for increased flexibility when searching in the cost landscape of our problem. We showcase the performance of our trajectory optimization framework on multiple terrain conditions and validate our method in realistic simulation scenarios and experimental trials on a hydraulic, torque controlled quadruped robot.

title: Fast Trajectory Optimization for Legged Robots Using Vertex-Based ZMP Constraints
abstract: This letter combines the fast zero-moment-point approaches that work well in practice with the broader range of capabilities of a trajectory optimization formulation, by optimizing over body motion, footholds, and center of pressure simultaneously. We introduce a vertex-based representation of the support-area constraint, which can treat arbitrarily oriented point-, line-, and area-contacts uniformly. This generalization allows us to create motions, such as quadrupedal walking, trotting, bounding, pacing, combinations, and transitions between these, limping, bipedal walking, and push recovery all with the same approach. This formulation constitutes a minimal representation of the physical laws (unilateral contact forces) and kinematic restrictions (range of motion) in legged locomotion, which allows us to generate diverse motions in less than a second. We demonstrate the feasibility of the generated motions on a real quadruped robot.

title: Whole-body motion planning with centroidal dynamics and full kinematics
abstract: To plan dynamic, whole-body motions for robots, one conventionally faces the choice between a complex, full-body dynamic model containing every link and actuator of the robot, or a highly simplified model of the robot as a point mass. In this paper we explore a powerful middle ground between these extremes. We exploit the fact that while the full dynamics of humanoid robots are complicated, their centroidal dynamics (the evolution of the angular momentum and the center of mass (COM) position) are much simpler. By treating the dynamics of the robot in centroidal form and directly optimizing the joint trajectories for the actuated degrees of freedom, we arrive at a method that enjoys simpler dynamics, while still having the expressiveness required to handle kinematic constraints such as collision avoidance or reaching to a target. We further require that the robot's COM and angular momentum as computed from the joint trajectories match those given by the centroidal dynamics. This ensures that the dynamics considered by our optimization are equivalent to the full dynamics of the robot, provided that the robot's actuators can supply sufficient torque. We demonstrate that this algorithm is capable of generating highly-dynamic motion plans with examples of a humanoid robot negotiating obstacle course elements and gait optimization for a quadrupedal robot. Additionally, we show that we can plan without pre-specifying the contact sequence by exploiting the complementarity conditions between contact forces and contact distance.

title: 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 for forward simulation, the proposed algorithm poses the optimization problem as a Mathematical Program with Complementarity Constraints. We leverage Sequential Quadratic Programming 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.

title: Planning robust walking motion on uneven terrain via convex optimization
abstract: In this paper, we present a convex optimization problem to generate Center of Mass (CoM) and momentum trajectories of a walking robot, such that the motion robustly satisfies the friction cone constraints on uneven terrain. We adopt the Contact Wrench Cone (CWC) criterion to measure a robot's dynamical stability, which generalizes the venerable Zero Moment Point (ZMP) criterion. Unlike the ZMP criterion, which is ideal for walking on flat ground with unbounded tangential friction forces, the CWC criterion incorporates non-coplanar contacts with friction cone constraints. We measure the robustness of the motion using the margin in the Contact Wrench Cone at each time instance, which quantifies the capability of the robot to instantaneously resist external force/torque disturbance, without causing the foot to tip over or slide. For pre-specified footstep location and time, we formulate a convex optimization problem to search for robot linear and angular momenta that satisfy the CWC criterion. We aim to maximize the CWC margin to improve the robustness of the motion, and minimize the centroidal angular momentum (angular momentum about CoM) to make the motion natural. Instead of directly minimizing the non-convex centroidal angular momentum, we resort to minimizing a convex upper bound. We show that our CWC planner can generate motion similar to the result of the ZMP planner on flat ground with sufficient friction. Moreover, on an uneven terrain course with friction cone constraints, our CWC planner can still find feasible motion, while the outcome of the ZMP planner violates the friction limit.

title: Trajectory Optimization Through Contacts and Automatic Gait Discovery for Quadrupeds
abstract: In this letter, we present a trajectory optimization framework for whole-body motion planning through contacts. We demonstrate how the proposed approach can be applied to automatically discover different gaits and dynamic motions on a quadruped robot. In contrast to most previous methods, we do not prespecify contact-switches, -timings, -points or gait patterns, but they are a direct outcome of the optimization. Furthermore, we optimize over the entire dynamics of the robot, which enables the optimizer to fully leverage the capabilities of the robot. To illustrate the spectrum of achievable motions, we show eight different tasks, which would require very different control structures when solved with state-of-the-art methods. Using our trajectory optimization approach, we are solving each task with a simple, high level cost function and without any changes in the control structure. Furthermore, we fully integrate our approach with the robot's control and estimation framework such that we are able to run the optimization online. Through several hardware experiments, we show that the optimized trajectories and control inputs can be directly applied to physical systems.

title: A convex model of humanoid momentum dynamics for multi-contact motion generation
abstract: Linear models for control and motion generation of humanoid robots have received significant attention in the past years, not only due to their well known theoretical guarantees, but also because of practical computational advantages. However, to tackle more challenging tasks and scenarios such as locomotion on uneven terrain, a more expressive model is required. In this paper, we are interested in contact interaction-centered motion optimization based on the momentum dynamics model. This model is non-linear and non-convex; however, we find a relaxation of the problem that allows us to formulate it as a single convex quadratically-constrained quadratic program (QCQP) that can be very efficiently optimized and is useful for multi-contact planning. This convex model is then coupled to the optimization of end-effector contact locations using a mixed integer program, which can also be efficiently solved. This becomes relevant e.g. to recover from external pushes, where a predefined stepping plan is likely to fail and an online adaptation of the contact location is needed. The performance of our algorithm is demonstrated in several multi-contact scenarios for a humanoid robot.

title: Mixed-integer convex optimization for planning aggressive motions of legged robots over rough terrain
abstract: Sc. D., Massachusetts Institute of Technology, Department of Mechanical Engineering, 2016.

title: Centroidal dynamics of a humanoid robot
abstract: The center of mass (CoM) of a humanoid robot occupies a special place in its dynamics. As the location of its effective total mass, and consequently, the point of resultant action of gravity, the CoM is also the point where the robot’s aggregate linear momentum and angular momentum are naturally defined. The overarching purpose of this paper is to refocus our attention to centroidal dynamics: the dynamics of a humanoid robot projected at its CoM. In this paper we specifically study the properties, structure and computation schemes for the centroidal momentum matrix (CMM), which projects the generalized velocities of a humanoid robot to its spatial centroidal momentum. Through a transformation diagram we graphically show the relationship between this matrix and the well-known joint-space inertia matrix. We also introduce the new concept of “average spatial velocity” of the humanoid that encompasses both linear and angular components and results in a novel decomposition of the kinetic energy. Further, we develop a very efficient $$O(N)$$O(N) algorithm, expressed in a compact form using spatial notation, for computing the CMM, centroidal momentum, centroidal inertia, and average spatial velocity. Finally, as a practical use of centroidal dynamics we show that a momentum-based balance controller that directly employs the CMM can significantly reduce unnecessary trunk bending during balance maintenance against external disturbance.

title: Mixed-integer programming for control
abstract: The article describes how mixed-integer programming (MIP) can be employed for feedback control. MIP can be used to find optimal trajectories subject to integer constraints, which can encode discrete decisions or nonconvexity, for example. This optimization can be performed online within model predictive control (MPC) to implement a feedback control law. The article discusses how to model systems using MIP, the implementation as a MPC, and techniques for fast solutions of the optimizations to make them suitable for real-time use.

title: Hierarchical planning of dynamic movements without scheduled contact sequences
abstract: Most animal and human locomotion behaviors for solving complex tasks involve dynamic motions and rich contact interaction. In fact, complex maneuvers need to consider dynamic movement and contact events at the same time. We present a hierarchical trajectory optimization approach for planning dynamic movements with unscheduled contact sequences. We compute whole-body motions that achieve goals that cannot be reached in a kinematic fashion. First, we find a feasible CoM motion according to the centroidal dynamics of the robot. Then, we refine the solution by applying the robot's full-dynamics model, where the feasible CoM trajectory is used as a warm-start point. To accomplish the unscheduled contact behavior, we use complementarity constraints to describe the contact model, i.e. environment geometry and non-sliding active contacts. Both optimization phases are posed as Mathematical Program with Complementarity Constraints (MPCC). Experimental trials demonstrate the performance of our planning approach in a set of challenging tasks.

title: Fast algorithms to test robust static equilibrium for legged robots
abstract: Maintaining equilibrium is of primary importance for legged systems. It is not surprising then that static equilibrium is at the core of most control/planning algorithms for legged robots. Being able to check whether a system is in static equilibrium is thus important, and doing it efficiently is crucial. While this is straightforward for a system in contact with a flat ground only, it is not the case for arbitrary contact geometries. In this paper we propose two new techniques to test static equilibrium and we show that they are computationally faster than all other existing methods. Moreover, we address the issue of robustness to errors in the contact-force tracking, which could lead to slippage or rotation at the contacts. We extend all the discussed techniques to test for robust static equilibrium, that is the ability to maintain equilibrium while avoiding to lose contacts despite bounded force-tracking errors. Accounting for robustness does not affect the computation time of the equilibrium tests, while it qualitatively improves the contact postures generated by our reachability-based multicontact planner.

title: The Actuation-consistent Wrench Polytope (AWP) and the Feasible Wrench Polytope (FWP)
abstract: The motivation of our current research is to devise motion planners for legged locomotion that are able to exploit the robot's actuation capabilities. This means, when possible, to minimize joint torques or to propel as much as admissible when required. For this reason we define two new 6 dimensional bounded polytopes that we name Actuation-consistent Wrench Polytope (AWP) and Feasible Wrench Polytope (FWP). These objects turn out to be very useful in motion planning for the definition of constraints on the accelerations of the Center of Mass of the robot that respect the friction cones and the actuation limits. The AWP and the FWP could be used also in the robot design phase to size the actuators of the system based on some predefined reference motion.

title: Theory and Applications of Robust Optimization
abstract: In this paper we survey the primary research, both theoretical and applied, in the area of robust optimization (RO). Our focus is on the computational attractiveness of RO approaches, as well as the modeling power and broad applicability of the methodology. In addition to surveying prominent theoretical results of RO, we also present some recent results linking RO to adaptable models for multistage decision-making problems. Finally, we highlight applications of RO across a wide spectrum of domains, including finance, statistics, learning, and various areas of engineering.

title: Motion Planning for Quadrupedal Locomotion: Coupled Planning, Terrain Mapping, and Whole-Body Control
abstract: Planning whole-body motions while taking into account the terrain conditions is a challenging problem for legged robots since the terrain model might produce many local minima. Our coupled planning method uses stochastic and derivatives-free search to plan both foothold locations and horizontal motions due to the local minima produced by the terrain model. It jointly optimizes body motion, step duration and foothold selection, and it models the terrain as a cost-map. Due to the novel attitude planning method, the horizontal motion plans can be applied to various terrain conditions. The attitude planner ensures the robot stability by imposing limits to the angular acceleration. Our whole-body controller tracks compliantly trunk motions while avoiding slippage, as well as kinematic and torque limits. Despite the use of a simplified model, which is restricted to flat terrain, our approach shows remarkable capability to deal with a wide range of noncoplanar terrains. The results are validated by experimental trials and comparative evaluations in a series of terrains of progressively increasing complexity.

title: Design of HyQ – a hydraulically and electrically actuated quadruped robot
abstract: A new versatile hydraulically powered quadruped robot (HyQ) has been developed to serve as a platform to study not only highly dynamic motions, such as running and jumping, but also careful navigation over very rough terrain. HyQ stands 1 m tall, weighs roughly 90 kg, and features 12 torque-controlled joints powered by a combination of hydraulic and electric actuators. The hydraulic actuation permits the robot to perform powerful and dynamic motions that are hard to achieve with more traditional electrically actuated robots. This paper describes design and specifications of the robot and presents details on the hardware of the quadruped platform, such as the mechanical design of the four articulated legs and of the torso frame, and the configuration of the hydraulic power system. Results from the first walking experiments are presented, along with test studies using a previously built prototype leg.

title: Heterogeneous Sensor Fusion for Accurate State Estimation of Dynamic Legged Robots
abstract: In this paper we present a system for the state estimation of a dynamically walking and trotting quadruped. The approach fuses four heterogeneous sensor sources (inertial, kinematic, stereo vision and LIDAR) to maintain an accurate and consistent estimate of the robot’s base link velocity and position in the presence of disturbances such as slips and missteps. We demonstrate the performance of our system, which is robust to changes in the structure and lighting of the environment, as well as the terrain over which the robot crosses. Our approach builds upon a modular inertial-driven Extended Kalman Filter which incorporates a rugged, probabilistic leg odometry component with additional inputs from stereo visual odometry and LIDAR registration. The simultaneous use of both stereo vision and LIDAR helps combat operational issues which occur in real applications. To the best of our knowledge, this paper is the first to discuss the complexity of consistent estimation of pose and velocity states, as well as the fusion of multiple exteroceptive signal sources at largely different frequencies and latencies, in a manner which is acceptable for a quadruped’s feedback controller. A substantial experimental evaluation demonstrates the robustness and accuracy of our system, achieving continuously accurate localization and drift per distance traveled below 1cm/m.