Drake中Box间有向距离查询不支持AutoDiffXd的问题及扩展方案问询

Drake的有向距离查询的AutoDiffXd支持是分几何类型实现的：

• 球体型几何的有向距离函数是全局光滑的解析表达式，梯度计算逻辑简单，因此原生支持AutoDiffXd标量类型下的查询
• 两个Box的有向距离会随位姿变化出现接触模式切换（顶点对面、棱对棱、面对角等），存在不可导的尖点，且目前Drake官方未实现Box-Box、Mesh类几何在AutoDiffXd标量下的有向距离查询后端，因此会抛出对应报错。

• 优先使用double类型Plant构建问题：StaticEquilibriumProblem本身支持直接传入double标量类型的MultibodyPlant和Context，不需要强制转换为AutoDiffXd类型。此时求解器会调用适配非光滑接触约束的求解逻辑，原生支持Box、Mesh等任意几何类型的静态平衡计算，修改后的代码示例如下：

# 替换为double类型的plant和context，无需转换为AutoDiffXd
static_equilibrium_problem = StaticEquilibriumProblem(double_plant, double_plant.GetMyContextFromRoot(double_context), set())
# 选用支持非光滑约束的求解器求解
solver = SnoptSolver()
result = solver.Solve(static_equilibrium_problem.prog())

• 替换接触约束实现：如果必须使用AutoDiffXd的场景，可以将默认的有向距离约束替换为平滑近似的接触约束，例如对Box边缘做圆角平滑处理规避不可导点，或者将碰撞约束拆解为多组线性互补约束，手动添加到static_equilibrium_problem.prog()中。
• 点接触近似：对Box、Mesh表面采样离散接触点，将碰撞约束转换为采样点之间的非穿透约束，这类约束的求导逻辑已适配AutoDiffXd，不需要依赖原生的Box-Box有向距离查询。

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