Call2 Monitoring GOP

Public Summary Month 10/2012

Tue, 09 Oct 2012 14:26:47 GMT

Work done at LAAS-CNRS

The last two months were dedicated to generate feasible optimized motions for the ECHORD final experimental setup with MUSCOD. This demonstrator shows the effectiveness of our two-stage optimal motion planning framework.

Work done at Uni Heidelberg

The last two months were mostly spent to prepare the Demonstrator. Improvements were made for the software that directly controls the robot. Furthermore the Framework developed at LAAS-CNRS that computes distances using capsules was integrated in our optimization code base.

Public Summary Month 5/2012

Fri, 22 Jun 2012 11:22:43 GMT

During April and May 2012 our current results were presented at the workshop "Industry-Academia collaboration in the ECHORD project: a bridge for European robotics innovation" which was held at the ICRA 2012 conference in St. Paul, Minnesota. A draft for the multimedia report was created and work on both the simulated and real demonstrator was done. Furthermore the framework for the generation of optimal paths was created together with LAAS-CNRS.

For our demonstrator we designed a test-problem in which a glass that contains water is moved by the robot. The motion is computed using trajectory optimization that minimize acceleration of the glass in certain directions to avoid spilling of water. The acquired results were presented at the workshop.

In our demonstrator the robot has to move a glass with liquid in a complex environment without spilling or collision with the environment or with the robot itself. A sample scenario of the demonstrator was designed in the path planning tool KINEO and various real world counterparts created that allow us to recreate the virtual scenarios in reality.

Together with LAAS-CNRS we created and formulated a framework for the generation of optimal paths. The path planner is used to compute an initial path whereas the trajectory optimizer uses the collision detection facilities of the path planner to ensure collision free paths for the optimal paths.

At LAAS-CNRS the last two months were dedicated to generate feasible optimized motions with MUSCOD. We finalized the work on the path-planning framework: once the humanoid robot model is loaded, the user can choose the support joints (right foot, left foot, or both). The constrained manifold is then built from these constraints and collision-free quasi-static paths can be planned in any kind of environment.

Once the solution path is ready, it is fed to MUSCOD. So far we have been able to sucessfully generate minimum-jerk trajectories while keeping both feet on the ground and guaranteeing the robot balance. To do so we use the jerk and external forces as control inputs, and the state variables are the concatenation of the robot configuration, velocity and acceleration. We verified that the final motion was feasible on the OpenHRP dynamic simulator.

To guarantee robot balance and actuator torques validity, we need to compute the torques from the robot configuration, velocity, acceleration and external forces using an inverse dynamics algorithm. To do so we use a fast implementation of the recursive Newton-Euler algorithm that allows fast torques and gradients computation.

We have finalized the scenario of the demonstrator. The robot will start in the resting position facing a big shelf and oter obstacles , and will be asked to retrieve an object from the lower shelf and place it on the upper shelf. This will require the robot to avoid collisions with all obstacles and between its own bodies.

We have implemented the collision avoidance constraints in MUSCOD: self-collision pairs are added using the robot model, and collision pairs are added after loading the given environment. We are currently testing those constraints.

Public Summary Month 3/2012

Fri, 22 Jun 2012 11:21:55 GMT

During February 2012 and March 2012 we created a new formulation for optimal motions that are subject to acceleration constraints. Using this formulation, multiple motions were generated and successfully transferred onto the KUKA robot arm. For this a custom glass holder for the robot was designed and then printed on a RepRap 3D printer. Also we successfully submitted a paper that will present these results at the ICRA 2012 workshop “Industry-Academia collaboration in the ECHORD project: a bridge for European robotic innovation”. We further intend to extend this new formulation so that it will be used in our demonstrator.

During the visit of Antonio El Khoury to Heidelberg University in January / February 2012, we came up with a new idea for a motion optimization for the KUKA arm. Instead of generating motions that are e.g. minimized by torques, we could also minimize the linear acceleration of the gripper of the robot. The application of this would be highly dynamical motions, that allow the robot to move a glass of water very quickly without spilling any water.

We formulated an optimal-control problem that minimizes the linear accelerations towards the sides of the glass, but still allows for larger accelerations along the longitudinal axis of a glass. This ensures, that the water stays inside the glass. The formulation allows to specify start- and end-postures for which the connecting trajectories are then optimized. To verify the motions on the KUKA arm, we designed a glass holder for the robot. The glass holder is designed such that the glass is not fixed to the robot, instead the glass can still slip out of the holder, if the end-effector accelerates down the vertical axis too fast. This enforces that the motions fulfil the acceleration constraints also for an empty glass of water.

As the results of the acceleration minimization formulation were very promising, we decided to further explore in this direction. So far we can generate optimal trajectories, however there is no collision checking or other path planning. Therefore we want to add obstacles that allow us to use path planning methods to compute initial guesses that are then refined by trajectory optimizations subject to acceleration constraints. Using this formulation in the demonstrator would also be a better example for a combination of the two approaches (trajectory optimization and path planning), as neither one of the two would be suficient to generate such motions.

