1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm): SimTKmath *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2006-12 Stanford University and the Authors. *
10 * Authors: Jack Middleton *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 #include "SimTKmath.h"
25
26 #include <iostream>
27 using std::cout;
28 using std::endl;
29 using SimTK::Vector;
30 using SimTK::Matrix;
31 using SimTK::Real;
32 using SimTK::Optimizer;
33 using SimTK::OptimizerSystem;
34
35
36 static int NUMBER_OF_PARAMETERS = 4;
37 static int NUMBER_OF_EQUALITY_CONSTRAINTS = 1;
38 static int NUMBER_OF_INEQUALITY_CONSTRAINTS = 1;
39
40 /*
41 * Adapted from Ipopt's hs071 example problem
42 *
43 * min x1*x4*(x1 + x2 + x3) + x3
44 * s.t. x1*x2*x3*x4 >= 25
45 * x1**2 + x2**2 + x3**2 + x4**2 = 40
46 * 1 <= x1,x2,x3,x4 <= 5
47 *
48 * Starting point:
49 * x = (1, 5, 5, 1)
50 *
51 * Optimal solution:
52 * x = (1.00000000, 4.74299963, 3.82114998, 1.37940829)
53 *
54 */
55
56 class ProblemSystem : public OptimizerSystem {
57 public:
58
59
objectiveFunc(const Vector & coefficients,bool new_coefficients,Real & f) const60 int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const override {
61 const Real *x;
62
63 x = &coefficients[0];
64
65 f = x[0] * x[3] * (x[0] + x[1] + x[2]) + x[2];
66 return( 0 );
67 }
68
gradientFunc(const Vector & coefficients,bool new_coefficients,Vector & gradient) const69 int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const override{
70 const Real *x;
71
72 x = &coefficients[0];
73
74 gradient[0] = x[0] * x[3] + x[3] * (x[0] + x[1] + x[2]);
75 gradient[1] = x[0] * x[3];
76 gradient[2] = x[0] * x[3] + 1;
77 gradient[3] = x[0] * (x[0] + x[1] + x[2]);
78
79 return(0);
80
81 }
constraintFunc(const Vector & coefficients,bool new_coefficients,Vector & constraints) const82 int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const override{
83 const Real *x;
84
85 x = &coefficients[0];
86 constraints[0] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3] - 40.0;
87 constraints[1] = x[0] * x[1] * x[2] * x[3] - 25.0;
88
89 return(0);
90 }
91
constraintJacobian(const Vector & coefficients,bool new_coefficients,Matrix & jac) const92 int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const override{
93 // int constraintJacobian( const Vector& coefficients, const bool new_coefficients, Vector& jac) const{
94 const Real *x;
95
96 x = &coefficients[0];
97
98 jac(0,0) = 2*x[0];
99 jac(0,1) = 2*x[1];
100 jac(0,2) = 2*x[2];
101 jac(0,3) = 2*x[3];
102 jac(1,0) = x[1]*x[2]*x[3];
103 jac(1,1) = x[0]*x[2]*x[3];
104 jac(1,2) = x[0]*x[1]*x[3];
105 jac(1,3) = x[0]*x[1]*x[2];
106
107 return(0);
108 }
109
110 /* ProblemSystem() : OptimizerSystem( NUMBER_OF_PARAMETERS, NUMBER_OF_CONSTRAINTS ) {} */
111
ProblemSystem(const int numParams,const int numEqualityConstraints,const int numInequalityConstraints)112 ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
113 OptimizerSystem( numParams )
114 {
115 setNumEqualityConstraints( numEqualityConstraints );
116 setNumInequalityConstraints( numInequalityConstraints );
117 }
118
119 };
120
121
main()122 int main() {
123
124 Real f;
125 int i;
126
127 /* create the system to be optimized */
128 ProblemSystem sys(NUMBER_OF_PARAMETERS, NUMBER_OF_EQUALITY_CONSTRAINTS, NUMBER_OF_INEQUALITY_CONSTRAINTS);
129
130 Vector results(NUMBER_OF_PARAMETERS);
131 Vector lower_bounds(NUMBER_OF_PARAMETERS);
132 Vector upper_bounds(NUMBER_OF_PARAMETERS);
133
134 /* set initial conditions */
135 results[0] = 1.0;
136 results[1] = 5.0;
137 results[2] = 5.0;
138 results[3] = 1.0;
139
140 /* set bounds */
141 for(i=0;i<NUMBER_OF_PARAMETERS;i++) {
142 lower_bounds[i] = 1.0;
143 upper_bounds[i] = 5.0;
144 }
145
146 sys.setParameterLimits( lower_bounds, upper_bounds );
147
148
149 int returnValue = 0; // assume success
150
151 try {
152
153 Optimizer opt( sys );
154
155 opt.setConvergenceTolerance( 1e-4 );
156
157 opt.setDiagnosticsLevel( 7 );
158 opt.setLimitedMemoryHistory(500); // works well for our small systems
159
160 opt.setAdvancedBoolOption("warm_start",true);
161
162 opt.setAdvancedRealOption("obj_scaling_factor",1);
163
164 opt.setAdvancedRealOption("nlp_scaling_max_gradient",1);
165
166 /* compute optimization */
167 f = opt.optimize( results );
168 }
169 catch (const std::exception& e) {
170 std::cout << e.what() << std::endl;
171 returnValue = 1; // failure
172 printf("IpoptTest.cpp: Caught exception \n");
173 }
174
175 printf("IpoptTest.cpp: f = %f params = ",f);
176 for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
177 printf(" %f",results[i]);
178 }
179 printf("\n");
180
181 static const Real TOL = 1e-4;
182 Real expected[] = { 1.00000000, 4.74299963, 3.82114998, 1.37940829 };
183 for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
184 if( results[i] > expected[i]+TOL || results[i] < expected[i]-TOL) {
185 printf(" IpoptTest.cpp: error results[%d] = %f expected=%f \n",i,results[i], expected[i]);
186 returnValue = 1;
187 }
188 }
189
190 return( returnValue );
191 }
192