// 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;
}
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;
}
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;
}
