/**
* 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;
}
int main()
{
RigidBodyTree rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
RigidBodyTree * model = &rbm;
if(!model)
{
cerr<<"ERROR: Failed to load model"<<endl;
}
Vector2d tspan;
tspan<<0,1;
int l_hand;
int r_hand;
//int l_foot;
//int r_foot;
for(int i = 0;i<model->bodies.size();i++)
{
if(model->bodies[i]->linkname.compare(string("l_hand")))
{
l_hand = i;
}
else if(model->bodies[i]->linkname.compare(string("r_hand")))
{
r_hand = i;
}
//else if(model->bodies[i].linkname.compare(string("l_foot")))
//{
// l_foot = i;
//}
//else if(model->bodies[i].linkname.compare(string("r_foot")))
//{
// r_foot = i;
//}
}
int nq = model->num_positions;
VectorXd qstar = VectorXd::Zero(nq);
qstar(3) = 0.8;
KinematicsCache<double> cache = model->doKinematics(qstar);
Vector3d com0 = model->centerOfMass(cache);
Vector3d r_hand_pt = Vector3d::Zero();
Vector3d rhand_pos0 = model->forwardKin(cache, r_hand_pt, r_hand, 0, 0);
int nT = 4;
double* t = new double[nT];
double dt = 1.0/(nT-1);
for(int i = 0;i<nT;i++)
{
t[i] = dt*i;
}
MatrixXd q0 = qstar.replicate(1,nT);
VectorXd qdot0 = VectorXd::Zero(model->num_velocities);
Vector3d com_lb = com0;
com_lb(0) = std::numeric_limits<double>::quiet_NaN();
com_lb(1) = std::numeric_limits<double>::quiet_NaN();
Vector3d com_ub = com0;
com_ub(0) = std::numeric_limits<double>::quiet_NaN();
com_ub(1) = std::numeric_limits<double>::quiet_NaN();
com_ub(2) = com0(2)+0.5;
WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
Vector3d rhand_pos_lb = rhand_pos0;
rhand_pos_lb(0) +=0.1;
rhand_pos_lb(1) +=0.05;
rhand_pos_lb(2) +=0.25;
Vector3d rhand_pos_ub = rhand_pos_lb;
rhand_pos_ub(2) += 0.25;
Vector2d tspan_end;
tspan_end<<t[nT-1],t[nT-1];
WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
int num_constraints = 2;
RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
constraint_array[0] = com_kc;
constraint_array[1] = kc_rhand;
IKoptions ikoptions(model);
MatrixXd q_sol(model->num_positions,nT);
MatrixXd qdot_sol(model->num_velocities,nT);
MatrixXd qddot_sol(model->num_positions,nT);
int info = 0;
vector<string> infeasible_constraint;
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
ikoptions.setFixInitialState(false);
ikoptions.setMajorIterationsLimit(500);
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
RowVectorXd t_inbetween(5);
t_inbetween << 0.1,0.15,0.3,0.4,0.6;
ikoptions.setAdditionaltSamples(t_inbetween);
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
delete com_kc;
delete[] constraint_array;
delete[] t;
return 0;
}
