Online Multi-Contact Receding Horizon Planning via Value Function Approximation

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.

Demonstration-guided Optimal Control for Long-term Non-prehensile Planar Manipulation

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 contact points and directions, which involves joint logic and geometrical reasoning in the modes of the dynamics model. 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 cases, 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 real Franka Emika robot in the presence of external disturbances.

Probabilistic Iterative LQR for Short Time Horizon MPC

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 a 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.

Generative Adversarial Network to Learn Valid Distributions of Robot Configurations for Inverse Kinematics and Constrained Motion Planning

In high dimensional robotic system, the manifold of the valid configuration space often has complex shape, especially under constraints such as end-effector orientation, static stability, and obstacles. We propose a generative adversarial network approach to learn the distribution of valid robot configurations. It can generate configurations that are close to the constraint manifold. We present two applications of this method. First, by learning the conditional distribution with respect to the desired end-effector position, we can do fast inverse kinematics even for very high degrees-of-freedom (DoF) systems. Then, it can be used to generate samples in sampling based constrained motion planning algorithms to reduce the necessary projection steps, speeding up computation. We validate the approach in simulation using the 7-DoF Panda manipulator and the 28-DoF humanoid robot Talos.

A memory of motion for visual predictive control tasks

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.

Learning How to Walk: Warm-starting Optimal Control Solver with Memory of Motion

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 $\sim$9.5 to only $\sim$3.0 iterations for the single-step motion and from $\sim$6.2 to $\sim$4.5 iterations for the multi-step motion, while maintaining the solution's quality.

Memory of Motion for Warm-starting Trajectory Optimization

Trajectory optimization for motion planning requires a good initial guess to obtain good performance. In our proposed approach, we build a memory of motion based on a database of robot paths to provide a good initial guess online. The memory of motion relies on function approximators and dimensionality reduction techniques to learn the mapping between the task and the robot paths. Three function approximators are compared: k-Nearest Neighbor, Gaussian Process Regression, and Bayesian Gaussian Mixture Regression. In addition, we show that the usage of the memory of motion can be improved by using an ensemble method, and that the memory can also be used as a metric to choose between several possible goals. We demonstrate the proposed approach with the motion planning on a dual-arm PR2 robot.

SCALAR: Simultaneous Calibration of 2D Laser and Robot Kinematic Parameters Using Planarity and Distance Constraints

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 2D 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 planar and distance constraints, and the optimization problem is solved using Levenberg-Marquardt algorithm. We demonstrate through experiments that SCALAR can reduce the mean and the maximum tool position error from 0.44 mm to 0.19 mm and from 1.41 mm to 0.50 mm, respectively.

SCALAR - Simultaneous Calibration of 2D Laser and Robot's Kinematic Parameters Using Three Planar Constraints

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) which 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.6mm and 4.05 degrees to 0.09mm and 0.02 degrees.

RoboTSP - A Fast Solution to the Robotic Task Sequencing Problem

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.