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.

The contribution of this article is to propose an approach that solves the motion planning problem for an extensible elastic rod using contacts with the environment. We first show how motion planning for a deformable rod can be done by coupling both quasi-static and dynamic rod models with sampling-based methods. Sampling directly in the submanifold of static equilibrium and contact-free configurations allows to take advantage of the dynamic model to improve the exploration of the state space. Then, thanks to the contact information (point, forces, direction, and the number of contacts), the exploration of the rapidly exploring random tree (RRT) approach can be improved. We present a new RRT-SLIDE algorithm, which guides the roadmap extension with a sliding contact mode based on some principles of human reasoning. We show that our approach is probabilistically complete. We also demonstrate the necessity of considering contacts on complex scenarios with several simulation experiments. Besides its performances, our algorithm does not require further tuning phase for a new scenario. Note to Practitioners-This article was done under the industrial project Flecto. It aims at solving the assembly/disassembly task for a rod while satisfying the elasticity parameters of its material in a digital mockup. For industrial applications, the resolution time is a critical point. On the one hand, probabilistic motion planning methods require to efficiently build a roadmap of valid rod configurations. On the other hand, accurate rod modeling implies the use of a simulator based on the finite-element method (FEM). Nevertheless, the very large size of the roadmap that leads to a high number of calls to the simulator is conflicting with the high computational cost of FEM simulation. To overcome this problem, one solution is to reduce the number of simulator calls. This can be achieved by sampling the free space with an efficient parameterization and by limiting the use of the simulator to roadmap extension in the free space or in the contact space. We introduce heuristics based on contact information returned by the simulator to significantly reduce the computational time. One of the main advantages of our algorithm is that it does not require any tuning phase for each scenario. Although we do not solve the more general gripper manipulation planning problem, this approach could be used as a first step before computing the grippers' motion. In the framework of our project, we did not consider disassembling operations implying undoing rod knots. Consequently, we do not consider friction in our approach (friction simulation is necessary to handle knots).

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.

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.

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 focuses on interactive motion planning processes intended to assist a human operator when simulating industrial tasks in Virtual Reality. Such applications need motion planning on surfaces. We propose an original haptic path planning algorithm with contact, H-RRT-C, based on a RRT planner and a real-time interactive approach involving a haptic device for computer-operator authority sharing. Force feedback allows the human operator to keep contact consistently and provides the user with the feel of the contact, and the force applied by the operator on the haptic device is used to control the roadmap extension. Our approach has been validated through two experimental examples, and brings significant improvement over state of the art methods in both free and contact space to solve path-planning queries and contact operations such as insertion or sliding in highly constrained environments.

transHumUs is an artistic work recently exhibited at the 56th Venice Biennale. The work aims at freeing trees from their roots. How to translate this poetic ambition into technological terms? This paper reports on the setup and the implementation of the project. It shows how state of the art mobile robotics technology can contribute to contemporary art development. The challenge has been to design original mobile platforms carrying charges of three tones, while moving noiseless according to tree metabolism, in operational spaces populated by visitors.

This work deals with interactive motion planning processes intended to assist a human operator when simulating industrial tasks such as assembly, maintenance or disassembly in Virtual Reality. Such applications need motion planning on surfaces. We propose an original interactive path planning algorithm with contact, I-RRT-C, based on a RRT-Connect approach. This algorithm is based on a real-time interactive approach allowing both an automatic motion planner and a human operator to jointly explore the workspace. A parameter balances the authority between the computer and the operator to reduce processing times. We improve the guidance by allowing to sample on the surfaces of obstacles. Our method allows to find a path in cluttered environments or to solve contact operations such as insertion or sliding tasks. Last, we present experimental results showing that our interactive path planner with contact brings a significant improvement over state of the art methods in both free and contact space.

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.

