Teguh Santoso Lembono

Francisco Suárez-Ruiz, Teguh Santoso Lembono, Quang-Cuong Pham

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.

Teguh Santoso Lembono, Sylvain Calinon

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.

Teguh Santoso Lembono, Emmanuel Pignat, Julius Jankowski et al.

In high dimensional robotic system, the manifold of the valid configuration space often has a complex shape, especially under constraints such as end-effector orientation or static stability. We propose a generative adversarial network approach to learn the distribution of valid robot configurations under such constraints. 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, we use it to generate samples in sampling-based constrained motion planning algorithms to reduce the necessary projection steps, speeding up the computation. We validate the approach in simulation using the 7-DoF Panda manipulator and the 28-DoF humanoid robot Talos.

Antonio Paolillo, Teguh Santoso Lembono, Sylvain Calinon

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.

Teguh Santoso Lembono, Carlos Mastalli, Pierre Fernbach et al.

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.

Teguh Santoso Lembono, Antonio Paolillo, Emmanuel Pignat et al.

Trajectory optimization for motion planning requires good initial guesses to obtain good performance. In our proposed approach, we build a memory of motion based on a database of robot paths to provide good initial guesses. The memory of motion relies on function approximators and dimensionality reduction techniques to learn the mapping between the tasks 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 memory can be used as a metric to choose between several possible goals, and using an ensemble method to combine different function approximators results in a significantly improved warm-starting performance. We demonstrate the proposed approach with motion planning examples on the dual-arm robot PR2 and the humanoid robot Atlas.

Teguh Santoso Lembono, Francisco Suárez-Ruiz, Quang-Cuong Pham

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.

Teguh Santoso Lembono, Fransisco Suarez-Ruiz, Quang-Cuong Pham

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.