int main(int argc, char* argv[])
{
if (argc<2) {
cerr << "Usage: urdfCollisionTest urdf_filename" << endl;
exit(-1);
}
RigidBodyManipulator* model = new RigidBodyManipulator(argv[1]);
if (!model) {
cerr << "ERROR: Failed to load model from " << argv[1] << endl;
return -1;
}
// run kinematics with second derivatives 100 times
VectorXd q = VectorXd::Zero(model->num_positions);
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, 0);
// }
VectorXd phi;
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;
}
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;
RigidBodyManipulator *model = static_cast<RigidBodyManipulator*>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double>* cache = static_cast<KinematicsCache<double>*>(getDrakeMexPointer(prhs[arg_num++]));
// 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());
}
}
