如何在Drake中固定MultiBodyPlant指定位置，生成6自由度关联模型？

问题 #

我拥有一个包含6自由度机器人和2自由度静止障碍物的MultibodyPlant，其位置向量为8维：[object_x_offset, object_y_offset, robot_joint_1, ..., robot_joint_6]。这种设置在IrisInConfigurationSpace中十分便捷，可在运行时确定障碍物位姿后，通过已知的[object_x, object_y]坐标将8维无碰撞凸区域切片为6维。但这对Drake轨迹规划算法不便，算法会默认[object_x, object_y]坐标可被控制，我需手动向MathematicalProgram添加线性约束来固定这些坐标。我希望创建一个关联的MultibodyPlant，使其num_positions() == 6，能将原模型的前两个坐标固定为指定值，是否有简便方法？

解决方案 #

有几种简便的方法可以实现需求：

• 冻结障碍物关节（推荐）
  直接找到障碍物对应x、y方向的关节，调用Joint::Freeze()方法。冻结后这些关节的自由度会被移除，MultibodyPlant的num_positions()会自动变为6，轨迹规划算法会直接忽略这些固定自由度，无需手动添加约束。
  示例代码：

  // 获取障碍物的x、y关节
  const auto& object_x_joint = plant.GetJointByName("object_x_joint");
  const auto& object_y_joint = plant.GetJointByName("object_y_joint");
  
  // 冻结关节
  object_x_joint.Freeze();
  object_y_joint.Freeze();
  
  // 设置固定的坐标值
  Eigen::VectorXd fixed_pos = plant.GetPositions(context);
  fixed_pos(0) = desired_object_x;
  fixed_pos(1) = desired_object_y;
  plant.SetPositions(context, fixed_pos);
  

• 克隆原模型后修改
  如果需要保留原8维配置空间的Plant用于Iris计算，可以克隆一个新的MultibodyPlant实例，在新实例中执行上述冻结关节的操作。这样原Plant不受影响，新Plant可直接用于轨迹规划。

• 使用关节位置约束
  若不想冻结关节，也可以通过AddJointConstraint添加固定位置约束，但这种方式仍会保留8维自由度，需要规划算法处理约束，不如冻结关节高效。

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