移除所有Proximity几何后添加MinimumDistanceConstraint仍致逆运动学求解失败

问题背景 #

我在使用Drake求解逆运动学（IK）时，尝试加入自碰撞规避约束（MinimumDistanceConstraint），采用NLOPT作为求解器。未添加该约束时IK求解正常，但添加后同一IK问题求解失败。我已通过视觉验证解是可行的，即使移除所有几何体的Proximity角色，IK求解仍然失败。

相关代码 #

string sdf_filepath = "my_robot.sdf";
float timestep = 0.0;
drake::systems::DiagramBuilder<double> builder;
drake::multibody::MultibodyPlant<double>* plant{};
drake::geometry::SceneGraph<double>* scene_graph{};
std::tie(plant, scene_graph) = drake::multibody::AddMultibodyPlantSceneGraph(&builder, timestep);
plant->set_name("plant");
scene_graph->set_name("scene_graph");
drake::multibody::Parser parser(plant, scene_graph);

const auto robot_model_index = parser.AddModelFromFile(sdf_filepath, "robot");

plant->Finalize();

auto diagram = builder.Build();
auto diagram_context= diagram->CreateDefaultContext();
auto plant_context = &(diagram->GetMutableSubsystemContext(*plant,
diagram_context.get()));

auto all_geom_ids = inspector.GetAllGeometryIds();
for (const auto& geom_id : all_geom_ids) {
  const auto source_id = inspector.GetOwningSourceId(geom_id);  // NOTE: custom function making a private function public 
  scene_graph->RemoveRole(source_id, geom_id,
      drake::geometry::Role::kProximity);
}

// Verified this is empty
auto collision_pairs = inspector.GetCollisionCandidates();

drake::multibody::InverseKinematics ik_solver(*plant, plant_context, with_joint_limits);
float min_distance = 0.1;  // padding distance
ik_solver.AddMinimumDistanceConstraint(min_distance);

// Solve IK
..... // setting pos/ori constraints for EE, solver parameters
solver_id = drake::solvers::NloptSolver::id();
std::optional<drake::solvers::SolverId> solver_id_optional = solver_id;
auto result = drake::solvers::Solve(ik_solver.prog(), solver_id_optional);  // custom change: force NLOPT to be used

// solver_results.status is 4, but result.is_success() is False, ProgramAttributesSatisfied(ik_solver.prog) is True
auto solver_results = result.get_solver_details<drake::solvers::NloptSolver>();  

可能的问题 #

• NLOPT对非光滑约束的处理缺陷：MinimumDistanceConstraint是非光滑约束（距离函数在碰撞边界不可导），NLOPT的默认梯度类算法（如SLSQP）处理这类约束时容易陷入局部最优或收敛停滞，导致求解失败。
• 约束尺度不匹配：最小距离约束（0.1m）与关节变量的尺度差异过大，优化器难以平衡不同约束的优先级，进而无法收敛到可行解。
• 初始点选择不当：即使存在可行解，若初始关节配置距离可行域过远，加上碰撞约束后，优化器可能找不到通往可行解的路径。
• Proximity角色修改时机错误：在plant->Finalize()之后修改Proximity角色无效，因为此时场景图的碰撞检测配置已经固化，无法再修改。
• 碰撞候选对清理不彻底：虽然GetCollisionCandidates()返回空，但可能存在未被正确移除的内部碰撞配置，导致约束仍被添加到优化问题中。

调试技巧 #

• 切换求解器验证：尝试使用SNOPT、IPOPT等Drake支持的其他求解器，若其他求解器能成功求解，说明问题源于NLOPT对该约束的适配性。可去掉强制指定NLOPT的代码，让Solve()自动选择合适的求解器。
• 手动设置初始可行点：将视觉验证过的可行关节配置作为初始猜测，通过ik_solver.prog().SetInitialGuess()设置，看是否能收敛。
• 放松距离约束：暂时减小最小距离值（如设为0.01m），验证求解是否成功，再逐步增大到目标值，判断是否是约束过于严格导致的问题。
• 分步添加约束：先只添加碰撞约束，求解无碰撞的关节配置；再逐步添加末端执行器位姿约束，定位是哪个约束组合导致失败。
• 检查约束有效性：遍历并打印优化问题中的所有约束，确认MinimumDistanceConstraint的数量、上下限是否符合预期，可使用ik_solver.prog().GetNumConstraints()和约束的evaluator()方法查看详情。
• 调整Proximity角色修改时机：将移除Proximity角色的代码移到plant->Finalize()之前执行，确保场景图配置在固化前被修改。
• 使用光滑化约束：替换AddMinimumDistanceConstraint为AddSmoothMinimumDistanceConstraint，用光滑近似处理距离约束，更适合梯度优化器。
• 启用求解器详细日志：设置NLOPT的set_verbose(1)参数，查看优化过程中的约束违反情况、梯度变化，定位收敛失败的具体原因。

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