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