Planning multi-contact motions in a receding horizon fashion requires a value function to guide the planning with respect to the future, e.g., building momentum to traverse large obstacles. Traditionally, the value function is approximated by computing trajectories in a prediction horizon (never executed) that foresees the future beyond the execution horizon. However, given the non-convex dynamics of multi-contact motions, this approach is computationally expensive. To enable online Receding Horizon Planning (RHP) of multi-contact motions, we find efficient approximations of the value function. Specifically, we propose a trajectory-based and a learning-based approach. In the former, namely RHP with Multiple Levels of Model Fidelity, we approximate the value function by computing the prediction horizon with a convex relaxed model. In the latter, namely Locally-Guided RHP, we learn an oracle to predict local objectives for locomotion tasks, and we use these local objectives to construct local value functions for guiding a short-horizon RHP. We evaluate both approaches in simulation by planning centroidal trajectories of a humanoid robot walking on moderate slopes, and on large slopes where the robot cannot maintain static balance. Our results show that locally-guided RHP achieves the best computation efficiency (95%-98.6% cycles converge online). This computation advantage enables us to demonstrate online receding horizon planning of our real-world humanoid robot Talos walking in dynamic environments that change on-the-fly.

State-of-the-art approaches to footstep planning assume reduced-order dynamics when solving the combinatorial problem of selecting contact surfaces in real time. However, in exchange for computational efficiency, these approaches ignore joint torque limits and limb dynamics. In this work, we address these limitations by presenting a topology-based approach that enables model predictive control (MPC) to simultaneously plan full-body motions, torque commands, footstep placements, and contact surfaces in real time. To determine if a robot's foot is inside a contact surface, we borrow the winding number concept from topology. We then use this winding number and potential field to create a contact-surface penalty function. By using this penalty function, MPC can select a contact surface from all candidate surfaces in the vicinity and determine footstep placements within it. We demonstrate the benefits of our approach by showing the impact of considering full-body dynamics, which includes joint torque limits and limb dynamics, on the selection of footstep placements and contact surfaces. Furthermore, we validate the feasibility of deploying our topology-based approach in an MPC scheme and explore its potential capabilities through a series of experimental and simulation trials.

Optimal control (OC) using inverse dynamics provides numerical benefits, such as coarse optimization, cheaper computation of derivatives, and a high convergence rate. However, to take advantage of these benefits in model predictive control (MPC) for legged robots, it is crucial to handle efficiently its large number of equality constraints. To accomplish this, we first propose a novel approach to handle equality constraints based on nullspace parameterization. Our approach balances optimality, and both dynamics and equality-constraint feasibility appropriately, which increases the basin of attraction to high-quality local minima. To do so, we modify our feasibility-driven search by incorporating a merit function. Furthermore, we introduce a condensed formulation of inverse dynamics that considers arbitrary actuator models. We also propose a novel MPC based on inverse dynamics within a perceptive locomotion framework. Finally, we present a theoretical comparison of OC with forward and inverse dynamics and evaluate both numerically. Our approach enables the first application of inverse-dynamics MPC on hardware, resulting in the state-of-the-art dynamic climbing on the ANYmal robot. We benchmark it over a wide range of robotics problems and generate agile and complex maneuvers. We show the computational reduction of our nullspace resolution and condensed formulation (up to${47.3}\boldsymbol{\%}$). We provide evidence of the benefits of our approach by solving coarse optimization problems with a high convergence rate (up to 10 Hz of discretization). Our algorithm is publicly available insideCrocoddyl.

Derivative based optimization methods are efficient at solving optimal control problems near local optima. However, their ability to converge halts when derivative information vanishes. The inference approach to optimal control does not have strict requirements on the objective landscape. However, sampling, the primary tool for solving such problems, tends to be much slower in computation time. We propose a new method that combines second order methods with inference. We utilise the Kullback Leibler (KL) control framework to formulate an inference problem that computes the optimal controls from an adaptive distribution approximating the solution of the second order method. Our method allows for combining simple convex and non convex cost functions. This simplifies the process of cost function design and leverages the strengths of both inference and second order optimization. We compare our method to Model Predictive Path Integral (MPPI) and iterative Linear Quadratic Gaussian controller (iLQG), outperforming both in sample efficiency and quality on manipulation and obstacle avoidance tasks.

