Collocation methods for numerical optimal control commonly assume that the system dynamics is expressed as a first order ODE of the form xdot = f(x,u,t), where x is the state and u the control vector. However, in many systems in robotics, the dynamics adopts the second order form qddot = g(q,qdot,u,t), where q is the configuration. To preserve the first order form, the usual procedure is to introduce the velocity variable v = qdot and define the state as x=(q,v), where q and v are treated as independent in the collocation method. As a consequence, the resulting trajectories do not fulfill the mandatory relationships v(t) = qdot(t) for all times, and even violate qddot = g(q,qdot,u,t) at the collocation points. This prevents the possibility of reaching a correct solution for the problem, and makes the trajectories less compliant with the system dynamics. In this paper we propose a formulation for the trapezoidal and Hermite-Simpson collocation methods that is able to deal with second order dynamics and grants the mutual consistency of the trajectories for q and v while ensuring qddot = g(q,qdot,u,t) at the collocation points. As a result, we obtain trajectories with a much smaller dynamical error in similar computation times, so the robot will behave closer to what is predicted by the solution. We illustrate these points by way of examples, using well-established benchmark problems from the literature.

Trabajo presentado en el Robotics: Science and Systems, celebrado en Nueva York (Estados Unidos), del 27 de junio al 1 de julio de 2022