snopt_adapter.cc
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2017, Alexander W. Winkler, ETH Zurich. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification,
5 are permitted provided that the following conditions are met:
6  * Redistributions of source code must retain the above copyright notice,
7  this list of conditions and the following disclaimer.
8  * Redistributions in binary form must reproduce the above copyright notice,
9  this list of conditions and the following disclaimer in the documentation
10  and/or other materials provided with the distribution.
11  * Neither the name of ETH ZURICH nor the names of its contributors may be
12  used to endorse or promote products derived from this software without
13  specific prior written permission.
14 
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT,
19 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
20 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
21 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
22 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
23 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
24 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 ******************************************************************************/
26 
27 #include <ifopt/snopt_adapter.h>
28 
29 namespace ifopt {
30 
32 
34 {
35  nlp_ = &ref;
36 }
37 
38 void
40 {
41  int obj_count = nlp_->HasCostTerms()? 1 : 0;
43  neF = nlp_->GetNumberOfConstraints() + obj_count;
44 
45  x = new double[n];
46  xlow = new double[n];
47  xupp = new double[n];
48  xmul = new double[n];
49  xstate = new int[n];
50 
51  F = new double[neF];
52  Flow = new double[neF];
53  Fupp = new double[neF];
54  Fmul = new double[neF];
55  Fstate = new int[neF];
56 
57  // Set the upper and lower bounds.
58  // no bounds on the spline coefficients or footholds
59  auto bounds_x = nlp_->GetBoundsOnOptimizationVariables();
60  for (std::size_t _n=0; _n<bounds_x.size(); ++_n) {
61  xlow[_n] = bounds_x.at(_n).lower_;
62  xupp[_n] = bounds_x.at(_n).upper_;
63  xstate[_n] = 0;
64  }
65 
66  // bounds on the cost function if it exists
67  int c = 0;
68  if (nlp_->HasCostTerms()) {
69  Flow[c] = -1e20; Fupp[c] = 1e20; Fmul[c] = 0.0; // no bounds on cost function
70  c++;
71  }
72 
73  // bounds on equality and inequality constraints
74  auto bounds_g = nlp_->GetBoundsOnConstraints();
75  for (const auto& b : bounds_g) {
76  Flow[c] = b.lower_; Fupp[c] = b.upper_; Fmul[c] = 0.0;
77  c++;
78  }
79 
80  // initial values of the optimization
81  VectorXd x_all = nlp_->GetVariableValues();
82  Eigen::Map<VectorXd>(&x[0], x_all.rows()) = x_all;
83 
84  ObjRow = nlp_->HasCostTerms()? 0 : -1; // the row in user function that corresponds to the objective function
85  ObjAdd = 0.0; // the constant to be added to the objective function
86 
87  // no linear derivatives/just assume all are nonlinear
88  lenA = 0;
89  neA = 0;
90  iAfun = nullptr;
91  jAvar = nullptr;
92  A = nullptr;
93 
94  // derivatives of nonlinear part
95  lenG = obj_count*n + nlp_->GetJacobianOfConstraints().nonZeros();
96  iGfun = new int[lenG];
97  jGvar = new int[lenG];
98 
99  // the gradient terms of the cost function
100  neG=0; // nonzero cells in jacobian of Cost Function AND Constraints
101 
102  // the nonzero elements of cost function (assume all)
103  if(nlp_->HasCostTerms()) {
104  for (int var=0; var<n; ++var) {
105  iGfun[neG] = 0;
106  jGvar[neG] = var;
107  neG++;
108  }
109  }
110 
111  // the nonzero elements of constraints (assume all)
112  auto jac = nlp_->GetJacobianOfConstraints();
113  for (int k=0; k<jac.outerSize(); ++k) {
114  for (Jacobian::InnerIterator it(jac,k); it; ++it) {
115  iGfun[neG] = it.row() + obj_count;
116  jGvar[neG] = it.col();
117  neG++;
118  }
119  }
120 
122 }
123 
124 void
125 SnoptAdapter::ObjectiveAndConstraintFct (int* Status, int* n, double x[],
126  int* needF, int* neF, double F[],
127  int* needG, int* neG, double G[],
128  char* cu, int* lencu, int iu[],
129  int* leniu, double ru[], int* lenru)
130 {
131  if ( *needF > 0 ) {
132  int i=0;
133 
134  // the scalar objective function value
135  if (nlp_->HasCostTerms())
136  F[i++] = nlp_->EvaluateCostFunction(x);
137 
138  // the vector of constraint values
139  VectorXd g_eig = nlp_->EvaluateConstraints(x);
140  Eigen::Map<VectorXd>(F+i, g_eig.rows()) = g_eig;
141  }
142 
143 
144  if ( *needG > 0 ) {
145  int i=0;
146 
147  // the jacobian of the first row (cost function)
148  if (nlp_->HasCostTerms()) {
149  Eigen::VectorXd grad = nlp_->EvaluateCostFunctionGradient(x, false);
150  i = grad.rows();
151  Eigen::Map<VectorXd>(G, i) = grad;
152  }
153 
154  // the jacobian of all the constraints
155  nlp_->EvalNonzerosOfJacobian(x, G+i);
156  nlp_->SaveCurrent();
157  }
158 }
159 
160 void
162 {
163  nlp_->SetVariables(x);
164 }
165 
166 } /* namespace opt */
int GetNumberOfConstraints() const
The number of individual constraints.
Definition: problem.cc:133
Problem::VectorXd VectorXd
Definition: snopt_adapter.h:51
A generic optimization problem with variables, costs and constraints.
Definition: problem.h:97
static void ObjectiveAndConstraintFct(int *Status, int *n, double x[], int *needF, int *neF, double F[], int *needG, int *neG, double G[], char *cu, int *lencu, int iu[], int *leniu, double ru[], int *lenru)
double EvaluateCostFunction(const double *x)
The scalar cost for current optimization variables x.
Definition: problem.cc:89
VectorXd EvaluateCostFunctionGradient(const double *x, bool use_finite_difference_approximation=false, double epsilon=std::numeric_limits< double >::epsilon())
The column-vector of derivatives of the cost w.r.t. each variable.
Definition: problem.cc:100
VectorXd GetVariableValues() const
The current value of the optimization variables.
Definition: problem.cc:77
void SaveCurrent()
Saves the current values of the optimization variables in x_prev.
Definition: problem.cc:174
int GetNumberOfOptimizationVariables() const
The number of optimization variables.
Definition: problem.cc:65
static NLPPtr nlp_
Definition: snopt_adapter.h:72
void SetVariables(const double *x)
Updates the variables with the values of the raw pointer x.
Definition: problem.cc:83
Jacobian GetJacobianOfConstraints() const
The sparse-matrix representation of Jacobian of the constraints.
Definition: problem.cc:162
VecBound GetBoundsOnConstraints() const
The upper and lower bound of each individual constraint.
Definition: problem.cc:127
bool HasCostTerms() const
True if the optimization problem includes a cost, false if merely a feasibility problem is defined...
Definition: problem.cc:146
common namespace for all elements in this library.
Definition: bounds.h:33
SnoptAdapter(Problem &nlp)
Creates an Adapter Object around the problem to conform to the Snopt interface.
void EvalNonzerosOfJacobian(const double *x, double *values)
Extracts those entries from constraint Jacobian that are not zero.
Definition: problem.cc:152
VectorXd EvaluateConstraints(const double *x)
Each constraint value g(x) for current optimization variables x.
Definition: problem.cc:139
VecBound GetBoundsOnOptimizationVariables() const
The maximum and minimum value each optimization variable is allowed to have.
Definition: problem.cc:71


ifopt
Author(s): Alexander W. Winkler
autogenerated on Fri Sep 16 2022 02:15:26