Non-linear trajectory optimisation methods require good initial guesses to converge to a locally optimal solution. A feasible guess can often be obtained by allocating a large amount of time for the trajectory to be complete. However for unstable dynamical systems such as humanoid robots, this quasi-static assumption does not always hold. We propose a conservative formulation of the trajectory problem that simultaneously computes a feasible path and its time allocation. The problem is solved as a convex optimisation problem guaranteed to converge to a feasible local optimum. The approach is evaluated with the computation of feasible trajectories that traverse sequentially a sequence of polytopes. We demonstrate that on instances of the problem where quasi static solutions are not admissible, our approach is able to find a feasible solution with a success rate above 80% in all the scenarios considered, in less than 10ms for problems involving traversing less than 5 polytopes and less than 1s for problems involving 20 polytopes, thus demonstrating its ability to reliably provide initial guesses to advanced non linear solvers.

We present a versatile framework for the computational co-design of legged robots and dynamic maneuvers. Current state-of-the-art approaches are typically based on random sampling or concurrent optimization. We propose a novel bilevel optimization approach that exploits the derivatives of the motion planning sub-problem (i.e., the lower level). These motion-planning derivatives allow us to incorporate arbitrary design constraints and costs in an general-purpose nonlinear program (i.e., the upper level). Our approach allows for the use of any differentiable motion planner in the lower level and also allows for an upper level that captures arbitrary design constraints and costs. It efficiently optimizes the robot's morphology, payload distribution and actuator parameters while considering its full dynamics, joint limits and physical constraints such as friction cones. We demonstrate these capabilities by designing quadruped robots that jump and trot. We show that our method is able to design a more energy-efficient Solo robot for these tasks.

Quadruped robots have proved their robustness to cross complex terrain despite little environment knowledge. Yet advanced locomotion controllers are expected to take advantage of exteroceptive information. This paper presents a complete method to plan and control the locomotion of quadruped robots when 3D information about the surrounding obstacles is available, based on several stages of decision. We first propose a contact planner formulated as a mixed-integer program, optimized on-line at each new robot step. It selects a surface from a set of convex surfaces describing the environment for the next footsteps while ensuring kinematic constraints. We then propose to optimize the exact contact location and the feet trajectories at control frequency to avoid obstacles, thanks to an efficient formulation of quadratic programs optimizing Bezier curves. By relying on the locomotion controller of our quadruped robot Solo, we finally implement the complete method, provided as an open-source package. Its efficiency is asserted by statistical evaluation of the importance of each component in simulation. We have a 100% success rate for our framework, and we show that the deactivation of the contact planning, footstep adaptation and collision avoidance, respectively induced a drop to 70%, 62% and 83% success rate in the worst case, justifying the complete architecture.

In Receding Horizon Planning (RHP), it is critical that the motion being executed facilitates the completion of the task, e.g. building momentum to overcome large obstacles. This requires a value function to inform the desirability of robot states. However, given the complex dynamics, value functions are often approximated by expensive computation of trajectories in an extended planning horizon. In this work, to achieve online multi-contact Receding Horizon Planning (RHP), we propose to learn an oracle that can predict local objectives (intermediate goals) for a given task based on the current robot state and the environment. Then, we use these local objectives to construct local value functions to guide a short-horizon RHP. To obtain the oracle, we take a supervised learning approach, and we present an incremental training scheme that can improve the prediction accuracy by adding demonstrations on how to recover from failures. We compare our approach against the baseline (long-horizon RHP) for planning centroidal trajectories of humanoid walking on moderate slopes as well as large slopes where static stability cannot be achieved. We validate these trajectories by tracking them via a whole-body inverse dynamics controller in simulation. We show that our approach can achieve online RHP for 95%-98.6% cycles, outperforming the baseline (8%-51.2%).

