/**
 * 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;
}
Exemple #2
0
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;
}