void testUserGradients(const RigidBodyManipulator &model, int ntests) {
VectorXd q(model.num_positions);
KinematicsCache<double> cache(model.bodies, 1);
for (int i = 0; i < ntests; i++) {
model.getRandomConfiguration(q, generator);
cache.initialize(q);
model.doKinematics(cache, false);
auto H_gradientvar = model.massMatrix(cache, 1);
}
}
void testAutoDiffScalarGradients(const RigidBodyManipulator &model, int ntests) {
const int NUM_POSITIONS = Dynamic;
typedef Matrix<double, NUM_POSITIONS, 1> DerivativeType;
typedef AutoDiffScalar<DerivativeType> TaylorVar;
VectorXd q(model.num_positions);
Matrix<TaylorVar, NUM_POSITIONS, 1> q_taylorvar;
auto grad = Matrix<double, NUM_POSITIONS, NUM_POSITIONS>::Identity(model.num_positions, model.num_positions).eval();
KinematicsCache<TaylorVar> cache(model.bodies, 0);
for (int i = 0; i < ntests; i++) {
model.getRandomConfiguration(q, generator);
q_taylorvar = q.cast<TaylorVar>().eval();
gradientMatrixToAutoDiff(grad, q_taylorvar);
cache.initialize(q_taylorvar);
model.doKinematics(cache, false);
auto H_taylorvar = model.massMatrix(cache, 0);
}
}
