如何正确使用Drake C++中的GetJointActuatorIndices()方法？

我正在学习Drake C++，尝试在Drake中模拟Allegro Hand与WAM机械臂。原本参考Allegro Hand控制示例，用以下代码配置执行器：

for (JointActuatorIndex actuator_index(0); actuator_index < plant.num_actuators(); ++actuator_index) {...}

但因在plant中添加了其他模型，需要指定特定执行器。我参考示例调用GetJointActuatorIndices()，代码如下：

for (JointActuatorIndex joint_actuator_index : plant.GetJointActuatorIndices(model_hand[0])) {...}

其中model_hand由const auto& model_hand = parser.AddModels(...);预先定义。但运行时出现如下错误：

terminate called after throwing an instance of 'std::out_of_range'
what(): vector::_M_range_check: __n (which is 2) >= this->size() (which is 0)

解决方案 #

• 确认model_hand模型加载有效性：
  parser.AddModels()返回std::vector<ModelInstanceIndex>，加载失败会返回空向量。添加检查确保model_hand[0]有效：

  if (model_hand.empty()) {
    throw std::runtime_error("Failed to load Allegro Hand model");
  }
  

• 检查模型文件的执行器定义：
  Allegro Hand的URDF/SDF文件必须包含joint actuator元素。如果缺少该定义，GetJointActuatorIndices()会返回空向量，遍历触发越界。确认模型文件中有类似定义：

  <joint name="joint_1" type="revolute">
    <!-- 关节属性 -->
    <actuator name="actuator_1" joint="joint_1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </joint>
  

• 确保Plant已完成Finalize：
  MultibodyPlant需调用plant.Finalize()后，才能正确获取执行器索引。调整代码顺序：

  // 完成所有模型添加后
  plant.Finalize();
  // 再获取执行器索引并遍历
  auto hand_actuators = plant.GetJointActuatorIndices(model_hand[0]);
  for (JointActuatorIndex idx : hand_actuators) {
    // 执行器配置逻辑
  }
  

• 提前检查执行器向量是否为空：
  遍历前验证返回的执行器索引向量长度，避免空向量遍历：

  auto hand_actuators = plant.GetJointActuatorIndices(model_hand[0]);
  if (hand_actuators.empty()) {
    throw std::runtime_error("Allegro Hand model has no joint actuators defined");
  }
  for (JointActuatorIndex idx : hand_actuators) {
    // 配置执行器
  }
  

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