This work deals with planning processes for the assistance to manipulation in order to simulate industrial tasks such as assembly, maintenance or disassembly in Virtual Reality. This paper presents a novel interactive path planning algorithm with contact based on an BiRRT approach. First, we propose a real-time interactive planner where both a computer and a human operator simultaneously search the workspace therefore largely speeding up the process. An authority sharing parameter can control the autonomy of the computer. Then we present a novel contact-space algorithm able to sample on the surface of obstacles. This method helps finding paths in cluttered environments or solving specific contact tasks such as insertion or sliding operations. We finish by presenting the results of our interactive path planner with contact through three examples showing significant improvement over usual methods in both free and contact space.

We consider the manipulation planning problem of an extensible elastic rod in collision-free or contact space. We assume the rod can be handled by grippers either at both or at only one of its extremities and during the manipulation, the grasped end may change. We show that the use of both quasi-static and dynamic models can be coupled efficiently with sampling-based methods. By sampling directly on the submanifold of static equilibrium and contact-free configurations, we can take advantage of the dynamic model to improve the exploration in the state space. We show the necessity of considering contacts for this type of problems with several simulation experiments on various scenarios.

This paper aims at combining state of the art developments of path planning and optimal control and to create the algorithmic foundations to tackle optimal control problems in cluttered environments. Our contribution is three-fold: first, we describe a simple method to automatically generate minimum bounding capsules around exact robot body geometries represented by meshes. Second, we use the bounding capsules to implement distance constraints for an optimal control problem solver and achieve (self-)collision avoidance. Finally, we propose a complete two-stage framework for optimal motion planning on complex robots. This framework is successfully applied to generate optimal collision-free trajectories both in simulation and on the humanoid robot HRP-2.

This paper presents a method aimed at enhancing interaction between a path planning algorithm and a user to guide a robot motion task in a virtual environment. Existing works use a two-step decomposition which limits the interaction between the user and the ongoing process. We propose a modification of a classic motion planning method, the Rapidly-exploring Random Tree to build a Interactive-RRT. This method is based on exchanging forces between the algorithm and the user, and on data gathering (labels) from the virtual scene. The Interactive-RRT is combined with a locomotion controller and then applied to a virtual character for generating a locomotion movement along a path suggested by the user.

Even if the probabilistic motion planner methods (PRM or RRT) have been successful for robot path planning, it remains a challenge in a constrained cases with narrow passages. The RRT is a powerful tool for a simple request, but the performances of approaches falls sharply when the search has several narrow passages. The introduction of several trees can reduce this problem, but has the disadvantage of requiring the control of the number and the growth of these trees. However, this can be done using the properties of the Visibility-PRM. Combining the ideas of the Visibility-PRM with a multi-RRT (local trees), a new algorithm is presented and experimental results show the importance and effectiveness of the method.

This paper deals with the problem of generating realistic human-like reaching movements from a small set of movement primitives. Two kinds of movement databases are used as reference. The first one is obtained numerically, by applying biological principles of motor control on the dynamic model of the robot arm. The second one is obtained by recording reaching movements of human subjects. From these databases, primitives are extracted and analyzed by using Principal Component Analysis. An original generalization method is then proposed for generating movements that did not belong to the initial database. We show that twenty primitives allow to produce new movements, having characteristics similar to that of humans. Experiments on the humanoid robot HRP-2 are presented to illustrate the result.

This paper presents an application of a motion planning algorithm to interactive systems. The proposed approach aims at improving the guidance of an operator during an assembling/dis-assembling task in a virtual environment with the help of an automatic path planning algorithm in PLM operations (PLM stands for product lifecycle management, which is the process of managing the lifecycle of a product, from conception to service). Existing works use a two-step decomposition which limits the interaction between the user and the ongoing process. We propose a modification of a classic motion planning method, the rapidly-exploring random tree to build an Interactive-RRT. This method is based on exchanging forces between the algorithm and the user, and on data gathering (labels) from the virtual scene. Examples are shown to illustrate the interactive motion planning system.

