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) 2007-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
28 using namespace SimTK;
29
30 static int NUMBER_OF_PARAMETERS = 2;
31 static int NUMBER_OF_EQUALITY_CONSTRAINTS = 0;
32 static int NUMBER_OF_INEQUALITY_CONSTRAINTS = 2;
33
34 /*
35 *
36 * Problem statement:
37 *
38 * minimize: (x1+5)^2 + (x2 - 4)^2
39 *
40 * s.t. x1 - x2^2 <= 0 // inequality constraint
41 x1 + x2 >= -2 // inequality constraint
42 *
43 * Starting point:
44 * x = ( 5, 5) will be used for the initial conditions
45 *
46 * Optimal solution:
47 * x = (1.00000000 )
48 *
49 */
50
51 class ProblemSystem : public OptimizerSystem {
52 public:
53
54
objectiveFunc(const Vector & coefficients,bool new_coefficients,Real & f) const55 int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const override {
56 const Real *x;
57
58 x = &coefficients[0];
59
60 f = (x[0] - 5.0)*(x[0] - 5.0) + (x[1] - 1.0)*(x[1] - 1.0);
61 return( 0 );
62 }
63
gradientFunc(const Vector & coefficients,bool new_coefficients,Vector & gradient) const64 int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const override{
65 const Real *x;
66
67 x = &coefficients[0];
68
69 gradient[0] = 2.0*(x[0] - 5.0);
70 gradient[1] = 2.0*(x[1] - 1.0);
71
72 return(0);
73
74 }
75
76 /*
77 ** Method to compute the value of the constraints.
78 ** Equality constraints are first followed by the any inequality constraints
79 */
constraintFunc(const Vector & coefficients,bool new_coefficients,Vector & constraints) const80 int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const override{
81 const Real *x;
82
83 x = &coefficients[0];
84 constraints[0] = x[0] - x[1]*x[1];
85 constraints[1] = x[1] - x[0] + 2.0;
86
87 return(0);
88 }
89
90
91 /*
92 ** Method to compute the jacobian of the constraints.
93 **
94 */
constraintJacobian(const Vector & coefficients,bool new_coefficients,Matrix & jac) const95 int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const override{
96 const Real *x;
97
98 x = &coefficients[0];
99 jac(0,0) = 1.0;
100 jac(0,1) = -2.0*x[1];
101 jac(1,0) = -1.0;
102 jac(1,1) = 1.0;
103
104
105 return(0);
106 }
107
ProblemSystem(const int numParams,const int numEqualityConstraints,const int numInequalityConstraints)108 ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
109 OptimizerSystem( numParams )
110 {
111 setNumEqualityConstraints( numEqualityConstraints );
112 setNumInequalityConstraints( numInequalityConstraints );
113 }
114
115 };
116
117
main()118 int main() {
119 Vector lower_limits(NUMBER_OF_PARAMETERS);
120 Vector upper_limits(NUMBER_OF_PARAMETERS);
121
122 /* create the system to be optimized */
123 ProblemSystem sys(NUMBER_OF_PARAMETERS, NUMBER_OF_EQUALITY_CONSTRAINTS, NUMBER_OF_INEQUALITY_CONSTRAINTS);
124
125 /* set bounds */
126 lower_limits[0] = 1.0;
127 upper_limits[0] = 3.0;
128 lower_limits[1] = -2e19;
129 upper_limits[1] = 2e19;
130
131 sys.setParameterLimits( lower_limits, upper_limits );
132
133 Vector results(NUMBER_OF_PARAMETERS);
134 /* set initial conditions */
135 results[0] = 5.0;
136 results[1] = 5.0;
137
138 Real f = NaN;
139 try {
140 Optimizer opt( sys );
141
142 opt.setConvergenceTolerance( .0000001 );
143
144 /* compute optimization */
145 f = opt.optimize( results );
146 }
147 catch (const std::exception& e) {
148 std::cout << "ConstrainedOptimization.cpp Caught exception:" << std::endl;
149 std::cout << e.what() << std::endl;
150 }
151
152 printf("Optimal Solution: f = %f parameters = %f %f \n",f,results[0],results[1]);
153
154 return 0;
155 }
156