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