void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs < 2 || nlhs < 2) {
    mexErrMsgIdAndTxt(
        "Drake:jointLimitConstraintsmex:InvalidCall",
        "Usage: [phi, J] = jointLimitConstraintsmex(mex_model_ptr, q) ");
  }

  RigidBodyTree *model = (RigidBodyTree *)getDrakeMexPointer(prhs[0]);

  if (mxGetNumberOfElements(prhs[1]) != model->number_of_positions()) {
    mexErrMsgIdAndTxt(
        "Drake:jointLimitConstraintsmex:InvalidPositionVectorLength",
        "q contains the wrong number of elements");
  }

  Map<VectorXd> q(mxGetPrSafe(prhs[1]), model->number_of_positions());

  size_t numJointConstraints = model->getNumJointLimitConstraints();

  plhs[0] = mxCreateDoubleMatrix(numJointConstraints, 1, mxREAL);
  plhs[1] =
      mxCreateDoubleMatrix(numJointConstraints, model->number_of_positions(),
        mxREAL);

  Map<VectorXd> phi(mxGetPrSafe(plhs[0]), numJointConstraints);
  Map<MatrixXd> J(mxGetPrSafe(plhs[1]), numJointConstraints,
                  model->number_of_positions());

  model->jointLimitConstraints(q, phi, J);
}
Beispiel #2
0
//[z, Mvn, wvn] = setupLCPmex(mex_model_ptr, cache_ptr, u, phiC, n, D, h, z_inactive_guess_tol)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { 
  
  if (nlhs != 5 || nrhs != 13) {
    mexErrMsgIdAndTxt("Drake:setupLCPmex:InvalidUsage","Usage: [z, Mvn, wvn, zqp] = setupLCPmex(mex_model_ptr, cache_ptr, u, phiC, n, D, h, z_inactive_guess_tol, z_cached, H, C, B)");
  }
  static unique_ptr<MexWrapper> lcp_mex = unique_ptr<MexWrapper>(new MexWrapper(PATHLCP_MEXFILE));

  int arg_num = 0;
  RigidBodyTree *model = static_cast<RigidBodyTree *>(getDrakeMexPointer(prhs[arg_num++]));
  KinematicsCache<double>& cache = fromMex(prhs[arg_num++], static_cast<KinematicsCache<double>*>(nullptr));
  cache.checkCachedKinematicsSettings(true, true, "solveLCPmex");

  const int nq = model->num_positions;
  const int nv = model->num_velocities;
  
  //input mappings
  const mxArray* u_array = prhs[arg_num++];
  const mxArray* phiC_array = prhs[arg_num++];
  const mxArray* n_array = prhs[arg_num++];
  const mxArray* D_array = prhs[arg_num++];
  const mxArray* h_array = prhs[arg_num++];
  const mxArray* inactive_guess_array = prhs[arg_num++];
  const mxArray* z_cached_array = prhs[arg_num++];
  const mxArray* H_array = prhs[arg_num++];
  const mxArray* C_array = prhs[arg_num++];
  const mxArray* B_array = prhs[arg_num++];
  const mxArray* enable_fastqp_array = prhs[arg_num++];

  const size_t num_z_cached = mxGetNumberOfElements(z_cached_array);
  const size_t num_contact_pairs = mxGetNumberOfElements(phiC_array);
  const size_t mC = mxGetNumberOfElements(D_array);
  const double z_inactive_guess_tol = mxGetScalar(inactive_guess_array);
  const double h = mxGetScalar(h_array);
  const auto& q = cache.getQ();
  const auto& v = cache.getV();
  const Map<VectorXd> u(mxGetPrSafe(u_array), mxGetNumberOfElements(u_array));
  const Map<VectorXd> phiC(mxGetPrSafe(phiC_array), num_contact_pairs);
  const Map<MatrixXd> n(mxGetPrSafe(n_array), num_contact_pairs, nq);
  const Map<VectorXd> z_cached(mxGetPrSafe(z_cached_array), num_z_cached);
  const Map<MatrixXd> H(mxGetPrSafe(H_array), nv, nv);
  const Map<VectorXd> C(mxGetPrSafe(C_array), nv);
  const Map<MatrixXd> B(mxGetPrSafe(B_array), mxGetM(B_array), mxGetN(B_array));
 
  const bool enable_fastqp = mxIsLogicalScalarTrue(enable_fastqp_array);

  VectorXd phiL, phiL_possible, phiC_possible, phiL_check, phiC_check;
  MatrixXd JL, JL_possible, n_possible, JL_check, n_check;

  auto phiP = model->positionConstraints(cache);
  auto JP = model->positionConstraintsJacobian(cache);
  model->jointLimitConstraints(q, phiL, JL);  
  
  const size_t nP = phiP.size();
  
  //Convert jacobians to velocity mappings
  const MatrixXd n_velocity = cache.transformPositionDotMappingToVelocityMapping(n);
  const MatrixXd JL_velocity = cache.transformPositionDotMappingToVelocityMapping(JL);
  const auto JP_velocity = cache.transformPositionDotMappingToVelocityMapping(JP);
  
  plhs[2] = mxCreateDoubleMatrix(nv, 1, mxREAL);
  Map<VectorXd> wvn(mxGetPrSafe(plhs[2]), nv);

  LLT<MatrixXd> H_cholesky(H); // compute the Cholesky decomposition of H
  wvn = H_cholesky.solve(B * u - C);
  wvn *= h;
  wvn += v;

  //use forward euler step in joint space as
  //initial guess for active constraints
  vector<bool> possible_contact;
  vector<bool> possible_jointlimit;
  vector<bool> z_inactive;

  getThresholdInclusion((phiC + h * n_velocity * v).eval(), z_inactive_guess_tol, possible_contact);
  getThresholdInclusion((phiL + h * JL_velocity * v).eval(), z_inactive_guess_tol, possible_jointlimit);

  while (true) {
  //continue from here if our inactive guess fails
    const size_t nC = getNumTrue(possible_contact);
    const size_t nL = getNumTrue(possible_jointlimit);
    const size_t lcp_size = nL + nP + (mC + 2) * nC;

    plhs[0] = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(nv, lcp_size, mxREAL);

    Map<VectorXd> z(mxGetPrSafe(plhs[0]), lcp_size);
    Map<MatrixXd> Mvn(mxGetPrSafe(plhs[1]), nv, lcp_size);

    if (lcp_size == 0) {
      plhs[3] = mxCreateDoubleMatrix(1, possible_contact.size(), mxREAL);
      plhs[4] = mxCreateDoubleMatrix(1, possible_jointlimit.size(), mxREAL);
      return;
    } 
    z = VectorXd::Zero(lcp_size);

    vector<size_t> possible_contact_indices;
    getInclusionIndices(possible_contact, possible_contact_indices, true);

    partitionVector(possible_contact, phiC, phiC_possible, phiC_check);
    partitionVector(possible_jointlimit, phiL, phiL_possible, phiL_check);
    partitionMatrix(possible_contact, n_velocity, n_possible, n_check);
    partitionMatrix(possible_jointlimit, JL_velocity, JL_possible, JL_check);
    
    MatrixXd D_possible(mC * nC, nv);
    for (size_t i = 0; i < mC ; i++) {
      Map<MatrixXd> D_i(mxGetPrSafe(mxGetCell(D_array, i)), num_contact_pairs , nq);
      MatrixXd D_i_possible, D_i_exclude;
      filterByIndices(possible_contact_indices, D_i, D_i_possible);
      D_possible.block(nC * i, 0, nC, nv) = cache.transformPositionDotMappingToVelocityMapping(D_i_possible);
    }

    // J in velocity coordinates
    MatrixXd J(lcp_size, nv);
    J << JL_possible, JP_velocity, n_possible, D_possible, MatrixXd::Zero(nC, nv);
    Mvn = H_cholesky.solve(J.transpose());
    
    //solve LCP problem 
    //TODO: call path from C++ (currently only 32-bit C libraries available)
    mxArray* mxw = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    mxArray* mxlb = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    mxArray* mxub = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);  
    
    Map<VectorXd> lb(mxGetPrSafe(mxlb), lcp_size);
    Map<VectorXd> ub(mxGetPrSafe(mxub), lcp_size);
    lb << VectorXd::Zero(nL),
          -BIG * VectorXd::Ones(nP),
          VectorXd::Zero(nC + mC * nC + nC);
    ub = BIG * VectorXd::Ones(lcp_size);

    MatrixXd M(lcp_size, lcp_size);
    Map<VectorXd> w(mxGetPrSafe(mxw), lcp_size);

    //build LCP matrix
    M << h * JL_possible * Mvn,
         h * JP_velocity * Mvn,
         h * n_possible * Mvn,
         D_possible * Mvn,
         MatrixXd::Zero(nC, lcp_size);


    if (nC > 0) {
      for (size_t i = 0; i < mC ; i++) {
        M.block(nL + nP + nC + nC * i, nL + nP + nC + mC * nC, nC, nC) = MatrixXd::Identity(nC, nC);
        M.block(nL + nP + nC + mC*nC, nL + nP + nC + nC * i, nC, nC) = -MatrixXd::Identity(nC, nC);
      }
      double mu = 1.0; //TODO: pull this from contactConstraints
      M.block(nL + nP + nC + mC * nC, nL + nP, nC, nC) = mu * MatrixXd::Identity(nC, nC);
    }

    //build LCP vector
    w << phiL_possible + h *  JL_possible * wvn,
         phiP + h *  JP_velocity * wvn,
         phiC_possible + h * n_possible * wvn,
         D_possible * wvn,
         VectorXd::Zero(nC);

    //try fastQP first
    bool qp_failed = true;
    
    if (enable_fastqp) {
      if (num_z_cached != lcp_size) {
        z_inactive.clear();
        for (int i = 0; i < lcp_size; i++) {
          z_inactive.push_back(true);
        }
      } else {
        getThresholdInclusion((lb - z_cached).eval(), -SMALL, z_inactive);
      }
      qp_failed = !callFastQP(M, w, lb, z_inactive, nL+nP+nC, z);
    }
    
    int nnz;
    mxArray* mxM_sparse = eigenToMatlabSparse(M, nnz);
    mxArray* mxnnzJ = mxCreateDoubleScalar(static_cast<double>(nnz));
    mxArray* mxn = mxCreateDoubleScalar(static_cast<double>(lcp_size));
    mxArray* mxz = mxCreateDoubleMatrix(lcp_size, 1, mxREAL);
    mxArray *lhs[2];
    mxArray *rhs[] = {mxn, mxnnzJ, mxz, mxlb, mxub, mxM_sparse, mxw};

    Map<VectorXd> z_path(mxGetPr(mxz), lcp_size);
    z_path = VectorXd::Zero(lcp_size);
    //fall back to pathlcp
    if(qp_failed) {
      lcp_mex->mexFunction(2, lhs, 7, const_cast<const mxArray**>(rhs));
      z = z_path;
      mxDestroyArray(lhs[0]);
      mxDestroyArray(lhs[1]);
    }

    mxDestroyArray(mxz);
    mxDestroyArray(mxn);
    mxDestroyArray(mxnnzJ);
    mxDestroyArray(mxub);
    mxDestroyArray(mxlb);
    mxDestroyArray(mxw);
    mxDestroyArray(mxM_sparse);

    VectorXd vn = Mvn * z + wvn;

    vector<size_t> impossible_contact_indices, impossible_limit_indices;
    getInclusionIndices(possible_contact, impossible_contact_indices, false);
    getInclusionIndices(possible_jointlimit, impossible_limit_indices, false);

    vector<bool> penetrating_joints, penetrating_contacts;
    getThresholdInclusion((phiL_check + h * JL_check * vn).eval(), 0.0, penetrating_joints);
    getThresholdInclusion((phiC_check + h * n_check * vn).eval(), 0.0, penetrating_contacts);

    const size_t num_penetrating_joints = getNumTrue(penetrating_joints);
    const size_t num_penetrating_contacts = getNumTrue(penetrating_contacts);
    const size_t penetrations = num_penetrating_joints + num_penetrating_contacts;
    //check nonpenetration assumptions
    if (penetrations > 0) {
      //revise joint limit active set
      for (size_t i = 0; i < impossible_limit_indices.size(); i++) {
        if (penetrating_joints[i]) {
          possible_jointlimit[impossible_limit_indices[i]] = true;
        }
      }

      //revise contact constraint active set
      for (size_t i = 0; i < impossible_contact_indices.size(); i++) {
        if (penetrating_contacts[i]) {
          possible_contact[impossible_contact_indices[i]] = true;
        }
      }
      //throw away our old solution and try again
      mxDestroyArray(plhs[0]);
      mxDestroyArray(plhs[1]);
      continue;
    }
    //our initial guess was correct. we're done
    break;
  }

  plhs[3] = mxCreateDoubleMatrix(1, possible_contact.size(), mxREAL);
  plhs[4] = mxCreateDoubleMatrix(1, possible_jointlimit.size(), mxREAL);

  Map<VectorXd> possible_contact_map(mxGetPrSafe(plhs[3]), possible_contact.size());
  Map<VectorXd> possible_jointlimit_map(mxGetPrSafe(plhs[4]), possible_jointlimit.size());

  for (size_t i = 0; i < possible_contact.size(); i++) {
    possible_contact_map[i] = static_cast<double>(possible_contact[i]);
  }

  for (size_t i = 0; i < possible_jointlimit.size(); i++) {
    possible_jointlimit_map[i] = static_cast<double>(possible_jointlimit[i]);
  }

}