Пример #1
0
// Find the joint position indices corresponding to 'name'
vector<int> getJointPositionVectorIndices(const RigidBodyTree &model, const std::string &name) {
  shared_ptr<RigidBody> joint_parent_body = model.findJoint(name);
  int num_positions = joint_parent_body->getJoint().getNumPositions();
  vector<int> ret(static_cast<size_t>(num_positions));

  // fill with sequentially increasing values, starting at joint_parent_body->position_num_start:
  iota(ret.begin(), ret.end(), joint_parent_body->position_num_start);
  return ret;
}
Пример #2
0
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.findJoint(joint_name);
    for (int i = 0; i < body.getJoint().getNumPositions(); i++) {
      position_indices.push_back(body.get_position_start_index() + i);
    }
  }
  return position_indices;
}
Пример #3
0
RobotPropertyCache parseKinematicTreeMetadata(const YAML::Node& metadata,
                                              const RigidBodyTree& 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.findJoint(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.findJoint(joint_group_names["back_bkz"].as<string>())
          ->get_position_start_index();
  ret.position_indices.back_bky =
      robot.findJoint(joint_group_names["back_bky"].as<string>())
          ->get_position_start_index();

  return ret;
}