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