Work in the ECHORD GOP project continued with a close interaction of the project partners in France and Germany, based on regular (at least bi-weekly) Skype meetings of all project members and some personal meetings of the responsible persons in the project.  

A central topic of discussion were the pros and cons (in the context of the problem class addressed in GOP) of feasible optimization techniques with respect to optimization techniques that allow infeasibility in the course of the solution. The direct multiple shooting optimal control code MUSCOD of IWR, Heidelberg University, uses a specially tailored SQP (sequential quadratic programming) method for the solution of the unserlying nonlinear programming problem (NLP) which is only guaranteed to satisfy all constraints in the solution but can still violate constraints in the iterations before which is considered to be advantageous in the case of complex constraint sets. However, this requires the constraints to be formulated in such a way that they can also give back a meaningful value in the case where they are violated. But this is not the case in the efficient constraint evaluation routines such as KCD (Kineo Collision detector) used at LAAS which give only a qualitative but no quantitative answer in the case of constraint violation. So the coupling of optimal control and motion planning techniques is not possible in a straightforward way. Two approaches are discussed:

1.     adding a computation (or at least an estimation) of the constraint violation in the motions planning functions evaluating distance to obstacles

2.     using feasible optimization solvers such as interior point techniques for optimization instead. 

Work at LAAS during this period has concentrated on installing and evaluating an own optimization library called Roboptim based on feasible optimization techniques. Trajectories are discretized using splines. For the resulting NLP, the optimization solvers IPOPT and CFSQP are implemented.  The setup of the optimal control problem of the humanoid robot is in progress, as well as the direct coupling with KCD which is easily feasible iin this case.

Work at IWR, Heidelberg in the ECHORD project has focused on the practical side, i.e. the set up of the robotic demonstrator and some modeling aspects since the position responsible for the optimal control aspects has been vacant for this two-month period.  Optimal control work will be taken up again on October 1^st.

The KUKA robot arm has arrived at the beginning of August and has been set up and has been thoroughly tested.  We have also mounted the pneumatic gripper along wit its pressure control system.  The robot has been set up on a specially manufactured solid table in order to be able to run the robot at full speed. We have also started to familiarize with the theory of realtime robot control via XML strings on ethernet. Example trajectories have been executed on the robot. Different options for the dynamic test environment for the KUKA arm have been evaluated. In addition we have worked on expanding the VRML model of the KUKA robot arm to include the gripper and integrated the VRML model and the kinematik chain into the Kineo software. The integration of the Kineo library into own C++ code has been started.

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.

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.