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