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