MathematicalProgram和MinimumDistanceConstraint:你能用更通用的凸形状和AutoDiff吗？

Question

我试图将最初为TrajOpt编写的弹道优化代码移植到Drake中。TrajOpt的优点之一是它可以用约束约束解决SQP轨迹优化问题，使机器人与周围障碍物之间的距离最小，并且支持相当广泛的几何形状(所有标准的凸原语加上简单的凸网格)。由于许多原因，TrajOpt不再是我的项目的正确选择，所以我将我的轨迹优化代码移植到Drake。我认为MinimumDistanceConstraint是我想要复制这个功能的东西，但是看起来Drake只允许对球面和半空间进行AutoDiffXd签名的距离查询，而不是对更一般的凸形状(比如盒子或圆柱)进行查询。

我的所有其他约束都支持AutoDiff (我有一些用于“碰撞概率”的自定义约束，但这些约束提供了一个可在AutoDiff中使用的解析导数)。为了增加一个支持更一般几何的MinimumDistanceConstraint，我是否需要完全用双数表示MathematicalProgram？这会不会降低求解器的性能(例如，必须做有限差分而不是使用AutoDiffXd中的梯度信息)？

在一个理想的世界里，我想避免诉诸于“泡泡包装”我的机器人和环境(用球体代替所有碰撞几何)，因为自定义约束的运行时，我使用的是场景中可碰撞对数的比例(我目前正在使用凸几何来保持这个数字相对较低)。

任何帮助都将不胜感激！

Answer 1

但是，德雷克似乎只允许对球面和半空间进行AutoDiffXd符号距离查询，而不允许对更一般的凸形状(如盒或圆柱)进行有符号距离查询。

1. 我认为MinimumDistanceConstraint可以为MultibodyPlant<double>和SceneGraph<double>处理更一般的几何图形(包括盒子和圆柱)。它调用FCL来计算这些几何之间的有符号距离(包括见证点)。确实，这些带符号的距离查询还不支持AutodiffXd，而只支持双重类型。但是，正如您稍后将看到的，您不需要使用带AutoDiffXd的带符号距离查询来计算距离的梯度。
2. 您可以尝试用这个MinimumDistanceConstraint为MultibodyPlant<double>构造API接口。虽然您使用的是MultibodyPlant<double>而不是MultibodyPlant<AutoDiffXd>，但MinimumDistanceConstraint仍然可以使用AutodiffXd进行计算。具体来说，它计算有符号距离这里的梯度。要计算有符号距离查询的梯度，符号距离查询不需要支持AutoDiffXd。我们可以用见证点和法向量计算梯度，如n̂_BA_Wᵀd/∂q =∂* (∂p_CbCa_W /∂q)。 其中d是符号距离，q是机器人姿态，n̂_BA_W是接触法，∂p_CbCa_W / ∂q是从B体上的见证点到A体上的见证点向量的雅可比，在此梯度下，我们用MultibodyPlant<double>求解了无碰撞逆运动学问题。