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
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 *model = (RigidBodyTree *)getDrakeMexPointer(prhs[0]);

  const size_t nq = model->number_of_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);
}
Beispiel #3
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->number_of_velocities());
  int i;

  if (argc >= 2 + model->number_of_positions()) {
    for (i = 0; i < model->number_of_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 < static_cast<int>(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].name_ << " is at
    //    " << pt.transpose() << endl;
    cout << model->bodies[i]->name_ << " ";
    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;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
    if (nrhs != 3 && nrhs != 4) {
        mexErrMsgIdAndTxt("Drake:testQuasiStaticConstraintmex:BadInputs",
                          "Usage [active, num_weights, c, dc] = "
                          "testQuasiStaticConstraintmex(qsc_ptr, q, weights, t)");
    }
    double t;
    double* t_ptr;
    if (nrhs == 4 && mxGetNumberOfElements(prhs[3]) == 1) {
        t = mxGetScalar(prhs[3]);
        t_ptr = &t;
    } else {
        t_ptr = nullptr;
    }
    QuasiStaticConstraint* qsc =
        (QuasiStaticConstraint*)getDrakeMexPointer(prhs[0]);
    bool active = qsc->isActive();
    RigidBodyTree* model = qsc->getRobotPointer();
    int nq = model->number_of_positions();
    Map<VectorXd> q(mxGetPrSafe(prhs[1]), nq);
    int num_weights = qsc->getNumWeights();
    double* weights = new double[num_weights];
    memcpy(weights, mxGetPrSafe(prhs[2]), sizeof(double) * num_weights);
    int num_qsc_cnst = qsc->getNumConstraint(t_ptr);
    VectorXd c(num_qsc_cnst - 1);
    MatrixXd dc = MatrixXd::Zero(num_qsc_cnst - 1, nq + num_weights);
    KinematicsCache<double> cache = model->doKinematics(q);
    qsc->eval(t_ptr, cache, weights, c, dc);
    VectorXd lb, ub;
    lb.resize(num_qsc_cnst - 1);
    ub.resize(num_qsc_cnst - 1);
    qsc->bounds(t_ptr, lb, ub);
    plhs[0] = mxCreateLogicalScalar(active);
    plhs[1] = mxCreateDoubleScalar((double)num_weights);
    plhs[2] = mxCreateDoubleMatrix(num_qsc_cnst - 1, 1, mxREAL);
    memcpy(mxGetPrSafe(plhs[2]), c.data(), sizeof(double) * (num_qsc_cnst - 1));
    plhs[3] = mxCreateDoubleMatrix(num_qsc_cnst - 1, nq + num_weights, mxREAL);
    memcpy(mxGetPrSafe(plhs[3]), dc.data(), sizeof(double) * dc.size());
    plhs[4] = mxCreateDoubleMatrix(num_qsc_cnst - 1, 1, mxREAL);
    memcpy(mxGetPrSafe(plhs[4]), lb.data(), sizeof(double) * (num_qsc_cnst - 1));
    plhs[5] = mxCreateDoubleMatrix(num_qsc_cnst - 1, 1, mxREAL);
    memcpy(mxGetPrSafe(plhs[5]), ub.data(), sizeof(double) * (num_qsc_cnst - 1));
    delete[] weights;
}
int main(int argc, char* argv[]) {
  RigidBodyTree tree;

  for (int i = 0; i < 10; ++i) {
    drake::parsers::urdf::AddModelInstanceFromURDF(
        drake::GetDrakePath() + "/systems/plants/test/PointMass.urdf",
            DrakeJoint::ROLLPITCHYAW, &tree);
  }

  drake::parsers::urdf::AddModelInstanceFromURDF(
      drake::GetDrakePath() + "/systems/plants/test/FallingBrick.urdf",
          DrakeJoint::FIXED, &tree);

  VectorXd q = VectorXd::Random(tree.number_of_positions());
  VectorXd v = VectorXd::Random(tree.number_of_velocities());
  auto kinsol = tree.doKinematics(q, v);

  VectorXd phi;
  Matrix3Xd normal, xA, xB;
  std::vector<int> bodyA_idx, bodyB_idx;
  tree.collisionDetect(kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx,
                       false);
}
Beispiel #6
0
int contactConstraintsBV(
    const RigidBodyTree &r, const KinematicsCache<double> &cache, int nc,
    std::vector<double> support_mus,
    std::vector<SupportStateElement,
                Eigen::aligned_allocator<SupportStateElement>> &supp,
    MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, VectorXd &Jpdotv,
    MatrixXd &normals) {
  int j, k = 0, nq = r.number_of_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 (std::vector<SupportStateElement,
                   Eigen::aligned_allocator<SupportStateElement>>::iterator
           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;
}
void testScenario1(const RigidBodyTree& model) {
  int ntests = 1000;

  vector<VectorXd> qs_double;
  vector<Matrix<AutoDiffFixedMaxSize, Dynamic, 1>> qs_autodiff_fixed;
  vector<Matrix<AutoDiffDynamicSize, Dynamic, 1>> qs_autodiff_dynamic;
  default_random_engine generator;

  for (int i = 0; i < ntests; i++) {
    auto q = model.getRandomConfiguration(generator);
    qs_double.push_back(q);

    MatrixXd grad = MatrixXd::Identity(model.number_of_positions(),
                                       model.number_of_positions());

    auto q_autodiff_fixed = q.cast<AutoDiffFixedMaxSize>().eval();
    gradientMatrixToAutoDiff(grad, q_autodiff_fixed);
    qs_autodiff_fixed.push_back(q_autodiff_fixed);

    auto q_autodiff_dynamic = q.cast<AutoDiffDynamicSize>().eval();
    gradientMatrixToAutoDiff(grad, q_autodiff_dynamic);
    qs_autodiff_dynamic.push_back(q_autodiff_dynamic);
  }

  map<int, Matrix3Xd> body_fixed_points;
  int npoints_feet = 4;
  int npoints_hands = 1;
  int npoints_head = 1;
  vector<string> sides{"l", "r"};
  for (const auto& side : sides) {
    int hand_id = model.FindBodyIndex(side + "_hand");
    body_fixed_points.insert(
        make_pair(hand_id, Matrix3Xd::Random(3, npoints_hands)));

    int foot_id = model.FindBodyIndex(side + "_foot");
    body_fixed_points.insert(
        make_pair(foot_id, Matrix3Xd::Random(3, npoints_feet)));
  }
  int head_id = model.FindBodyIndex("head");
  body_fixed_points.insert(
      make_pair(head_id, Matrix3Xd::Random(3, npoints_head)));

  KinematicsCache<double> cache_double(model.bodies);
  KinematicsCache<AutoDiffFixedMaxSize> cache_autodiff_fixed(model.bodies);
  KinematicsCache<AutoDiffDynamicSize> cache_autodiff_dynamic(model.bodies);

  cout << "scenario 1:" << endl;
  cout << "no gradients: "
       << measure<>::execution(scenario1<double>, model, cache_double,
                               qs_double, body_fixed_points) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff fixed max size: "
       << measure<>::execution(scenario1<AutoDiffFixedMaxSize>, model,
                               cache_autodiff_fixed, qs_autodiff_fixed,
                               body_fixed_points) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff dynamic size: "
       << measure<>::execution(scenario1<AutoDiffDynamicSize>, model,
                               cache_autodiff_dynamic, qs_autodiff_dynamic,
                               body_fixed_points) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << endl;
}
void testScenario2(const RigidBodyTree& model) {
  int ntests = 1000;

  vector<pair<VectorXd, VectorXd>> states_double;
  vector<pair<Matrix<AutoDiffFixedMaxSize, Dynamic, 1>,
              Matrix<AutoDiffFixedMaxSize, Dynamic, 1>>>
      states_autodiff_fixed;
  vector<pair<Matrix<AutoDiffDynamicSize, Dynamic, 1>,
              Matrix<AutoDiffDynamicSize, Dynamic, 1>>>
      states_autodiff_dynamic;
  default_random_engine generator;

  for (int i = 0; i < ntests; i++) {
    VectorXd q = model.getRandomConfiguration(generator);
    VectorXd v = VectorXd::Random(model.number_of_velocities());
    VectorXd x(q.rows() + v.rows());
    x << q, v;
    states_double.push_back(make_pair(q, v));

    MatrixXd grad = MatrixXd::Identity(x.size(), x.size());

    auto x_autodiff_fixed = x.cast<AutoDiffFixedMaxSize>().eval();
    gradientMatrixToAutoDiff(grad, x_autodiff_fixed);
    Matrix<AutoDiffFixedMaxSize, Dynamic, 1> q_autodiff_fixed =
        x_autodiff_fixed.topRows(model.number_of_positions());
    Matrix<AutoDiffFixedMaxSize, Dynamic, 1> v_autodiff_fixed =
        x_autodiff_fixed.bottomRows(model.number_of_velocities());
    states_autodiff_fixed.push_back(
        make_pair(q_autodiff_fixed, v_autodiff_fixed));

    auto x_autodiff_dynamic = x.cast<AutoDiffDynamicSize>().eval();
    gradientMatrixToAutoDiff(grad, x_autodiff_dynamic);
    Matrix<AutoDiffDynamicSize, Dynamic, 1> q_autodiff_dynamic =
        x_autodiff_dynamic.topRows(model.number_of_positions());
    Matrix<AutoDiffDynamicSize, Dynamic, 1> v_autodiff_dynamic =
        x_autodiff_dynamic.bottomRows(model.number_of_velocities());
    states_autodiff_dynamic.push_back(
        make_pair(q_autodiff_dynamic, v_autodiff_dynamic));
  }

  KinematicsCache<double> cache_double(model.bodies);
  KinematicsCache<AutoDiffFixedMaxSize> cache_autodiff_fixed(model.bodies);
  KinematicsCache<AutoDiffDynamicSize> cache_autodiff_dynamic(model.bodies);

  cout << "scenario 2:" << endl;
  cout << "no gradients: "
       << measure<>::execution(scenario2<double>, model, cache_double,
                               states_double) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff fixed max size: "
       << measure<>::execution(scenario2<AutoDiffFixedMaxSize>, model,
                               cache_autodiff_fixed, states_autodiff_fixed) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << "autodiff dynamic size: "
       << measure<>::execution(scenario2<AutoDiffDynamicSize>, model,
                               cache_autodiff_dynamic,
                               states_autodiff_dynamic) /
              static_cast<double>(ntests)
       << " ms" << endl;
  cout << endl;
}
Beispiel #9
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->number_of_positions();
  const int nv = model->number_of_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 (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]);
  }
}