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.

翻译：直接配点法是解决机器人轨迹优化问题的强大工具。尽管其生成的轨迹通常具有较高的动态精度，但在受约束的机械系统（即其状态坐标受完整或非完整约束，如闭环约束或滚动接触约束的系统）中，这些轨迹可能呈现出较大的运动学误差。这些约束将机器人轨迹限制在一个隐式定义的流形上，从而增加了求解精确解的难度。问题转述过程中固有的离散化误差容易使轨迹偏离该流形，导致产生物理不一致的运动，难以通过控制器进行跟踪。本文综述了解决该问题的现有方法，并提出了克服其局限性的新方法。现有方法要么忽略运动学约束（导致漂移累积），要么修改系统动力学以保持轨迹接近流形（从而向系统引入人工力或能量耗散）。相比之下，我们提出的方法无需对系统动力学进行人工修改即可实现离散轨迹上甚至连续轨迹上的完全漂移消除。我们通过多个不同复杂度的示例对所提方法进行了说明和比较。