At LAAS-CNRS the last two months were dedicated to using the MUSCOD-II optimization solver to generate minimum-jerk constrained trajectories. We settled for a minimum-jerk optimization problem where the jerk is the control input of the problem. This allows us to generate smooth motion on the HRP-2 robot, and to compute the objective function without requiring any heavy computation since it directly relates to the controls. The trajectory time is assumed to be fixed for the moment but we plan to minimize as a next step. We set the controls to be piecewise linear functions of time, and we integrate them 3 times to obtain the robot state, namely the configuration, speed and acceleration.

In order to generate a feasible motion on the HRP-2 robot, a certain number of constraints needs to be verified. In our case, we plan to optimize a motion where the robot does not step, and support foot or feet need to be at a fixed position during the whole motion. Apart from the robot dynamics, we thus add a 6D position constraint for one foot (in single support).

Furthermore, we need to make sure we are simulating correctly the robot motion. From the robot state, we use inverse dynamics to compute the actuator torques on all joints, including on the free-floating base. But by definition this base is not actuated, so we add an additional 6-dimensional constraint to make sure the 6 free-floating joint torques are equal to zero. This can be only achieved by adding 6 controls that represent the spatial external force (3 linear forces and 3 torques) on the support foot. In the particular case of a quasistatic motion, the spatial force norm is equal to the robot weight and oriented vertically upwards. Finally joint angular and speed limit constraints are added to the problem, and we plan to add actuator torque limits.

If we want to find a trajectory connecting a start and a goal configurations in a reasonable time, it is recommended giving the optimization solver an initial guess that is not too far away from the solution. This can be achieved by using constrained motion planners. They allow to find a geometrical path connecting the two configurations while staying on a submanifold of the configuration space. In our case the submanifold is defined by the fixed support foot position and the quasi-static balance of the robot.

Furthermore, in the case of an unconstrained problem, the minimum-jerk trajectory between two configurations can be analytically derived for a fixed time without any computational overhead. We feed this very good initial guess to the solver that converges much faster than in the case of other trajectories.

Public Summary Month 1/2012

Fri, 22 Jun 2012 11:20:15 GMT

During December 2011 and January 2012 the optimal control problem formulation was extended and an optimized motion was successfully transferred onto the KUKA robot arm. The demonstrator was changed to better match the workspace of the robot. Furthermore, there was an exchange of PhD students between Heidelberg University and LAAS-CNRS.

Previous problems with additional constraints such as ground collisions could be resolved which was due to an error in the formulation and the optimizations work now as expected. The code for the robot model was robustified by adding additional tests which will guarantee correctness also in the future.

Computed motions can now be exported and executed on the robot. So far the motions have to be checked manually to ensure that the robot does not collide with itself as self-collision is not yet implemented. Furthermore the motion has to be performed at a slower speed as the constraints posed in the optimal control formulation do not fully represent the safety parameters in the robot.

Previously we planned the demonstrator to be a track which moves around the robot in a non-linear path in the horizontal plane. However in this plane, the end-effector can only reach a limited dextrous workspace especially when obstacles are involved. We therefore changed it so that the path will be on a path that is aligned vertically. Furthermore, a smaller test-case scenario for the demonstrator was designed to test the motion generation pipeline. The work at LAAS-CNRS was dedicated to writing effecicient collision avoidance constraints, and starting to use the MUSCOD solver.

In order to implement fast distance constraints, we implement, using the Kineo Collision Detection (KCD) library, two new geometry types: segments and capsules. A segment is defined by its two end points, and a capsule is defined by its two axis end points and a radius. Collision detection and proximity query are also implemented for capsule-capsule, segment-segment, capsule-polyhedron, and segment-polyhedron tests.

The reason for which we use capsules and segments is the following: while the capsule type is very useful for fast collision detection, KCD does not allow the user to return a negative distance when two capsules are colliding. We hence define the segment geometry, retrieve the distance separating two segments (or a segment and a polyhedron). The user can then subtract the sum of capsule radii to obtain the real distance separating the equivalent capsules; if the distance is positive, then the capsules do not collide, otherwise they collide and the distance is equal to the penetration distance. This is an important feature for numerical optimization, as we can get a non-zero gradient even if the objects collide. We packaged all these classes in one library that can be used easily both by LAAS and IWR to define distance constraints.

Now that a capsule can be defined, a simplified model of the robot can be built by replacing all polyhedrons representing the bodies by their bounding capsules. The bounding capsule being a conservative approximation of a the real body geometry, this requires finding the minimum volume capsule that contains the set of all points of the polyhedron.

We proceed in two steps: first, the least-squares method is applied on the set of points to find the best line approximation. To make sure all points lie inside the capsule, the radius is set to the maximum distance between the line and the points, and the segment is defined as the minimum-length portion of line such that all points are insisde. The obtained capsule volume is not minimal, but it is a good approximation of the optimal solution.

Second, we take the computed capsule as initial guess and we solve the following numerical optimization problem: find the capsule paramters (end points and radius) that minimize the volume, such that the distance between the capsule and all points is negative, i.e. all points lie inside the capsule. We use the Ipopt solver that is available in RobOptim. We apply this two-stage optimization on all bodies of HRP-2. The obtained bounding capsules represent a fairly tight fit of the underlying geometries, especially convex volumes such as the arms and the legs.

