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;
}
void testScenario1(const RigidBodyTree& model) {
int ntests = 1000;
vector<VectorXd> qs_double;
vector<Matrix<AutoDiffFixedMaxSize, Dynamic, 1>> qs_autodiff_fixed;
vector<Matrix<AutoDiffDynamicSize, Dynamic, 1>> qs_autodiff_dynamic;
default_random_engine generator;
for (int i = 0; i < ntests; i++) {
auto q = model.getRandomConfiguration(generator);
qs_double.push_back(q);
MatrixXd grad = MatrixXd::Identity(model.number_of_positions(),
model.number_of_positions());
auto q_autodiff_fixed = q.cast<AutoDiffFixedMaxSize>().eval();
gradientMatrixToAutoDiff(grad, q_autodiff_fixed);
qs_autodiff_fixed.push_back(q_autodiff_fixed);
auto q_autodiff_dynamic = q.cast<AutoDiffDynamicSize>().eval();
gradientMatrixToAutoDiff(grad, q_autodiff_dynamic);
qs_autodiff_dynamic.push_back(q_autodiff_dynamic);
}
map<int, Matrix3Xd> body_fixed_points;
int npoints_feet = 4;
int npoints_hands = 1;
int npoints_head = 1;
vector<string> sides{"l", "r"};
for (const auto& side : sides) {
int hand_id = model.FindBodyIndex(side + "_hand");
body_fixed_points.insert(
make_pair(hand_id, Matrix3Xd::Random(3, npoints_hands)));
int foot_id = model.FindBodyIndex(side + "_foot");
body_fixed_points.insert(
make_pair(foot_id, Matrix3Xd::Random(3, npoints_feet)));
}
int head_id = model.FindBodyIndex("head");
body_fixed_points.insert(
make_pair(head_id, Matrix3Xd::Random(3, npoints_head)));
KinematicsCache<double> cache_double(model.bodies);
KinematicsCache<AutoDiffFixedMaxSize> cache_autodiff_fixed(model.bodies);
KinematicsCache<AutoDiffDynamicSize> cache_autodiff_dynamic(model.bodies);
cout << "scenario 1:" << endl;
cout << "no gradients: "
<< measure<>::execution(scenario1<double>, model, cache_double,
qs_double, body_fixed_points) /
static_cast<double>(ntests)
<< " ms" << endl;
cout << "autodiff fixed max size: "
<< measure<>::execution(scenario1<AutoDiffFixedMaxSize>, model,
cache_autodiff_fixed, qs_autodiff_fixed,
body_fixed_points) /
static_cast<double>(ntests)
<< " ms" << endl;
cout << "autodiff dynamic size: "
<< measure<>::execution(scenario1<AutoDiffDynamicSize>, model,
cache_autodiff_dynamic, qs_autodiff_dynamic,
body_fixed_points) /
static_cast<double>(ntests)
<< " ms" << endl;
cout << endl;
}