运行含自由体的IrisInConfigurationSpace报错，求替代解决方案

核心原因 #

报错是因为IrisInConfigurationSpace要求所有配置变量（包括自由体的位姿自由度）必须有有限的上下边界，而自由体默认没有位置限制，触发了断言检查。

最优方案：仅在机械臂配置空间运行IRIS #

你的核心需求是规划iiwa机械臂的避障路径，完全不需要将自由体的位姿纳入IRIS的配置空间范围。直接指定IRIS只处理机械臂的关节变量即可：

1. 获取iiwa机械臂所有关节的配置索引：

   const auto& iiwa_joints = plant.GetJointsOfModelInstance(iiwa_model_instance);
   std::vector<int> iiwa_config_indices;
   for (const auto& joint : iiwa_joints) {
       const auto& indices = plant.GetJointConfigurationIndices(joint);
       iiwa_config_indices.insert(iiwa_config_indices.end(), indices.begin(), indices.end());
   }
   

2. 调用IrisInConfigurationSpace时传入configuration_space_indices参数：

   IrisInConfigurationSpace(
       plant,
       scene_graph,
       start_configuration,
       iris_options,
       iiwa_config_indices  // 仅处理机械臂的配置变量
   );
   

这个方案直接规避了自由体的无界问题，同时完全匹配你的路径规划需求，是最简洁高效的做法。

备选方案：临时给自由体设置有限位置边界 #

如果必须包含自由体的配置空间（比如需要同时规划自由体和机械臂），可以给自由体的位姿设置极小的有限边界，模拟固定状态：

// 获取自由体的浮动关节
const auto& free_body_joint = plant.GetJointByName("free_body_joint");
const Eigen::VectorXd current_pose = plant.GetPositions(free_body_joint);

// 设置极小的上下限（数值有限但实际等价于固定）
plant.SetPositionLowerLimits(free_body_joint, current_pose.array() - 1e-6);
plant.SetPositionUpperLimits(free_body_joint, current_pose.array() + 1e-6);

// 运行IRIS...

// （可选）若后续需恢复自由体无界状态，可重置上下限
plant.SetPositionLowerLimits(free_body_joint, Eigen::VectorXd::Constant(current_pose.size(), -std::numeric_limits<double>::infinity()));
plant.SetPositionUpperLimits(free_body_joint, Eigen::VectorXd::Constant(current_pose.size(), std::numeric_limits<double>::infinity()));

不推荐方案：创建额外Plant焊接自由体 #

你提到的额外Plant方案虽然可行，但会增加代码冗余和维护成本，以上两种方案都比它更高效。

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