Ensuring constraint satisfaction is a key requirement for safety-critical systems, which include most robotic platforms. For example, constraints can be used for modeling joint position/velocity/torque limits and collision avoidance. Constrained systems are often controlled using Model Predictive Control, because of its ability to naturally handle constraints, relying on numerical optimization. However, ensuring constraint satisfaction is challenging for nonlinear systems/constraints. A well-known tool to make controllers safe is the so-called control-invariant set (a.k.a. safe set). In our previous work, we have shown that safety can be improved by letting the safe-set constraint recede along the MPC horizon. In this paper, we push that idea further by exploiting parallel computation to improve safety. We solve several MPC problems at the same time, where each problem instantiates the safe-set constraint at a different time step along the horizon. Finally, the controller can select the best solution according to some user-defined criteria. We validated this idea through extensive simulations with a 3 -joint robotic arm, showing that significant improvements can be achieved in terms of safety and performance, even using as little as 4 computational cores.

In recent years, advanced model-based and data-driven control methods are unlocking the potential of complex robotics systems, and we can expect this trend to continue at an exponential rate in the near future. However, ensuring safety with these advanced control methods remains a challenge. A well-known tool to make controllers (either Model Predictive Controllers or Reinforcement Learning policies) safe, is the so-called control-invariant set (a.k.a. safe set). Unfortunately, for nonlinear systems, such a set cannot be exactly computed in general. Numerical algorithms exist for computing approximate control-invariant sets, but classic theoretic control methods break down if the set is not exact. This paper presents our recent efforts to address this issue. We present a novel Model Predictive Control scheme that can guarantee recursive feasibility and/or safety under weaker assumptions than classic methods. In particular, recursive feasibility is guaranteed by making the safe-set constraint move backward over the horizon, and assuming that such set satisfies a condition that is weaker than control invariance. Safety is instead guaranteed under an even weaker assumption on the safe set, triggering a safe task-abortion strategy whenever a risk of constraint violation is detected. We evaluated our approach on a simulated robot manipulator, empirically demonstrating that it leads to less constraint violations than state-of-the-art approaches, while retaining reasonable performance in terms of tracking cost, number of completed tasks, and computation time.

In a world designed for legs, quadrupeds, bipeds, and humanoids have the opportunity to impact emerging robotics applications from logistics, to agriculture, to home assistance. The goal of this survey is to cover the recent progress toward these applications that have been driven by model-based optimization for the real-time generation and control of movement. The majority of the research community has converged on the idea of generating locomotion control laws by solving an optimal control problem (OCP) in either a model-based or data-driven manner. However, solving the most general of these problems online remains intractable due to complexities from intermittent unidirectional contacts with the environment, and from the many degrees of freedom of legged robots. This survey covers methods that have been pursued to make these OCPs computationally tractable, with a specific focus on how environmental contacts are treated, how the model can be simplified, and how these choices affect the numerical solution methods employed. The survey focuses on model-based optimization while paving its way for broader combination with learning-based formulations to accelerate progress in this growing field.

