00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <ifopt_ipopt/ipopt_adapter.h>
00028
00029 namespace ifopt {
00030
00031 void
00032 IpoptAdapter::Solve (Problem& nlp)
00033 {
00034 using namespace Ipopt;
00035 using IpoptPtr = SmartPtr<TNLP>;
00036 using IpoptApplicationPtr = SmartPtr<IpoptApplication>;
00037
00038
00039 IpoptApplicationPtr ipopt_app_ = new IpoptApplication();
00040 ipopt_app_->RethrowNonIpoptException(true);
00041 SetOptions(ipopt_app_);
00042
00043 ApplicationReturnStatus status_ = ipopt_app_->Initialize();
00044 if (status_ != Solve_Succeeded) {
00045 std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
00046 throw std::length_error("Ipopt could not initialize correctly");
00047 }
00048
00049
00050 IpoptPtr nlp_ptr = new IpoptAdapter(nlp);
00051 status_ = ipopt_app_->OptimizeTNLP(nlp_ptr);
00052
00053 if (status_ != Solve_Succeeded) {
00054 std::string msg = "Ipopt failed to find a solution. ReturnCode: " + std::to_string(status_);
00055 std::cerr << msg;
00056 }
00057 }
00058
00059 void
00060 IpoptAdapter::SetOptions (Ipopt::SmartPtr<Ipopt::IpoptApplication> ipopt_app_)
00061 {
00062
00063
00064
00065
00066 ipopt_app_->Options()->SetStringValue("linear_solver", "ma27");
00067
00068 ipopt_app_->Options()->SetStringValue("hessian_approximation", "limited-memory");
00069 ipopt_app_->Options()->SetNumericValue("tol", 0.001);
00070 ipopt_app_->Options()->SetNumericValue("max_cpu_time", 40.0);
00071 ipopt_app_->Options()->SetIntegerValue("print_level", 5);
00072 ipopt_app_->Options()->SetStringValue("print_user_options", "yes");
00073 ipopt_app_->Options()->SetStringValue("print_timing_statistics", "no");
00074
00075
00076
00077
00078
00079 }
00080
00081 IpoptAdapter::IpoptAdapter(Problem& nlp)
00082 {
00083 nlp_ = &nlp;
00084 }
00085
00086 bool IpoptAdapter::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
00087 Index& nnz_h_lag, IndexStyleEnum& index_style)
00088 {
00089 n = nlp_->GetNumberOfOptimizationVariables();
00090 m = nlp_->GetNumberOfConstraints();
00091
00092 nnz_jac_g = nlp_->GetJacobianOfConstraints().nonZeros();
00093 nnz_h_lag = n*n;
00094
00095
00096 index_style = C_STYLE;
00097
00098 return true;
00099 }
00100
00101 bool IpoptAdapter::get_bounds_info(Index n, double* x_lower, double* x_upper,
00102 Index m, double* g_l, double* g_u)
00103 {
00104 auto bounds_x = nlp_->GetBoundsOnOptimizationVariables();
00105 for (uint c=0; c<bounds_x.size(); ++c) {
00106 x_lower[c] = bounds_x.at(c).lower_;
00107 x_upper[c] = bounds_x.at(c).upper_;
00108 }
00109
00110
00111 auto bounds_g = nlp_->GetBoundsOnConstraints();
00112 for (uint c=0; c<bounds_g.size(); ++c) {
00113 g_l[c] = bounds_g.at(c).lower_;
00114 g_u[c] = bounds_g.at(c).upper_;
00115 }
00116
00117 return true;
00118 }
00119
00120 bool IpoptAdapter::get_starting_point(Index n, bool init_x, double* x,
00121 bool init_z, double* z_L, double* z_U,
00122 Index m, bool init_lambda,
00123 double* lambda)
00124 {
00125
00126 assert(init_x == true);
00127 assert(init_z == false);
00128 assert(init_lambda == false);
00129
00130 VectorXd x_all = nlp_->GetVariableValues();
00131 Eigen::Map<VectorXd>(&x[0], x_all.rows()) = x_all;
00132
00133 return true;
00134 }
00135
00136 bool IpoptAdapter::eval_f(Index n, const double* x, bool new_x, double& obj_value)
00137 {
00138 obj_value = nlp_->EvaluateCostFunction(x);
00139 return true;
00140 }
00141
00142 bool IpoptAdapter::eval_grad_f(Index n, const double* x, bool new_x, double* grad_f)
00143 {
00144 Eigen::VectorXd grad = nlp_->EvaluateCostFunctionGradient(x);
00145 Eigen::Map<Eigen::MatrixXd>(grad_f,n,1) = grad;
00146 return true;
00147 }
00148
00149 bool IpoptAdapter::eval_g(Index n, const double* x, bool new_x, Index m, double* g)
00150 {
00151 VectorXd g_eig = nlp_->EvaluateConstraints(x);
00152 Eigen::Map<VectorXd>(g,m) = g_eig;
00153 return true;
00154 }
00155
00156 bool IpoptAdapter::eval_jac_g(Index n, const double* x, bool new_x,
00157 Index m, Index nele_jac, Index* iRow, Index *jCol,
00158 double* values)
00159 {
00160
00161 if (values == NULL) {
00162
00163 auto jac = nlp_->GetJacobianOfConstraints();
00164 int nele=0;
00165 for (int k=0; k<jac.outerSize(); ++k) {
00166 for (Jacobian::InnerIterator it(jac,k); it; ++it) {
00167 iRow[nele] = it.row();
00168 jCol[nele] = it.col();
00169 nele++;
00170 }
00171 }
00172
00173 assert(nele == nele_jac);
00174 }
00175 else {
00176
00177 nlp_->EvalNonzerosOfJacobian(x, values);
00178 }
00179
00180 return true;
00181 }
00182
00183 bool IpoptAdapter::intermediate_callback(Ipopt::AlgorithmMode mode,
00184 Index iter, double obj_value,
00185 double inf_pr, double inf_du,
00186 double mu, double d_norm,
00187 double regularization_size,
00188 double alpha_du, double alpha_pr,
00189 Index ls_trials,
00190 const Ipopt::IpoptData* ip_data,
00191 Ipopt::IpoptCalculatedQuantities* ip_cq)
00192 {
00193 nlp_->SaveCurrent();
00194 return true;
00195 }
00196
00197 void IpoptAdapter::finalize_solution(Ipopt::SolverReturn status,
00198 Index n, const double* x, const double* z_L, const double* z_U,
00199 Index m, const double* g, const double* lambda,
00200 double obj_value,
00201 const Ipopt::IpoptData* ip_data,
00202 Ipopt::IpoptCalculatedQuantities* ip_cq)
00203 {
00204 nlp_->SetVariables(x);
00205 nlp_->SaveCurrent();
00206 }
00207
00208 }