The combinatorics inherent to the issue of planning legged locomotion can be addressed by decomposing the problem: first, select a guide path abstracting the contacts with a heuristic model; then compute the contact sequence to balance the robot gait along the guide path. While several models have been proposed to compute such a path, none have yet managed to efficiently capture the complexity of legged locomotion on arbitrary terrain. In this paper, we present a novel method to automatically build a local controller, or steering method, to generate a guide path along which a feasible contact sequence can be built. Our reinforcement learning approach is coupled with a geometric condition for feasibility during the training, which improves the convergence rate without incurring a loss in generality. We have designed a dedicated environment along with an associated reward function to run a classical reinforcement learning algorithm that computes the steering method. The policy takes a target velocity and a local heightmap of the terrain around the robot as inputs, and steers the path where new contacts should be created. It is then coupled with a contact generator that creates the contacts to support the robot movement. We show that the trained policy has an improved generalization and higher success rate at generating feasible contact plans than previous approaches. As a result, this policy can be used with a path planning algorithm to navigate complex environments.

This paper presents the first successful experiment implementing whole-body model predictive control with state feedback on a torque-control humanoid robot. We demonstrate that our control scheme is able to do whole-body target tracking, control the balance in front of strong external perturbations and avoid collision with an external object. The key elements for this success are threefold. First, optimal control over a receding horizon is implemented with Crocoddyl, an optimal control library based on differential dynamics programming, providing state-feedback control in less than 10 ms. Second, a warm start strategy based on memory of motion has been implemented to overcome the sensitivity of the optimal control solver to initial conditions. Finally, the optimal trajectories are executed by a low-level torque controller, feedbacking on direct torque measurement at high frequency. This paper provides the details of the method, along with analytical benchmarks with the real humanoid robot Talos.A video of the experiment is available at https://peertube.laas.fr/videos/watch/cbc25927-337c-4635-a1bc-153b9aeb4135

One of the main challenges of planning legged locomotion in complex environments is the combinatorial contact selection problem. Recent contributions propose to use integer variables to represent which contact surface is selected, and then to rely on modern mixed-integer (MI) optimization solvers to handle this combinatorial issue. To reduce the computational cost of MI, we exploit the sparsity properties of L1 norm minimization techniques to relax the contact planning problem into a feasibility linear program. Our approach accounts for kinematic reachability of the center of mass (COM) and of the contact effectors. We ensure the existence of a quasi-static COM trajectory by restricting our plan to quasi-flat contacts. For planning 10 steps with less than 10 potential contact surfaces for each phase, our approach is 50 to 100 times faster that its MI counterpart, which suggests potential applications for online contact re-planning. The method is demonstrated in simulation with the humanoid robots HRP-2 and Talos over various scenarios.

An important issue when synthesizing legged locomotion plans is the combinatorial complexity that arises from gait pattern selection. Though it can be defined manually, the gait pattern plays an important role in the feasibility and optimality of a motion with respect to a task. Replacing human intuition with an automatic and efficient approach for gait pattern selection would allow for more autonomous robots, responsive to task and environment changes. To this end, we propose the idea of building a map from task to gait pattern selection for given environment and performance objective. Indeed, we show that for a 2D half-cheetah model and a quadruped robot, a direct mapping between a given task and an optimal gait pattern can be established. We use supervised learning to capture the structure of this map in a form of gait regions. Furthermore, we propose to construct a warm-starting trajectory for each gait region. We empirically show that these warm-starting trajectories improve the convergence speed of our trajectory optimization problem up to 60 times when compared with random initial guesses. Finally, we conduct experimental trials on the ANYmal robot to validate our method.

Synthesizing legged locomotion requires planning one or several steps ahead (literally): when and where, and with which effector should the next contact(s) be created between the robot and the environment? Validating a contact candidate implies a minima the resolution of a slow, nonlinear optimization problem, to demonstrate that a center of mass (CoM) trajectory, compatible with the contact transition constraints, exists. We propose a conservative reformulation of this trajectory generation problem as a convex 3-D linear program, named convex resolution of centroidal dynamic trajectories (CROC). It results from the observation that if the CoM trajectory is a polynomial with only one free variable coefficient, the nonlinearity of the problem disappears. This has two consequences. On the positive side, in terms of computation times, CROC outperforms the state of the art by at least one order of magnitude, and allows to consider interactive applications (with a planning time roughly equal to the motion time). On the negative side, in our experiments, our approach finds a majority of the feasible trajectories found by a nonlinear solver, but not all of them. Still, we demonstrate that the solution space covered by CROC is large enough to achieve the automated planning of a large variety of locomotion tasks for different robots, demonstrated in simulation and on the real HRP-2 robot, several of which were rarely seen before. Another significant contribution is the introduction of a Bezier curve representation of the problem, which guarantees that the constraints of the CoM trajectory are verified continuously, and not only at discrete points as traditionally done. This formulation is lossless, and results in more robust trajectories. It is not restricted to CROC, but could rather be integrated with any method from the state of the art.