Apart from that Antonio El Khoury from LAAS stayed at the IWR for two weeks. During the first week, he gave presentations about latest results in motion planning for anthropomorphic systems, and about the latest software implementations for the ECHORD project. The rest of the week was spent on following an introductory course to the MUSCOD solver, installing it and starting to use it on simple examples. During his stay, Antonio was able to successfully solve a simple optimization problem for HRP-2, using the dynamics model without any constraints.

Public Summary Month 11/2011

Sun, 18 Dec 2011 08:16:36 GMT

Work at Heidelberg University in the last two months focused on the control of the KUKA arm and the simulation and optimization of the model. In addition progress on the optimal control of humanoid walking motions for HRP-2 has been made.

Originally the KUKA arm is controlled by specification of joint configurations or end effector positions at certain points in time, and the motion in between is automatically generated by KUKA modules. In order to be able to reproduce motions determined by optimal control an alternative way to specify the motion for the robot was required. Therefore a bi-directional connection between the KUKA robot and the PC has been established. For that purpose, a C++ interface was written that now allows us to send trajectories to the robot. First tests using simpler analytic functions show that the robot follows the given trajectory quite accurately.
As a first test scenario, the previously established optimal control formulation was extended to allow a simplified specification of a path that has to be executed by the end-effector of the robot: the path is specified by 3 points in Cartesian coordinates and constraints in the optimization make sure that the robot is at rest at the beginning and the end of the motion. So far, trajectories for various points in the robot’s work space could be generated which fulfill the constraints. The results will be transferred to the robot. The success of the optimization highly depends on the initial values for both joint positions and velocities and the control trajectories. So far, calculation of initial joint states using inverse kinematics and linear interpolation works for the 3-point problem. However when adding more complex constraints such as prevention of ground or obstacle penetration, other approaches are required: later the initial trajectory will be delivered by the path planner in order to avoid being trapped by constraints, select between different local minima, improve convergence and speed up the optimization by saving iterations.

For the demonstrator we are planning to use a train-like vehicle that transports objects in a non-linear path around the base of the KUKA arm. The arm then has to remove objects from that train and put them into a container while having to navigate around different obatscles in between. The task is to compute at any instant the best possible trajectory for the robot arm to reach the objects on the train using a combination of path planning and optimal control and then execute it on he robot.

An optimal control problem for the generation of optimal walking motions for HRP-2 has been established using MUSCOD. Optimization of the swing phase according to different criteria is possible. The multi-phase problem combining swing phase and double support phase is currently established. These motions will be compared with the results generated by different approaches at LAAS and will be tested on the robot during a stay of a Heidelberg ECHORD team member in Toulouse.

At LAAS-CNRS, the last two months were dedicated to implementing collision avoidance constraints into the optimal control problem. Besides joint angle and angular rate limits, other collision avoidance constraints were added to the optimization problem by implementing a function that computes the distance between any two objects, namely between two bodies of the robot or between a body of the robot and the environment. This function computes as well the gradient that may be needed by the optimization solver Roboptim. In order to ensure that no auto-collision occurs during motion, a distance constraint must be added to each pair of bodies that may enter in collision. For a complex robot such as HRP-2, this results in adding about 100 constraints for auto-collision avoidance, and an additional 100 constraints for collision avoidance with the environment which strictly speaking would have to be checked along the trajectory. As test have shown a collision avoidance test of the whole robot based on a 3D mesh of the bodies is far too complex to be performed online. Instead, we evaluated the use of simple representations of the robot segments as capsules, i.e. cylinders topped with half-spheres on each side. Such a representation allows fast distance computation between two bodies while being a good fit of the original 3D mesh. Indeed we can simply compute the distance between the two capsule axes then subtract their radius to get the real distance. In case the two capsules are in collision, the distance will be negative, thus giving a penetration distance and a non-zero gradient. The previously used version of Kineo does not support capsule geometries, so we had to upgrade to a newer version and update packages that have a dependency to Kineo. This has just been finished.
The optimization approaches used at LAAS (based on Roboptim described previously) will be compared to MUSCOD of the University of Heidelberg. For this purpose, a member of the ECHORD team at LAAS will spend time in Heidelberg to learn to use MUSCOD.
For the HRP-2 ECHORD demonstrator, the idea is to manipulate an object such as a ball in a constrained environment (a table and a shelf for example). The ball needs to be put in a box which can be placed anywhere on the table and is detected using the cameras on HRP-2. This involves a dynamic whole-body motion of the robot.

Public Summary Month 09/2011

Sun, 18 Dec 2011 06:12:42 GMT

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.

Summary Month 07/2011

Sun, 18 Dec 2011 05:29:24 GMT

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.

Public Summary Month 07/2011

Sun, 18 Dec 2011 04:44:04 GMT

1. A collision-free path through the complex environment is computed using RRT-like methods and pre-computed roadmaps (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.

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