Oct 9, 2012 , by

## Public Summary Month 10/2012

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

Jun 22, 2012 , by

## Public Summary Month 5/2012

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.

Jun 22, 2012 , by

## Public Summary Month 3/2012

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.

Jun 22, 2012 , by

## Public Summary Month 1/2012

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.

Dec 18, 2011 , by

## Public Summary Month 11/2011

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.