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.

Long-term non-prehensile planar manipulation is a challenging task for robot planning and feedback control. It is characterized by underactuation, hybrid control, and contact uncertainty. One main difficulty is to determine both the continuous and discrete contact configurations, e.g., contact points and modes, which requires joint logical and geometrical reasoning. To tackle this issue, we propose a demonstration-guided hierarchical optimization framework to achieve offline task and motion planning (TAMP). Our work extends the formulation of the dynamics model of the pusher-slider system to include separation mode with face switching mechanism, and solves a warm-started TAMP problem by exploiting human demonstrations. We show that our approach can cope well with the local minima problems currently present in the state-of-the-art solvers and determine a valid solution to the task. We validate our results in simulation and demonstrate its applicability on a pusher-slider system with a real Franka Emika robot in the presence of external disturbances. Project webpage: https://sites.google.com/view/dg-oc/.

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%).

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

Optimal control is often used in robotics for planning a trajectory to achieve some desired behavior, as expressed by the cost function. Most works in optimal control focus on finding a single optimal trajectory, which is then typically tracked by another controller. In this work, we instead consider trajectory distribution as the solution of an optimal control problem, resulting in better tracking performance and a more stable controller. A Gaussian distribution is first obtained from an iterative Linear Quadratic Regulator (iLQR) solver. A short horizon Model Predictive Control (MPC) is then used to track this distribution. We show that tracking the distribution is more cost-efficient and robust as compared to tracking the mean or using iLQR feedback control. The proposed method is validated with kinematic control of 7-DoF Panda manipulator and dynamic control of 6-DoF quadcopter in simulation.

In this paper, we propose a framework to build a memory of motion for warm-starting an optimal control solver for the locomotion task of a humanoid robot. We use HPP Loco3D, a versatile locomotion planner, to generate offline a set of dynamically consistent whole-body trajectory to be stored as the memory of motion. The learning problem is formulated as a regression problem to predict a single-step motion given the desired contact locations, which is used as a building block for producing multi-step motions. The predicted motion is then used as a warm-start for the fast optimal control solver Crocoddyl. We have shown that the approach manages to reduce the required number of iterations to reach the convergence from ~9.5 to only ~3.0 iterations for the single-step motion and from ~6.2 to ~4.5 iterations for the multi-step motion, while maintaining the solution's quality.

This paper addresses the problem of efficiently achieving visual predictive control tasks. To this end, a memory of motion, containing a set of trajectories built off-line, is used for leveraging precomputation and dealing with difficult visual tasks. Standard regression techniques, such as k-nearest neighbors and Gaussian process regression, are used to query the memory and provide on-line a warm-start and a way point to the control optimization process. The proposed technique allows the control scheme to achieve high performance and, at the same time, keep the computational time limited. Simulation and experimental results, carried out with a 7-axis manipulator, show the effectiveness of the approach.

We propose to formulate the problem of representing a distribution of robot configurations (e.g. joint angles) as that of approximating a product of experts. Our approach uses variational inference, a popular method in Bayesian computation, which has several practical advantages over sampling-based techniques. To be able to represent complex and multimodal distributions of configurations, mixture models are used as approximate distribution. We show that the problem of approximating a distribution of robot configurations while satisfying multiple objectives arises in a wide range of problems in robotics, for which the properties of the proposed approach have relevant consequences. Several applications are discussed, including learning objectives from demonstration, planning, and warm-starting inverse kinematics problems. Simulated experiments are presented with a 7-DoF Panda arm and a 28-DoF Talos humanoid.

In this paper, we propose SCALAR, a calibration method to simultaneously calibrate the kinematic parameters of a 6-DoF robot and the extrinsic parameters of a 2-D laser range finder (LRF) attached to the robot's flange. The calibration setup requires only a flat plate with two small holes carved on it at a known distance from each other and a sharp tool-tip attached to the robot's flange. The calibration is formulated as a nonlinear optimization problem where the laser and the tool-tip are used to provide the planar and distance constraints, and the optimization problem is solved using the Levenberg-Marquardt algorithm. We demonstrate through experiments that SCALAR can reduce the mean and the maximum tool position error from 0.44 to 0.19 mm and from 1.41 to 0.50 mm, respectively.

