使用PiecewisePolynomial.FirstOrderHold遇SNOPT碰撞检测错误求助

核心原因 #

Drake的碰撞检测模块中，Box与Box的穿透查询（penetration query）不支持drake::AutoDiffXd标量类型。当DirectCollocation优化过程中（使用自动微分计算代价/约束导数）触发了这类碰撞检测，就会抛出该错误。仅初始+最终状态时正常，说明中间断点或轨迹插值后的状态存在碰撞，导致优化时执行了不支持的穿透计算。

排查与解决步骤 #

• 检查所有断点对应的状态是否存在碰撞：
  手动验证每个断点的x[t]对应的机器人构型，确认是否有Box形状的连杆发生碰撞。如果某个断点本身就处于碰撞状态，优化过程中会尝试计算碰撞约束的导数，触发错误。
• 禁用不必要的碰撞约束：
  如果你不需要碰撞避免约束，检查是否在DirectCollocation或场景图中意外启用了碰撞检测约束。可以通过RemoveCollisionPair或配置SceneGraph的碰撞过滤规则，关闭Box-Box之间的碰撞检测。
• 替换碰撞查询类型：
  若必须保留碰撞约束，将Box形状替换为支持AutoDiffXd穿透查询的形状（比如Sphere、Capsule），或者改用距离查询（distance query）替代穿透查询——Drake对多数形状组合的距离查询支持AutoDiff，可通过MinimumDistanceConstraint来实现碰撞避免，而非依赖穿透检测。
• 检查轨迹插值的影响：
  即使断点状态无碰撞，轨迹插值（比如DirectCollocation默认的分段多项式插值）可能在断点间生成碰撞状态。可以尝试：
  1. 增加断点数量，缩小插值区间，避免中间构型碰撞；
  2. 改用更保守的插值方式，或在相邻断点间添加碰撞约束。
• 验证约束配置：
  确认是否误将碰撞相关约束（如PenetrationConstraint）添加到了DirectCollocation的约束列表中。这类约束在AutoDiff上下文下对Box-Box组合不兼容，需替换为MinimumDistanceConstraint。

内容的提问来源于stack exchange，提问作者kevin