具有 MinimumDistanceConstraint 的 MathematicalProgram：您可以将更一般的凸形与 AutoDiff 一起使用吗？
MathematicalProgram with MinimumDistanceConstraint: can you use more general convex shapes with AutoDiff?
我正在尝试将最初为 TrajOpt 编写的轨迹优化代码移植到 Drake 中。 TrajOpt 的优点之一是它可以通过强制执行机器人与周围障碍物之间的最小距离的约束来解决 SQP 轨迹优化问题，并且它支持相当广泛的几何形状（所有标准凸基元加上简单的凸网格） .由于多种原因，TrajOpt 不再是我项目的正确选择，因此我将轨迹优化代码移植到 Drake。我认为 MinimumDistanceConstraint 是我想要复制此功能的东西，但似乎 Drake 只允许对球体和半空间进行 AutoDiffXd 有符号距离查询，而不是更一般的凸形（如盒子或圆柱体） .
我的所有其他约束都支持 AutoDiff（我有一些针对 "probability of collision," 的自定义约束，但这些约束提供了可在 AutoDiff 中使用的分析导数）。为了添加支持更一般几何的 MinimumDistanceConstraint，我是否必须完全用双打来表达 MathematicalProgram？这会降低求解器的性能吗（例如，必须进行有限差分而不是使用 AutoDiffXd 中的梯度信息）？
在理想世界中，我想避免求助于 "bubble-wrapping" 我的机器人和环境（用球体替换所有碰撞几何体），因为我使用的自定义约束的运行时间与场景中可碰撞对的数量（我目前使用凸几何来保持这个数量相对较低）。
如有任何帮助，我们将不胜感激！
but it seems that Drake allows AutoDiffXd signed distance queries only for spheres and half-spaces, not for more general convex shapes (like boxes or cylinders)
- 我认为
MinimumDistanceConstraint 可以为 MultibodyPlant<double> 和 SceneGraph<double> 处理更一般的几何形状（包括长方体和圆柱体）。它调用 FCL 来计算这些几何图形（包括见证点）之间的有符号距离。确实，这些带符号的距离查询还不支持 AutodiffXd，但只支持 double 类型。但是正如您稍后将看到的，您不需要使用 AutoDiffXd 的有符号距离查询来计算距离的梯度。
- 您可以尝试使用此 API. Although you use
MultibodyPlant<double> not MultibodyPlant<AutoDiffXd>, MinimumDistanceConstraint can still evaluate with AutodiffXd. Specifically, it computes the gradient of the signed distance here 为 MultibodyPlant<double> 构建 MinimumDistanceConstraint。要计算带符号距离查询的梯度，带符号距离查询不需要支持 AutoDiffXd。我们可以使用见证点和法向量来计算梯度
∂d / ∂q = n̂_BA_Wᵀ * (∂p_CbCa_W / ∂q)
其中 d 是带符号的距离，q 是机器人姿态，n̂_BA_W 是接触法线，∂p_CbCa_W / ∂q 是 [=32 上见证点的矢量的雅可比矩阵=] B 到 body A 上的见证点。利用这个梯度，我们用 MultibodyPlant<double>. 解决了 collision-free 逆运动学问题
我正在尝试将最初为 TrajOpt 编写的轨迹优化代码移植到 Drake 中。 TrajOpt 的优点之一是它可以通过强制执行机器人与周围障碍物之间的最小距离的约束来解决 SQP 轨迹优化问题，并且它支持相当广泛的几何形状（所有标准凸基元加上简单的凸网格） .由于多种原因，TrajOpt 不再是我项目的正确选择，因此我将轨迹优化代码移植到 Drake。我认为 MinimumDistanceConstraint 是我想要复制此功能的东西，但似乎 Drake 只允许对球体和半空间进行 AutoDiffXd 有符号距离查询，而不是更一般的凸形（如盒子或圆柱体） .
我的所有其他约束都支持 AutoDiff（我有一些针对 "probability of collision," 的自定义约束，但这些约束提供了可在 AutoDiff 中使用的分析导数）。为了添加支持更一般几何的 MinimumDistanceConstraint，我是否必须完全用双打来表达 MathematicalProgram？这会降低求解器的性能吗（例如，必须进行有限差分而不是使用 AutoDiffXd 中的梯度信息）？
在理想世界中，我想避免求助于 "bubble-wrapping" 我的机器人和环境（用球体替换所有碰撞几何体），因为我使用的自定义约束的运行时间与场景中可碰撞对的数量（我目前使用凸几何来保持这个数量相对较低）。
如有任何帮助，我们将不胜感激！
but it seems that Drake allows AutoDiffXd signed distance queries only for spheres and half-spaces, not for more general convex shapes (like boxes or cylinders)
- 我认为
MinimumDistanceConstraint可以为MultibodyPlant<double>和SceneGraph<double>处理更一般的几何形状（包括长方体和圆柱体）。它调用 FCL 来计算这些几何图形（包括见证点）之间的有符号距离。确实，这些带符号的距离查询还不支持 AutodiffXd，但只支持 double 类型。但是正如您稍后将看到的，您不需要使用 AutoDiffXd 的有符号距离查询来计算距离的梯度。 - 您可以尝试使用此 API. Although you use
MultibodyPlant<double>notMultibodyPlant<AutoDiffXd>,MinimumDistanceConstraintcan still evaluate with AutodiffXd. Specifically, it computes the gradient of the signed distance here 为MultibodyPlant<double>构建 MinimumDistanceConstraint。要计算带符号距离查询的梯度，带符号距离查询不需要支持 AutoDiffXd。我们可以使用见证点和法向量来计算梯度
其中∂d / ∂q = n̂_BA_Wᵀ * (∂p_CbCa_W / ∂q)d是带符号的距离，q是机器人姿态，n̂_BA_W是接触法线，∂p_CbCa_W / ∂q是 [=32 上见证点的矢量的雅可比矩阵=] B 到 body A 上的见证点。利用这个梯度，我们用MultibodyPlant<double>. 解决了 collision-free 逆运动学问题