  Direct collocation methods are powerful tools to solve trajectory
optimization problems in robotics. While their resulting trajectories tend to
be dynamically accurate, they may also present large kinematic errors in the
case of constrained mechanical systems, i.e., those whose state coordinates are
subject to holonomic or nonholonomic constraints, like loop-closure or
rolling-contact constraints. These constraints confine the robot trajectories
to an implicitly-defined manifold, which complicates the computation of
accurate solutions. Discretization errors inherent to the transcription of the
problem easily make the trajectories drift away from this manifold, which
results in physically inconsistent motions that are difficult to track with a
controller. This paper reviews existing methods to deal with this problem and
proposes new ones to overcome their limitations. Current approaches either
disregard the kinematic constraints (which leads to drift accumulation) or
modify the system dynamics to keep the trajectory close to the manifold (which
adds artificial forces or energy dissipation to the system). The methods we
propose, in contrast, achieve full drift elimination on the discrete
trajectory, or even along the continuous one, without artificial modifications
of the system dynamics. We illustrate and compare the methods using various
examples of different complexity.
