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.