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;
}
void testScenario2(const RigidBodyTree& model) {
  int ntests = 1000;

  vector<pair<VectorXd, VectorXd>> states_double;
  vector<pair<Matrix<AutoDiffFixedMaxSize, Dynamic, 1>,
              Matrix<AutoDiffFixedMaxSize, Dynamic, 1>>>
      states_autodiff_fixed;
  vector<pair<Matrix<AutoDiffDynamicSize, Dynamic, 1>,
              Matrix<AutoDiffDynamicSize, Dynamic, 1>>>
      states_autodiff_dynamic;
  default_random_engine generator;

  for (int i = 0; i < ntests; i++) {
    VectorXd q = model.getRandomConfiguration(generator);
    VectorXd v = VectorXd::Random(model.number_of_velocities());
    VectorXd x(q.rows() + v.rows());
    x << q, v;
    states_double.push_back(make_pair(q, v));

    MatrixXd grad = MatrixXd::Identity(x.size(), x.size());

    auto x_autodiff_fixed = x.cast<AutoDiffFixedMaxSize>().eval();
    gradientMatrixToAutoDiff(grad, x_autodiff_fixed);
    Matrix<AutoDiffFixedMaxSize, Dynamic, 1> q_autodiff_fixed =
        x_autodiff_fixed.topRows(model.number_of_positions());
    Matrix<AutoDiffFixedMaxSize, Dynamic, 1> v_autodiff_fixed =
        x_autodiff_fixed.bottomRows(model.number_of_velocities());
    states_autodiff_fixed.push_back(
        make_pair(q_autodiff_fixed, v_autodiff_fixed));

    auto x_autodiff_dynamic = x.cast<AutoDiffDynamicSize>().eval();
    gradientMatrixToAutoDiff(grad, x_autodiff_dynamic);
    Matrix<AutoDiffDynamicSize, Dynamic, 1> q_autodiff_dynamic =
        x_autodiff_dynamic.topRows(model.number_of_positions());
    Matrix<AutoDiffDynamicSize, Dynamic, 1> v_autodiff_dynamic =
        x_autodiff_dynamic.bottomRows(model.number_of_velocities());
    states_autodiff_dynamic.push_back(
        make_pair(q_autodiff_dynamic, v_autodiff_dynamic));
  }

  KinematicsCache<double> cache_double(model.bodies);
  KinematicsCache<AutoDiffFixedMaxSize> cache_autodiff_fixed(model.bodies);
  KinematicsCache<AutoDiffDynamicSize> cache_autodiff_dynamic(model.bodies);

  cout << "scenario 2:" << endl;
  cout << "no gradients: "
       << measure<>::execution(scenario2<double>, model, cache_double,
                               states_double) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff fixed max size: "
       << measure<>::execution(scenario2<AutoDiffFixedMaxSize>, model,
                               cache_autodiff_fixed, states_autodiff_fixed) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff dynamic size: "
       << measure<>::execution(scenario2<AutoDiffDynamicSize>, model,
                               cache_autodiff_dynamic,
                               states_autodiff_dynamic) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << endl;
}