void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs != 5) {
mexErrMsgIdAndTxt("Drake:doKinematicsmex:NotEnoughInputs", "Usage doKinematicsmex(model_ptr,q,compute_gradients,v,compute_JdotV)");
}
// first get the model_ptr back from matlab
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);
Map<VectorXd> q(mxGetPrSafe(prhs[1]), mxGetNumberOfElements(prhs[1]));
if (q.rows() != model->num_positions)
mexErrMsgIdAndTxt("Drake:doKinematicsmex:BadInputs", "q must be size %d x 1", model->num_positions);
bool compute_gradients = (bool) (mxGetLogicals(prhs[2]))[0];
bool compute_Jdotv = (bool) (mxGetLogicals(prhs[4]))[0];
if (mxGetNumberOfElements(prhs[3]) > 0) {
auto v = Map<VectorXd>(mxGetPrSafe(prhs[3]), mxGetNumberOfElements(prhs[3]));
if (v.rows() != model->num_velocities)
mexErrMsgIdAndTxt("Drake:doKinematicsmex:BadInputs", "v must be size %d x 1", model->num_velocities);
model->doKinematics(q, v, compute_gradients, compute_Jdotv);
}
else {
Map<VectorXd> v(nullptr, 0, 1);
model->doKinematics(q, v, compute_gradients, compute_Jdotv);
}
}
void testUserGradients(const RigidBodyManipulator &model, int ntests) {
VectorXd q(model.num_positions);
KinematicsCache<double> cache(model.bodies, 1);
for (int i = 0; i < ntests; i++) {
model.getRandomConfiguration(q, generator);
cache.initialize(q);
model.doKinematics(cache, false);
auto H_gradientvar = model.massMatrix(cache, 1);
}
}
int main(int argc, char* argv[])
{
if (argc<2) {
cerr << "Usage: urdfCollisionTest urdf_filename" << endl;
exit(-1);
}
RigidBodyManipulator* model = new RigidBodyManipulator(argv[1]);
if (!model) {
cerr << "ERROR: Failed to load model from " << argv[1] << endl;
return -1;
}
// run kinematics with second derivatives 100 times
VectorXd q = VectorXd::Zero(model->num_positions);
int i;
if (argc>=2+model->num_positions) {
for (i=0; i<model->num_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, 0);
// }
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
model->collisionDetect(cache, phi,normal,xA,xB,bodyA_idx,bodyB_idx);
cout << "=======" << endl;
for (int j=0; j<phi.rows(); ++j) {
cout << phi(j) << " ";
for (int i=0; i<3; ++i) {
cout << normal(i,j) << " ";
}
for (int i=0; i<3; ++i) {
cout << xA(i,j) << " ";
}
for (int i=0; i<3; ++i) {
cout << xB(i,j) << " ";
}
cout << model->bodies[bodyA_idx.at(j)]->linkname << " ";
cout << model->bodies[bodyB_idx.at(j)]->linkname << endl;
}
delete model;
return 0;
}
int main(int argc, char* argv[])
{
if (argc<2) {
cerr << "Usage: urdfKinTest urdf_filename" << endl;
exit(-1);
}
RigidBodyManipulator* model = new RigidBodyManipulator(argv[1]);
if (!model) {
cerr << "ERROR: Failed to load model from " << argv[1] << endl;
return -1;
}
cout << "=======" << endl;
// run kinematics with second derivatives 100 times
Eigen::VectorXd q = Eigen::VectorXd::Zero(model->num_positions);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
int i;
if (argc>=2+model->num_positions) {
for (i=0; i<model->num_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, 0);
// }
// const Vector4d zero(0,0,0,1);
Eigen::Vector3d zero = Eigen::Vector3d::Zero();
Eigen::Matrix<double,6,1> pt;
for (i=0; i<model->bodies.size(); i++) {
// model->forwardKin(i,zero,1,pt);
auto pt = model->forwardKin(cache, zero, i, 0, 1, 0);
// cout << i << ": forward kin: " << model->bodies[i].linkname << " is at " << pt.transpose() << endl;
cout << model->bodies[i]->linkname << " ";
cout << pt.value().transpose() << endl;
// for (int j=0; j<pt.size(); j++)
// cout << pt(j) << " ";
}
auto phi = model->positionConstraints<double>(cache,1);
cout << "phi = " << phi.value().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();
RigidBodyManipulator* model = qsc->getRobotPointer();
int nq = model->num_dof;
double *q = new double[nq];
memcpy(q,mxGetPr(prhs[1]),sizeof(double)*nq);
int num_weights = qsc->getNumWeights();
double* weights = new double[num_weights];
memcpy(weights,mxGetPr(prhs[2]),sizeof(double)*num_weights);
int num_qsc_cnst = qsc->getNumConstraint(t_ptr);
VectorXd c(num_qsc_cnst-1);
MatrixXd dc(num_qsc_cnst-1,nq+num_weights);
model->doKinematics(q);
qsc->eval(t_ptr,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(mxGetPr(plhs[2]),c.data(),sizeof(double)*(num_qsc_cnst-1));
plhs[3] = mxCreateDoubleMatrix(num_qsc_cnst-1,nq+num_weights,mxREAL);
memcpy(mxGetPr(plhs[3]),dc.data(),sizeof(double)*dc.size());
plhs[4] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
memcpy(mxGetPr(plhs[4]),lb.data(),sizeof(double)*(num_qsc_cnst-1));
plhs[5] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
memcpy(mxGetPr(plhs[5]),ub.data(),sizeof(double)*(num_qsc_cnst-1));
delete[] q;
delete[] weights;
}
QPLocomotionPlan::QPLocomotionPlan(RigidBodyManipulator& robot, const QPLocomotionPlanSettings& settings, const std::string& lcm_channel) :
robot(robot),
settings(settings),
start_time(std::numeric_limits<double>::quiet_NaN()),
pelvis_id(robot.findLinkId(settings.pelvis_name)),
foot_body_ids(createFootBodyIdMap(robot, settings.foot_names)),
knee_indices(createJointIndicesMap(robot, settings.knee_names)),
akx_indices(createJointIndicesMap(robot, settings.akx_names)),
aky_indices(createJointIndicesMap(robot, settings.aky_names)),
plan_shift(Vector3d::Zero()),
shifted_zmp_trajectory(settings.zmp_trajectory)
{
for (int i = 1; i < settings.support_times.size(); i++) {
if (settings.support_times[i] < settings.support_times[i - 1])
throw std::runtime_error("support times must be increasing");
}
for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
toe_off_active[*it] = false;
knee_pd_active[*it] = false;
knee_pd_settings[*it] = QPLocomotionPlanSettings::createDefaultKneeSettings();
foot_shifts[*it] = Vector3d::Zero();
}
if (!lcm.good())
{
cerr << "ERROR: lcm is not good()" << endl;
}
}
void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {
if (nrhs < 5) {
mexErrMsgIdAndTxt("Drake:collisionRaycastmex:NotEnoughInputs","Usage collisionRaycastmex(model_ptr, cache_ptr, origin_vector, ray_endpoint, use_margins)");
}
int arg_num = 0;
RigidBodyManipulator *model = static_cast<RigidBodyManipulator*>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double>* cache = static_cast<KinematicsCache<double>*>(getDrakeMexPointer(prhs[arg_num++]));
const mxArray* origin_vector_mex = prhs[arg_num++];
const mxArray* ray_endpoint_mex = prhs[arg_num++];
bool use_margins = (mxGetScalar(prhs[arg_num++])!=0.0);
Matrix3Xd origins(3, mxGetN(origin_vector_mex)), ray_endpoints(3, mxGetN(ray_endpoint_mex));
if (!mxIsNumeric(origin_vector_mex)) {
mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputNotNumeric","Expected a numeric value, got something else.");
}
if (!mxIsNumeric(ray_endpoint_mex)) {
mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputNotNumeric","Expected a numeric value, got something else.");
}
if (mxGetM(origin_vector_mex) != 3) {
mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputSizeWrong","Expected a 3 x N matrix, got %d x %d.", mxGetM(origin_vector_mex), mxGetN(origin_vector_mex));
}
if (mxGetM(ray_endpoint_mex) != 3) {
mexErrMsgIdAndTxt("Drake:collisionRaycastmex:InputSizeWrong","Expected a 3-element vector for ray_endpoint, got %d elements", mxGetNumberOfElements(ray_endpoint_mex));
}
memcpy(origins.data(), mxGetPrSafe(origin_vector_mex), sizeof(double)*mxGetNumberOfElements(origin_vector_mex));
memcpy(ray_endpoints.data(), mxGetPrSafe(ray_endpoint_mex), sizeof(double)*mxGetNumberOfElements(ray_endpoint_mex));
VectorXd distances;
model->collisionRaycast(*cache, origins, ray_endpoints, distances, use_margins);
if (nlhs>0) {
plhs[0] = mxCreateDoubleMatrix(static_cast<int>(distances.size()),1,mxREAL);
memcpy(mxGetPrSafe(plhs[0]), distances.data(), sizeof(double)*distances.size());
}
}
const std::map<Side, int> QPLocomotionPlan::createFootBodyIdMap(RigidBodyManipulator& robot, const std::map<Side, std::string>& foot_names)
{
std::map<Side, int> foot_body_ids;
for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
foot_body_ids[*it] = robot.findLinkId(foot_names.at(*it));
}
return foot_body_ids;
}
int main()
{
RigidBodyManipulator rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
RigidBodyManipulator* model = &rbm;
if(!model)
{
cerr<<"ERROR: Failed to load model"<<endl;
}
Vector2d tspan;
tspan<<0,1;
VectorXd q0 = VectorXd::Zero(model->num_positions);
// The state frame of cpp model does not match with the state frame of MATLAB model, since the dofname_to_dofnum is different in cpp and MATLAB
q0(3) = 0.8;
Vector3d com_lb = Vector3d::Zero();
Vector3d com_ub = Vector3d::Zero();
com_lb(2) = 0.9;
com_ub(2) = 1.0;
WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub,tspan);
int num_constraints = 1;
RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
constraint_array[0] = com_kc;
IKoptions ikoptions(model);
VectorXd q_sol(model->num_positions);
int info;
vector<string> infeasible_constraint;
inverseKin(model,q0,q0,num_constraints,constraint_array,q_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
return 1;
}
VectorXd v = VectorXd::Zero(0);
model->doKinematics(q_sol, v);
Vector3d com = model->centerOfMass<double>(0).value();
printf("%5.2f\n%5.2f\n%5.2f\n",com(0),com(1),com(2));
/*MATFile *presultmat;
presultmat = matOpen("q_sol.mat","w");
mxArray* pqsol = mxCreateDoubleMatrix(model->num_dof,1,mxREAL);
memcpy(mxGetPrSafe(pqsol),q_sol.data(),sizeof(double)*model->num_dof);
matPutVariable(presultmat,"q_sol",pqsol);
matClose(presultmat);*/
delete com_kc;
delete[] constraint_array;
return 0;
}
// Find the joint position indices corresponding to 'name'
vector<int> getJointPositionVectorIndices(const RigidBodyManipulator &model, const std::string &name) {
shared_ptr<RigidBody> joint_parent_body = model.findJoint(name);
int num_positions = joint_parent_body->getJoint().getNumPositions();
vector<int> ret(static_cast<size_t>(num_positions));
// fill with sequentially increasing values, starting at joint_parent_body->position_num_start:
iota(ret.begin(), ret.end(), joint_parent_body->position_num_start);
return ret;
}
const std::map<Side, int> QPLocomotionPlan::createJointIndicesMap(RigidBodyManipulator& robot, const std::map<Side, std::string>& joint_names)
{
std::map<Side, int> joint_indices;
for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
int joint_id = robot.findJointId(joint_names.at(*it));
joint_indices[*it] = robot.bodies[joint_id]->position_num_start;
}
return joint_indices;
}
void testAutoDiffScalarGradients(const RigidBodyManipulator &model, int ntests) {
const int NUM_POSITIONS = Dynamic;
typedef Matrix<double, NUM_POSITIONS, 1> DerivativeType;
typedef AutoDiffScalar<DerivativeType> TaylorVar;
VectorXd q(model.num_positions);
Matrix<TaylorVar, NUM_POSITIONS, 1> q_taylorvar;
auto grad = Matrix<double, NUM_POSITIONS, NUM_POSITIONS>::Identity(model.num_positions, model.num_positions).eval();
KinematicsCache<TaylorVar> cache(model.bodies, 0);
for (int i = 0; i < ntests; i++) {
model.getRandomConfiguration(q, generator);
q_taylorvar = q.cast<TaylorVar>().eval();
gradientMatrixToAutoDiff(grad, q_taylorvar);
cache.initialize(q_taylorvar);
model.doKinematics(cache, false);
auto H_taylorvar = model.massMatrix(cache, 0);
}
}
void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
string usage = "Usage [M, dM] = massMatrixmex(model_ptr, cache_ptr)";
if (nrhs != 2) {
mexErrMsgIdAndTxt("Drake:geometricJacobianmex:WrongNumberOfInputs", usage.c_str());
}
if (nlhs > 2) {
mexErrMsgIdAndTxt("Drake:geometricJacobianmex:WrongNumberOfOutputs", usage.c_str());
}
int gradient_order = nlhs - 1;
int arg_num = 0;
RigidBodyManipulator *model = static_cast<RigidBodyManipulator*>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double>* cache = static_cast<KinematicsCache<double>*>(getDrakeMexPointer(prhs[arg_num++]));
auto ret = model->massMatrix(*cache, gradient_order);
plhs[0] = eigenToMatlab(ret.value());
if (gradient_order > 0)
plhs[1] = eigenToMatlab(ret.gradient().value());
}
//[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;
RigidBodyManipulator *model = static_cast<RigidBodyManipulator*>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double>* cache = static_cast<KinematicsCache<double>*>(getDrakeMexPointer(prhs[arg_num++]));
cache->checkCachedKinematicsSettings(false, true, true, "solveLCPmex");
const int nq = model->num_positions;
const int nv = model->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 phiPgrad = model->positionConstraints<double>(*cache,1);
auto phiP = phiPgrad.value();
auto JP = phiPgrad.gradient().value();
model->jointLimitConstraints(q, phiL, JL);
const size_t nP = phiP.size();
//Convert jacobians to velocity mappings
const MatrixXd n_velocity = model->transformPositionDotMappingToVelocityMapping(*cache,n);
const MatrixXd JL_velocity = model->transformPositionDotMappingToVelocityMapping(*cache,JL);
const auto JP_velocity = model->transformPositionDotMappingToVelocityMapping(*cache,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) = model->transformPositionDotMappingToVelocityMapping(*cache,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: 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: 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]);
}
}
int main()
{
RigidBodyManipulator rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
RigidBodyManipulator* model = &rbm;
if(!model)
{
cerr<<"ERROR: Failed to load model"<<endl;
}
Vector2d tspan;
tspan<<0,1;
int l_hand;
int r_hand;
//int l_foot;
//int r_foot;
for(int i = 0;i<model->bodies.size();i++)
{
if(model->bodies[i]->linkname.compare(string("l_hand")))
{
l_hand = i;
}
else if(model->bodies[i]->linkname.compare(string("r_hand")))
{
r_hand = i;
}
//else if(model->bodies[i].linkname.compare(string("l_foot")))
//{
// l_foot = i;
//}
//else if(model->bodies[i].linkname.compare(string("r_foot")))
//{
// r_foot = i;
//}
}
int nq = model->num_positions;
VectorXd qstar = VectorXd::Zero(nq);
qstar(3) = 0.8;
KinematicsCache<double> cache = model->doKinematics(qstar, 0);
Vector3d com0 = model->centerOfMass(cache, 0).value();
Vector3d r_hand_pt = Vector3d::Zero();
Vector3d rhand_pos0 = model->forwardKin(cache, r_hand_pt, r_hand, 0, 0, 0).value();
int nT = 4;
double* t = new double[nT];
double dt = 1.0/(nT-1);
for(int i = 0;i<nT;i++)
{
t[i] = dt*i;
}
MatrixXd q0 = qstar.replicate(1,nT);
VectorXd qdot0 = VectorXd::Zero(model->num_velocities);
Vector3d com_lb = com0;
com_lb(0) = std::numeric_limits<double>::quiet_NaN();
com_lb(1) = std::numeric_limits<double>::quiet_NaN();
Vector3d com_ub = com0;
com_ub(0) = std::numeric_limits<double>::quiet_NaN();
com_ub(1) = std::numeric_limits<double>::quiet_NaN();
com_ub(2) = com0(2)+0.5;
WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
Vector3d rhand_pos_lb = rhand_pos0;
rhand_pos_lb(0) +=0.1;
rhand_pos_lb(1) +=0.05;
rhand_pos_lb(2) +=0.25;
Vector3d rhand_pos_ub = rhand_pos_lb;
rhand_pos_ub(2) += 0.25;
Vector2d tspan_end;
tspan_end<<t[nT-1],t[nT-1];
WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
int num_constraints = 2;
RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
constraint_array[0] = com_kc;
constraint_array[1] = kc_rhand;
IKoptions ikoptions(model);
MatrixXd q_sol(model->num_positions,nT);
MatrixXd qdot_sol(model->num_velocities,nT);
MatrixXd qddot_sol(model->num_positions,nT);
int info = 0;
vector<string> infeasible_constraint;
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
ikoptions.setFixInitialState(false);
ikoptions.setMajorIterationsLimit(500);
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
RowVectorXd t_inbetween(5);
t_inbetween << 0.1,0.15,0.3,0.4,0.6;
ikoptions.setAdditionaltSamples(t_inbetween);
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
delete com_kc;
delete[] constraint_array;
delete[] t;
return 0;
}
void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {
if (nrhs < 4) {
mexErrMsgIdAndTxt("Drake:smoothDistancePenaltymex:NotEnoughInputs","Usage smoothDistancePenaltymex(model_ptr, cache_ptr, min_distance, active_collision_options)");
}
int arg_num = 0;
RigidBodyManipulator *model = static_cast<RigidBodyManipulator*>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double>* cache = static_cast<KinematicsCache<double>*>(getDrakeMexPointer(prhs[arg_num++]));
// Get the minimum allowable distance
double min_distance = mxGetScalar(prhs[arg_num++]);
// Parse `active_collision_options`
vector<int> active_bodies_idx;
set<string> active_group_names;
// First get the list of body indices for which to compute distances
const mxArray* active_collision_options = prhs[arg_num++];
const mxArray* body_idx = mxGetField(active_collision_options,0,"body_idx");
if (body_idx != NULL) {
int n_active_bodies = static_cast<int>(mxGetNumberOfElements(body_idx));
active_bodies_idx.resize(n_active_bodies);
if (mxGetClassID(body_idx) == mxINT32_CLASS) {
memcpy(active_bodies_idx.data(),(int*) mxGetData(body_idx),
sizeof(int)*n_active_bodies);
} else if(mxGetClassID(body_idx) == mxDOUBLE_CLASS) {
double* ptr = mxGetPrSafe(body_idx);
for (int i=0; i<n_active_bodies; i++)
active_bodies_idx[i] = static_cast<int>(ptr[i]);
} else {
mexErrMsgIdAndTxt("Drake:smoothDistancePenaltymex:WrongInputClass","active_collision_options.body_idx must be an int32 or a double array");
}
transform(active_bodies_idx.begin(),active_bodies_idx.end(),
active_bodies_idx.begin(),
[](int i){return --i;});
}
// Then get the group names for which to compute distances
const mxArray* collision_groups = mxGetField(active_collision_options,0,
"collision_groups");
if (collision_groups != NULL) {
int num = static_cast<int>(mxGetNumberOfElements(collision_groups));
for (int i=0; i<num; i++) {
const mxArray *ptr = mxGetCell(collision_groups,i);
int buflen = static_cast<int>(mxGetN(ptr)*sizeof(mxChar))+1;
char* str = (char*)mxMalloc(buflen);
mxGetString(ptr, str, buflen);
active_group_names.insert(str);
mxFree(str);
}
}
double penalty;
vector<int> bodyA_idx, bodyB_idx;
Matrix3Xd ptsA, ptsB, normals;
MatrixXd dpenalty;
VectorXd dist;
if (active_bodies_idx.size() > 0) {
if (active_group_names.size() > 0) {
model->collisionDetect(*cache, dist, normals, ptsA, ptsB, bodyA_idx, bodyB_idx,
active_bodies_idx,active_group_names);
} else {
model->collisionDetect(*cache, dist, normals, ptsA, ptsB, bodyA_idx, bodyB_idx,
active_bodies_idx);
}
} else {
if (active_group_names.size() > 0) {
model->collisionDetect(*cache, dist, normals, ptsA, ptsB, bodyA_idx, bodyB_idx,
active_group_names);
} else {
model->collisionDetect(*cache, dist, normals, ptsA, ptsB, bodyA_idx, bodyB_idx);
}
}
smoothDistancePenalty(penalty, dpenalty, model, *cache, min_distance, dist, normals, ptsA, ptsB, bodyA_idx, bodyB_idx);
vector<int32_T> idxA(bodyA_idx.size());
transform(bodyA_idx.begin(),bodyA_idx.end(),idxA.begin(),
[](int i){return ++i;});
vector<int32_T> idxB(bodyB_idx.size());
transform(bodyB_idx.begin(),bodyB_idx.end(),idxB.begin(),
[](int i){return ++i;});
if (nlhs>0) {
plhs[0] = mxCreateDoubleScalar(penalty);
}
if (nlhs>1) {
plhs[1] = mxCreateDoubleMatrix(static_cast<int>(dpenalty.rows()),static_cast<int>(dpenalty.cols()),mxREAL);
memcpy(mxGetPrSafe(plhs[1]),dpenalty.data(),sizeof(double)*dpenalty.size());
}
}
int App::getConstraints(Eigen::VectorXd q_star, Eigen::VectorXd &q_sol){
Vector2d tspan;
tspan << 0, 1;
// 0 Pelvis Position and Orientation Constraints
int pelvis_link = model_.findLinkId("pelvis");
Vector3d pelvis_pt = Vector3d::Zero();
Vector3d pelvis_pos0;
pelvis_pos0(0) = rstate_.pose.translation.x;
pelvis_pos0(1) = rstate_.pose.translation.y;
pelvis_pos0(2) = rstate_.pose.translation.z;
Vector3d pelvis_pos_lb = pelvis_pos0;
//pelvis_pos_lb(0) += 0.001;
//pelvis_pos_lb(1) += 0.001;
//pelvis_pos_lb(2) += 0.001;
Vector3d pelvis_pos_ub = pelvis_pos_lb;
//pelvis_pos_ub(2) += 0.001;
WorldPositionConstraint kc_pelvis_pos(&model_, pelvis_link, pelvis_pt, pelvis_pos_lb, pelvis_pos_ub, tspan);
Eigen::Vector4d pelvis_quat_des(rstate_.pose.rotation.w , rstate_.pose.rotation.x, rstate_.pose.rotation.y, rstate_.pose.rotation.z);
double pelvis_tol = 0;//0.0017453292519943296;
WorldQuatConstraint kc_pelvis_quat(&model_, pelvis_link, pelvis_quat_des, pelvis_tol, tspan);
// 1 Back Posture Constraint
PostureConstraint kc_posture_back(&model_, tspan);
std::vector<int> back_idx;
findJointAndInsert(model_, "torsoYaw", back_idx);
findJointAndInsert(model_, "torsoPitch", back_idx);
findJointAndInsert(model_, "torsoRoll", back_idx);
VectorXd back_lb = VectorXd::Zero(3);
VectorXd back_ub = VectorXd::Zero(3);
kc_posture_back.setJointLimits(3, back_idx.data(), back_lb, back_ub);
// 2 Upper Neck Constraint
PostureConstraint kc_posture_neck(&model_, tspan);
std::vector<int> neck_idx;
findJointAndInsert(model_, "upperNeckPitch", neck_idx);
VectorXd neck_lb = VectorXd::Zero(1);
neck_lb(0) = 0.0;
VectorXd neck_ub = VectorXd::Zero(1);
neck_ub(0) = 0.0;
kc_posture_neck.setJointLimits(1, neck_idx.data(), neck_lb, neck_ub);
// 3 Look At constraint:
int head_link = model_.findLinkId("head");
Eigen::Vector3d gaze_axis = Eigen::Vector3d(1,0,0);
Eigen::Vector3d target = cl_cfg_.gazeGoal;
Eigen::Vector3d gaze_origin = Eigen::Vector3d(0,0, 0);//0.45);// inserting this offset almost achieves the required look-at
double conethreshold = 0;
WorldGazeTargetConstraint kc_gaze(&model_, head_link, gaze_axis, target, gaze_origin, conethreshold, tspan);
// Assemble Constraint Set
std::vector<RigidBodyConstraint *> constraint_array;
constraint_array.push_back(&kc_pelvis_pos);
constraint_array.push_back(&kc_pelvis_quat);
constraint_array.push_back(&kc_gaze);
constraint_array.push_back(&kc_posture_back); // leave this out to also use the back
constraint_array.push_back(&kc_posture_neck); // upper neck pitch
// Solve
IKoptions ikoptions(&model_);
int info;
vector<string> infeasible_constraint;
inverseKin(&model_, q_star, q_star, constraint_array.size(), constraint_array.data(), q_sol, info, infeasible_constraint, ikoptions);
printf("INFO = %d\n", info);
return info;
}
void App::solveGazeProblem(){
// Publish the query for visualisation in Director
bot_core::pose_t goalMsg;
goalMsg.utime = rstate_.utime;
goalMsg.pos[0] = cl_cfg_.gazeGoal(0);
goalMsg.pos[1] = cl_cfg_.gazeGoal(1);
goalMsg.pos[2] = cl_cfg_.gazeGoal(2);
goalMsg.orientation[0] = 1;
goalMsg.orientation[1] = 0;
goalMsg.orientation[2] = 0;
goalMsg.orientation[3] = 0;
lcm_->publish("POSE_BODY_ALT", &goalMsg);
// Solve the IK problem for the neck:
VectorXd q_star(robotStateToDrakePosition(rstate_, dofMap_, model_.num_positions));
VectorXd q_sol(model_.num_positions);
int info = getConstraints(q_star, q_sol);
if (info != 1) {
std::cout << "Problem not solved\n";
return;
}
bool mode = 0;
if (mode==0){ // publish utorso-to-head as orientation, not properly tracking but works with different orientations
// Get the utorso to head frame:
int pelvis_link = model_.findLinkId("pelvis");
int head_link = model_.findLinkId("head");
int utorso_link = model_.findLinkId("torso");
KinematicsCache<double> cache = model_.doKinematics(q_sol,0);
Eigen::Isometry3d world_to_pelvis = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), pelvis_link, 0, 2, 0).value() );
Eigen::Isometry3d world_to_head = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), head_link, 0, 2, 0).value() );
Eigen::Isometry3d pelvis_to_head = world_to_head.inverse()*world_to_pelvis;
Eigen::Isometry3d pelvis_to_utorso = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), utorso_link, 0, 2, 0).value() ).inverse()*world_to_pelvis;
Eigen::Isometry3d utorso_to_head = pelvis_to_utorso.inverse()*pelvis_to_head;
// Apply 180 roll as head orientation control seems to be in z-up frame
Eigen::Isometry3d rotation_frame;
rotation_frame.setIdentity();
Eigen::Quaterniond quat = euler_to_quat( M_PI, 0, 0);
rotation_frame.rotate(quat);
utorso_to_head = utorso_to_head*rotation_frame;
bot_core::pose_t world_to_head_frame_pose_msg = getPoseAsBotPose(world_to_head, rstate_.utime);
lcm_->publish("POSE_VICON",&world_to_head_frame_pose_msg);// for debug
bot_core::pose_t utorso_to_head_frame_pose_msg = getPoseAsBotPose(utorso_to_head, rstate_.utime);
lcm_->publish("DESIRED_HEAD_ORIENTATION",&utorso_to_head_frame_pose_msg);// temp
}else{ // publish neck pitch and yaw joints as orientation. this works ok when robot is facing 1,0,0,0
// Fish out the two neck joints (in simulation) and send as a command:
std::vector<string> jointNames;
for (int i=0 ; i <model_.num_positions ; i++){
// std::cout << model.getPositionName(i) << " " << i << "\n";
jointNames.push_back( model_.getPositionName(i) ) ;
}
drc::robot_state_t robot_state_msg;
getRobotState(robot_state_msg, 0*1E6, q_sol , jointNames);
lcm_->publish("CANDIDATE_ROBOT_ENDPOSE",&robot_state_msg);
std::vector<std::string>::iterator it1 = find(jointNames.begin(),
jointNames.end(), "lowerNeckPitch");
int lowerNeckPitchIndex = std::distance(jointNames.begin(), it1);
float lowerNeckPitchAngle = q_sol[lowerNeckPitchIndex];
std::vector<std::string>::iterator it2 = find(jointNames.begin(),
jointNames.end(), "neckYaw");
int neckYawIndex = std::distance(jointNames.begin(), it2);
float neckYawAngle = q_sol[neckYawIndex];
std::cout << lowerNeckPitchAngle << " (" << lowerNeckPitchAngle*180.0/M_PI << ") is lowerNeckPitchAngle\n";
std::cout << neckYawAngle << " (" << neckYawAngle*180.0/M_PI << ") is neckYawAngle\n";
bot_core::pose_t headOrientationMsg;
headOrientationMsg.utime = rstate_.utime;
headOrientationMsg.pos[0] = 0;
headOrientationMsg.pos[1] = 0;
headOrientationMsg.pos[2] = 0;
Eigen::Quaterniond quat = euler_to_quat(0, lowerNeckPitchAngle, neckYawAngle);
headOrientationMsg.orientation[0] = quat.w();
headOrientationMsg.orientation[1] = quat.x();
headOrientationMsg.orientation[2] = quat.y();
headOrientationMsg.orientation[3] = quat.z();
lcm_->publish("DESIRED_HEAD_ORIENTATION",&headOrientationMsg);
lcm_->publish("POSE_VICON",&headOrientationMsg); // for debug
}
//std::cout << "Desired orientation sent, exiting\n";
//exit(-1);
}
