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.