Ejemplo n.º 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;
}
Ejemplo n.º 2
0
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;
}