Example #1
0
int main(int argc, char* argv[])
{
	if (argc<2) {
		cerr << "Usage: urdfCollisionTest urdf_filename" << endl;
		exit(-1);
	}
  RigidBodyTree * model = new RigidBodyTree(argv[1]);
  if (!model) {
  	cerr << "ERROR: Failed to load model from " << argv[1] << endl;
  	return -1;
  }

  // run kinematics with second derivatives 100 times
  Eigen::VectorXd q = model->getZeroConfiguration();
  int i;

  if (argc>=2+model->num_positions) {
  	for (i=0; i<model->num_positions; i++)
  		sscanf(argv[2+i],"%lf",&q(i));
  }

// for (i=0; i<model->num_dof; i++)
// 	 q(i)=(double)rand() / RAND_MAX;
  KinematicsCache<double> cache = model->doKinematics(q);
//  }

  Eigen::VectorXd phi;
  Eigen::Matrix3Xd normal, xA, xB;
  vector<int> bodyA_idx, bodyB_idx;

  model->collisionDetect(cache, phi,normal,xA,xB,bodyA_idx,bodyB_idx);

  cout << "=======" << endl;
  for (int j=0; j<phi.rows(); ++j) {
    cout << phi(j) << " ";
    for (int i=0; i<3; ++i) {
      cout << normal(i,j) << " ";
    }
    for (int i=0; i<3; ++i) {
      cout << xA(i,j) << " ";
    }
    for (int i=0; i<3; ++i) {
      cout << xB(i,j) << " ";
    }
    cout << model->bodies[bodyA_idx.at(j)]->linkname << " ";
    cout << model->bodies[bodyB_idx.at(j)]->linkname << endl;
  }

  delete model;
  return 0;
}
int main(int argc, char* argv[])
{
  RigidBodyTree tree;

  for (int i=0; i<10; i++) {
    tree.addRobotFromURDF(getDrakePath()+"/systems/plants/test/PointMass.urdf",DrakeJoint::ROLLPITCHYAW);
  }
  tree.addRobotFromURDF(getDrakePath()+"/systems/plants/test/FallingBrick.urdf",DrakeJoint::FIXED);

  VectorXd q = VectorXd::Random(tree.num_positions);
  VectorXd v = VectorXd::Random(tree.num_velocities);
  auto kinsol = tree.doKinematics(q,v);

  VectorXd phi;
  Matrix3Xd normal, xA, xB;
  std::vector<int> bodyA_idx, bodyB_idx;
  tree.collisionDetect(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx,false);
}
int main(int argc, char* argv[]) {
  RigidBodyTree tree;

  for (int i = 0; i < 10; ++i) {
    drake::parsers::urdf::AddModelInstanceFromURDF(
        drake::GetDrakePath() + "/systems/plants/test/PointMass.urdf",
            DrakeJoint::ROLLPITCHYAW, &tree);
  }

  drake::parsers::urdf::AddModelInstanceFromURDF(
      drake::GetDrakePath() + "/systems/plants/test/FallingBrick.urdf",
          DrakeJoint::FIXED, &tree);

  VectorXd q = VectorXd::Random(tree.number_of_positions());
  VectorXd v = VectorXd::Random(tree.number_of_velocities());
  auto kinsol = tree.doKinematics(q, v);

  VectorXd phi;
  Matrix3Xd normal, xA, xB;
  std::vector<int> bodyA_idx, bodyB_idx;
  tree.collisionDetect(kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx,
                       false);
}
Example #4
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  // check number of arguments
  if (nrhs < 4) {
    mexErrMsgIdAndTxt("Drake:collisionDetectmex:NotEnoughInputs",
                      "Usage: [xA, xB, normal, distance, idxA, idxB] = "
                      "collisionDetectmex(mex_model_ptr, cache_ptr, "
                      "allow_multiple_contacts, active_collision_options)");
  }

  // check argument types
  if (!mxIsClass(prhs[0], "DrakeMexPointer")) {
    mexErrMsgIdAndTxt(
        "Drake:collisionDetectmex:InvalidInputType",
        "Expected a DrakeMexPointer for mex_model_ptr but got something else.");
  }

  if (!mxIsClass(prhs[1], "DrakeMexPointer")) {
    mexErrMsgIdAndTxt(
        "Drake:collisionDetectmex:InvalidInputType",
        "Expected a DrakeMexPointer for cache_ptr but got something else.");
  }

  if (!mxIsLogical(prhs[2])) {
    mexErrMsgIdAndTxt("Drake:collisionDetectmex:InvalidInputType",
                      "Expected a boolean logic type for "
                      "allow_multiple_collisions but got something else.");
  }

  if (!mxIsStruct(prhs[3])) {
    mexErrMsgIdAndTxt("Drake:collisionDetectmex:InvalidInputType",
                      "Expected a struct type for active_collision_options but "
                      "got something else.");
  }

  int arg_num = 0;
  RigidBodyTree<double>* model =
      static_cast<RigidBodyTree<double>*>(getDrakeMexPointer(prhs[arg_num++]));

  KinematicsCache<double>& cache =
      fromMex(prhs[arg_num++], static_cast<KinematicsCache<double>*>(nullptr));

  // Parse `active_collision_options`
  vector<int> active_bodies_idx;
  set<string> active_group_names;

  const mxArray* allow_multiple_contacts = prhs[arg_num++];
  const mxArray* active_collision_options = prhs[arg_num++];

  const mxArray* body_idx = mxGetField(active_collision_options, 0, "body_idx");
  if (body_idx != NULL) {
    size_t n_active_bodies =
        static_cast<size_t>(mxGetNumberOfElements(body_idx));

    active_bodies_idx.resize(n_active_bodies);
    memcpy(active_bodies_idx.data(), (int*)mxGetData(body_idx),
           sizeof(int) * n_active_bodies);
    transform(active_bodies_idx.begin(), active_bodies_idx.end(),
              active_bodies_idx.begin(), [](int i) { return --i; });
  }

  const mxArray* collision_groups =
      mxGetField(active_collision_options, 0, "collision_groups");

  if (collision_groups != NULL) {
    size_t num_collision_groups =
        static_cast<size_t>(mxGetNumberOfElements(collision_groups));
    for (size_t i = 0; i < num_collision_groups; i++) {
      const mxArray* ptr = mxGetCell(collision_groups, i);
      size_t buflen = static_cast<size_t>(mxGetN(ptr) * sizeof(mxChar)) + 1;
      char* str = (char*)mxMalloc(buflen);
      mxGetString(ptr, str, buflen);
      active_group_names.insert(str);
      mxFree(str);
    }
  }

  vector<int> bodyA_idx, bodyB_idx;
  Matrix3Xd ptsA, ptsB, normals;
  MatrixXd JA, JB, Jd;
  VectorXd dist;

  if (active_bodies_idx.size() > 0) {
    if (active_group_names.size() > 0) {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_bodies_idx, active_group_names);
    } else {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_bodies_idx);
    }
  } else {
    const bool multiple_contacts =
        mxIsLogicalScalarTrue(allow_multiple_contacts);
    if (multiple_contacts) {
      model->potentialCollisions(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                                 bodyB_idx);
    } else if (active_group_names.size() > 0) {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_group_names);
    } else {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx);
    }
  }

  vector<int32_T> idxA(bodyA_idx.size());
  transform(bodyA_idx.begin(), bodyA_idx.end(), idxA.begin(),
            [](int i) { return ++i; });
  vector<int32_T> idxB(bodyB_idx.size());
  transform(bodyB_idx.begin(), bodyB_idx.end(), idxB.begin(),
            [](int i) { return ++i; });

  if (nlhs > 0) {
    plhs[0] = mxCreateDoubleMatrix(3, static_cast<int>(ptsA.cols()), mxREAL);
    memcpy(mxGetPrSafe(plhs[0]), ptsA.data(), sizeof(double) * 3 * ptsA.cols());
  }
  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleMatrix(3, static_cast<int>(ptsB.cols()), mxREAL);
    memcpy(mxGetPrSafe(plhs[1]), ptsB.data(), sizeof(double) * 3 * ptsB.cols());
  }
  if (nlhs > 2) {
    plhs[2] = mxCreateDoubleMatrix(3, static_cast<int>(normals.cols()), mxREAL);
    memcpy(mxGetPrSafe(plhs[2]), normals.data(),
           sizeof(double) * 3 * normals.cols());
  }
  if (nlhs > 3) {
    plhs[3] = mxCreateDoubleMatrix(1, static_cast<int>(dist.size()), mxREAL);
    memcpy(mxGetPrSafe(plhs[3]), dist.data(), sizeof(double) * dist.size());
  }
  if (nlhs > 4) {
    plhs[4] = mxCreateNumericMatrix(1, static_cast<int>(idxA.size()),
                                    mxINT32_CLASS, mxREAL);
    memcpy(mxGetData(plhs[4]), idxA.data(), sizeof(int32_T) * idxA.size());
  }
  if (nlhs > 5) {
    plhs[5] = mxCreateNumericMatrix(1, static_cast<int>(idxB.size()),
                                    mxINT32_CLASS, mxREAL);
    memcpy(mxGetData(plhs[5]), idxB.data(), sizeof(int32_T) * idxB.size());
  }
}
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs < 4) {
    mexErrMsgIdAndTxt("Drake:smoothDistancePenaltymex:NotEnoughInputs",
                      "Usage smoothDistancePenaltymex(model_ptr, cache_ptr, "
                      "min_distance, active_collision_options)");
  }

  int arg_num = 0;
  RigidBodyTree* model =
      static_cast<RigidBodyTree*>(getDrakeMexPointer(prhs[arg_num++]));
  KinematicsCache<double>& cache =
      fromMex(prhs[arg_num++], static_cast<KinematicsCache<double>*>(nullptr));

  // Get the minimum allowable distance
  double min_distance = mxGetScalar(prhs[arg_num++]);

  // Parse `active_collision_options`
  vector<int> active_bodies_idx;
  set<string> active_group_names;
  // First get the list of body indices for which to compute distances
  const mxArray* active_collision_options = prhs[arg_num++];
  const mxArray* body_idx = mxGetField(active_collision_options, 0, "body_idx");
  if (body_idx != NULL) {
    int n_active_bodies = static_cast<int>(mxGetNumberOfElements(body_idx));
    active_bodies_idx.resize(n_active_bodies);
    if (mxGetClassID(body_idx) == mxINT32_CLASS) {
      memcpy(active_bodies_idx.data(), (int*)mxGetData(body_idx),
             sizeof(int) * n_active_bodies);
    } else if (mxGetClassID(body_idx) == mxDOUBLE_CLASS) {
      double* ptr = mxGetPrSafe(body_idx);
      for (int i = 0; i < n_active_bodies; i++)
        active_bodies_idx[i] = static_cast<int>(ptr[i]);
    } else {
      mexErrMsgIdAndTxt("Drake:smoothDistancePenaltymex:WrongInputClass",
                        "active_collision_options.body_idx must be an int32 or "
                        "a double array");
    }
    transform(active_bodies_idx.begin(), active_bodies_idx.end(),
              active_bodies_idx.begin(), [](int i) { return --i; });
  }

  // Then get the group names for which to compute distances
  const mxArray* collision_groups =
      mxGetField(active_collision_options, 0, "collision_groups");
  if (collision_groups != NULL) {
    int num = static_cast<int>(mxGetNumberOfElements(collision_groups));
    for (int i = 0; i < num; i++) {
      const mxArray* ptr = mxGetCell(collision_groups, i);
      int buflen = static_cast<int>(mxGetN(ptr) * sizeof(mxChar)) + 1;
      char* str = (char*)mxMalloc(buflen);
      mxGetString(ptr, str, buflen);
      active_group_names.insert(str);
      mxFree(str);
    }
  }

  double penalty;
  vector<int> bodyA_idx, bodyB_idx;
  Matrix3Xd ptsA, ptsB, normals;
  MatrixXd dpenalty;
  VectorXd dist;
  if (active_bodies_idx.size() > 0) {
    if (active_group_names.size() > 0) {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_bodies_idx, active_group_names);
    } else {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_bodies_idx);
    }
  } else {
    if (active_group_names.size() > 0) {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_group_names);
    } else {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx);
    }
  }

  smoothDistancePenalty(penalty, dpenalty, model, cache, min_distance, dist,
                        normals, ptsA, ptsB, bodyA_idx, bodyB_idx);

  vector<int32_T> idxA(bodyA_idx.size());
  transform(bodyA_idx.begin(), bodyA_idx.end(), idxA.begin(),
            [](int i) { return ++i; });
  vector<int32_T> idxB(bodyB_idx.size());
  transform(bodyB_idx.begin(), bodyB_idx.end(), idxB.begin(),
            [](int i) { return ++i; });

  if (nlhs > 0) {
    plhs[0] = mxCreateDoubleScalar(penalty);
  }
  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleMatrix(static_cast<int>(dpenalty.rows()),
                                   static_cast<int>(dpenalty.cols()), mxREAL);
    memcpy(mxGetPrSafe(plhs[1]), dpenalty.data(),
           sizeof(double) * dpenalty.size());
  }
}