Ejemplo n.º 1
0
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*>(&current_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;
}