void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs != 5) {
mexErrMsgIdAndTxt("Drake:doKinematicsmex:NotEnoughInputs", "Usage doKinematicsmex(model_ptr,q,compute_gradients,v,compute_JdotV)");
}
// first get the model_ptr back from matlab
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);
Map<VectorXd> q(mxGetPrSafe(prhs[1]), mxGetNumberOfElements(prhs[1]));
if (q.rows() != model->num_positions)
mexErrMsgIdAndTxt("Drake:doKinematicsmex:BadInputs", "q must be size %d x 1", model->num_positions);
bool compute_gradients = (bool) (mxGetLogicals(prhs[2]))[0];
bool compute_Jdotv = (bool) (mxGetLogicals(prhs[4]))[0];
if (mxGetNumberOfElements(prhs[3]) > 0) {
auto v = Map<VectorXd>(mxGetPrSafe(prhs[3]), mxGetNumberOfElements(prhs[3]));
if (v.rows() != model->num_velocities)
mexErrMsgIdAndTxt("Drake:doKinematicsmex:BadInputs", "v must be size %d x 1", model->num_velocities);
model->doKinematicsNew(q, v, compute_gradients, compute_Jdotv);
}
else {
Map<VectorXd> v(nullptr, 0, 1);
model->doKinematicsNew(q, v, compute_gradients, compute_Jdotv);
}
}
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;
model->doKinematicsNew(q,VectorXd::Zero(model->num_velocities).eval());
// }
VectorXd phi;
MatrixXd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
model->collisionDetect(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;
}