Predictive control is an efficient model-based methodology to control complex dynamical systems. In general, it boils down to the resolution at each control cycle of a large nonlinear optimization problem. A critical issue is then to provide a good guess to initialize the nonlinear solver so as to speed up convergence. This is particularly important when disturbances or changes in the environment prevent the use of the trajectory computed at the previous control cycle as initial guess. In this paper, we introduce an original and very efficient solution to automatically build this initial guess. We propose to rely on off-line computation to build an approximation of the optimal trajectories, that can be used on-line to initialize the predictive controller. To that end, we combined the use of sampling-based planning, policy learning with generic representations (such as neural networks), and direct optimal control. We first propose an algorithm to simultaneously build a kinodynamic probabilistic roadmap (PRM) and approximate value function and control policy. This algorithm quickly converges toward an approximation of the optimal state-control trajectories (along with an optimal PRM). Then, we propose two methods to store the optimal trajectories and use them to initialize the predictive controller. We experimentally show that directly storing the state-control trajectories leads the predictive controller to quickly converges (2 to 5 iterations) toward the (global) optimal solution. The results are validated in simulation with an unmanned aerial vehicle (UAV) and other dynamical systems.

We tackle the transition feasibility problem, that is the issue of determining whether there exists a feasible motion connecting two configurations of a legged robot. To achieve this we introduce CROC, a novel method for computing centroidal dynamics trajectories in multi-contact planning contexts. Our approach is based on a conservative and convex reformulation of the problem, where we represent the center of mass trajectory as a Bezier curve comprising a single free control point as a variable. Under this formulation, the transition problem is solved efficiently with a Linear Program (LP)of low dimension. We use this LP as a feasibility criterion, incorporated in a sampling-based contact planner, to discard efficiently unfeasible contact plans. We are thus able to produce robust contact sequences, likely to define feasible motion synthesis problems. We illustrate this application on various multi-contact scenarios featuring HRP2 and HyQ. We also show that we can use CROC to compute valuable initial guesses, used to warm-start non-linear solvers for motion generation methods. This method could also be used for the 0 and 1-Step capturability problem. The source code of CROC is available under an open source BSD-2 License.

Synthesizing motions for legged characters in arbitrary environments is a long-standing problem that has recently received a lot of attention from the computer graphics community. We tackle this problem with a procedural approach that is generic, fully automatic, and independent from motion capture data. The main contribution of this article is a point-mass-model-based method to synthesize Center Of Mass trajectories. These trajectories are then used to generate the whole-body motion of the character. The use of a point mass model results in physically inconsistent motions and joint limit violations when mapped back to a full- body motion. We mitigate these issues through the use of a novel formulation of the kinematic constraints that allows us to generate a quasi-static Center Of Mass trajectory in a way that is both user-friendly and computationally efficient. We also show that the quasi-static constraint can be relaxed to generate motions usable for computer animation at the cost of a moderate violation of the dynamic constraints. Our method was integrated in our open-source contact planner and tested with different scenarios—some never addressed before—featuring legged characters performing non-gaited motions in cluttered environments. The computational efficiency of our trajectory generation algorithm (under one ms to compute one second of trajectory) enables us to synthesize motions in a few seconds, one order of magnitude faster than state-of-the-art methods. Although our method is empirically able to synthesize collision-free motions, the formal handling of environmental constraints is not part of the proposed method and left for future work.

The ability to anticipate a fall is fundamental for any robot that has to balance. Currently, fast fall-prediction algorithms only exist for simple models, such as the linear inverted pendulum model (LIPM), whose validity breaks down in multicontact scenarios (i.e., when contacts are not limited to a flat ground). This paper presents a fast fall-prediction algorithm based on the point-mass model, which remains valid in multicontact scenarios. The key assumption of our algorithm is that, in order to come to a stop without changing its contacts, a robot only needs to accelerate its center of mass in the direction opposite to its velocity. This assumption allows us to predict the fall by means of a convex optimal control problem, which we solve with a fast custom algorithm (less than 11 ms of computation time). We validated the approach through extensive simulations with the humanoid robot HRP-2 in randomly-sampled scenarios. Comparisons with standard LIPM-based methods demonstrate the superiority of our algorithm in predicting the fall of the robot, when controlled with a state-of-the-art balance controller. This paper lays the foundations for the solution of the challenging problem of push recovery in multicontact scenarios.

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.

