This article presents a novel optimization method that handles collision constraints with complex, non-convex 3D geometries. The optimization problem is cast as a semi-infinite program in which each collision constraint is implicitly treated as an infinite number of numeric constraints. The approach progressively generates some of these constraints for inclusion in a finite nonlinear program. Constraint generation uses an oracle to detect points of deepest penetration, and this oracle is implemented efficiently via signed distance field (SDF) versus point cloud collision detection. This approach is applied to pose optimization and trajectory optimization for both free-flying rigid bodies and articulated robots. Experiments demonstrate performance improvements compared with optimizers that handle only convex polyhedra, and demonstrate efficient collision avoidance between non-convex CAD models and point clouds in a variety of pose and trajectory optimization settings.

本文提出了一种新颖的优化方法，该方法可以处理具有复杂，非凸面3D几何形状的碰撞约束。优化问题被强制转换为半无限程序，其中每个碰撞约束都隐式地视为无限数量的数字约束。该方法逐步生成其中一些约束，以包含在有限的非线性程序中。约束生成使用一个预言机来检测最深的穿透点，并且该预言机是通过有符号距离场（SDF）与点云碰撞检测有效地实现的。该方法适用于自由飞行刚体和多关节机器人的姿态优化和轨迹优化。实验表明，与仅处理凸多面体的优化器相比，性能得到了改善，