void approximateIK(RigidBodyTree* model, const MatrixBase<DerivedA>& q_seed,
const MatrixBase<DerivedB>& q_nom, const int num_constraints,
RigidBodyConstraint* *const constraint_array,
const IKoptions& ikoptions,
MatrixBase<DerivedC>* q_sol, int* info) {
int num_kc = 0;
int nq = model->number_of_positions();
SingleTimeKinematicConstraint** kc_array =
new SingleTimeKinematicConstraint* [num_constraints];
double* joint_lb = new double[nq];
double* joint_ub = new double[nq];
for (int j = 0; j < nq; j++) {
joint_lb[j] = model->joint_limit_min[j];
joint_ub[j] = model->joint_limit_max[j];
}
for (int i = 0; i < num_constraints; i++) {
int constraint_category = constraint_array[i]->getCategory();
if (constraint_category ==
RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
kc_array[num_kc] =
static_cast<SingleTimeKinematicConstraint*>(constraint_array[i]);
num_kc++;
} else if (constraint_category ==
RigidBodyConstraint::PostureConstraintCategory) {
VectorXd joint_min, joint_max;
PostureConstraint* pc =
static_cast<PostureConstraint*>(constraint_array[i]);
pc->bounds(nullptr, joint_min, joint_max);
for (int j = 0; j < nq; j++) {
joint_lb[j] = (joint_lb[j] < joint_min[j] ? joint_min[j] : joint_lb[j]);
joint_ub[j] = (joint_ub[j] > joint_max[j] ? joint_max[j] : joint_ub[j]);
if (joint_lb[j] > joint_ub[j]) {
cerr << "Drake:approximateIK:posture constraint has lower bound "
"larger than upper bound" << endl;
}
}
} else {
cerr
<< "Drake:approximateIK: The constraint category is not supported yet"
<< endl;
}
}
MatrixXd Q;
ikoptions.getQ(Q);
int error;
GRBenv* grb_env = nullptr;
GRBmodel* grb_model = nullptr;
VectorXd qtmp = -2 * Q * q_nom;
// create gurobi environment
error = GRBloadenv(&grb_env, nullptr);
if (error) {
printf("Load Gurobi environment error %s\n", GRBgeterrormsg(grb_env));
}
// set solver params
// (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
error = GRBsetintparam(grb_env, "outputflag", 0);
if (error) {
printf("Set Gurobi outputflag error %s\n", GRBgeterrormsg(grb_env));
}
/*error = GRBsetintparam(grb_env,"method", 2);
error = GRBsetintparam(grb_env,"presolve", 0);
error = GRBsetintparam(grb_env,"bariterlimit", 20);
error = GRBsetintparam(grb_env,"barhomogenous", 0);
error = GRBsetdblparam(grb_env,"barconvtol", 1e-4);*/
error = GRBnewmodel(grb_env, &grb_model, "ApproximateIK", nq, qtmp.data(),
joint_lb, joint_ub, nullptr, nullptr);
if (error) {
printf("Create Gurobi model error %s\n", GRBgeterrormsg(grb_env));
}
// set up cost function
// cost: (q-q_nom)'*Q*(q-q_nom)
error = GRBsetdblattrarray(grb_model, "Obj", 0, nq, qtmp.data());
if (error) {
printf("Set Gurobi obj error %s\n", GRBgeterrormsg(grb_env));
}
for (int i = 0; i < nq; i++) {
for (int j = 0; j < nq; j++) {
if (abs(Q(i, j)) > 1e-10) {
error = GRBaddqpterms(grb_model, 1, &i, &j, Q.data() + i + j * nq);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
}
}
}
int* allIndsData = new int[nq];
for (int j = 0; j < nq; j++) {
allIndsData[j] = j;
}
// TODO(tkoolen): pass this into the function?
KinematicsCache<double> cache = model->doKinematics(q_seed);
int kc_idx, c_idx;
for (kc_idx = 0; kc_idx < num_kc; kc_idx++) {
int nc = kc_array[kc_idx]->getNumConstraint(nullptr);
VectorXd lb(nc);
VectorXd ub(nc);
VectorXd c(nc);
MatrixXd dc(nc, nq);
kc_array[kc_idx]->bounds(nullptr, lb, ub);
kc_array[kc_idx]->eval(nullptr, cache, c, dc);
for (c_idx = 0; c_idx < nc; c_idx++) {
VectorXd rowVec = dc.row(c_idx);
double* Jrow = rowVec.data();
double c_seed = c(c_idx) - dc.row(c_idx) * q_seed;
double rhs_row;
if (std::isinf(-lb(c_idx))) {
rhs_row = ub(c_idx) - c_seed;
int gerror = GRBaddconstr(grb_model, nq, allIndsData, Jrow,
GRB_LESS_EQUAL, rhs_row, nullptr);
if (gerror) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
} else if (std::isinf(ub(c_idx))) {
if (std::isinf(lb(c_idx))) {
cerr << "Drake:approximateIK: lb and ub cannot be both infinity, "
"check the getConstraintBnds output of the "
"KinematicConstraint" << endl;
}
rhs_row = lb(c_idx) - c_seed;
error = GRBaddconstr(grb_model, nq, allIndsData, Jrow,
GRB_GREATER_EQUAL, rhs_row, nullptr);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
} else if (ub(c_idx) - lb(c_idx) < 1e-10) {
rhs_row = lb(c_idx) - c_seed;
error = GRBaddconstr(grb_model, nq, allIndsData, Jrow, GRB_EQUAL,
rhs_row, nullptr);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
} else {
double rhs_row1 = ub(c_idx) - c_seed;
error = GRBaddconstr(grb_model, nq, allIndsData, Jrow, GRB_LESS_EQUAL,
rhs_row1, nullptr);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
double rhs_row2 = lb(c_idx) - c_seed;
error = GRBaddconstr(grb_model, nq, allIndsData, Jrow,
GRB_GREATER_EQUAL, rhs_row2, nullptr);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
}
}
}
error = GRBupdatemodel(grb_model);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
error = GRBoptimize(grb_model);
if (error) {
printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
}
VectorXd q_sol_data(nq);
error =
GRBgetdblattrarray(grb_model, GRB_DBL_ATTR_X, 0, nq, q_sol_data.data());
(*q_sol) = q_sol_data;
error = GRBgetintattr(grb_model, GRB_INT_ATTR_STATUS, info);
if ((*info) == 2) {
(*info) = 0;
} else {
(*info) = 1;
}
// debug only
/*GRBwrite(grb_model,"gurobi_approximateIK.lp");
int num_gurobi_cnst;
GRBgetintattr(grb_model, GRB_INT_ATTR_NUMCONSTRS,&num_gurobi_cnst);
MatrixXd J(num_gurobi_cnst, nq);
VectorXd rhs(num_gurobi_cnst);
for(int i = 0;i<num_gurobi_cnst;i++)
{
for(int j = 0;j<nq;j++)
{
GRBgetcoeff(grb_model, i, j,&J(i, j));
}
GRBgetdblattrarray(grb_model, GRB_DBL_ATTR_RHS, 0, num_gurobi_cnst, rhs.data());
}
*/
GRBfreemodel(grb_model);
GRBfreeenv(grb_env);
delete[] joint_lb;
delete[] joint_ub;
delete[] allIndsData;
delete[] kc_array;
return;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int error;
if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct QPControllerData* pdata;
mxArray* pm;
double* pr;
int i,j;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct QPControllerData;
// get control object properties
const mxArray* pobj = prhs[1];
pm = myGetProperty(pobj,"slack_limit");
pdata->slack_limit = mxGetScalar(pm);
pm = myGetProperty(pobj,"W_kdot");
assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
memcpy(pdata->W_kdot.data(),mxGetPr(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));
pm= myGetProperty(pobj,"w_grf");
pdata->w_grf = mxGetScalar(pm);
pm= myGetProperty(pobj,"w_slack");
pdata->w_slack = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_ang");
pdata->Kp_ang = mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_inputs");
pdata->n_body_accel_inputs = mxGetScalar(pm);
pm = myGetProperty(pobj,"body_accel_input_weights");
pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
memcpy(pdata->body_accel_input_weights.data(),mxGetPr(pm),sizeof(double)*pdata->n_body_accel_inputs);
pdata->n_body_accel_constraints = 0;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0)
pdata->n_body_accel_constraints++;
}
// get robot mex model ptr
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("DRC:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));
pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
memcpy(pdata->B.data(),mxGetPr(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));
int nq = pdata->r->num_dof, nu = pdata->B.cols();
pm = myGetProperty(pobj,"w_qdd");
pdata->w_qdd.resize(nq);
memcpy(pdata->w_qdd.data(),mxGetPr(pm),sizeof(double)*nq);
pdata->umin.resize(nu);
pdata->umax.resize(nu);
memcpy(pdata->umin.data(),mxGetPr(prhs[4]),sizeof(double)*nu);
memcpy(pdata->umax.data(),mxGetPr(prhs[5]),sizeof(double)*nu);
pdata->B_act.resize(nu,nu);
pdata->B_act = pdata->B.bottomRows(nu);
// get the map ptr back from matlab
if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
mexErrMsgIdAndTxt("DRC:QPControllermex:BadInputs","the seventh argument should be the map ptr");
memcpy(&pdata->map_ptr,mxGetPr(prhs[6]),sizeof(pdata->map_ptr));
// pdata->map_ptr = NULL;
if (!pdata->map_ptr)
mexWarnMsgTxt("Map ptr is NULL. Assuming flat terrain at z=0");
// create gurobi environment
error = GRBloadenv(&(pdata->env),NULL);
// set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
mxArray* psolveropts = myGetProperty(pobj,"gurobi_options");
int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
// CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
if (method==2) {
CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
}
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:constructModelmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
// preallocate some memory
pdata->H.resize(nq,nq);
pdata->H_float.resize(6,nq);
pdata->H_act.resize(nu,nq);
pdata->C.resize(nq);
pdata->C_float.resize(6);
pdata->C_act.resize(nu);
pdata->J.resize(3,nq);
pdata->Jdot.resize(3,nq);
pdata->J_xy.resize(2,nq);
pdata->Jdot_xy.resize(2,nq);
pdata->Hqp.resize(nq,nq);
pdata->fqp.resize(nq);
pdata->Ag.resize(6,nq);
pdata->Agdot.resize(6,nq);
pdata->Ak.resize(3,nq);
pdata->Akdot.resize(3,nq);
pdata->vbasis_len = 0;
pdata->cbasis_len = 0;
pdata->vbasis = NULL;
pdata->cbasis = NULL;
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("DRC:QPControllermex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
// for (i=0; i<pdata->r->num_bodies; i++)
// mexPrintf("body %d (%s) has %d contact points\n", i, pdata->r->bodies[i].linkname.c_str(), pdata->r->bodies[i].contact_pts.cols());
int nu = pdata->B.cols(), nq = pdata->r->num_dof;
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
int narg=1;
int use_fast_qp = (int) mxGetScalar(prhs[narg++]);
Map< VectorXd > qddot_des(mxGetPr(prhs[narg++]),nq);
double *q = mxGetPr(prhs[narg++]);
double *qd = &q[nq];
vector<VectorXd,aligned_allocator<VectorXd>> body_accel_inputs;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
assert(mxGetM(prhs[narg])==7); assert(mxGetN(prhs[narg])==1);
VectorXd v = VectorXd::Zero(7,1);
memcpy(v.data(),mxGetPr(prhs[narg++]),sizeof(double)*7);
body_accel_inputs.push_back(v);
}
int num_condof;
VectorXd condof;
if (!mxIsEmpty(prhs[narg])) {
assert(mxGetN(prhs[narg])==1);
num_condof=mxGetM(prhs[narg]);
condof = VectorXd::Zero(num_condof);
memcpy(condof.data(),mxGetPr(prhs[narg++]),sizeof(double)*num_condof);
}
else {
num_condof=0;
narg++; // skip over empty vector
}
int desired_support_argid = narg++;
assert(mxGetM(prhs[narg])==4); assert(mxGetN(prhs[narg])==4);
Map< Matrix4d > A_ls(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==4); assert(mxGetN(prhs[narg])==2);
Map< Matrix<double,4,2> > B_ls(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==2); assert(mxGetN(prhs[narg])==2);
Map< Matrix2d > Qy(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==2); assert(mxGetN(prhs[narg])==2);
Map< Matrix2d > R_ls(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==2); assert(mxGetN(prhs[narg])==4);
Map< Matrix<double,2,4> > C_ls(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==2); assert(mxGetN(prhs[narg])==2);
Map< Matrix2d > D_ls(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==4); assert(mxGetN(prhs[narg])==4);
Map< Matrix4d > S(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==4); assert(mxGetN(prhs[narg])==1);
Map< Vector4d > s1(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==4); assert(mxGetN(prhs[narg])==1);
Map< Vector4d > s1dot(mxGetPr(prhs[narg++]));
double s2dot = mxGetScalar(prhs[narg++]);
assert(mxGetM(prhs[narg])==4); assert(mxGetN(prhs[narg])==1);
Map< Vector4d > x0(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==2); assert(mxGetN(prhs[narg])==1);
Map< Vector2d > u0(mxGetPr(prhs[narg++]));
assert(mxGetM(prhs[narg])==2); assert(mxGetN(prhs[narg])==1);
Map< Vector2d > y0(mxGetPr(prhs[narg++]));
double mu = mxGetScalar(prhs[narg++]);
double terrain_height = mxGetScalar(prhs[narg++]); // nonzero if we're using DRCFlatTerrainMap
Matrix2d R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
// Compute active support from desired supports -----------------------
vector<SupportStateElement> active_supports;
set<int> contact_bodies; // redundant, clean up later
int num_active_contact_pts=0;
if (!mxIsEmpty(prhs[desired_support_argid])) {
VectorXd phi;
mxArray* mxBodies = myGetField(prhs[desired_support_argid],"bodies");
if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
double* pBodies = mxGetPr(mxBodies);
mxArray* mxContactPts = myGetField(prhs[desired_support_argid],"contact_pts");
if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
mxArray* mxContactSurfaces = myGetField(prhs[desired_support_argid],"contact_surfaces");
if (!mxContactSurfaces) mexErrMsgTxt("couldn't get contact surfaces");
double* pContactSurfaces = mxGetPr(mxContactSurfaces);
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
int nc = mxGetNumberOfElements(mxBodyContactPts);
if (nc<1) continue;
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
pr = mxGetPr(mxBodyContactPts);
for (j=0; j<nc; j++) {
se.contact_pt_inds.insert((int)pr[j]-1);
}
se.contact_surface = (int) pContactSurfaces[i]-1;
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
pdata->r->HandC(q,qd,(MatrixXd*)NULL,pdata->H,pdata->C,(MatrixXd*)NULL,(MatrixXd*)NULL,(MatrixXd*)NULL);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
// copy to xy versions for computations below (avoid doing topRows a bunch of times)
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
Map<VectorXd> qdvec(qd,nq);
MatrixXd Jz,Jp,Jpdot,D;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,Jz,D,Jp,Jpdot,terrain_height);
int neps = nc*dim;
Vector4d x_bar,xlimp;
MatrixXd D_float(6,D.cols()), D_act(nu,D.cols());
if (nc>0) {
xlimp << xcom.topRows(2),pdata->J_xy*qdvec;
x_bar = xlimp-x0;
D_float = D.topRows(6);
D_act = D.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qdvec;
kdot_des = -pdata->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: quad(Jdot*qd + J*qdd,R_ls)+quad(C*x+D*(Jdot*qd + J*qdd),Qy) + (2*x'*S + s1')*(A*x + B*(Jdot*qd + J*qdd)) + w*quad(qddot_ref - qdd) + quad(u,R) + quad(epsilon)
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
pdata->fqp = (C_ls*xlimp).transpose()*Qy*D_ls*pdata->J_xy;
pdata->fqp += (pdata->Jdot_xy*qdvec).transpose()*R_DQyD_ls*pdata->J_xy;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*pdata->J_xy;
pdata->fqp -= u0.transpose()*R_DQyD_ls*pdata->J_xy;
pdata->fqp -= y0.transpose()*Qy*D_ls*pdata->J_xy;
pdata->fqp -= (pdata->w_qdd.array()*qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += qdvec.transpose()*pdata->Akdot.transpose()*pdata->W_kdot*pdata->Ak;
pdata->fqp -= kdot_des.transpose()*pdata->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*pdata->n_body_accel_constraints+num_condof;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
// beq.segment(6,neps) = (-Jpdot - 1.0*Jp)*qdvec; // TODO: parameterize
beq.segment(6,neps) = -Jpdot*qdvec; // TODO: parameterize
}
// add in body spatial equality constraints
VectorXd body_vdot;
MatrixXd orig = MatrixXd::Zero(4,1);
orig(3,0) = 1;
int body_idx;
int equality_ind = 6+neps;
MatrixXd Jb(6,nq);
MatrixXd Jbdot(6,nq);
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0) {
// negative implies constraint
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
pdata->r->forwardJac(body_idx,orig,1,Jb);
pdata->r->forwardJacDot(body_idx,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot.row(j)*qdvec + body_vdot[j];
}
}
}
}
}
if (num_condof>0) {
// add joint acceleration constraints
for (int i=0; i<num_condof; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = qddot_des[(int)condof[i]-1];
}
}
MatrixXd Ain = MatrixXd::Zero(2*nu,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(2*nu);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
GRBmodel * model = NULL;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = -1e3*VectorXd::Ones(nq);
ub.head(nq) = 1e3*VectorXd::Ones(nq);
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -pdata->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = pdata->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (pdata->w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
bool include_body_accel_cost_terms = pdata->n_body_accel_inputs > 0 && pdata->body_accel_input_weights.array().maxCoeff() > 1e-10;
if (use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(pdata->w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*pdata->J_xy.transpose()*(R_DQyD_ls.inverse() + pdata->J_xy*Wi*pdata->J_xy.transpose()).inverse()*pdata->J_xy*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (pdata->J_xy.transpose()*R_DQyD_ls*pdata->J_xy + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(2*nu+2*nparams,nparams);
VectorXd bin_lb_ub(2*nu+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = pdata->J_xy.transpose()*R_DQyD_ls*pdata->J_xy;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*pdata->W_kdot*pdata->Ak;
}
pdata->Hqp += pdata->w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = MatrixXd::Constant(nq,1,1+REG);
}
// add in body spatial acceleration cost terms
int w_i;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
w_i=pdata->body_accel_input_weights(i);
if (w_i > 0) {
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
pdata->r->forwardJac(body_idx,orig,1,Jb);
pdata->r->forwardJacDot(body_idx,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
pdata->Hqp += w_i*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += w_i*(qdvec.transpose()*Jbdot.row(j).transpose() - body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,pdata->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,pdata->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(2*nu+2*nparams,nparams);
VectorXd bin_lb_ub(2*nu+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->vbasis,pdata->vbasis_len,pdata->cbasis,pdata->cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&pdata->vbasis_len), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&pdata->cbasis_len), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
VectorXd y(nu);
VectorXd qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
y = pdata->B_act.transpose()*(pdata->H_act*qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
if (nlhs>0) {
plhs[0] = eigenToMatlab(y);
}
if (nlhs>1) {
plhs[1] = eigenToMatlab(qdd);
}
if (nlhs>2) {
plhs[2] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
memcpy(mxGetData(plhs[2]),&info,sizeof(int));
}
if (nlhs>3) {
plhs[3] = mxCreateDoubleMatrix(1,active_supports.size(),mxREAL);
pr = mxGetPr(plhs[3]);
int i=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
pr[i++] = (double) (iter->body_idx + 1);
}
}
if (nlhs>4) {
plhs[4] = eigenToMatlab(alpha);
}
if (nlhs>5) {
plhs[5] = eigenToMatlab(pdata->Hqp);
}
if (nlhs>6) {
plhs[6] = eigenToMatlab(f);
}
if (nlhs>7) {
plhs[7] = eigenToMatlab(Aeq);
}
if (nlhs>8) {
plhs[8] = eigenToMatlab(beq);
}
if (nlhs>9) {
plhs[9] = eigenToMatlab(Ain_lb_ub);
}
if (nlhs>10) {
plhs[10] = eigenToMatlab(bin_lb_ub);
}
if (nlhs>11) {
plhs[11] = eigenToMatlab(Qnfdiag);
}
if (nlhs>12) {
plhs[12] = eigenToMatlab(Qneps);
}
if (nlhs>13) {
double Vdot;
if (nc>0)
// note: Sdot is 0 for ZMP/double integrator dynamics, so we omit that term here
Vdot = ((2*x_bar.transpose()*S + s1.transpose())*(A_ls*x_bar + B_ls*(pdata->Jdot_xy*qdvec + pdata->J_xy*qdd)) + s1dot.transpose()*x_bar)(0) + s2dot;
else
Vdot = 0;
plhs[13] = mxCreateDoubleScalar(Vdot);
}
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
}
int main()
{
URDFRigidBodyManipulator* model = loadURDFfromFile("examples/Atlas/urdf/atlas_minimal_contact.urdf");
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->num_bodies;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_dof;
VectorXd qstar = VectorXd::Zero(nq);
qstar(3) = 0.8;
model->doKinematics(qstar.data());
Vector3d com0;
model->getCOM(com0);
Vector4d l_hand_pt;
l_hand_pt<<0.0,0.0,0.0,1.0;
Vector4d r_hand_pt;
r_hand_pt<<0.0,0.0,0.0,1.0;
Vector3d lhand_pos0;
model->forwardKin(l_hand,l_hand_pt,0,lhand_pos0);
Vector3d rhand_pos0;
model->forwardKin(r_hand,r_hand_pt,0,rhand_pos0);
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_dof);
Vector3d com_lb = com0;
com_lb(0) = nan("");;
com_lb(1) = nan("");
Vector3d com_ub = com0;
com_ub(0) = nan("");
com_ub(1) = 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_dof,nT);
MatrixXd qdot_sol(model->num_dof,nT);
MatrixXd qddot_sol(model->num_dof,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;
}
double HamSolver::lanczos(const MatrixX_t& mat, rmMatrixX_t& seed,
double lancTolerance)
{
int matSize = mat.rows();
if(matSize == 1)
return re(mat(0, 0));
const int minIters = std::min(matSize, globalMinLancIters),
maxIters = std::min(matSize, globalMaxLancIters);
std::vector<double> a,
b;
a.reserve(minIters);
b.reserve(minIters);
MatrixX_t basisVecs = seed;
VectorX_t x = mat * basisVecs;
a.push_back(re(seed.col(0).dot(x)));
b.push_back(0.);
VectorX_t oldGS;
int i = 0; // iteration counter
char JOBZ = 'V', // define dstemr arguments
RANGE = 'I';
int N = 1;
std::vector<double> D,
E;
D.reserve(minIters);
E.reserve(minIters);
double VL,
VU;
int IL = 1,
IU = 1,
M;
std::vector<double> W;
W.reserve(minIters);
VectorXd Z;
int LDZ,
NZC = 1,
ISUPPZ[2];
bool TRYRAC = true;
double optLWORK;
std::vector<double> WORK;
int LWORK,
optLIWORK;
std::vector<int> IWORK;
int LIWORK,
INFO;
double gStateDiff;
// change in ground state vector across subsequent Lanczos iterations
do
{
i++;
oldGS = seed;
// Lanczos stage 1: Lanczos iteration
x -= a[i - 1] * basisVecs.col(i - 1);
b.push_back(x.norm());
basisVecs.conservativeResize(NoChange, i + 1);
basisVecs.col(i) = x / b[i];
x.noalias() = mat * basisVecs.col(i) - b[i] * basisVecs.col(i - 1);
a.push_back(re(basisVecs.col(i).dot(x)));
// Lanczos stage 2: diagonalize tridiagonal matrix
N++;
D = a;
E.assign(b.begin() + 1, b.end());
E.resize(N);
W.resize(N);
Z.resize(N);
LDZ = N;
LWORK = -1;
LIWORK = -1;
dstemr_(&JOBZ, &RANGE, &N, D.data(), E.data(), &VL, &VU, &IL, &IU, &M,
W.data(), Z.data(), &LDZ, &NZC, ISUPPZ, &TRYRAC, &optLWORK,
&LWORK, &optLIWORK, &LIWORK, &INFO);
// query for optimal workspace allocations
LWORK = int(optLWORK);
WORK.resize(LWORK);
LIWORK = optLIWORK;
IWORK.resize(LIWORK);
dstemr_(&JOBZ, &RANGE, &N, D.data(), E.data(), &VL, &VU, &IL, &IU, &M,
W.data(), Z.data(), &LDZ, &NZC, ISUPPZ, &TRYRAC, WORK.data(),
&LWORK, IWORK.data(), &LIWORK, &INFO); // calculate ground state
seed = (basisVecs * Z).normalized();
gStateDiff = std::abs(1 - std::abs(seed.col(0).dot(oldGS)));
} while(N < minIters || (N < maxIters && gStateDiff > lancTolerance));
if(N == maxIters && gStateDiff > lancTolerance)
// check if last iteration converges to an eigenstate
{
double gStateError = std::abs(1 - std::abs(seed.col(0)
.dot((mat * seed).col(0)
.normalized())));
std::cout << "Warning: final Lanczos iteration reached. The inner "
<< "product of the final approximate ground state and its "
<< "normalized image differs from 1 by " << gStateError
<< std::endl;
if(gStateError > fallbackLancTolerance)
{
std::cerr << "Lanczos algorithm failed to converge after "
<< maxIters << " iterations." << std::endl;
exit(EXIT_FAILURE);
};
};
return W.front();
};
VectorXd operator()(const VectorXd& x) const {
py::object outarr = np_mod.attr("array")(m_pyfunc(toNdarray1<double>(x.data(), x.size())), "float64");
VectorXd out = Map<const VectorXd>(getPointer<double>(outarr), py::extract<int>(outarr.attr("size")));
return out;
}
void shearsteps(double deltaboxdx,
int NumberStepsRight,
int NumberStepsLeft,
vector<spring> &springlist,
vector<vector<int>> &springpairs,
VectorXd &XY,
int bendingOn,
double kappa,
int Nit,
double tolGradE,
ofstream &shearcoordinates,
ofstream &shearenergy,
ofstream &Strestens,
ofstream &EN)
{
double g11,g12,g22; //The components of the metric tensor
double boxdx=0.0;
double EBEND,ESTRETCH,ETOT;
VectorXd gradE(XY.size());
VectorXd s0(XY.size());
int conjsteps;
double lenGrad;
int iter;
double fret;
networkinfo info(springlist,springpairs);
info.g11=1.0;
info.g12=0.0;
info.g22=1.0;
// info.springlist=springlist;
// info.springpairs=springpairs;
info.size=XY.size();
info.bendingon=bendingOn;
info.sheardeformation=boxdx;
info.kappa=kappa;
//XY is in BOX coordinates
frprmn(XY.data(),XY.size(),1e-20,&iter,&fret,Efunc2use,grad2use,info,EN); //Do one calibration before shearing
// frprmn(XY.data(),XY.size(),1e-12,&iter,&fret,NULL,NULL,info); //Do one calibration before shearing
// CGAGONY(XY,springlist,springpairs,bendingOn,kappa,1.0,0.0,1.0,0.0);
stresstensor S=StressTensor(springlist,XY,0.0);
Strestens<<boxdx<<"\t"<<S.sxx<<"\t"<<S.sxy<<"\t"<<S.syy<<"\t"<<0.5*(S.sxx+S.syy)<<endl;
for(int k=0;k<(NumberStepsLeft+NumberStepsRight);k++){
g11=1.0;
g12=boxdx;
g22=1.0+boxdx*boxdx;
info.g11=1.0;
info.g12=boxdx;
info.g22=1.0+boxdx*boxdx;
info.sheardeformation=boxdx;
// CGAGONY(XY,springlist,springpairs,bendingOn,kappa,g11,g12,g22,boxdx); //fast
frprmn(XY.data(),XY.size(),1e-15,&iter,&fret,Efunc2use,grad2use,info,EN); //slow but better THIS ONE
S=StressTensor(springlist,XY,boxdx);
Write_ShearCoordinates_2txt(shearcoordinates,XY);
//ESTRETCH=EnergyNetworkn(XY.data(),info);
ESTRETCH=StretchEnergy(springlist,XY,boxdx);
// cout << "Now EBEND: " << XY(600) << "\t" << kappa << "\t" << boxdx << "\t" << springpairs[80][0] << "\t" << springlist[80].one << endl;
EBEND=BendEnergy(springpairs,springlist,XY,kappa,boxdx);
// cout << "RESULT: " << EBEND << endl;
Write_ShearEnergy_2txt(shearenergy,boxdx,ESTRETCH+EBEND,ESTRETCH,EBEND,lenGrad,iter);
if(k<NumberStepsRight){
boxdx=boxdx+deltaboxdx;
} else {
boxdx=boxdx-deltaboxdx;
}
cout<<"This was a shearstep "<<k<<" with "<<iter<<endl;
}
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
// check number of arguments
if (nrhs < 4) {
mexErrMsgIdAndTxt("Drake:collisionDetectmex:NotEnoughInputs",
"Usage: [xA,xB,normal,distance,idxA,idxB] = "
"collisionDetectmex(mex_model_ptr, cache_ptr, "
"allow_multiple_contacts, active_collision_options)");
}
// check argument types
if (!mxIsClass(prhs[0], "DrakeMexPointer")) {
mexErrMsgIdAndTxt(
"Drake:collisionDetectmex:InvalidInputType",
"Expected a DrakeMexPointer for mex_model_ptr but got something else.");
}
if (!mxIsClass(prhs[1], "DrakeMexPointer")) {
mexErrMsgIdAndTxt(
"Drake:collisionDetectmex:InvalidInputType",
"Expected a DrakeMexPointer for cache_ptr but got something else.");
}
if (!mxIsLogical(prhs[2])) {
mexErrMsgIdAndTxt("Drake:collisionDetectmex:InvalidInputType",
"Expected a boolean logic type for "
"allow_multiple_collisions but got something else.");
}
if (!mxIsStruct(prhs[3])) {
mexErrMsgIdAndTxt("Drake:collisionDetectmex:InvalidInputType",
"Expected a struct type for active_collision_options but "
"got something else.");
}
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));
// Parse `active_collision_options`
vector<int> active_bodies_idx;
set<string> active_group_names;
const mxArray* allow_multiple_contacts = prhs[arg_num++];
const mxArray* active_collision_options = prhs[arg_num++];
const mxArray* body_idx = mxGetField(active_collision_options, 0, "body_idx");
if (body_idx != NULL) {
size_t n_active_bodies =
static_cast<size_t>(mxGetNumberOfElements(body_idx));
active_bodies_idx.resize(n_active_bodies);
memcpy(active_bodies_idx.data(), (int*)mxGetData(body_idx),
sizeof(int) * n_active_bodies);
transform(active_bodies_idx.begin(), active_bodies_idx.end(),
active_bodies_idx.begin(), [](int i) { return --i; });
}
const mxArray* collision_groups =
mxGetField(active_collision_options, 0, "collision_groups");
if (collision_groups != NULL) {
size_t num_collision_groups =
static_cast<size_t>(mxGetNumberOfElements(collision_groups));
for (size_t i = 0; i < num_collision_groups; i++) {
const mxArray* ptr = mxGetCell(collision_groups, i);
size_t buflen = static_cast<size_t>(mxGetN(ptr) * sizeof(mxChar)) + 1;
char* str = (char*)mxMalloc(buflen);
mxGetString(ptr, str, buflen);
active_group_names.insert(str);
mxFree(str);
}
}
vector<int> bodyA_idx, bodyB_idx;
Matrix3Xd ptsA, ptsB, normals;
MatrixXd JA, JB, Jd;
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 {
const bool multiple_contacts =
mxIsLogicalScalarTrue(allow_multiple_contacts);
if (multiple_contacts) {
model->potentialCollisions(cache, dist, normals, ptsA, ptsB, bodyA_idx,
bodyB_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);
}
}
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] = mxCreateDoubleMatrix(3, static_cast<int>(ptsA.cols()), mxREAL);
memcpy(mxGetPrSafe(plhs[0]), ptsA.data(), sizeof(double) * 3 * ptsA.cols());
}
if (nlhs > 1) {
plhs[1] = mxCreateDoubleMatrix(3, static_cast<int>(ptsB.cols()), mxREAL);
memcpy(mxGetPrSafe(plhs[1]), ptsB.data(), sizeof(double) * 3 * ptsB.cols());
}
if (nlhs > 2) {
plhs[2] = mxCreateDoubleMatrix(3, static_cast<int>(normals.cols()), mxREAL);
memcpy(mxGetPrSafe(plhs[2]), normals.data(),
sizeof(double) * 3 * normals.cols());
}
if (nlhs > 3) {
plhs[3] = mxCreateDoubleMatrix(1, static_cast<int>(dist.size()), mxREAL);
memcpy(mxGetPrSafe(plhs[3]), dist.data(), sizeof(double) * dist.size());
}
if (nlhs > 4) {
plhs[4] = mxCreateNumericMatrix(1, static_cast<int>(idxA.size()),
mxINT32_CLASS, mxREAL);
memcpy(mxGetData(plhs[4]), idxA.data(), sizeof(int32_T) * idxA.size());
}
if (nlhs > 5) {
plhs[5] = mxCreateNumericMatrix(1, static_cast<int>(idxB.size()),
mxINT32_CLASS, mxREAL);
memcpy(mxGetData(plhs[5]), idxB.data(), sizeof(int32_T) * idxB.size());
}
}
void merPredD::setBeta0(const VectorXd& nBeta) {
if (nBeta.size() != d_p) throw invalid_argument("setBeta0: dimension mismatch");
std::copy(nBeta.data(), nBeta.data() + d_p, d_beta0.data());
}
void merPredD::setDelu(const VectorXd& newDelu) {
if (newDelu.size() != d_q)
throw invalid_argument("setDelu: dimension mismatch");
std::copy(newDelu.data(), newDelu.data() + d_q, d_delu.data());
}
drwnCompressionBuffer drwnCompressVector(const VectorXd& x)
{
drwnCompressionBuffer buffer;
buffer.compress((unsigned char *)x.data(), x.rows() * sizeof(double));
return buffer;
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
// This is useful for debugging whether Matlab is caching the mex binary
//mexPrintf("%s %s\n",__TIME__,__DATE__);
igl::matlab::MexStream mout;
std::streambuf *outbuf = std::cout.rdbuf(&mout);
using namespace std;
using namespace Eigen;
using namespace igl;
using namespace igl::matlab;
MatrixXd P,V,C;
VectorXi I;
VectorXd sqrD;
MatrixXi F;
if(nrhs < 3)
{
mexErrMsgTxt("nrhs < 3");
}
parse_rhs_double(prhs,P);
parse_rhs_double(prhs+1,V);
parse_rhs_index(prhs+2,F);
mexErrMsgTxt(P.cols()==3 || P.cols()==2,"P must be #P by (3|2)");
mexErrMsgTxt(V.cols()==3 || V.cols()==2,"V must be #V by (3|2)");
mexErrMsgTxt(V.cols()==P.cols(),"dim(V) must be dim(P)");
mexErrMsgTxt(F.cols()==3 || F.cols()==2 || F.cols()==1,"F must be #F by (3|2|1)");
point_mesh_squared_distance(P,V,F,sqrD,I,C);
// Prepare left-hand side
switch(nlhs)
{
case 3:
{
// Treat indices as reals
plhs[2] = mxCreateDoubleMatrix(C.rows(),C.cols(), mxREAL);
double * Cp = mxGetPr(plhs[2]);
copy(&C.data()[0],&C.data()[0]+C.size(),Cp);
// Fallthrough
}
case 2:
{
// Treat indices as reals
plhs[1] = mxCreateDoubleMatrix(I.rows(),I.cols(), mxREAL);
double * Ip = mxGetPr(plhs[1]);
VectorXd Id = (I.cast<double>().array()+1).matrix();
copy(&Id.data()[0],&Id.data()[0]+Id.size(),Ip);
// Fallthrough
}
case 1:
{
plhs[0] = mxCreateDoubleMatrix(sqrD.rows(),sqrD.cols(), mxREAL);
double * sqrDp = mxGetPr(plhs[0]);
copy(&sqrD.data()[0],&sqrD.data()[0]+sqrD.size(),sqrDp);
break;
}
default:break;
}
// Restore the std stream buffer Important!
std::cout.rdbuf(outbuf);
}
mfloat_t CKroneckerLMM::nLLeval(mfloat_t ldelta, const MatrixXdVec& A,const MatrixXdVec& X, const MatrixXd& Y, const VectorXd& S_C1, const VectorXd& S_R1, const VectorXd& S_C2, const VectorXd& S_R2)
{
//#define debugll
muint_t R = (muint_t)Y.rows();
muint_t C = (muint_t)Y.cols();
assert(A.size() == X.size());
assert(R == (muint_t)S_R1.rows());
assert(C == (muint_t)S_C1.rows());
assert(R == (muint_t)S_R2.rows());
assert(C == (muint_t)S_C2.rows());
muint_t nWeights = 0;
for(muint_t term = 0; term < A.size();++term)
{
assert((muint_t)(X[term].rows())==R);
assert((muint_t)(A[term].cols())==C);
nWeights+=(muint_t)(A[term].rows()) * (muint_t)(X[term].cols());
}
mfloat_t delta = exp(ldelta);
mfloat_t ldet = 0.0;//R * C * ldelta;
//build D and compute the logDet of D
MatrixXd D = MatrixXd(R,C);
for (muint_t r=0; r<R;++r)
{
if(S_R2(r)>1e-10)
{
ldet += (mfloat_t)C * log(S_R2(r));//ldet
}
else
{
std::cout << "S_R2(" << r << ")="<< S_R2(r)<<"\n";
}
}
#ifdef debugll
std::cout << ldet;
std::cout << "\n";
#endif
for (muint_t c=0; c<C;++c)
{
if(S_C2(c)>1e-10)
{
ldet += (mfloat_t)R * log(S_C2(c));//ldet
}
else
{
std::cout << "S_C2(" << c << ")="<< S_C2(c)<<"\n";
}
}
#ifdef debugll
std::cout << ldet;
std::cout << "\n";
#endif
for (muint_t r=0; r<R;++r)
{
for (muint_t c=0; c<C;++c)
{
mfloat_t SSd = S_R1.data()[r]*S_C1.data()[c] + delta;
ldet+=log(SSd);
D(r,c) = 1.0/SSd;
}
}
#ifdef debugll
std::cout << ldet;
std::cout << "\n";
#endif
MatrixXd DY = Y.array() * D.array();
VectorXd XYA = VectorXd(nWeights);
muint_t cumSumR = 0;
MatrixXd covW = MatrixXd(nWeights,nWeights);
for(muint_t termR = 0; termR < A.size();++termR){
muint_t nW_AR = A[termR].rows();
muint_t nW_XR = X[termR].cols();
muint_t rowsBlock = nW_AR * nW_XR;
MatrixXd XYAblock = X[termR].transpose() * DY * A[termR].transpose();
XYAblock.resize(rowsBlock,1);
XYA.block(cumSumR,0,rowsBlock,1) = XYAblock;
muint_t cumSumC = 0;
for(muint_t termC = 0; termC < A.size(); ++termC){
muint_t nW_AC = A[termC].rows();
muint_t nW_XC = X[termC].cols();
muint_t colsBlock = nW_AC * nW_XC;
MatrixXd block = MatrixXd::Zero(rowsBlock,colsBlock);
if (R<C)
{
for(muint_t r=0; r<R; ++r)
{
MatrixXd AD = A[termR];
AD.array().rowwise() *= D.row(r).array();
MatrixXd AA = AD * A[termC].transpose();
//sum up col matrices
MatrixXd XX = X[termR].row(r).transpose() * X[termC].row(r);
akron(block,AA,XX,true);
}
}
else
{//sum up col matrices
for(muint_t c=0; c<C; ++c)
{
MatrixXd XD = X[termR];
XD.array().colwise() *= D.col(c).array();
MatrixXd XX = XD.transpose() * X[termC];
//sum up col matrices
MatrixXd AA = A[termR].col(c) * A[termC].col(c).transpose();
akron(block,AA,XX,true);
}
}
covW.block(cumSumR, cumSumC, rowsBlock, colsBlock) = block;
cumSumC+=colsBlock;
}
cumSumR+=rowsBlock;
}
//std::cout << "covW = " << covW<<std::endl;
MatrixXd W_vec = covW.colPivHouseholderQr().solve(XYA);
//MatrixXd W_vec = covW * XYA;
//std::cout << "W = " << W_vec<<std::endl;
//std::cout << "XYA = " << XYA<<std::endl;
mfloat_t res = (Y.array()*DY.array()).sum();
mfloat_t varPred = (W_vec.array() * XYA.array()).sum();
res-= varPred;
mfloat_t sigma = res/(mfloat_t)(R*C);
mfloat_t nLL = 0.5 * ( R * C * (L2pi + log(sigma) + 1.0) + ldet);
#ifdef returnW
covW = covW.inverse();
//std::cout << "covW.inverse() = " << covW<<std::endl;
muint_t cumSum = 0;
VectorXd F_vec = W_vec.array() * W_vec.array() /covW.diagonal().array() / sigma;
for(muint_t term = 0; term < A.size();++term)
{
muint_t currSize = X[term].cols() * A[term].rows();
//W[term] = MatrixXd(X[term].cols(),A[term].rows());
W[term] = W_vec.block(cumSum,0,currSize,1);//
W[term].resize(X[term].cols(),A[term].rows());
//F_tests[term] = MatrixXd(X[term].cols(),A[term].rows());
F_tests[term] = F_vec.block(cumSum,0,currSize,1);//
F_tests[term].resize(X[term].cols(),A[term].rows());
cumSum+=currSize;
}
#endif
return nLL;
}
void eigs_gen_F77(MatrixXd &M, VectorXd &init_resid, int k, int m,
double &time_used, double &prec_err, int &nops)
{
double start, end;
prec_err = -1.0;
start = get_wall_time();
// Begin ARPACK
//
// Initial value of ido
int ido = 0;
// 'I' means standard eigen value problem, A * x = lambda * x
char bmat = 'I';
// dimension of A (n by n)
int n = M.rows();
// Specify selection criteria
// "LM": largest magnitude
char which[3] = {'L', 'M', '\0'};
// Number of eigenvalues requested
int nev = k;
// Precision
double tol = 1e-10;
// Residual vector
double *resid = new double[n]();
std::copy(init_resid.data(), init_resid.data() + n, resid);
// Number of Ritz values used
int ncv = m;
// Vector of eigenvalues
VectorXd evals_re(nev + 1);
VectorXd evals_im(nev + 1);
// Matrix of eigenvectors
MatrixXd evecs(n, ncv);
// Store final results of eigenvectors
// double *V = new double[n * ncv]();
double *V = evecs.data();
// Leading dimension of V, required by FORTRAN
int ldv = n;
// Control parameters
int *iparam = new int[11]();
iparam[1 - 1] = 1; // ishfts
iparam[3 - 1] = 1000; // maxitr
iparam[7 - 1] = 1; // mode
// Some pointers
int *ipntr = new int[14]();
/* workd has 3 columns.
* ipntr[2] - 1 ==> first column to store B * X,
* ipntr[1] - 1 ==> second to store Y,
* ipntr[0] - 1 ==> third to store X. */
double *workd = new double[3 * n]();
int lworkl = 3 * ncv * ncv + 6 * ncv;
double *workl = new double[lworkl]();
// Error flag. 0 means random initialization,
// otherwise using resid as initial value
int info = 1;
naupd(ido, bmat, n, which,
nev, tol, resid,
ncv, V, ldv,
iparam, ipntr, workd,
workl, lworkl, info);
// ido == -1 or ido == 1 means more iterations needed
while (ido == -1 || ido == 1)
{
MapVec vec_in(&workd[ipntr[0] - 1], n);
MapVec vec_out(&workd[ipntr[1] - 1], n);
vec_out.noalias() = M * vec_in;
naupd(ido, bmat, n, which,
nev, tol, resid,
ncv, V, ldv,
iparam, ipntr, workd,
workl, lworkl, info);
}
// info > 0 means warning, < 0 means error
if(info > 0)
std::cout << "warnings occured" << std::endl;
if(info < 0)
{
delete [] workl;
delete [] workd;
delete [] ipntr;
delete [] iparam;
delete [] resid;
std::cout << "errors occured" << std::endl;
end = get_wall_time();
time_used = (end - start) * 1000;
return;
}
// Retrieve results
//
// Whether to calculate eigenvectors or not.
bool rvec = true;
// 'A' means to calculate Ritz vectors
// 'P' to calculate Schur vectors
char howmny = 'A';
// Vector of eigenvalues
double *dr = evals_re.data();
double *di = evals_im.data();
// Used to store results, will use V instead.
double *Z = V;
// Leading dimension of Z, required by FORTRAN
int ldz = n;
// Shift
double sigmar = 0;
double sigmai = 0;
// Working space
double *workv = new double[3 * ncv]();
// Error information
int ierr = 0;
// Number of converged eigenvalues
int nconv = 0;
// Number of iterations
int niter = 0;
// Use seupd() to retrieve results
neupd(rvec, howmny, dr, di,
Z, ldz, sigmar, sigmai, workv,
bmat, n, which, nev, tol,
resid, ncv, V, ldv, iparam,
ipntr, workd, workl, lworkl, ierr);
// Obtain 'nconv' converged eigenvalues
nconv = iparam[5 - 1];
// 'niter' number of iterations
niter = iparam[9 - 1];
// Free memory of temp arrays
delete [] workv;
delete [] workl;
delete [] workd;
delete [] ipntr;
delete [] iparam;
delete [] resid;
// ierr < 0 means error
if (ierr < 0)
{
std::cout << "errors occured" << std::endl;
end = get_wall_time();
time_used = (end - start) * 1000;
return;
}
/*
VectorXcd evals(evals_re.size());
evals.real() = evals_re;
evals.imag() = evals_im;
std::cout << "computed eigenvalues D = \n" << evals << std::endl;
std::cout << "first 5 rows of computed eigenvectors U = \n" <<
evecs.topLeftCorner(5, nconv) << std::endl;
std::cout << "nconv = " << nconv << std::endl;
std::cout << "nops = " << niter << std::endl;
*/
end = get_wall_time();
time_used = (end - start) * 1000;
nops = niter;
}
void randVec(VectorXd& M)
{
for (int nn = 0; nn < M.size(); nn++)
M.data()[nn] = -1.0 + 2.0 * Uniform(myrng);
}
Vector<double> as_vector (const VectorXd & x)
{
return Vector<double>(x.size(), const_cast<double *>(x.data()));
}
void merPredD::setU0(const VectorXd& newU0) {
if (newU0.size() != d_q) throw invalid_argument("setU0: dimension mismatch");
std::copy(newU0.data(), newU0.data() + d_q, d_u0.data());
}
void display()
{
using namespace Eigen;
using namespace igl;
using namespace std;
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// All smooth points
glEnable( GL_POINT_SMOOTH );
lights();
push_scene();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
push_object();
if(trackball_on)
{
// Draw a "laser" line
glLineWidth(3.0);
glDisable(GL_LIGHTING);
glEnable(GL_DEPTH_TEST);
glBegin(GL_LINES);
glColor3f(1,0,0);
glVertex3fv(s.data());
glColor3f(1,0,0);
glVertex3fv(d.data());
glEnd();
// Draw the start and end points used for ray
glPointSize(10.0);
glBegin(GL_POINTS);
glColor3f(1,0,0);
glVertex3fv(s.data());
glColor3f(0,0,1);
glVertex3fv(d.data());
glEnd();
}
// Draw the model
glEnable(GL_LIGHTING);
draw_mesh(V,F,N,C);
// Draw all hits
glBegin(GL_POINTS);
glColor3f(0,0.2,0.2);
for(vector<igl::embree::Hit>::iterator hit = hits.begin();
hit != hits.end();
hit++)
{
const double w0 = (1.0-hit->u-hit->v);
const double w1 = hit->u;
const double w2 = hit->v;
VectorXd hitP =
w0 * V.row(F(hit->id,0)) +
w1 * V.row(F(hit->id,1)) +
w2 * V.row(F(hit->id,2));
glVertex3dv(hitP.data());
}
glEnd();
pop_object();
// Draw a nice floor
glPushMatrix();
glEnable(GL_LIGHTING);
glTranslated(0,-1,0);
draw_floor();
glPopMatrix();
// draw a transparent "projection screen" show model at time of hit (aka
// mouse down)
push_object();
if(trackball_on)
{
glColor4f(0,0,0,1.0);
glPointSize(10.0);
glBegin(GL_POINTS);
glVertex3fv(SW.data());
glVertex3fv(SE.data());
glVertex3fv(NE.data());
glVertex3fv(NW.data());
glEnd();
glDisable(GL_LIGHTING);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, tex_id);
glColor4f(1,1,1,0.7);
glBegin(GL_QUADS);
glTexCoord2d(0,0);
glVertex3fv(SW.data());
glTexCoord2d(1,0);
glVertex3fv(SE.data());
glTexCoord2d(1,1);
glVertex3fv(NE.data());
glTexCoord2d(0,1);
glVertex3fv(NW.data());
glEnd();
glBindTexture(GL_TEXTURE_2D, 0);
glDisable(GL_TEXTURE_2D);
}
pop_object();
pop_scene();
// Draw a faint point over mouse
if(!trackball_on)
{
glDisable(GL_LIGHTING);
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f(1.0,0.3,0.3,0.6);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0,width,0,height);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glPointSize(20.0);
glBegin(GL_POINTS);
glVertex2fv(win_s.data());
glEnd();
}
report_gl_error();
glutSwapBuffers();
glutPostRedisplay();
}
/**
* Use Frank's fastQP code (mexed)
* [q, info] = approximateIKEIQPmex(objgetMexModelPtr, q0, q_nom, Q, varargin)
* info = 0 on success, 1 on failure
**/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
if (nrhs < 4) {
mexErrMsgIdAndTxt("Drake:approximateIKmex:NotEnoughInputs",
"Usage approximateIKmex(model_ptr, q0, q_nom, Q,...)");
}
if (nlhs < 1) return;
// first get the model_ptr back from matlab
RigidBodyTree *model = (RigidBodyTree *)getDrakeMexPointer(prhs[0]);
int i, j, error, nq = model->num_positions;
static RigidBodyTree *lastModel = NULL;
static int lastNumJointLimits = 0;
int equality_ind = 0;
int inequality_ind = 0;
// Constraint preallocation
Matrix<double, -1, 1, 0, MAX_CONSTRS, 1> beq(MAX_CONSTRS);
Matrix<double, -1, 1, 0, MAX_CONSTRS, 1> bin(MAX_CONSTRS);
Matrix<double, -1, -1, RowMajor, MAX_CONSTRS, -1> Aeq(MAX_CONSTRS, nq);
Matrix<double, -1, -1, RowMajor, MAX_CONSTRS, -1> Ain(MAX_CONSTRS, nq);
// Add joint limits
if (lastModel != model || true) {
lastModel = model;
for (i = 0; i < nq; i++) {
if (!mxIsInf(model->joint_limit_max[i])) {
bin(inequality_ind) = model->joint_limit_max[i];
Ain.row(inequality_ind) = MatrixXd::Zero(1, nq);
Ain(inequality_ind, i) = 1;
// cout << bin(inequality_ind) << endl << endl;
inequality_ind++;
}
if (!mxIsInf(model->joint_limit_min[i])) {
bin(inequality_ind) = -model->joint_limit_min[i];
Ain.row(inequality_ind) << MatrixXd::Zero(1, nq);
Ain(inequality_ind, i) = -1;
// cout << bin(inequality_ind) << endl << endl;
inequality_ind++;
}
}
lastNumJointLimits = inequality_ind;
} else {
inequality_ind = lastNumJointLimits;
}
// cost: (q-q_nom)'*Q*(q-q_nom) \equiv q'*Q*q - 2*q_nom'*Q*q (const term
// doesn't matter)
Map<VectorXd> q_nom(mxGetPrSafe(prhs[2]), nq);
Map<MatrixXd> Q(mxGetPrSafe(prhs[3]), nq, nq);
VectorXd c = -Q * q_nom;
double *q0 = mxGetPrSafe(prhs[1]);
model->doKinematics(q0);
VectorXd q0vec = Map<VectorXd>(q0, nq);
i = 4;
while (i < nrhs) {
MatrixXd body_pos;
Map<MatrixXd> *world_pos = NULL;
int rows;
int body_ind = mxGetScalar(prhs[i]) - 1;
mxArray *min = NULL;
mxArray *max = NULL;
int n_pts;
if (body_ind == -1) {
int n_pts = mxGetN(prhs[i + 1]);
body_pos.resize(4, 1);
body_pos << 0, 0, 0, 1;
world_pos = new Map<MatrixXd>(mxGetPrSafe(prhs[i + 1]), 3, n_pts);
rows = 3;
i += 2;
} else {
if (mxIsClass(prhs[i + 2], "struct")) { // isstruct(worldpos)
min = mxGetField(prhs[i + 2], 0, "min");
max = mxGetField(prhs[i + 2], 0, "max");
if (min == NULL || max == NULL) {
mexErrMsgIdAndTxt(
"Drake:approximateIKMex:BadInputs",
"if world_pos is a struct, it must have fields .min and .max");
}
rows = mxGetM(min);
if (rows != 3 && rows != 6) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"world_pos.min must have 3 or 6 rows");
}
if (mxGetM(max) != rows) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"world_pos.max must have the same number of rows "
"as world_pos.min");
}
} else {
rows = mxGetM(prhs[i + 2]);
int n_pts = mxGetN(prhs[i + 2]);
world_pos = new Map<MatrixXd>(mxGetPrSafe(prhs[i + 2]), rows, n_pts);
}
if (mxIsClass(prhs[i + 1], "char") || mxGetM(prhs[i + 1]) == 1) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:NotImplemented",
"collision group not implemented in mex (mposa)");
} else {
if (mxGetM(prhs[i + 1]) != 3) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"bodypos must be 3xmi");
}
n_pts = mxGetN(prhs[i + 1]);
Map<MatrixXd> pts_tmp(mxGetPrSafe(prhs[i + 1]), 3, n_pts);
body_pos.resize(4, n_pts);
body_pos << pts_tmp, MatrixXd::Ones(1, n_pts);
}
i += 3;
}
MatrixXd x;
MatrixXd J;
if (body_ind == -1) {
x = VectorXd::Zero(3, 1);
Vector3d x_com;
model->getCOM(x_com);
model->getCOMJac(J);
x.resize(3, 1);
x << x_com;
} else {
J.resize(rows * n_pts, nq);
model->forwardKin(body_ind, body_pos, (int)rows == 6, x);
model->forwardJac(body_ind, body_pos, (int)rows == 6, J);
if (rows == 6 && min == NULL && max == NULL) {
VectorXd delta;
angleDiff(x.block(3, 0, 3, n_pts), (*world_pos).block(3, 0, 3, n_pts),
&delta);
(*world_pos).block(3, 0, 3, n_pts) = x.block(3, 0, 3, n_pts) + delta;
}
}
if (max != NULL) {
double *val = mxGetPrSafe(max);
// add inequality constraint
for (j = 0; j < rows; j++) {
if (!mxIsNaN(val[j])) {
VectorXd rowVec = J.row(j);
double rhs = val[j] - x(j) + J.row(j) * q0vec;
if (mxIsInf(rhs)) {
if (rhs < 0) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"RHS of a constraint evaluates to -inf");
}
} else {
Ain.row(inequality_ind) << J.row(j);
bin(inequality_ind) = rhs;
inequality_ind++;
}
}
}
}
if (min != NULL) {
double *val = mxGetPrSafe(min);
// add inequality constraint
for (j = 0; j < rows; j++) {
if (!mxIsNaN(val[j])) {
VectorXd rowVec = J.row(j);
double rhs = -(val[j] - x(j) + J.row(j) * q0vec);
if (mxIsInf(rhs)) {
if (rhs < 0) {
mexErrMsgIdAndTxt("Drake:approximateIKMex:BadInputs",
"RHS of a constraint evaluates to -inf");
}
} else {
Ain.row(inequality_ind) << J.row(j);
bin(inequality_ind) = rhs;
inequality_ind++;
}
}
}
}
if (min == NULL && max == NULL) {
for (j = 0; j < rows; j++) {
if (!mxIsNaN((*world_pos)(j))) {
double rhs = (*world_pos)(j)-x(j) + J.row(j) * q0vec;
Aeq.row(equality_ind) << J.row(j);
beq(equality_ind) = rhs;
equality_ind++;
}
}
}
delete world_pos;
if (min) mxDestroyArray(min);
if (max) mxDestroyArray(max);
}
Aeq.conservativeResize(equality_ind, nq);
Ain.conservativeResize(inequality_ind, nq);
beq.conservativeResize(equality_ind);
bin.conservativeResize(inequality_ind);
cout << "c is " << c.rows() << endl;
cout << "Aeq is " << Aeq.rows() << " by " << Aeq.cols() << endl;
VectorXd q = model->getZeroConfiguration();
// double result = solve_quadprog(Q, c, -Aeq, beq, -Ain, bin, q);
VectorXd Qdiag = Q.diagonal();
vector<MatrixBase<VectorXd> > blkQ;
blkQ.push_back(Qdiag);
set<int> active;
double result = fastQP(blkQ, c, Aeq, beq, Ain, bin, active, q);
// Enable the block below to use stephens' qp code
#if USE_EIQUADPROG_BACKUP
if (result == 1) {
Q += 1e-8 * MatrixXd::Identity(nq, nq);
result =
solve_quadprog(Q, c, -Aeq.transpose(), beq, -Ain.transpose(), bin, q);
if (mxIsInf(result)) {
result = 1;
} else {
result = 0;
}
}
#endif
plhs[0] = mxCreateDoubleMatrix(nq, 1, mxREAL);
memcpy(mxGetPrSafe(plhs[0]), q.data(), sizeof(double) * nq);
if (nlhs > 1) {
plhs[1] = mxCreateDoubleScalar(result);
}
return;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int error;
if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct QPControllerData* pdata;
mxArray* pm;
double* pr;
int i,j;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct QPControllerData;
// get control object properties
const mxArray* pobj = prhs[1];
pm = myGetProperty(pobj,"slack_limit");
pdata->slack_limit = mxGetScalar(pm);
pm = myGetProperty(pobj,"W_kdot");
assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
memcpy(pdata->W_kdot.data(),mxGetPrSafe(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));
pm= myGetProperty(pobj,"w_grf");
pdata->w_grf = mxGetScalar(pm);
pm= myGetProperty(pobj,"w_slack");
pdata->w_slack = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_ang");
pdata->Kp_ang = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_accel");
pdata->Kp_accel = mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_inputs");
pdata->n_body_accel_inputs = (int) mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_bounds");
pdata->n_body_accel_bounds = (int) mxGetScalar(pm);
mxArray* body_accel_bounds = myGetProperty(pobj,"body_accel_bounds");
Vector6d vecbound;
for (int i=0; i<pdata->n_body_accel_bounds; i++) {
pdata->accel_bound_body_idx.push_back((int) mxGetScalar(mxGetField(body_accel_bounds,i,"body_idx"))-1);
pm = mxGetField(body_accel_bounds,i,"min_acceleration");
assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
memcpy(vecbound.data(),mxGetPrSafe(pm),sizeof(double)*6);
pdata->min_body_acceleration.push_back(vecbound);
pm = mxGetField(body_accel_bounds,i,"max_acceleration");
assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
memcpy(vecbound.data(),mxGetPrSafe(pm),sizeof(double)*6);
pdata->max_body_acceleration.push_back(vecbound);
}
pm = myGetProperty(pobj,"body_accel_input_weights");
pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
memcpy(pdata->body_accel_input_weights.data(),mxGetPrSafe(pm),sizeof(double)*pdata->n_body_accel_inputs);
pdata->n_body_accel_eq_constraints = 0;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0)
pdata->n_body_accel_eq_constraints++;
}
// get robot mex model ptr
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));
pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
memcpy(pdata->B.data(),mxGetPrSafe(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));
int nq = pdata->r->num_positions, nu = pdata->B.cols();
pm = myGetProperty(pobj,"w_qdd");
pdata->w_qdd.resize(nq);
memcpy(pdata->w_qdd.data(),mxGetPrSafe(pm),sizeof(double)*nq);
pdata->umin.resize(nu);
pdata->umax.resize(nu);
memcpy(pdata->umin.data(),mxGetPrSafe(prhs[4]),sizeof(double)*nu);
memcpy(pdata->umax.data(),mxGetPrSafe(prhs[5]),sizeof(double)*nu);
pdata->B_act.resize(nu,nu);
pdata->B_act = pdata->B.bottomRows(nu);
// get the map ptr back from matlab
if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the seventh argument should be the map ptr");
memcpy(&pdata->map_ptr,mxGetPrSafe(prhs[6]),sizeof(pdata->map_ptr));
// pdata->map_ptr = NULL;
if (!pdata->map_ptr)
mexWarnMsgTxt("Map ptr is NULL. Assuming flat terrain at z=0");
// create gurobi environment
error = GRBloadenv(&(pdata->env),NULL);
// set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
mxArray* psolveropts = myGetProperty(pobj,"gurobi_options");
int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
// CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
if (method==2) {
CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
}
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:constructModelmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
// preallocate some memory
pdata->H.resize(nq,nq);
pdata->H_float.resize(6,nq);
pdata->H_act.resize(nu,nq);
pdata->C.resize(nq);
pdata->C_float.resize(6);
pdata->C_act.resize(nu);
pdata->J.resize(3,nq);
pdata->J_xy.resize(2,nq);
pdata->Hqp.resize(nq,nq);
pdata->fqp.resize(nq);
pdata->Ag.resize(6,nq);
pdata->Ak.resize(3,nq);
pdata->vbasis_len = 0;
pdata->cbasis_len = 0;
pdata->vbasis = NULL;
pdata->cbasis = NULL;
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
// for (i=0; i<pdata->r->num_bodies; i++)
// mexPrintf("body %d (%s) has %d contact points\n", i, pdata->r->bodies[i].linkname.c_str(), pdata->r->bodies[i].contact_pts.cols());
int nu = pdata->B.cols(), nq = pdata->r->num_positions, nv = pdata->r->num_velocities;
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
int narg=1;
int use_fast_qp = (int) mxGetScalar(prhs[narg++]);
Map< VectorXd > qddot_des(mxGetPrSafe(prhs[narg++]),nq);
Map<VectorXd> q(mxGetPrSafe(prhs[narg]), nq);
Map<VectorXd> qd(mxGetPrSafe(prhs[narg++]) + nq, nv);
vector<VectorXd,aligned_allocator<VectorXd>> body_accel_inputs;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
assert(mxGetM(prhs[narg])==7); assert(mxGetN(prhs[narg])==1);
VectorXd v = VectorXd::Zero(7,1);
memcpy(v.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*7);
body_accel_inputs.push_back(v);
}
int num_condof;
VectorXd condof;
if (!mxIsEmpty(prhs[narg])) {
assert(mxGetN(prhs[narg])==1);
num_condof=mxGetM(prhs[narg]);
condof = VectorXd::Zero(num_condof);
memcpy(condof.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*num_condof);
}
else {
num_condof=0;
narg++; // skip over empty vector
}
int desired_support_argid = narg++;
Map<MatrixXd> A_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> B_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> Qy (mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> R_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> C_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> D_ls(mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> S (mxGetPrSafe(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<VectorXd> s1(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
Map<VectorXd> s1dot(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
double s2dot = mxGetScalar(prhs[narg++]);
Map<VectorXd> x0(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
Map<VectorXd> u0(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
Map<VectorXd> y0(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
Map<VectorXd> qdd_lb(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
Map<VectorXd> qdd_ub(mxGetPrSafe(prhs[narg]),mxGetNumberOfElements(prhs[narg])); narg++;
memcpy(pdata->w_qdd.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*nq);
double mu = mxGetScalar(prhs[narg++]);
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
KinematicsCache<double> cache = pdata->r->doKinematics(q, qd);
//---------------------------------------------------------------------
// Compute active support from desired supports -----------------------
MatrixXd all_body_contact_pts;
vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> active_supports;
set<int> contact_bodies; // redundant, clean up later
int num_active_contact_pts=0;
if (!mxIsEmpty(prhs[desired_support_argid])) {
VectorXd phi;
mxArray* mxBodies = myGetField(prhs[desired_support_argid],"bodies");
if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
double* pBodies = mxGetPrSafe(mxBodies);
mxArray* mxContactPts = myGetField(prhs[desired_support_argid],"contact_pts");
if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
assert(mxGetM(mxBodyContactPts) == 3);
int nc = static_cast<int>(mxGetN(mxBodyContactPts));
if (nc<1) continue;
all_body_contact_pts.resize(mxGetM(mxBodyContactPts),mxGetN(mxBodyContactPts));
pr = mxGetPrSafe(mxBodyContactPts);
memcpy(all_body_contact_pts.data(),pr,sizeof(double)*mxGetM(mxBodyContactPts)*mxGetN(mxBodyContactPts));
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
for (j=0; j<nc; j++) {
se.contact_pts.push_back(all_body_contact_pts.col(j));
}
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
std::unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
pdata->H = pdata->r->massMatrix(cache).value();
pdata->C = pdata->r->dynamicsBiasTerm(cache, f_ext).value();
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head<6>();
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->Ag = pdata->r->centroidalMomentumMatrix(cache, 0).value();
pdata->Agdot_times_v = pdata->r->centroidalMomentumMatrixDotTimesV(cache, 0).value();
pdata->Ak = pdata->Ag.topRows<3>();
pdata->Akdot_times_v = pdata->Agdot_times_v.topRows<3>();
}
// consider making all J's into row-major
Vector3d xcom = pdata->r->centerOfMass(cache);
pdata->J = pdata->r->centerOfMassJacobian(cache, 0).value();
pdata->Jdotv = pdata->r->centerOfMassJacobianDotTimesV(cache, 0).value();
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdotv_xy = pdata->Jdotv.head<2>();
MatrixXd Jcom;
VectorXd Jcomdotv;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdotv = pdata->Jdotv;
}
else {
Jcom = pdata->J_xy;
Jcomdotv = pdata->Jdotv_xy;
}
MatrixXd B,JB,Jp,normals;
VectorXd Jpdotv;
std::vector<double> support_mus(active_supports.size(), mu);
int nc = contactConstraintsBV(pdata->r, cache, num_active_contact_pts, support_mus, active_supports, B, JB, Jp, Jpdotv, normals);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qd;
}
else {
xlimp.resize(4);
xlimp.topRows(2) = xcom.topRows(2);
xlimp.bottomRows(2) = Jcom*qd;
}
x_bar = xlimp-x0;
D_float = JB.topRows(6);
D_act = JB.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qd;
kdot_des = -pdata->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
// w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
// w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
VectorXd tmp = C_ls*xlimp;
VectorXd tmp1 = Jcomdotv;
MatrixXd tmp2 = R_DQyD_ls*Jcom;
pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
pdata->fqp += tmp1.transpose()*tmp2;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
pdata->fqp -= u0.transpose()*tmp2;
pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
pdata->fqp -= (pdata->w_qdd.array()*qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += pdata->Akdot_times_v.transpose() * pdata->W_kdot * pdata->Ak;
pdata->fqp -= kdot_des.transpose()*pdata->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*pdata->n_body_accel_eq_constraints+num_condof;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
beq.segment(6,neps) = -Jpdotv -pdata->Kp_accel*Jp*qd;
}
// add in body spatial equality constraints
VectorXd body_vdot;
int body_idx;
int equality_ind = 6+neps;
Vector3d origin = Vector3d::Zero();
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0) {
// negative implies constraint
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
auto Jb = pdata->r->forwardKinJacobian(cache, origin, body_idx, 0, 1, false, 0).value();
auto Jbdot_times_v = pdata->r->forwardJacDotTimesV(cache, origin, body_idx, 0, 1, 0).value();
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot_times_v(j) + body_vdot[j];
}
}
}
}
}
if (num_condof>0) {
// add joint acceleration constraints
for (int i=0; i<num_condof; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = qddot_des[(int)condof[i]-1];
}
}
int n_ineq = 2*nu+2*6*pdata->n_body_accel_bounds;
MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(n_ineq);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
int body_index;
int constraint_start_index = 2*nu;
for (int i=0; i<pdata->n_body_accel_bounds; i++) {
body_index = pdata->accel_bound_body_idx[i];
auto Jb = pdata->r->forwardKinJacobian(cache, origin, body_index, 0, 1, false, 0).value();
auto Jbdot_times_v = pdata->r->forwardJacDotTimesV(cache, origin, body_index, 0, 1, 0).value();
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb;
bin.segment(constraint_start_index,6) = -Jbdot_times_v + pdata->max_body_acceleration[i];
constraint_start_index += 6;
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb;
bin.segment(constraint_start_index,6) = Jbdot_times_v - pdata->min_body_acceleration[i];
constraint_start_index += 6;
}
for (int i=0; i<n_ineq; i++) {
// remove inf constraints---needed by gurobi
if (std::isinf(double(bin(i)))) {
Ain.row(i) = 0*Ain.row(i);
bin(i)=0;
}
}
GRBmodel * model = NULL;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = qdd_lb;
ub.head(nq) = qdd_ub;
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -pdata->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = pdata->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (pdata->w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
bool include_body_accel_cost_terms = pdata->n_body_accel_inputs > 0 && pdata->body_accel_input_weights.array().maxCoeff() > 1e-10;
if (use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(pdata->w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
//if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*pdata->W_kdot*pdata->Ak;
}
pdata->Hqp += pdata->w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
}
// add in body spatial acceleration cost terms
double w_i;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
w_i=pdata->body_accel_input_weights(i);
if (w_i > 0) {
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
auto Jb = pdata->r->forwardKinJacobian(cache, origin, body_idx, 0, 1, false, 0).value();
auto Jbdot_times_v = pdata->r->forwardJacDotTimesV(cache, origin, body_idx, 0, 1, 0).value();
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
pdata->Hqp += w_i*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += w_i*(Jbdot_times_v(j) - body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,pdata->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,pdata->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
//if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->vbasis,pdata->vbasis_len,pdata->cbasis,pdata->cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&pdata->vbasis_len), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&pdata->cbasis_len), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
//if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
VectorXd y(nu);
VectorXd qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
y = pdata->B_act.transpose()*(pdata->H_act*qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
if (nlhs>0) {
plhs[0] = eigenToMatlab(y);
}
if (nlhs>1) {
plhs[1] = eigenToMatlab(qdd);
}
if (nlhs>2) {
plhs[2] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
memcpy(mxGetData(plhs[2]),&info,sizeof(int));
}
if (nlhs>3) {
plhs[3] = mxCreateDoubleMatrix(1,active_supports.size(),mxREAL);
pr = mxGetPrSafe(plhs[3]);
int i=0;
for (vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
pr[i++] = (double) (iter->body_idx + 1);
}
}
if (nlhs>4) {
plhs[4] = eigenToMatlab(alpha);
}
if (nlhs>5) {
plhs[5] = eigenToMatlab(pdata->Hqp);
}
if (nlhs>6) {
plhs[6] = eigenToMatlab(f);
}
if (nlhs>7) {
plhs[7] = eigenToMatlab(Aeq);
}
if (nlhs>8) {
plhs[8] = eigenToMatlab(beq);
}
if (nlhs>9) {
plhs[9] = eigenToMatlab(Ain_lb_ub);
}
if (nlhs>10) {
plhs[10] = eigenToMatlab(bin_lb_ub);
}
if (nlhs>11) {
plhs[11] = eigenToMatlab(Qnfdiag);
}
if (nlhs>12) {
plhs[12] = eigenToMatlab(Qneps);
}
if (nlhs>13) {
double Vdot;
if (nc>0)
// note: Sdot is 0 for ZMP/double integrator dynamics, so we omit that term here
Vdot = ((2*x_bar.transpose()*S + s1.transpose())*(A_ls*x_bar + B_ls*(Jcomdotv + Jcom*qdd)) + s1dot.transpose()*x_bar)(0) + s2dot;
else
Vdot = 0;
plhs[13] = mxCreateDoubleScalar(Vdot);
}
if (nlhs>14) {
RigidBodyManipulator* r = pdata->r;
VectorXd individual_cops = individualSupportCOPs(r, cache, active_supports, normals, B, beta);
plhs[14] = eigenToMatlab(individual_cops);
}
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
}
double operator()(const VectorXd& x) const {
return py::extract<double>(m_pyfunc(toNdarray1<double>(x.data(), x.size())));
}
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;
RigidBodyTree *model =
static_cast<RigidBodyTree *>(getDrakeMexPointer(prhs[arg_num++]));
KinematicsCache<double> &cache =
fromMex(prhs[arg_num++], static_cast<KinematicsCache<double> *>(nullptr));
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;
Matrix3Xd normals;
model->collisionRaycast(cache, origins, ray_endpoints, distances, normals,
use_margins);
if (nlhs >= 1) {
plhs[0] =
mxCreateDoubleMatrix(static_cast<int>(distances.size()), 1, mxREAL);
memcpy(mxGetPrSafe(plhs[0]), distances.data(),
sizeof(double) * distances.size());
}
if (nlhs >= 2) {
plhs[1] =
mxCreateDoubleMatrix(3, static_cast<int>(normals.size()) / 3, mxREAL);
memcpy(mxGetPrSafe(plhs[1]), normals.data(),
sizeof(double) * normals.size());
}
}
void load(const string &filename_id, const string &filename_exp) {
cout << "loading prior data ..." << endl;
const string fnwid = filename_id;
ifstream fwid(fnwid, ios::in | ios::binary);
int ndims;
fwid.read(reinterpret_cast<char*>(&ndims), sizeof(int));
cout << "identity prior dim = " << ndims << endl;
Wid_avg.resize(ndims);
Wid0.resize(ndims);
sigma_Wid.resize(ndims, ndims);
fwid.read(reinterpret_cast<char*>(Wid_avg.data()), sizeof(double)*ndims);
fwid.read(reinterpret_cast<char*>(Wid0.data()), sizeof(double)*ndims);
fwid.read(reinterpret_cast<char*>(sigma_Wid.data()), sizeof(double)*ndims*ndims);
int m, n;
fwid.read(reinterpret_cast<char*>(&m), sizeof(int));
fwid.read(reinterpret_cast<char*>(&n), sizeof(int));
cout << "Uid size: " << m << 'x' << n << endl;
Uid.resize(m, n);
fwid.read(reinterpret_cast<char*>(Uid.data()), sizeof(double)*m*n);
fwid.close();
message("identity prior loaded.");
/*
cout << "Wid_avg = " << Wid_avg << endl;
cout << "Wid0 = " << Wid0 << endl;
cout << "sigma_Wid = " << sigma_Wid << endl;
cout << "Uid = " << Uid << endl;
*/
message("processing identity prior.");
inv_sigma_Wid = sigma_Wid.inverse();
message("done");
const string fnwexp = filename_exp;
ifstream fwexp(fnwexp, ios::in | ios::binary);
fwexp.read(reinterpret_cast<char*>(&ndims), sizeof(int));
cout << "expression prior dim = " << ndims << endl;
Wexp0.resize(ndims);
Wexp_avg.resize(ndims);
sigma_Wexp.resize(ndims, ndims);
fwexp.read(reinterpret_cast<char*>(Wexp_avg.data()), sizeof(double)*ndims);
fwexp.read(reinterpret_cast<char*>(Wexp0.data()), sizeof(double)*ndims);
fwexp.read(reinterpret_cast<char*>(sigma_Wexp.data()), sizeof(double)*ndims*ndims);
fwexp.read(reinterpret_cast<char*>(&m), sizeof(int));
fwexp.read(reinterpret_cast<char*>(&n), sizeof(int));
cout << "Uexp size: " << m << 'x' << n << endl;
Uexp.resize(m, n);
fwexp.read(reinterpret_cast<char*>(Uexp.data()), sizeof(double)*m*n);
fwexp.close();
message("expression prior loaded.");
/*
cout << "Wexp_avg = " << Wexp_avg << endl;
cout << "Wexp0 = " << Wexp0 << endl;
cout << "sigma_Wexp = " << sigma_Wexp << endl;
cout << "Uexp = " << Uexp << endl;
*/
message("processing expression prior.");
inv_sigma_Wexp = sigma_Wexp.inverse();
message("done.");
}
