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_adapter.h>
00028
00029 namespace Ipopt {
00030
00031 IpoptAdapter::IpoptAdapter(Problem& nlp, bool finite_diff)
00032 {
00033 nlp_ = &nlp;
00034 finite_diff_ = finite_diff;
00035 }
00036
00037 bool IpoptAdapter::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
00038 Index& nnz_h_lag, IndexStyleEnum& index_style)
00039 {
00040 n = nlp_->GetNumberOfOptimizationVariables();
00041 m = nlp_->GetNumberOfConstraints();
00042
00043 if (finite_diff_)
00044 nnz_jac_g = m*n;
00045 else
00046 nnz_jac_g = nlp_->GetJacobianOfConstraints().nonZeros();
00047
00048 nnz_h_lag = n*n;
00049
00050
00051 index_style = C_STYLE;
00052
00053 return true;
00054 }
00055
00056 bool IpoptAdapter::get_bounds_info(Index n, double* x_lower, double* x_upper,
00057 Index m, double* g_l, double* g_u)
00058 {
00059 auto bounds_x = nlp_->GetBoundsOnOptimizationVariables();
00060 for (uint c=0; c<bounds_x.size(); ++c) {
00061 x_lower[c] = bounds_x.at(c).lower_;
00062 x_upper[c] = bounds_x.at(c).upper_;
00063 }
00064
00065
00066 auto bounds_g = nlp_->GetBoundsOnConstraints();
00067 for (uint c=0; c<bounds_g.size(); ++c) {
00068 g_l[c] = bounds_g.at(c).lower_;
00069 g_u[c] = bounds_g.at(c).upper_;
00070 }
00071
00072 return true;
00073 }
00074
00075 bool IpoptAdapter::get_starting_point(Index n, bool init_x, double* x,
00076 bool init_z, double* z_L, double* z_U,
00077 Index m, bool init_lambda,
00078 double* lambda)
00079 {
00080
00081 assert(init_x == true);
00082 assert(init_z == false);
00083 assert(init_lambda == false);
00084
00085 VectorXd x_all = nlp_->GetVariableValues();
00086 Eigen::Map<VectorXd>(&x[0], x_all.rows()) = x_all;
00087
00088 return true;
00089 }
00090
00091 bool IpoptAdapter::eval_f(Index n, const double* x, bool new_x, double& obj_value)
00092 {
00093 obj_value = nlp_->EvaluateCostFunction(x);
00094 return true;
00095 }
00096
00097 bool IpoptAdapter::eval_grad_f(Index n, const double* x, bool new_x, double* grad_f)
00098 {
00099 Eigen::VectorXd grad = nlp_->EvaluateCostFunctionGradient(x);
00100 Eigen::Map<Eigen::MatrixXd>(grad_f,n,1) = grad;
00101 return true;
00102 }
00103
00104 bool IpoptAdapter::eval_g(Index n, const double* x, bool new_x, Index m, double* g)
00105 {
00106 VectorXd g_eig = nlp_->EvaluateConstraints(x);
00107 Eigen::Map<VectorXd>(g,m) = g_eig;
00108 return true;
00109 }
00110
00111 bool IpoptAdapter::eval_jac_g(Index n, const double* x, bool new_x,
00112 Index m, Index nele_jac, Index* iRow, Index *jCol,
00113 double* values)
00114 {
00115
00116 if (values == NULL) {
00117
00118 Index nele=0;
00119 if (finite_diff_) {
00120 for (Index row = 0; row < m; row++) {
00121 for (Index col = 0; col < n; col++) {
00122 iRow[nele] = row;
00123 jCol[nele] = col;
00124 nele++;
00125 }
00126 }
00127 }
00128 else {
00129 auto jac = nlp_->GetJacobianOfConstraints();
00130 for (int k=0; k<jac.outerSize(); ++k) {
00131 for (Jacobian::InnerIterator it(jac,k); it; ++it) {
00132 iRow[nele] = it.row();
00133 jCol[nele] = it.col();
00134 nele++;
00135 }
00136 }
00137 }
00138
00139 assert(nele == nele_jac);
00140 }
00141 else {
00142
00143 nlp_->EvalNonzerosOfJacobian(x, values);
00144 }
00145
00146 return true;
00147 }
00148
00149 bool IpoptAdapter::intermediate_callback(AlgorithmMode mode,
00150 Index iter, double obj_value,
00151 double inf_pr, double inf_du,
00152 double mu, double d_norm,
00153 double regularization_size,
00154 double alpha_du, double alpha_pr,
00155 Index ls_trials,
00156 const IpoptData* ip_data,
00157 IpoptCalculatedQuantities* ip_cq)
00158 {
00159 nlp_->SaveCurrent();
00160 return true;
00161 }
00162
00163 void IpoptAdapter::finalize_solution(SolverReturn status,
00164 Index n, const double* x, const double* z_L, const double* z_U,
00165 Index m, const double* g, const double* lambda,
00166 double obj_value,
00167 const IpoptData* ip_data,
00168 IpoptCalculatedQuantities* ip_cq)
00169 {
00170 nlp_->SetVariables(x);
00171 nlp_->SaveCurrent();
00172 }
00173
00174 }