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[]) {
if (argc < 2) {
cerr << "Usage: urdfKinTest urdf_filename" << endl;
exit(-1);
}
RigidBodyTree* model = new RigidBodyTree(argv[1]);
cout << "=======" << endl;
// run kinematics with second derivatives 100 times
Eigen::VectorXd q = model->getZeroConfiguration();
Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
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, v);
// }
// const Vector4d zero(0,0,0,1);
Eigen::Vector3d zero = Eigen::Vector3d::Zero();
Eigen::Matrix<double, 6, 1> pt;
for (i = 0; i < model->bodies.size(); i++) {
// model->forwardKin(i,zero,1,pt);
auto pt = model->transformPoints(cache, zero, i, 0);
auto rpy = model->relativeRollPitchYaw(cache, i, 0);
Eigen::Matrix<double, 6, 1> x;
x << pt, rpy;
// cout << i << ": forward kin: " << model->bodies[i].linkname << " is at
// " << pt.transpose() << endl;
cout << model->bodies[i]->linkname << " ";
cout << x.transpose() << endl;
// for (int j=0; j<pt.size(); j++)
// cout << pt(j) << " ";
}
auto phi = model->positionConstraints<double>(cache);
cout << "phi = " << phi.transpose() << endl;
delete model;
return 0;
}
/**
* Use Frank's fastQP code (mexed)
* [q, info] = approximateIKEIQPmex(objgetMexModelPtr, q0, q_nom, Q, varargin)
* info = 0 on success, 1 on failure
**/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
if (nrhs < 4) {
mexErrMsgIdAndTxt("Drake:approximateIKmex:NotEnoughInputs",
"Usage approximateIKmex(model_ptr, q0, q_nom, Q,...)");
}
if (nlhs < 1) return;
// first get the model_ptr back from matlab
RigidBodyTree *model = (RigidBodyTree *)getDrakeMexPointer(prhs[0]);
int i, j, error, nq = model->num_positions;
static RigidBodyTree *lastModel = NULL;
static int lastNumJointLimits = 0;
int equality_ind = 0;
int inequality_ind = 0;
// Constraint preallocation
Matrix<double, -1, 1, 0, MAX_CONSTRS, 1> beq(MAX_CONSTRS);
Matrix<double, -1, 1, 0, MAX_CONSTRS, 1> bin(MAX_CONSTRS);
Matrix<double, -1, -1, RowMajor, MAX_CONSTRS, -1> Aeq(MAX_CONSTRS, nq);
Matrix<double, -1, -1, RowMajor, MAX_CONSTRS, -1> Ain(MAX_CONSTRS, nq);
// Add joint limits
if (lastModel != model || true) {
lastModel = model;
for (i = 0; i < nq; i++) {
if (!mxIsInf(model->joint_limit_max[i])) {
bin(inequality_ind) = model->joint_limit_max[i];
Ain.row(inequality_ind) = MatrixXd::Zero(1, nq);
Ain(inequality_ind, i) = 1;
// cout << bin(inequality_ind) << endl << endl;
inequality_ind++;
}
if (!mxIsInf(model->joint_limit_min[i])) {
bin(inequality_ind) = -model->joint_limit_min[i];
Ain.row(inequality_ind) << MatrixXd::Zero(1, nq);
Ain(inequality_ind, i) = -1;
// cout << bin(inequality_ind) << endl << endl;
inequality_ind++;
}
}
lastNumJointLimits = inequality_ind;
} else {
inequality_ind = lastNumJointLimits;
}
// cost: (q-q_nom)'*Q*(q-q_nom) \equiv q'*Q*q - 2*q_nom'*Q*q (const term
// doesn't matter)
Map<VectorXd> q_nom(mxGetPrSafe(prhs[2]), nq);
Map<MatrixXd> Q(mxGetPrSafe(prhs[3]), nq, nq);
VectorXd c = -Q * q_nom;
double *q0 = mxGetPrSafe(prhs[1]);
model->doKinematics(q0);
VectorXd q0vec = Map<VectorXd>(q0, nq);
i = 4;
while (i < nrhs) {
MatrixXd body_pos;
Map<MatrixXd> *world_pos = NULL;
int rows;
int body_ind = mxGetScalar(prhs[i]) - 1;
mxArray *min = NULL;
mxArray *max = NULL;
int n_pts;
if (body_ind == -1) {
int n_pts = mxGetN(prhs[i + 1]);
body_pos.resize(4, 1);
body_pos << 0, 0, 0, 1;
world_pos = new Map<MatrixXd>(mxGetPrSafe(prhs[i + 1]), 3, n_pts);
rows = 3;
i += 2;
} else {
if (mxIsClass(prhs[i + 2], "struct")) { // isstruct(worldpos)
min = mxGetField(prhs[i + 2], 0, "min");
max = mxGetField(prhs[i + 2], 0, "max");
if (min == NULL || max == NULL) {
mexErrMsgIdAndTxt(
"Drake:approximateIKMex:BadInputs",
"if world_pos is a struct, it must have fields .min and .max");
}
rows = mxGetM(min);
if (rows != 3 && rows != 6) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"world_pos.min must have 3 or 6 rows");
}
if (mxGetM(max) != rows) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"world_pos.max must have the same number of rows "
"as world_pos.min");
}
} else {
rows = mxGetM(prhs[i + 2]);
int n_pts = mxGetN(prhs[i + 2]);
world_pos = new Map<MatrixXd>(mxGetPrSafe(prhs[i + 2]), rows, n_pts);
}
if (mxIsClass(prhs[i + 1], "char") || mxGetM(prhs[i + 1]) == 1) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:NotImplemented",
"collision group not implemented in mex (mposa)");
} else {
if (mxGetM(prhs[i + 1]) != 3) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"bodypos must be 3xmi");
}
n_pts = mxGetN(prhs[i + 1]);
Map<MatrixXd> pts_tmp(mxGetPrSafe(prhs[i + 1]), 3, n_pts);
body_pos.resize(4, n_pts);
body_pos << pts_tmp, MatrixXd::Ones(1, n_pts);
}
i += 3;
}
MatrixXd x;
MatrixXd J;
if (body_ind == -1) {
x = VectorXd::Zero(3, 1);
Vector3d x_com;
model->getCOM(x_com);
model->getCOMJac(J);
x.resize(3, 1);
x << x_com;
} else {
J.resize(rows * n_pts, nq);
model->forwardKin(body_ind, body_pos, (int)rows == 6, x);
model->forwardJac(body_ind, body_pos, (int)rows == 6, J);
if (rows == 6 && min == NULL && max == NULL) {
VectorXd delta;
angleDiff(x.block(3, 0, 3, n_pts), (*world_pos).block(3, 0, 3, n_pts),
&delta);
(*world_pos).block(3, 0, 3, n_pts) = x.block(3, 0, 3, n_pts) + delta;
}
}
if (max != NULL) {
double *val = mxGetPrSafe(max);
// add inequality constraint
for (j = 0; j < rows; j++) {
if (!mxIsNaN(val[j])) {
VectorXd rowVec = J.row(j);
double rhs = val[j] - x(j) + J.row(j) * q0vec;
if (mxIsInf(rhs)) {
if (rhs < 0) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"RHS of a constraint evaluates to -inf");
}
} else {
Ain.row(inequality_ind) << J.row(j);
bin(inequality_ind) = rhs;
inequality_ind++;
}
}
}
}
if (min != NULL) {
double *val = mxGetPrSafe(min);
// add inequality constraint
for (j = 0; j < rows; j++) {
if (!mxIsNaN(val[j])) {
VectorXd rowVec = J.row(j);
double rhs = -(val[j] - x(j) + J.row(j) * q0vec);
if (mxIsInf(rhs)) {
if (rhs < 0) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"RHS of a constraint evaluates to -inf");
}
} else {
Ain.row(inequality_ind) << J.row(j);
bin(inequality_ind) = rhs;
inequality_ind++;
}
}
}
}
if (min == NULL && max == NULL) {
for (j = 0; j < rows; j++) {
if (!mxIsNaN((*world_pos)(j))) {
double rhs = (*world_pos)(j)-x(j) + J.row(j) * q0vec;
Aeq.row(equality_ind) << J.row(j);
beq(equality_ind) = rhs;
equality_ind++;
}
}
}
delete world_pos;
if (min) mxDestroyArray(min);
if (max) mxDestroyArray(max);
}
Aeq.conservativeResize(equality_ind, nq);
Ain.conservativeResize(inequality_ind, nq);
beq.conservativeResize(equality_ind);
bin.conservativeResize(inequality_ind);
cout << "c is " << c.rows() << endl;
cout << "Aeq is " << Aeq.rows() << " by " << Aeq.cols() << endl;
VectorXd q = model->getZeroConfiguration();
// double result = solve_quadprog(Q, c, -Aeq, beq, -Ain, bin, q);
VectorXd Qdiag = Q.diagonal();
vector<MatrixBase<VectorXd> > blkQ;
blkQ.push_back(Qdiag);
set<int> active;
double result = fastQP(blkQ, c, Aeq, beq, Ain, bin, active, q);
// Enable the block below to use stephens' qp code
#if USE_EIQUADPROG_BACKUP
if (result == 1) {
Q += 1e-8 * MatrixXd::Identity(nq, nq);
result =
solve_quadprog(Q, c, -Aeq.transpose(), beq, -Ain.transpose(), bin, q);
if (mxIsInf(result)) {
result = 1;
} else {
result = 0;
}
}
#endif
plhs[0] = mxCreateDoubleMatrix(nq, 1, mxREAL);
memcpy(mxGetPrSafe(plhs[0]), q.data(), sizeof(double) * nq);
if (nlhs > 1) {
plhs[1] = mxCreateDoubleScalar(result);
}
return;
}
