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);
}
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);
}
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);
}
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;
}
// [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]);
}
}