snopt_adapter.cc
Go to the documentation of this file.
00001 /******************************************************************************
00002 Copyright (c) 2017, Alexander W. Winkler, ETH Zurich. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification,
00005 are permitted provided that the following conditions are met:
00006     * Redistributions of source code must retain the above copyright notice,
00007       this list of conditions and the following disclaimer.
00008     * Redistributions in binary form must reproduce the above copyright notice,
00009       this list of conditions and the following disclaimer in the documentation
00010       and/or other materials provided with the distribution.
00011     * Neither the name of ETH ZURICH nor the names of its contributors may be
00012       used to endorse or promote products derived from this software without
00013       specific prior written permission.
00014 
00015 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00016 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00017 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00018 DISCLAIMED. IN NO EVENT SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT,
00019 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00020 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00021 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00022 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00023 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00024 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00025 ******************************************************************************/
00026 
00027 #include <ifopt_snopt/snopt_adapter.h>
00028 
00029 namespace ifopt {
00030 
00031 SnoptAdapter::NLPPtr SnoptAdapter::nlp_;
00032 
00033 void
00034 SnoptAdapter::Solve (Problem& ref)
00035 {
00036   SnoptAdapter snopt(ref);
00037   snopt.Init();
00038   SetOptions(snopt);
00039 
00040   // error codes as given in the manual.
00041   int Cold = 0; // Basis = 1, Warm = 2;
00042   int INFO = snopt.solve(Cold);
00043   int EXIT = INFO - INFO%10; // change least significant digit to zero
00044 
00045   if (EXIT != 0) {
00046     std::string msg = "Snopt failed to find a solution. EXIT:" + std::to_string(EXIT) + ", INFO:" + std::to_string(INFO);
00047     throw std::runtime_error(msg);
00048   }
00049 
00050   snopt.SetVariables();
00051 }
00052 
00053 void SnoptAdapter::SetOptions(SnoptAdapter& solver)
00054 {
00055   // A complete list of options can be found in the snopt user guide:
00056   // https://web.stanford.edu/group/SOL/guides/sndoc7.pdf
00057 
00058   solver.setProbName( "snopt" );
00059   solver.setIntParameter( "Major Print level", 1 );
00060   solver.setIntParameter( "Minor Print level", 1 );
00061   solver.setParameter( "Solution");
00062   solver.setIntParameter( "Derivative option", 1 ); // 1 = snopt will not calculate missing derivatives
00063   solver.setIntParameter( "Verify level ", 3 ); // full check on gradients, will throw error
00064   solver.setIntParameter("Iterations limit", 200000);
00065   solver.setRealParameter( "Major feasibility tolerance",  1.0e-3); // target nonlinear constraint violation
00066   solver.setRealParameter( "Minor feasibility tolerance",  1.0e-3); // for satisfying the QP bounds
00067   solver.setRealParameter( "Major optimality tolerance",   1.0e-2); // target complementarity gap
00068 }
00069 
00070 SnoptAdapter::SnoptAdapter (Problem& ref)
00071 {
00072   nlp_ = &ref;
00073 }
00074 
00075 void
00076 SnoptAdapter::Init ()
00077 {
00078   int obj_count = nlp_->HasCostTerms()? 1 : 0;
00079   n     = nlp_->GetNumberOfOptimizationVariables();
00080   neF   = nlp_->GetNumberOfConstraints() + obj_count;
00081 
00082   x      = new double[n];
00083   xlow   = new double[n];
00084   xupp   = new double[n];
00085   xmul   = new double[n];
00086   xstate = new    int[n];
00087 
00088   F      = new double[neF];
00089   Flow   = new double[neF];
00090   Fupp   = new double[neF];
00091   Fmul   = new double[neF];
00092   Fstate = new    int[neF];
00093 
00094   // Set the upper and lower bounds.
00095   // no bounds on the spline coefficients or footholds
00096   auto bounds_x = nlp_->GetBoundsOnOptimizationVariables();
00097   for (uint _n=0; _n<bounds_x.size(); ++_n) {
00098     xlow[_n] = bounds_x.at(_n).lower_;
00099     xupp[_n] = bounds_x.at(_n).upper_;
00100     xstate[_n] = 0;
00101   }
00102 
00103   // bounds on the cost function if it exists
00104   int c = 0;
00105   if (nlp_->HasCostTerms()) {
00106     Flow[c] = -1e20; Fupp[c] = 1e20; Fmul[c] = 0.0; // no bounds on cost function
00107     c++;
00108   }
00109 
00110   // bounds on equality and inequality constraints
00111   auto bounds_g = nlp_->GetBoundsOnConstraints();
00112   for (const auto& b : bounds_g) {
00113     Flow[c] = b.lower_; Fupp[c] = b.upper_; Fmul[c] = 0.0;
00114     c++;
00115   }
00116 
00117   // initial values of the optimization
00118   VectorXd x_all = nlp_->GetVariableValues();
00119   Eigen::Map<VectorXd>(&x[0], x_all.rows()) = x_all;
00120 
00121   ObjRow  = nlp_->HasCostTerms()? 0 : -1; // the row in user function that corresponds to the objective function
00122   ObjAdd  = 0.0;                          // the constant to be added to the objective function
00123 
00124   // no linear derivatives/just assume all are nonlinear
00125   lenA = 0;
00126   neA = 0;
00127   iAfun = nullptr;
00128   jAvar = nullptr;
00129   A = nullptr;
00130 
00131   // derivatives of nonlinear part
00132   lenG  = obj_count*n + nlp_->GetJacobianOfConstraints().nonZeros();
00133   iGfun = new int[lenG];
00134   jGvar = new int[lenG];
00135 
00136   // the gradient terms of the cost function
00137   neG=0; // nonzero cells in jacobian of Cost Function AND Constraints
00138 
00139   // the nonzero elements of cost function (assume all)
00140   if(nlp_->HasCostTerms()) {
00141     for (int var=0; var<n; ++var) {
00142       iGfun[neG] = 0;
00143       jGvar[neG] = var;
00144       neG++;
00145     }
00146   }
00147 
00148   // the nonzero elements of constraints (assume all)
00149   auto jac = nlp_->GetJacobianOfConstraints();
00150   for (int k=0; k<jac.outerSize(); ++k) {
00151     for (Jacobian::InnerIterator it(jac,k); it; ++it) {
00152       iGfun[neG] = it.row() + obj_count;
00153       jGvar[neG] = it.col();
00154       neG++;
00155     }
00156   }
00157 
00158   setUserFun(&SnoptAdapter::ObjectiveAndConstraintFct);
00159 }
00160 
00161 void
00162 SnoptAdapter::ObjectiveAndConstraintFct (int* Status, int* n, double x[],
00163                                          int* needF, int* neF, double F[],
00164                                          int* needG, int* neG, double G[],
00165                                          char* cu, int* lencu, int iu[],
00166                                          int* leniu, double ru[], int* lenru)
00167 {
00168   if ( *needF > 0 ) {
00169     int i=0;
00170 
00171     // the scalar objective function value
00172     if (nlp_->HasCostTerms())
00173       F[i++] = nlp_->EvaluateCostFunction(x);
00174 
00175     // the vector of constraint values
00176     VectorXd g_eig = nlp_->EvaluateConstraints(x);
00177     Eigen::Map<VectorXd>(F+i, g_eig.rows()) = g_eig;
00178   }
00179 
00180 
00181   if ( *needG > 0 ) {
00182     int i=0;
00183 
00184     // the jacobian of the first row (cost function)
00185     if (nlp_->HasCostTerms()) {
00186       Eigen::VectorXd grad = nlp_->EvaluateCostFunctionGradient(x);
00187       i = grad.rows();
00188       Eigen::Map<VectorXd>(G, i) = grad;
00189     }
00190 
00191     // the jacobian of all the constraints
00192     nlp_->EvalNonzerosOfJacobian(x, G+i);
00193     nlp_->SaveCurrent();
00194   }
00195 }
00196 
00197 void
00198 SnoptAdapter::SetVariables ()
00199 {
00200   nlp_->SetVariables(x);
00201 }
00202 
00203 } /* namespace opt */


ifopt_snopt
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 21 2018 03:01:50