DLL_EXPORT_SYM
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->get_num_positions()) {
    mexErrMsgIdAndTxt(
        "Drake:jointLimitConstraintsmex:InvalidPositionVectorLength",
        "q contains the wrong number of elements");
  }

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

  size_t numJointConstraints = model->getNumJointLimitConstraints();

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

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

  model->jointLimitConstraints(q, phi, J);
}
Esempio n. 2
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    if (nrhs < 2 || nlhs < 2) {
        mexErrMsgIdAndTxt(
            "Drake:positionConstraintsmex:InvalidCall",
            "Usage: [phi, J] = positionConstraintsmex(mex_model_ptr, q) ");
    }

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

    const size_t nq = model->get_num_positions();

    if (mxGetNumberOfElements(prhs[1]) != nq) {
        mexErrMsgIdAndTxt(
            "Drake:positionConstraintsmex:InvalidPositionVectorLength",
            "q contains the wrong number of elements");
    }

    Map<VectorXd> q(mxGetPrSafe(prhs[1]), nq);

    KinematicsCache<double> cache =
        model->doKinematics(q);  // FIXME: KinematicsCache should be passed in!

    auto phi = model->positionConstraints(cache);
    plhs[0] = eigenToMatlab(phi);

    auto dphi = model->positionConstraintsJacobian(cache);
    plhs[1] = eigenToMatlab(dphi);
}
Esempio n. 3
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs < 5) {
    mexErrMsgIdAndTxt("Drake:inverseKinmex:NotEnoughInputs",
                      "Usage "
                      "inverseKinmex(model_ptr, q_seed, q_nom, constraint1,"
                      "constraint2,..., ikoptions");
  }
  RigidBodyTree* model = (RigidBodyTree*)getDrakeMexPointer(prhs[0]);
  int nq = model->get_num_positions();
  Map<VectorXd> q_seed(mxGetPrSafe(prhs[1]), nq);
  Map<VectorXd> q_nom(mxGetPrSafe(prhs[2]), nq);
  int num_constraints = nrhs - 4;
  RigidBodyConstraint** constraint_array =
      new RigidBodyConstraint* [num_constraints];
  for (int i = 0; i < num_constraints; i++) {
    constraint_array[i] = (RigidBodyConstraint*)getDrakeMexPointer(prhs[3 + i]);
  }
  IKoptions* ikoptions = (IKoptions*)getDrakeMexPointer(prhs[nrhs - 1]);
  plhs[0] = mxCreateDoubleMatrix(nq, 1, mxREAL);
  Map<VectorXd> q_sol(mxGetPrSafe(plhs[0]), nq);
  int info;
  vector<string> infeasible_constraint;
  inverseKin(model, q_seed, q_nom, num_constraints, constraint_array,
             *ikoptions, &q_sol, &info, &infeasible_constraint);
  plhs[1] = mxCreateDoubleScalar((double)info);
  mwSize name_dim[1] = {static_cast<mwSize>(infeasible_constraint.size())};
  plhs[2] = mxCreateCellArray(1, name_dim);
  for (int i = 0; i < infeasible_constraint.size(); i++) {
    mxArray* name_ptr = mxCreateString(infeasible_constraint[i].c_str());
    mxSetCell(plhs[2], i, name_ptr);
  }
  delete[] constraint_array;
}
Esempio n. 4
0
int contactConstraintsBV(
    const RigidBodyTree<double>& r,
    const KinematicsCache<double>& cache, int nc,
    std::vector<double> support_mus,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    drake::eigen_aligned_std_vector<SupportStateElement>& supp, MatrixXd& B,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv, MatrixXd& normals) {
  int j, k = 0, nq = r.get_num_positions();

  B.resize(3, nc * 2 * m_surface_tangents);
  JB.resize(nq, nc * 2 * m_surface_tangents);
  Jp.resize(3 * nc, nq);
  Jpdotv.resize(3 * nc);
  normals.resize(3, nc);

  Vector3d contact_pos, pos, normal;
  MatrixXd J(3, nq);
  Matrix<double, 3, m_surface_tangents> d;

  for (auto iter = supp.begin(); iter != supp.end(); iter++) {
    double mu = support_mus[iter - supp.begin()];
    double norm = sqrt(1 + mu * mu);  // because normals and ds are orthogonal,
                                      // the norm has a simple form
    if (nc > 0) {
      for (auto pt_iter = iter->contact_pts.begin();
           pt_iter != iter->contact_pts.end(); pt_iter++) {
        contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
        J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);

        normal = iter->support_surface.head(3);
        surfaceTangents(normal, d);
        for (j = 0; j < m_surface_tangents; j++) {
          B.col(2 * k * m_surface_tangents + j) =
              (normal + mu * d.col(j)) / norm;
          B.col((2 * k + 1) * m_surface_tangents + j) =
              (normal - mu * d.col(j)) / norm;

          JB.col(2 * k * m_surface_tangents + j) =
              J.transpose() * B.col(2 * k * m_surface_tangents + j);
          JB.col((2 * k + 1) * m_surface_tangents + j) =
              J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
        }

        // store away kin sols into Jp and Jpdotv
        // NOTE: I'm cheating and using a slightly different ordering of J and
        // Jdot here
        Jp.block(3 * k, 0, 3, nq) = J;
        Vector3d pt = (*pt_iter).head(3);
        Jpdotv.block(3 * k, 0, 3, 1) =
            r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
        normals.col(k) = normal;

        k++;
      }
    }
  }

  return k;
}
Esempio n. 5
0
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs < 6) {
    mexErrMsgIdAndTxt("Drake:inverseKinPointwisemex:NotEnoughInputs",
                      "Usage "
                      "inverseKinPointwisemex(model_ptr, t, q_seed, q_nom,"
                      "constraint1, constraint2,..., ikoptions");
  }
  RigidBodyTree* model = (RigidBodyTree*)getDrakeMexPointer(prhs[0]);
  int nq = model->get_num_positions();
  int nT = static_cast<int>(mxGetNumberOfElements(prhs[1]));
  double* t = mxGetPrSafe(prhs[1]);
  Map<MatrixXd> q_seed(mxGetPrSafe(prhs[2]), nq, nT);
  Map<MatrixXd> q_nom(mxGetPrSafe(prhs[3]), nq, nT);
  int num_constraints = nrhs - 5;
  RigidBodyConstraint** constraint_array =
      new RigidBodyConstraint* [num_constraints];
  for (int i = 0; i < num_constraints; i++) {
    constraint_array[i] = (RigidBodyConstraint*)getDrakeMexPointer(prhs[4 + i]);
  }
  IKoptions* ikoptions = (IKoptions*)getDrakeMexPointer(prhs[nrhs - 1]);
  plhs[0] = mxCreateDoubleMatrix(nq, nT, mxREAL);
  Map<MatrixXd> q_sol(mxGetPrSafe(plhs[0]), nq, nT);
  int* info = new int[nT];
  vector<string> infeasible_constraint;
  inverseKinPointwise(model, nT, t, q_seed, q_nom, num_constraints,
                      constraint_array, *ikoptions,
                      &q_sol, info, &infeasible_constraint);

  plhs[1] = mxCreateDoubleMatrix(1, nT, mxREAL);
  for (int i = 0; i < nT; i++) {
    *(mxGetPrSafe(plhs[1]) + i) = (double)info[i];
  }
  mwSize name_dim[1] = {static_cast<mwSize>(infeasible_constraint.size())};
  plhs[2] = mxCreateCellArray(1, name_dim);
  for (int i = 0; i < infeasible_constraint.size(); i++) {
    mxArray* name_ptr = mxCreateString(infeasible_constraint[i].c_str());
    mxSetCell(plhs[2], i, name_ptr);
  }
  delete[] info;
  delete[] constraint_array;
}
Esempio n. 6
0
// [z, Mvn, wvn] = setupLCPmex(mex_model_ptr, cache_ptr, u, phiC, n, D, h,
//   z_inactive_guess_tol)
DLL_EXPORT_SYM
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<double>* model =
      static_cast<RigidBodyTree<double>*>(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->get_num_positions();
  const int nv = model->get_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(psiorx): 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(psiorx): 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 (size_t 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]);
  }
}