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.

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.

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.