This paper describes a global interactive scheme including fast motion planning and real time guiding force for 3D CAD part assembly or disassembly tasks. For real time purpose, the motion planner is divided into different steps. First, a preliminary workspace discretization is done without time limitations at the beginning of the simulation. Then, using those computed data, a second part tries to find a collision free path in real time. Once the path is found, an haptic artificial force is applied constraining the user on the path. The user can then influence the planner by not following the path and automatically order a new path research. The performance of this haptic assistance is measured on a test simulation based on an ALSTOM power components assembly simulation.

The robust execution of a planned trajectory is a critical issue in robotics. In this paper, we set the formal basis for a sensor-landmark-based motion planning approach. Our approach deals with inaccuracy of the map of the environment by producing motion features composed of a reference trajectory and of a set of pair sensor landmark over which the robot would control its motion. These motion features define along the trajectory closed-loop motion strategies for the robot. This approach enables us to produce secure motions by putting emphasis on landmarks that can represent a danger of collision. Our contribution is to state the problem in a generic formalism and to propose some simulations that show the relevance of our resolution approach.

This paper presents a path planning algorithm for a tractor to execute specific farmwork tasks requiring the complete coverage of a field while minimizing the overlapping between successive passages. The proposed method is based on the determination of a set of characteristic points from which a graph is defined. The covering trajectory is determined by means of a greedy algorithm associated to a heuristic and a cost function minimization which is used to determine the best Hamiltonian solution in the graph.

Deals with motion planning for a mobile robot on rough terrain. Hait and Simeon (1996) proposed a geometrical planner for articulated robots which can take into account uncertainty of the terrain and of the position of the robot. This paper aims at improving the robustness of the trajectory using a landmark based approach. We consider regions of the terrain where natural landmarks are visible. We propose a two-step planning approach taking advantage of these regions to reduce position uncertainty. First a path is determined between the initial and goal configurations, based on simplified models of the constraints. Then a trajectory is planned along this path, verifying the validity and visibility constraints.

This paper considers the problem of motion planning for a car-like robot (i.e., a mobile robot with a nonholonomic constraint whose turning radius is lower-bounded). We present a fast and exact planner for our mobile robot model, based upon recursive subdivision of a collision-free path generated by a lower-level geometric planner that ignores the motion constraints. The resultant trajectory is optimized to give a path that is of near-minimal length in its homotopy class. Our claims of high speed are supported by experimental results for implementations that assume a robot moving amid polygonal obstacles. The completeness and the complexity of the algorithm are proven using an appropriate metric in the configuration space R/sup 2//spl times/S/sup 1/ of the robot. This metric is defined by using the length of the shortest paths in the absence of obstacles as the distance between two configurations. We prove that the new induced topology and the classical one are the same. Although we concentrate upon the car-like robot, the generalization of these techniques leads to new theoretical issues involving sub-Riemannian geometry and to practical results for nonholonomic motion planning.>

Deals with the problem of motion planning for a car-like robot (i.e. a nonholonomic mobile robot whose turning radius is lower bounded). The authors present a fast and exact planner based upon recursive subdivisions of a collision-free path generated by a lower-level geometric planner which ignores the motion constraints. The resultant trajectory is optimized to give a path which is of near-minimal length in its homotopy class. The claims of high speed are supported by experimental results for several implementations which assume different geometric models of the robot.>

A system that automatically plans grasp operations is described. This work has been undertaken in the framework of a project for automatic planning of pick-and-place tasks. The originality of the system comes from the consideration of the strong interdependencies existing between the different planning modules that are involved. It deals explicitly with the interactions between transfer motions, approach motions, the propagation of position uncertainty constraints as well as constraints induced by the local environment of the next step in a pick-and-place context. The approach motion step is based on a motion planner that builds an exact representation of a three-dimensional (x, y, theta ) free configuration space.>