void SetIKSolverOptions(const IKoptions& ikoptions,
drake::solvers::OptimizationProblem* prog) {
prog->SetSolverOption("SNOPT", "Derivative option", 1);
prog->SetSolverOption("SNOPT", "Major optimality tolerance",
ikoptions.getMajorOptimalityTolerance());
prog->SetSolverOption("SNOPT", "Major feasibility tolerance",
ikoptions.getMajorFeasibilityTolerance());
prog->SetSolverOption("SNOPT", "Superbasics limit",
ikoptions.getSuperbasicsLimit());
prog->SetSolverOption("SNOPT", "Major iterations limit",
ikoptions.getMajorIterationsLimit());
prog->SetSolverOption("SNOPT", "Iterations limit",
ikoptions.getIterationsLimit());
}
void approximateIK(RigidBodyTree * model, const MatrixBase<DerivedA> &q_seed, const MatrixBase<DerivedB> &q_nom, const int num_constraints, RigidBodyConstraint** const constraint_array, MatrixBase<DerivedC> &q_sol, int &INFO, const IKoptions &ikoptions)
{
int num_kc = 0;
int nq = model->num_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;
}
KinematicsCache<double> cache = model->doKinematics(q_seed); // TODO: pass this into the function?
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[])
{
/*
IKoptionsmex(1,robot_ptr) // construct IK options
IKoptionsmex(2,ikoptions_pts) // destroy IK options
IKoptionsmex(3,ikoptions_ptr,field,varargin) // update IK options
*/
if (nrhs<2)
{
mexErrMsgIdAndTxt("Drake:IKoptionsmex:BadInputs","See IKoptionsmex.cpp for usage");
}
int COMMAND = (int) mxGetScalar(prhs[0]);
switch (COMMAND)
{
case 1:
{
RigidBodyTree * robot = (RigidBodyTree *) getDrakeMexPointer(prhs[1]);
IKoptions* ikoptions = new IKoptions(robot);
mxArray* additional_argument = mxCreateDoubleScalar(2);
plhs[0] = createDrakeMexPointer((void*) ikoptions,"IKoptions",-1,1,&additional_argument);
}
break;
case 2:
{
destroyDrakeMexPointer<IKoptions*>(prhs[1]);
return;
}
break;
case 3:
{
IKoptions* ikoptions = (IKoptions*) getDrakeMexPointer(prhs[1]);
mwSize strlen = static_cast<mwSize>(mxGetNumberOfElements(prhs[2]))+1;
char* field = new char[strlen];
mxGetString(prhs[2],field,strlen);
string field_str(field);
int nq = ikoptions->getRobotPtr()->num_positions;
IKoptions* ikoptions_new = new IKoptions(*ikoptions);
if(field_str == "Q")
{
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != nq || mxGetN(prhs[3]) != nq)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","Q must be nq x nq double matrix");
}
MatrixXd Q(nq,nq);
memcpy(Q.data(),mxGetPrSafe(prhs[3]),sizeof(double)*nq*nq);
ikoptions_new->setQ(Q);
}
else if(field_str == "Qa")
{
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != nq || mxGetN(prhs[3]) != nq)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","Qa must be nq x nq double matrix");
}
MatrixXd Qa(nq,nq);
memcpy(Qa.data(),mxGetPrSafe(prhs[3]),sizeof(double)*nq*nq);
ikoptions_new->setQa(Qa);
}
else if(field_str == "Qv")
{
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != nq || mxGetN(prhs[3]) != nq)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","Qv must be nq x nq double matrix");
}
MatrixXd Qv(nq,nq);
memcpy(Qv.data(),mxGetPrSafe(prhs[3]),sizeof(double)*nq*nq);
ikoptions_new->setQv(Qv);
}
else if (field_str == "debug")
{
if(!mxIsLogicalScalar(prhs[3]))
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","debug must be a single boolean");
}
bool flag = *mxGetLogicals(prhs[3]);
ikoptions_new->setDebug(flag);
}
else if(field_str == "sequentialSeedFlag")
{
if(!mxIsLogicalScalar(prhs[3]))
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","sequentialSeedFlag must be a boolean scalar");
}
bool flag = *mxGetLogicals(prhs[3]);
ikoptions_new->setSequentialSeedFlag(flag);
}
else if(field_str == "majorOptimalityTolerance")
{
if(!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","MajorOptimalityTolerance must be a double scalar");
}
double tol = mxGetScalar(prhs[3]);
if(tol<=0)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","MajorOptimalityTolerance must be positive");
}
ikoptions_new->setMajorOptimalityTolerance(tol);
}
else if(field_str == "majorFeasibilityTolerance")
{
if(!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","MajorFeasibilityTolerance must be a double scalar");
}
double tol = mxGetScalar(prhs[3]);
if(tol<=0)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","MajorFeasibilityTolerance must be positive");
}
ikoptions_new->setMajorFeasibilityTolerance(tol);
}
else if(field_str == "superbasicsLimit")
{
if(!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","SuperbasicsLimit must be an integer scalar");
}
int limit = (int) mxGetScalar(prhs[3]);
if(limit <=0)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","SuperbasicsLimit must be positive");
}
ikoptions_new->setSuperbasicsLimit(limit);
}
else if(field_str == "majorIterationsLimit")
{
if(!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","MajorIterationsLimit must be an integer scalar");
}
int limit = (int) mxGetScalar(prhs[3]);
if(limit <=0)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","MajorIterationsLimit must be positive");
}
ikoptions_new->setMajorIterationsLimit(limit);
}
else if(field_str == "iterationsLimit")
{
if(!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","IterationsLimit must be an integer scalar");
}
int limit = (int) mxGetScalar(prhs[3]);
if(limit <=0)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","IterationsLimit must be positive");
}
ikoptions_new->setIterationsLimit(limit);
}
else if(field_str == "fixInitialState")
{
if(!mxIsLogicalScalar(prhs[3]))
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","fixInitialState must be a boolean scalar");
}
bool flag = *mxGetLogicals(prhs[3]);
ikoptions_new->setFixInitialState(flag);
}
else if(field_str == "q0")
{
if(!mxIsNumeric(prhs[3]) || !mxIsNumeric(prhs[4]) || mxGetM(prhs[3]) != nq || mxGetM(prhs[4]) != nq || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","q0_lb,q0_ub must be nq x 1 column double vector");
}
VectorXd lb(nq);
VectorXd ub(nq);
memcpy(lb.data(),mxGetPrSafe(prhs[3]),sizeof(double)*nq);
memcpy(ub.data(),mxGetPrSafe(prhs[4]),sizeof(double)*nq);
ikoptions_new->setq0(lb,ub);
}
else if(field_str == "qd0")
{
if(!mxIsNumeric(prhs[3]) || !mxIsNumeric(prhs[4]) || mxGetM(prhs[3]) != nq || mxGetM(prhs[4]) != nq || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","qd0_lb,qd0_ub must be nq x 1 column double vector");
}
VectorXd lb(nq);
VectorXd ub(nq);
memcpy(lb.data(),mxGetPrSafe(prhs[3]),sizeof(double)*nq);
memcpy(ub.data(),mxGetPrSafe(prhs[4]),sizeof(double)*nq);
ikoptions_new->setqd0(lb,ub);
}
else if(field_str == "qdf")
{
if(!mxIsNumeric(prhs[3]) || !mxIsNumeric(prhs[4]) || mxGetM(prhs[3]) != nq || mxGetM(prhs[4]) != nq || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","qdf_lb,qdf_ub must be nq x 1 column double vector");
}
VectorXd lb(nq);
VectorXd ub(nq);
memcpy(lb.data(),mxGetPrSafe(prhs[3]),sizeof(double)*nq);
memcpy(ub.data(),mxGetPrSafe(prhs[4]),sizeof(double)*nq);
ikoptions_new->setqdf(lb,ub);
}
else if(field_str == "additionaltSamples")
{
RowVectorXd t_samples;
if(mxGetNumberOfElements(prhs[3]) == 0)
{
t_samples.resize(0);
}
else
{
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","additional_tSamples must be a row double vector");
}
t_samples.resize(mxGetN(prhs[3]));
memcpy(t_samples.data(),mxGetPrSafe(prhs[3]),sizeof(double)*mxGetN(prhs[3]));
}
ikoptions_new->setAdditionaltSamples(t_samples);
}
else if(field_str == "robot")
{
RigidBodyTree * new_robot = (RigidBodyTree *) getDrakeMexPointer(prhs[3]);
ikoptions_new->updateRobot(new_robot);
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrIKoptionsmex:BadInputs","Unsupported field");
}
mxArray* additional_argument = mxCreateDoubleScalar(2);
plhs[0] = createDrakeMexPointer((void*) ikoptions_new,"IKoptions",-1,1,&additional_argument);
delete[] field;
}
break;
default:
mexErrMsgIdAndTxt("Drake:IKoptionsmex:BadInputs","Unknown command");
break;
}
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
if(nlhs != 19 || nrhs != 1)
{
mexErrMsgIdAndTxt("Drake:testIKoptions:BadInputs","Usage [robot_address,Q,Qa,Qv,debug_mode, sequentialSeedFlag,majorFeasibilityTolerance,majorIterationsLimit,iterationsLimit,superbasicsLimit,majorOptimalityTolerance,additional_tSamples,fixInitialState,q0_lb,q0_ub,qd0_lb,qd0_ub,qdf_lb,qdf_ub] = testIKoptionsmex(ikoptions_ptr)");
}
IKoptions* ikoptions = (IKoptions*) getDrakeMexPointer(prhs[0]);
long long robot_address = reinterpret_cast<long long>(ikoptions->getRobotPtr());
int nq = ikoptions->getRobotPtr()->num_positions;
MatrixXd Q;
ikoptions->getQ(Q);
MatrixXd Qv;
ikoptions->getQv(Qv);
MatrixXd Qa;
ikoptions->getQa(Qa);
bool debug_mode = ikoptions->getDebug();
bool sequentialSeedFlag = ikoptions->getSequentialSeedFlag();
double majorFeasibilityTolerance = ikoptions->getMajorFeasibilityTolerance();
int majorIterationsLimit = ikoptions->getMajorIterationsLimit();
int iterationsLimit = ikoptions->getIterationsLimit();
int superbasicsLimit = ikoptions->getSuperbasicsLimit();
double majorOptimalityTolerance = ikoptions->getMajorOptimalityTolerance();
RowVectorXd t_samples;
ikoptions->getAdditionaltSamples(t_samples);
bool fixInitialState = ikoptions->getFixInitialState();
VectorXd q0_lb,q0_ub;
VectorXd qd0_lb,qd0_ub;
VectorXd qdf_lb,qdf_ub;
ikoptions->getq0(q0_lb,q0_ub);
ikoptions->getqd0(qd0_lb,qd0_ub);
ikoptions->getqdf(qdf_lb,qdf_ub);
plhs[0] = mxCreateDoubleScalar((double) robot_address);
plhs[1] = mxCreateDoubleMatrix(nq,nq,mxREAL);
memcpy(mxGetPr(plhs[1]),Q.data(),sizeof(double)*nq*nq);
plhs[2] = mxCreateDoubleMatrix(nq,nq,mxREAL);
memcpy(mxGetPr(plhs[2]),Qa.data(),sizeof(double)*nq*nq);
plhs[3] = mxCreateDoubleMatrix(nq,nq,mxREAL);
memcpy(mxGetPr(plhs[3]),Qv.data(),sizeof(double)*nq*nq);
plhs[4] = mxCreateLogicalScalar(debug_mode);
plhs[5] = mxCreateLogicalScalar(sequentialSeedFlag);
plhs[6] = mxCreateDoubleScalar(majorFeasibilityTolerance);
plhs[7] = mxCreateDoubleScalar((double) majorIterationsLimit);
plhs[8] = mxCreateDoubleScalar((double) iterationsLimit);
plhs[9] = mxCreateDoubleScalar((double) superbasicsLimit);
plhs[10] = mxCreateDoubleScalar(majorOptimalityTolerance);
if(t_samples.size()>0)
{
plhs[11] = mxCreateDoubleMatrix(1,static_cast<int>(t_samples.size()),mxREAL);
memcpy(mxGetPr(plhs[11]),t_samples.data(),sizeof(double)*t_samples.size());
}
else
{
plhs[11] = mxCreateDoubleMatrix(0,0,mxREAL);
}
plhs[12] = mxCreateLogicalScalar(fixInitialState);
plhs[13] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[13]),q0_lb.data(),sizeof(double)*nq);
plhs[14] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[14]),q0_ub.data(),sizeof(double)*nq);
plhs[15] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[15]),qd0_lb.data(),sizeof(double)*nq);
plhs[16] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[16]),qd0_ub.data(),sizeof(double)*nq);
plhs[17] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[17]),qdf_lb.data(),sizeof(double)*nq);
plhs[18] = mxCreateDoubleMatrix(nq,1,mxREAL);
memcpy(mxGetPr(plhs[18]),qdf_ub.data(),sizeof(double)*nq);
}
void inverseKinBackend(
RigidBodyTree* model, const int nT,
const double* t, 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, std::vector<std::string>* infeasible_constraint) {
// Validate some basic parameters of the input.
if (q_seed.rows() != model->number_of_positions() || q_seed.cols() != nT ||
q_nom.rows() != model->number_of_positions() || q_nom.cols() != nT) {
throw std::runtime_error(
"Drake::inverseKinBackend: q_seed and q_nom must be of size "
"nq x nT");
}
KinematicsCacheHelper<double> kin_helper(model->bodies);
// TODO(sam.creasey) I really don't like rebuilding the
// OptimizationProblem for every timestep, but it's not possible to
// enable/disable (or even remove) a constraint from an
// OptimizationProblem between calls to Solve() currently, so
// there's not actually another way.
for (int t_index = 0; t_index < nT; t_index++) {
OptimizationProblem prog;
SetIKSolverOptions(ikoptions, &prog);
DecisionVariableView vars =
prog.AddContinuousVariables(model->number_of_positions());
MatrixXd Q;
ikoptions.getQ(Q);
auto objective = std::make_shared<InverseKinObjective>(model, Q);
prog.AddCost(objective, {vars});
for (int i = 0; i < num_constraints; i++) {
RigidBodyConstraint* constraint = constraint_array[i];
const int constraint_category = constraint->getCategory();
if (constraint_category ==
RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
SingleTimeKinematicConstraint* stc =
static_cast<SingleTimeKinematicConstraint*>(constraint);
if (!stc->isTimeValid(&t[t_index])) { continue; }
auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>(
stc, &kin_helper);
prog.AddConstraint(wrapper, {vars});
} else if (constraint_category ==
RigidBodyConstraint::PostureConstraintCategory) {
PostureConstraint* pc = static_cast<PostureConstraint*>(constraint);
if (!pc->isTimeValid(&t[t_index])) { continue; }
VectorXd lb;
VectorXd ub;
pc->bounds(&t[t_index], lb, ub);
prog.AddBoundingBoxConstraint(lb, ub, {vars});
} else if (
constraint_category ==
RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) {
AddSingleTimeLinearPostureConstraint(
&t[t_index], constraint, model->number_of_positions(),
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::QuasiStaticConstraintCategory) {
AddQuasiStaticConstraint(&t[t_index], constraint, &kin_helper,
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) {
throw std::runtime_error(
"MultipleTimeKinematicConstraint is not supported"
" in pointwise mode.");
} else if (
constraint_category ==
RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) {
throw std::runtime_error(
"MultipleTimeLinearPostureConstraint is not supported"
" in pointwise mode.");
}
}
// Add a bounding box constraint from the model.
prog.AddBoundingBoxConstraint(
model->joint_limit_min, model->joint_limit_max,
{vars});
// TODO(sam.creasey) would this be faster if we stored the view
// instead of copying into a VectorXd?
objective->set_q_nom(q_nom.col(t_index));
if (!ikoptions.getSequentialSeedFlag() || (t_index == 0)) {
prog.SetInitialGuess(vars, q_seed.col(t_index));
} else {
prog.SetInitialGuess(vars, q_sol->col(t_index - 1));
}
SolutionResult result = prog.Solve();
prog.PrintSolution();
q_sol->col(t_index) = vars.value();
info[t_index] = GetIKSolverInfo(prog, result);
}
}
