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);
  }
}
Exemplo n.º 3
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());
}