SolutionResult Drake::SnoptSolver::Solve( OptimizationProblem& prog) const { auto d = prog.GetSolverData<SNOPTData>(); SNOPTRun cur(*d); Drake::OptimizationProblem const* current_problem = &prog; // Set the "maxcu" value to tell snopt to reserve one 8-char entry of user // workspace. We are then allowed to use cw(snopt_mincw+1:maxcu), as // expressed in Fortran array slicing. Use the space to pass our problem // instance pointer to our userfun. cur.snSeti("User character workspace", snopt_mincw + 1); { char const* const pcp = reinterpret_cast<char*>(¤t_problem); char* const cu_cp = d->cw.data() + 8 * snopt_mincw; std::copy(pcp, pcp + sizeof(current_problem), cu_cp); } snopt::integer nx = prog.num_vars(); d->min_alloc_x(nx); snopt::doublereal* x = d->x.data(); snopt::doublereal* xlow = d->xlow.data(); snopt::doublereal* xupp = d->xupp.data(); const Eigen::VectorXd x_initial_guess = prog.initial_guess(); for (int i = 0; i < nx; i++) { x[i] = static_cast<snopt::doublereal>(x_initial_guess(i)); xlow[i] = static_cast<snopt::doublereal>(-std::numeric_limits<double>::infinity()); xupp[i] = static_cast<snopt::doublereal>(std::numeric_limits<double>::infinity()); } for (auto const& binding : prog.bounding_box_constraints()) { auto const& c = binding.constraint(); for (const DecisionVariableView& v : binding.variable_list()) { auto const lb = c->lower_bound(), ub = c->upper_bound(); for (int k = 0; k < v.size(); k++) { xlow[v.index() + k] = std::max<snopt::doublereal>( static_cast<snopt::doublereal>(lb(k)), xlow[v.index() + k]); xupp[v.index() + k] = std::min<snopt::doublereal>( static_cast<snopt::doublereal>(ub(k)), xupp[v.index() + k]); } } } size_t num_nonlinear_constraints = 0, max_num_gradients = nx; for (auto const& binding : prog.generic_constraints()) { auto const& c = binding.constraint(); size_t n = c->num_constraints(); for (const DecisionVariableView& v : binding.variable_list()) { max_num_gradients += n * v.size(); } num_nonlinear_constraints += n; } size_t num_linear_constraints = 0; for (auto const& binding : prog.linear_equality_constraints()) { num_linear_constraints += binding.constraint()->num_constraints(); } snopt::integer nF = 1 + num_nonlinear_constraints + num_linear_constraints; d->min_alloc_F(nF); snopt::doublereal* Flow = d->Flow.data(); snopt::doublereal* Fupp = d->Fupp.data(); Flow[0] = static_cast<snopt::doublereal>(-std::numeric_limits<double>::infinity()); Fupp[0] = static_cast<snopt::doublereal>(std::numeric_limits<double>::infinity()); snopt::integer lenG = static_cast<snopt::integer>(max_num_gradients); d->min_alloc_G(lenG); snopt::integer* iGfun = d->iGfun.data(); snopt::integer* jGvar = d->jGvar.data(); for (snopt::integer i = 0; i < nx; i++) { iGfun[i] = 1; jGvar[i] = i + 1; } size_t constraint_index = 1, grad_index = nx; // constraint index starts at 1 // because the objective is the // first row for (auto const& binding : prog.generic_constraints()) { auto const& c = binding.constraint(); size_t n = c->num_constraints(); auto const lb = c->lower_bound(), ub = c->upper_bound(); for (int i = 0; i < n; i++) { Flow[constraint_index + i] = static_cast<snopt::doublereal>(lb(i)); Fupp[constraint_index + i] = static_cast<snopt::doublereal>(ub(i)); } for (const DecisionVariableView& v : binding.variable_list()) { for (size_t i = 0; i < n; i++) { for (size_t j = 0; j < v.size(); j++) { iGfun[grad_index] = constraint_index + i + 1; // row order jGvar[grad_index] = v.index() + j + 1; grad_index++; } } } constraint_index += n; } // http://eigen.tuxfamily.org/dox/group__TutorialSparse.html typedef Eigen::Triplet<double> T; std::vector<T> tripletList; tripletList.reserve(num_linear_constraints * prog.num_vars()); size_t linear_constraint_index = 0; for (auto const& binding : prog.linear_equality_constraints()) { auto const& c = binding.constraint(); size_t n = c->num_constraints(); size_t var_index = 0; Eigen::SparseMatrix<double> A_constraint = c->GetSparseMatrix(); for (const DecisionVariableView& v : binding.variable_list()) { for (size_t k = 0; k < v.size(); ++k) { for (Eigen::SparseMatrix<double>::InnerIterator it( A_constraint, var_index + k); it; ++it) { tripletList.push_back( T(linear_constraint_index + it.row(), v.index() + k, it.value())); } } var_index += v.size(); } auto const lb = c->lower_bound(), ub = c->upper_bound(); for (int i = 0; i < n; i++) { Flow[constraint_index + i] = static_cast<snopt::doublereal>(lb(i)); Fupp[constraint_index + i] = static_cast<snopt::doublereal>(ub(i)); } constraint_index += n; linear_constraint_index += n; } snopt::integer lenA = static_cast<snopt::integer>(tripletList.size()); d->min_alloc_A(lenA); snopt::doublereal* A = d->A.data(); snopt::integer* iAfun = d->iAfun.data(); snopt::integer* jAvar = d->jAvar.data(); size_t A_index = 0; for (auto const& it : tripletList) { A[A_index] = it.value(); iAfun[A_index] = 1 + num_nonlinear_constraints + it.row() + 1; jAvar[A_index] = it.col() + 1; A_index++; } snopt::integer nxname = 1, nFname = 1, npname = 0; char xnames[8 * 1]; // should match nxname char Fnames[8 * 1]; // should match nFname char Prob[200] = "drake.out"; snopt::integer nS, nInf; snopt::doublereal sInf; if (true) { // print to output file (todo: make this an option) cur.iPrint = 9; char print_file_name[50] = "snopt.out"; snopt::integer print_file_name_len = static_cast<snopt::integer>(strlen(print_file_name)); snopt::integer inform; snopt::snopenappend_(&cur.iPrint, print_file_name, &inform, print_file_name_len); cur.snSeti("Major print level", static_cast<snopt::integer>(11)); cur.snSeti("Print file", cur.iPrint); } snopt::integer minrw, miniw, mincw; cur.snMemA(nF, nx, nxname, nFname, lenA, lenG, &mincw, &miniw, &minrw); d->min_alloc_w(mincw, miniw, minrw); cur.snSeti("Total character workspace", d->lencw); cur.snSeti("Total integer workspace", d->leniw); cur.snSeti("Total real workspace", d->lenrw); snopt::integer Cold = 0; snopt::doublereal* xmul = d->xmul.data(); snopt::integer* xstate = d->xstate.data(); memset(xstate, 0, sizeof(snopt::integer) * nx); snopt::doublereal* F = d->F.data(); snopt::doublereal* Fmul = d->Fmul.data(); snopt::integer* Fstate = d->Fstate.data(); memset(Fstate, 0, sizeof(snopt::integer) * nF); snopt::doublereal ObjAdd = 0.0; snopt::integer ObjRow = 1; // feasibility problem (for now) // TODO(sam.creasey) These should be made into options when #1879 is // resolved or deleted. /* mysnseti("Derivative option",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"DerivativeOption"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Major iterations limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"MajorIterationsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Minor iterations limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"MinorIterationsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnsetr("Major optimality tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"MajorOptimalityTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnsetr("Major feasibility tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"MajorFeasibilityTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnsetr("Minor feasibility tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"MinorFeasibilityTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Superbasics limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"SuperbasicsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Verify level",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"VerifyLevel"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Iterations Limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"IterationsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Scale option",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"ScaleOption"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("New basis file",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"NewBasisFile"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Old basis file",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"OldBasisFile"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnseti("Backup basis file",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"BackupBasisFile"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); mysnsetr("Linesearch tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"LinesearchTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw); */ snopt::integer info; snopt::snopta_( &Cold, &nF, &nx, &nxname, &nFname, &ObjAdd, &ObjRow, Prob, snopt_userfun, iAfun, jAvar, &lenA, &lenA, A, iGfun, jGvar, &lenG, &lenG, xlow, xupp, xnames, Flow, Fupp, Fnames, x, xstate, xmul, F, Fstate, Fmul, &info, &mincw, &miniw, &minrw, &nS, &nInf, &sInf, d->cw.data(), &d->lencw, d->iw.data(), &d->leniw, d->rw.data(), &d->lenrw, // Pass the snopt workspace as the user workspace. We already set // the maxcu option to reserve some of it for our user code. d->cw.data(), &d->lencw, d->iw.data(), &d->leniw, d->rw.data(), &d->lenrw, npname, 8 * nxname, 8 * nFname, 8 * d->lencw, 8 * d->lencw); Eigen::VectorXd sol(nx); for (int i = 0; i < nx; i++) { sol(i) = static_cast<double>(x[i]); } prog.SetDecisionVariableValues(sol); prog.SetSolverResult("SNOPT", info); // todo: extract the other useful quantities, too. if (info >= 1 && info <= 6) { return SolutionResult::kSolutionFound; } else if (info >= 11 && info <= 16) { return SolutionResult::kInfeasibleConstraints; } else if (info == 91) { return SolutionResult::kInvalidInput; } return SolutionResult::kUnknownError; }