The main purpose of our experiment “Generating optimal paths for industrial and humanoid robots in complex environments (GOP)” is the determination of the best path in a cluttered, complex and potentially changing environment. In order to achieve this task, we plan to combine approaches of the two fields of research optimal control and kinematic motion planning represented by the two research groups in this project. Within this project, the techniques will be validated using two demonstrator platforms, the KUKA manipulator arm KR5 sixx 850 at IWR, Heidelberg University and the humanoid robot HRP-2 at LAAS-CNRS, Toulouse.

The discussions in the early phase of the project focused on the correct formulation of the optimal control problem under complex constraints and the different ways to efficiently combine the approaches of the two fields. We have decided to start with a two-level approach:

1. A collision-free path through the complex environment is computed using RRT-like methods and pre-computed roadmaps (see below)

2. The path obtained in the first stage is fed as initial trajectory into the optimal control code MUSCOD (Heidelberg University) which generates an optimal trajectory according to the selected objective function (see below).

This already is expected to improve initialization and convergence of the optimal control problem which might be hard to achieve otherwise, in particular in the case of many local minima.  

Later in this project, we additionally aim to include constraint handling techniques of motion planning in the optimal control code (i.e. in level 2 above).

The main purpose of motion planning techniques is to find a collision-free path taking into account only the geometry of the model, and no time or dynamics. In this first phase of the project, we explored several motion planning algorithms, which are mainly:
-  Rapidly-Exploring Random Trees (RRT) which relies on a randomized sampling of the configuration space (CS) and is very efficient for systems with a large number of degrees of freedom (DoF) such as HRP-2 which has 30 actuated DoF and 6 unactuated DoF

- RRT on sub-manifolds of CS Space which can take into account constraints to be satisfied along the path such as stability constraints for HRP-2

- pre-computed roadmaps which rely on a pre-specification of  the tree (or roadmap) and the determination of colliding edges beforehand to speed up computations.

Optimal control problems take into account the full dynamic model of the system to be optimized as well as various equality and inequality constraints and generate an optimal trajectory in time. Efficient direct multiple shooting techniques for the solution of optimal control problems are available at Heidelberg.  

We have addressed the optimal control problem formulation for the humanoid robot HRP-2 and for the arm robot KUKA KR5sixx 850.  We build the dynamic model of both robots using the kinematic and dynamic parameters, and developed a software package for that model. The dynamic model is taking as input the applied torques on the joints and giving acceleration, velocity and position of the generalized coordinates as output. Moreover, the dynamic model can incorporate the inertia of the motors in the joints to obtain an accurate estimation of the exerted torques by the motors. Later-on, also the multiple constraints of the environment will be included in the optimal control problem formulation. The problem of considering the minimum distance (with respect to a multitude of obstacles) as a constraint is a hard problem in numerical optimization. This is because of the discontinuity nature of this distance which could be resolved by a reformulation. In addition, our optimization solver requires the evaluation of the amount of violation of constraints in the infeasible areas which is not usually not performed in motion planning. This problem will be addressed in the course of this project.

For the humanoid robot HRP-2 there already exists a simulation kit that has been developed using KineoWorks, which is a motion planning software. We developed a new simulation kit for our arm robot at Heidelberg using KineoWorks as well. This kit allows to visualize the optimal path, and it will be useful to test the validity of the path before applying it on the real robots. Furthermore, it will serve to check and prevent the collision with the environment.

A KUKA arm robot KR5sixx 850 and a gripper for the demonstrator setup in Heidelberg have been ordered after the confirmation of the project start to arrive beginning of August. More precise planning fort this demonstrator will be done after the robot will have been tested.