We present a novel method for synthesizing collision-free, dynamic locomotion behaviors for legged robots, including jumping, going down a very steep slope, or recovering from a push using the arms of the robot. The approach is automatic and generic: non-gaited motions, comprising arbitrary contact postures can be generated along any environment. At the core of our framework is a new steering method that generates trajectories connecting two states of the robot. These trajectories account for the state-dependent, centroidal dynamic constraints inherent to legged robots. The method, of low dimension, formulated as a Linear Program, is really efficient to compute, and can find an application in various problems related to legged locomotion. By incorporating this steering method into an existing sampling-based contact planner, we propose the first kinodynamic contact planner for legged robots.

This paper presents a generic and efficient approach to generate dynamically consistent motions for under-actuated systems like humanoid or quadruped robots. The main contribution is a walking pattern generator, able to compute a stable trajectory of the center of mass of the robot along with the angular momentum, for any given configuration of contacts (e.g. on uneven, sloppy or slippery terrain, or with closed-gripper). Unlike existing methods, our solver is fast enough to be applied as a model-predictive controller. We then integrate this pattern generator in a complete framework: an acyclic contact planner is first used to automatically compute the contact sequence from a 3D model of the environment and a desired final posture; a stable walking pattern is then computed by the proposed solver; a dynamically-stable whole-body trajectory is finally obtained using a second-order hierarchical inverse kinematics. The implementation of the whole pipeline is fast enough to plan a step while the previous one is executed. The interest of the method is demonstrated by real experiments on the HRP-2 robot, by performing long-step walking and climbing a staircase with handrail support.

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.

We present HPP, a software designed for complex classes of motion planning problems, such as navigation among movable objects, manipulation, contact-rich multiped locomotion, or elastic rods in cluttered environments. HPP is an open-source answer to the lack of a standard framework for these important issues for robotics and graphics communities.

We present a framework capable of automatically planning and animating dynamic multi-contact jumping trajectories for arbitrary legged characters and environments. Our contribution is the introduction of an approximate yet efficient multi-contact impulse formulation, used at the motion planning phase. We combine this criterion with a heuristic for estimating the contact locations without explicitly computing them, which breaks the combinatorial aspect of contact generation. This low dimensional formulation is used to efficiently extend an existing ballistic motion planner to legged characters. We then propose a procedural method to animate the planned trajectory. Our approach thus results from a trade-off between accuracy of the law of physics and computational efficiency. We empirically justify this approach by demonstrating a large variety of behaviors for four legged characters in five scenarios.

We present an interactive motion planning algorithm to compute plausible trajectories for high-DOF human-like characters. Given a discrete sequence of contact configurations, we use a three-phase optimization approach to ensure that the resulting trajectory is collision-free, smooth, and satisfies dynamic balancing constraints. Our approach can directly compute dynamically balanced and natural-looking motions at interactive frame rates and is considerably faster than prior methods. We highlight its performance on complex human motion benchmarks corresponding to walking, climbing, crawling, and crouching, where the discrete configurations are generated from a kinematic planner or extracted from motion capture datasets.

Abstract Character animation based on motion capture provides intrinsically plausible results, but lacks the flexibility of procedural methods. Motion editing methods partially address this limitation by adapting the animation to small deformations of the environment. We extend one such method, the so‐called relationship descriptors, to tackle the issue of motion editing under large environment deformations. Large deformations often result in joint limits violation, loss of balance, or collisions. Our method handles these situations by automatically detecting and re‐positioning invalidated contacts. The new contact configurations are chosen to preserve the mechanical properties of the original contacts in order to provide plausible support phases. When it is not possible to find an equivalent contact, a procedural animation is generated and blended with the original motion. Thanks to an optimization scheme, the resulting motions are continuous and preserve the style of the reference motions. The method is fully interactive and enables the motion to be adapted on‐line even in case of large changes of the environment. We demonstrate our method on several challenging scenarios, proving its immediate application to 3D animation softwares and video games.