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