In many industrial robotics applications, such as spot-welding, spray-painting or drilling, the robot is required to visit successively multiple targets. The robot travel time among the targets is a significant component of the overall execution time. This travel time is in turn greatly affected by the order of visit of the targets, and by the robot configurations used to reach each target. Therefore, it is crucial to optimize these two elements, a problem known in the literature as the Robotic Task Sequencing Problem (RTSP). Our contribution in this paper is two-fold. First, we propose a fast, near-optimal, algorithm to solve RTSP. The key to our approach is to exploit the classical distinction between task space and configuration space, which, surprisingly, has been so far overlooked in the RTSP literature. Second, we provide an open-source implementation of the above algorithm, which has been carefully benchmarked to yield an efficient, ready-to-use, software solution. We discuss the relationship between RTSP and other Traveling Salesman Problem (TSP) variants, such as the Generalized Traveling Salesman Problem (GTSP), and show experimentally that our method finds motion sequences of the same quality but using several orders of magnitude less computation time than existing approaches.

Industrial robots are increasingly used in various applications where the robot accuracy becomes very important, hence calibrations of the robot's kinematic parameters and the measurement system's extrinsic parameters are required. However, the existing calibration approaches are either too cumbersome or require another expensive external measurement system such as laser tracker or measurement spinarm. In this paper, we propose SCALAR, a calibration method to simultaneously improve the kinematic parameters of a 6-DoF robot and the extrinsic parameters of a 2D Laser Range Finder (LRF) that is attached to the robot. Three flat planes are placed around the robot, and for each plane the robot moves to several poses such that the LRF's ray intersect the respective plane. Geometric planar constraints are then used to optimize the calibration parameters using Levenberg-Marquardt nonlinear optimization algorithm. We demonstrate through simulations that SCALAR can reduce the average position and orientation errors of the robot system from 14.6 mm and 4.05° to 0.09 mm and 0.02°.

In monocopter flight, two important parameters are required for control: angular rates and heading direction. Small monocopters fly at a very high speed (more than 600rpm), which can be out of the typical gyroscope limit. Very high speed gyroscopes do exist, but the price is high and it can only measure a single axis rotation. This paper presents an alternative approach to measure angular rates by using three accelerometers. The readings of the accelerometers are subtracted to calculate the angular rates in all three axes (x, y, and z). This paper also proposes to use the Extended Kalman Filter (EKF) to estimate the heading direction based on the magnetometer reading and the angular rates. The angular rates direction is used as the vertical direction reference. The proposed method has been applied on two setups: DC Motor setup (for quantifying the method's performance) and Monocopter setup. In the DC Motor setup, the motor encoder is used as the ground truth for the heading direction. The result is compared with the usual method of using only the magnetometer to obtain the heading direction of monocopters. The EKF result is more accurate and stable even in the presence of strong magnetic disturbances. In addition, the angle of attack and the coning angle can also be determined by the proposed method.

In surface treatment processes like plasma spraying and spray painting of workpieces, protecting the uninvolved surface by applying masking tape is a common process. Due to the operation complexity for different geometries, such taping tasks depend on a lot of manual works, which is tedious and tiring. This paper introduces an automatic agile robotic system and the corresponding algorithm to do the surface taping. The automatic taping system consists of a 3D scanner for workpiece 3D model reconstruction, a taping end-effector which is mounted on a robot manipulator to handle the taping task, and a rotating platform that is used to hold the workpiece. The surface covering method and the taping path planning algorithms using the scanned model are introduced. With the implementation of the compliance mechanism, the force feedback and the tape cutting mechanism, the system is able to tape flat, cylindrical, freeform, and grooved surfaces. Experiments conducted on taping an engine inner liner shows that the surface can be covered with uniform taping overlap and very little wrinkle. The proposed system is a useful taping package for industrial applications such as workpiece repairing and surface protection, where surface treatments are involved.