Rescue missions in mountain environments are hardly achievable by standard legged robots—because of the high slopes—or by flying robots—because of limited payload capacity. We present a concept for a rope-aided climbing robot which can negotiate up-to-vertical slopes and carry heavy payloads. The robot is attached to the mountain through a rope, and it is equipped with a leg to push against the mountain and initiate jumping maneuvers. Between jumps, a hoist is used to wind/unwind the rope to move vertically and affect the lateral motion. This simple (yet effective) two-fold actuation allows the system to achieve high safety and energy efficiency. Indeed, the rope prevents the robot from falling while compensating for most of its weight, drastically reducing the effort required by the leg actuator. We also present an optimal control strategy to generate point-to-point trajectories overcoming an obstacle. We achieve fast computation time ($16\ m$long jump, showing the effectiveness of the proposed approach, and confirming the interest of our concept. Finally, we performed a reachability analysis showing that the region of achievable targets is strongly affected by the friction properties of the foot-wall contact.

This paper deals with the robust control of fully-actuated robots subject to joint position, velocity and acceleration bounds. Robotic systems are subject to disturbances, which may arise from modeling errors, sensor noises or communication delays. This work presents mathematical and computational tools to ensure the robust satisfaction of joint bounds in the control of robot manipulators. We consider a system subject to bounded additive disturbances on the control inputs, with constant joint position, velocity and acceleration bounds. We compute the robust viability kernel, which is the set of states such that, starting from any such state, it is possible to avoid violating the constraints in the future, despite the presence of disturbances. Then we develop an efficient algorithm to compute the range of feasible accelerations that allow the state to remain inside the robust viability kernel. Our derivation ensures the continuous-time robust satisfaction of the joint bounds, while considering the discrete-time nature of the control inputs. Tests are performed in simulation with a single joint and a 6-DOF robot manipulator, demonstrating the effectiveness of the proposed approach compared to other state-of-the-art methods.

Model predictive control (MPC) has shown great success for controlling complex systems, such as legged robots. However, when closing the loop, the performance and feasibility of the finite horizon optimal control problem (OCP) solved at each control cycle are not guaranteed anymore. This is due to model discrepancies, the effect of low-level controllers, uncertainties, and sensor noise. To address these issues, we propose a modified version of a standard MPC approach used in legged locomotion with viability (weak forward invariance) guarantees. In this approach, instead of adding a (conservative) terminal constraint to the problem, we propose to use the measured state projected to the viability kernel in the OCP solved at each control cycle. Moreover, we use past experimental data to find the best cost weights, which measure a combination of performance, constraint satisfaction robustness, or stability (invariance). These interpretable costs measure the tradeoff between robustness and performance. For this purpose, we use Bayesian optimization to systematically design experiments that help efficiently collect data to learn a cost function leading to robust performance. Our simulation results with different realistic disturbances (i.e., external pushes, unmodeled actuator dynamics, and computational delay) show the effectiveness of our approach to create robust controllers for humanoid robots.

This paper presents a computational framework for the design of high-performance legged robotic systems. The framework relies on the concurrent optimization of hardware parameters and control trajectories to find the best robot design for a given task. In particular, we focus on energy efficiency, presenting novel electro-mechanical models to account for the losses of the actuators due to friction and Joule effects. Thanks to a bi-level optimization scheme, featuring a genetic algorithm in the outer loop, our framework can also optimize for the duration of the motion, the actuators, and the size of the robot. We present a novel approach to scale both the actuators and the robot structure in a way that ensures structural integrity by maintaining constant the normalized deflection of the links. We validated our approach by designing a two-joint monoped robot to execute a jumping task. Our simulation results show that our framework can lead to remarkable energy savings (up to 60%) thanks to the concurrent optimization of robot size, motion duration, and actuators.

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.

Recently, the centroidal momentum dynamics has received substantial attention to plan dynamically consistent motions for robots with arms and legs in multi-contact scenarios. However, it is also non convex which renders any optimization approach difficult and timing is usually kept fixed in most trajectory optimization techniques to not introduce additional non convexities to the problem. But this can limit the versatility of the algorithms. In our previous work, we proposed a convex relaxation of the problem that allowed to efficiently compute momentum trajectories and contact forces. However, our approach could not minimize a desired angular momentum objective which seriously limited its applicability. Noticing that the non-convexity introduced by the time variables is of similar nature as the centroidal dynamics one, we propose two convex relaxations to the problem based on trust regions and soft constraints. The resulting approaches can compute time-optimized dynamically consistent trajectories sufficiently fast to make the approach realtime capable. The performance of the algorithm is demonstrated in several multi-contact scenarios for a humanoid robot. In particular, we show that the proposed convex relaxation of the original problem finds solutions that are consistent with the original non-convex problem and illustrate how timing optimization allows to find motion plans that would be difficult to plan with fixed timing††Implementation details and demos can be found in the source code available at https://git-amd.tuebingen.mpg.de/bponton/timeoptimization.

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.

International audience

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.

The identification of inertial parameters is crucial to achieve high-performance model-based control of legged robots. The inertial parameters of the legs are typically not altered during expeditions and therefore are best identified offline. On the other hand, the trunk parameters depend on the modules mounted on the robot, like a motor to provide the hydraulic power, or different sets of cameras for perception. This motivates the use of recursive approaches to identify online mass and the position of the Center of Mass (CoM) of the robot trunk, when a payload change occurs. We propose two such approaches and analyze their robustness in simulation. Furthermore, experimental trials on our 80-kg quadruped robot HyQ show the applicability of our strategies during locomotion to cope with large payload changes that would otherwise severely compromise the balance of the robot.

This paper presents a new algorithm for optimal control (OC) of nonlinear dynamical systems. The main feature of this algorithm is that it allows the specification of the control objectives as a hierarchy of tasks, each task representing an action that the robot should perform. Each task is described by a cost function that the algorithm tries to minimize, while not affecting the tasks of higher priority. The concept of strict priority allows for an easier and more robust specification of the control objectives, without hand tuning of task weights. The hierarchy also makes it possible to properly regularize the behavior of each task independently. For the first time, we properly define the problem of regularizing the task cost functions in the presence of a hierarchy and propose an algorithm to compute an approximate solution. Several simulated scenarios with different robots compare our solution with other state-of-the-art methods, validating the interest of the hierarchy in OC and empirically demonstrating the importance of regularization to generate feasible behaviors.

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.

Task-space inverse dynamics (TSID) is a well-known optimization-based technique for the control of highly redundant mechanical systems, such as humanoid robots. One of its main flaws is that it does not take into account any of the uncertainties affecting these systems: poor torque tracking, sensor noises, delays, and model uncertainties. As a consequence, the resulting control-state trajectories may be feasible for the ideal system, but not for the real one. We propose to improve the robustness of TSID by modeling uncertainties in the joint torques, either as Gaussian random variables or as bounded deterministic variables. Then we try to immunize the constraints of the system to any—or at least most—of the realizations of these uncertainties. When the resulting optimization problem is computationally too expensive for online control, we propose ways to approximate it that lead to computation times below 1 ms. Extensive simulations in a realistic environment show that the proposed robust controllers greatly outperform the classic one, even when other unmodeled uncertainties affect the system (e.g., errors in the inertial parameters, delays in the velocity estimates).

This paper deals with the generation of motion for complex dynamical systems (such as humanoid robots) to achieve several concurrent objectives. Hierarchy of tasks and optimal control are two frameworks commonly used to this aim. The first one specifies control objectives as a number of quadratic functions to be minimized under strict priorities. The second one minimizes an arbitrary user-defined function of the future state of the system, thus considering its evolution in time. Our recent work on prioritized optimal control merges the advantages of both these methods. This paper reformulates the original prioritized optimal control algorithm with the precise goal of improving its computational speed. We extend the dynamic programming method to work with a hierarchy of tasks. We compared our approach in simulation with both our previous algorithm and classical optimal control. The measured computational improvement represents another step towards the application of prioritized optimal control for online model predictive control of humanoid robots. We believe that this could be the key to unlock the (so far unexploited) dynamic capabilities of these mechanical systems.

Classically robot force control passes through joint torques measurement or estimation. Within this context, classical torque sensing technologies rely on current sensing on motor windings and on torsion sensing on motor shaft. An alternative approach was recently proposed in [1] and combines whole-body distributed 6-axis force/torque (F/T) sensors, gyroscopes, accelerometers and tactile sensors (i.e. artificial skin). A further advantage of this method is that it simultaneously estimates (internal) joint torques and (external) contact forces with no need of joint redesign. As a drawback, the method relies on a model of the robot dynamics, as it consists on reordering the classical recursive Newton-Euler algorithm (RNEA). In this paper we consider the problem of the parametric identification of the robot dynamic model from embedded F/T sensors. We extend recent results on parametric identification [2] by considering an arbitrary reordering of the classical RNEA. The theoretical framework is validated on the iCub humanoid, which is equipped with both 6-axis F/T sensors and joint torque sensors. We estimated the system inertial parameters using only one F/T sensor. We used the obtained parameters to estimate the joint torques (as proposed in [1]) and compared the results with direct joint torque measurements, used in this context only as a ground truth.

Controlling the robot with a permanently-updated optimal trajectory, also known as model predictive control, is the Holy Grail of whole-body motion generation. Before obtaining it, several challenges should be faced: computation cost, non-linear local minima, algorithm stability, etc. In this paper, we address the problem of applying the updated optimal control in real-time on the physical robot. In particular, we focus on the problems raised by the delays due to computation and by the differences between the real robot and the simulated model. Based on the optimal-control solver MuJoCo, we implemented a complete model-predictive controller and we applied it in real-time on the physical HRP-2 robot. It is the first time that such a whole-body model predictive controller is applied in real-time on a complex dynamic robot. Aside from the technical contributions cited above, the main contribution of this paper is to report the experimental results of this première implementation.

This paper presents a new technique to control highly redundant mechanical systems, such as humanoid robots. We take inspiration from two approaches. Prioritized control is a widespread multi-task technique in robotics and animation: tasks have strict priorities and they are satisfied only as long as they do not conflict with any higher-priority task. Optimal control instead formulates an optimization problem whose solution is either a feedback control policy or a feedforward trajectory of control inputs. We introduce strict priorities in multi-task optimal control problems, as an alternative to weighting task errors proportionally to their importance. This ensures the respect of the specified priorities, while avoiding numerical conditioning issues. We compared our approach with both prioritized control and optimal control with tests on a simulated robot with 11 degrees of freedom.

Legged robots are typically in rigid contact with the environment at multiple locations, which add a degree of complexity to their control. We present a method to control the motion and a subset of the contact forces of a floating-base robot. We derive a new formulation of the lexicographic optimization problem typically arising in multi-task motion/force control frameworks. The structure of the constraints of the problem (i.e. the dynamics of the robot) allows us to find a sparse analytical solution. This leads to an equivalent optimization with reduced computational complexity, comparable to inverse-dynamics based approaches. At the same time, our method preserves the flexibility of optimization based control frameworks. Simulations were carried out to achieve different multi-contact behaviors on a 23-degree-of-freedom humanoid robot, validating the presented approach. A comparison with another state-of-the-art control technique with similar computational complexity shows the benefits of our controller, which can eliminate force/torque discontinuities.

This paper investigates the role of precise estimation of contact points in force control. This analysis is motivated by scenarios in which robots make contacts, either voluntarily or accidentally, with different parts of their body. Control paradigms that are usually implemented in robots with no tactile system, make the hypothesis that contacts occur at the end-effectors only. In this paper we try to investigate what happens when this assumption is not verified. First we consider a simple feedforward force control law, and then we extend it by introducing a proportional feedback term. For both controllers we find the error in the resulting contact force, that is induced by a hypothetic error in the estimation of the contact point. We show that, depending on the geometry of the contact, incorrect estimation of contact points can induce undesired joint accelerations. We validate the presented analysis with tests on a simulated robot arm. Moreover we consider a complex real world scenario, where most of the assumptions that we make in our analytical derivation do not hold. Through tests on the iCub humanoid robot we see how errors in contact localization affect the performance of a parallel force/position controller. In order to estimate contact points and contact forces on the forearm of the iCub we do not use any model of the environment, but we exploit its 6-axis force/torque sensor and its sensorized skin.

This paper deals with the problem of estimating the position of tactile elements (i.e. taxels) that are mounted on a robot body part. This problem arises with the adoption of tactile systems with a large number of sensors, and it is particularly critical in those cases in which the system is made of flexible material that is deployed on a curved surface. In this scenario the location of each taxel is partially unknown and difficult to determine manually. Placing the device is in fact an inaccurate procedure that is affected by displacements in both position and orientation. Our approach is based on the idea that it is possible to automatically infer the position of the taxels by measuring the interaction forces exchanged between the sensorized part and the environment. The location of the contact is estimated through force/torque (F/T) measures gathered by a sensor mounted on the kinematic chain of the robot. Our method requires few hypotheses and can be effectively implemented on a real platform, as demonstrated by the experiments with the iCub humanoid robot.