コード例 #1
0
RobotPropertyCache parseKinematicTreeMetadata(
    const YAML::Node& metadata,
    const RigidBodyTree<double>& robot) {
  RobotPropertyCache ret;
  std::map<Side, string> side_identifiers = {{Side::RIGHT, "r"},
                                             {Side::LEFT, "l"}};

  YAML::Node body_names = metadata["body_names"];
  YAML::Node feet = body_names["feet"];
  YAML::Node hands = body_names["hands"];
  for (const auto& side : Side::values) {
    ret.foot_ids[side] =
        robot.FindBodyIndex(feet[side_identifiers[side]].as<string>());
  }

  YAML::Node joint_group_names = metadata["joint_group_names"];
  for (const auto& side : Side::values) {
    auto side_id = side_identifiers.at(side);
    ret.position_indices.legs[side] = findPositionIndices(
        robot, joint_group_names["legs"][side_id].as<vector<string>>());
    ret.position_indices.knees[side] =
        robot
            .FindChildBodyOfJoint(
                joint_group_names["knees"][side_id].as<string>())
            ->get_position_start_index();
    ret.position_indices.ankles[side] = findPositionIndices(
        robot, joint_group_names["ankles"][side_id].as<vector<string>>());
    ret.position_indices.arms[side] = findPositionIndices(
        robot, joint_group_names["arms"][side_id].as<vector<string>>());
  }
  ret.position_indices.neck = findPositionIndices(
      robot, joint_group_names["neck"].as<vector<string>>());
  ret.position_indices.back_bkz =
      robot.FindChildBodyOfJoint(joint_group_names["back_bkz"].as<string>())
          ->get_position_start_index();
  ret.position_indices.back_bky =
      robot.FindChildBodyOfJoint(joint_group_names["back_bky"].as<string>())
          ->get_position_start_index();

  return ret;
}
コード例 #2
0
ファイル: yamlUtil.cpp プロジェクト: hsu/drake
vector<int> findPositionIndices(const RigidBodyTree& robot,
                                const vector<string>& joint_names) {
  vector<int> position_indices;
  position_indices.reserve(joint_names.size());
  for (const auto& joint_name : joint_names) {
    const RigidBody& body = *robot.FindChildBodyOfJoint(joint_name);
    for (int i = 0; i < body.getJoint().getNumPositions(); i++) {
      position_indices.push_back(body.get_position_start_index() + i);
    }
  }
  return position_indices;
}