VS300s机械臂导入Drake后，耦合关节用InverseDynamicsController遇错求助

问题背景 #

正在将VS300s机械臂移植到Drake环境，该机械臂手部包含2个棱柱形手指关节和1个连续型夹爪关节。目标是将三者耦合，仅保留1个驱动关节以简化控制。

已完成操作：

• 将两个手指关节的驱动力设置为0，转为非驱动关节
• 通过AddCouplerConstraint将夹爪关节与左、右手指关节分别建立耦合约束

遇到的问题：使用InverseDynamicsController时，触发「位置数多于驱动数」的报错。

解决步骤 #

1. 明确标记被动关节 #

仅设置驱动力为0不足以让Drake完全将手指关节排除出驱动集合。需要在加载模型时，明确将两个手指关节标记为被动关节：

• 方法一：修改URDF文件，在手指关节的<joint>标签内添加<dynamics passive="true"/>
• 方法二：在MultibodyPlant加载模型后，调用SetJointActuationIndex(joint, -1)，将这两个关节的驱动索引设为-1，彻底移除驱动属性

2. 约束控制器的作用关节集合 #

InverseDynamicsController默认会绑定模型的所有驱动关节，你需要手动指定仅作用于夹爪驱动关节：

• 创建控制器时，通过actuation_selection_matrix参数传入选择矩阵，仅保留夹爪关节对应的行；或者通过指定输入端口维度为1，确保输入命令与驱动关节数匹配
• 示例：如果夹爪关节是模型的第7个驱动关节，构造选择矩阵时仅第7行设为1，其余为0

3. 验证约束生效后的自由度 #

添加耦合约束后，模型的有效自由度会减少。可以通过以下代码验证：

std::cout << "Total positions: " << plant.num_positions() << std::endl;
std::cout << "Actuated DOFs: " << plant.num_actuated_dofs() << std::endl;
std::cout << "Number of constraints: " << plant.num_constraints() << std::endl;

正常情况下，num_positions() - num_actuated_dofs()应等于约束数量（此处为2），说明耦合约束已正确生效。

4. 匹配参考轨迹维度 #

确保传入InverseDynamicsController的参考轨迹（位置、速度、加速度）维度为1，仅对应夹爪关节的运动指令，不能再传入原3维的手部关节参考信号。

5. 检查耦合约束映射关系 #

确认AddCouplerConstraint中的运动映射是否正确，比如手指关节位置与夹爪关节位置的线性关系是否符合机械结构逻辑（例如：finger_joint_pos = 0.5 * gripper_joint_pos），错误的映射可能导致约束未正确限制自由度。

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