Collocation methods for second order systems

Conference Article


Robotics: Science and Systems Conference (RSS)





Doc link


Download the digital copy of the doc pdf document


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.


nonlinear programming, optimal control, robot dynamics.

Author keywords

direct collocation method, trapezoidal method, Hermite-Simpson method, second order systems

Scientific reference

S. Moreno, L. Ros and E. Celaya. Collocation methods for second order systems, XVIII Robotics: Science and Systems Conference, 2022, New York, pp. 1-11.