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);
}
}
void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
string usage = "Usage [M, dM] = massMatrixmex(model_ptr, cache_ptr)";
if (nrhs != 2) {
mexErrMsgIdAndTxt("Drake:geometricJacobianmex:WrongNumberOfInputs", usage.c_str());
}
if (nlhs > 2) {
mexErrMsgIdAndTxt("Drake:geometricJacobianmex:WrongNumberOfOutputs", usage.c_str());
}
int gradient_order = nlhs - 1;
int arg_num = 0;
RigidBodyManipulator *model = static_cast<RigidBodyManipulator*>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double>* cache = static_cast<KinematicsCache<double>*>(getDrakeMexPointer(prhs[arg_num++]));
auto ret = model->massMatrix(*cache, gradient_order);
plhs[0] = eigenToMatlab(ret.value());
if (gradient_order > 0)
plhs[1] = eigenToMatlab(ret.gradient().value());
}