1 /* -------------------------------------------------------------------------- *
2  *                   OpenSim:  testSimpleOptimizationExample.cpp              *
3  * -------------------------------------------------------------------------- *
4  * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
5  * See http://opensim.stanford.edu and the NOTICE file for more information.  *
6  * OpenSim is developed at Stanford University and supported by the US        *
7  * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
8  * through the Warrior Web program.                                           *
9  *                                                                            *
10  * Copyright (c) 2005-2017 Stanford University and the Authors                *
11  * Author(s): Ayman Habib                                                     *
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 // Author: Ayman Habib
25 
26 //==============================================================================
27 //==============================================================================
28 #include <OpenSim/OpenSim.h>
29 #include <ctime>  // clock(), clock_t, CLOCKS_PER_SEC
30 #include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>
31 
32 using namespace OpenSim;
33 using namespace SimTK;
34 using namespace std;
35 
36 // step count for troubleshooting
37 int stepCount = 0;
38 
39 double bestSoFar = Infinity;
40 
41 class ExampleOptimizationSystem : public OptimizerSystem {
42    public:
43 
44        /* Constructor class. Parameters passed are accessed in the objectiveFunc() class. */
ExampleOptimizationSystem(int numParameters,State & s,Model & aModel)45        ExampleOptimizationSystem(int numParameters, State& s, Model& aModel):
46              OptimizerSystem(numParameters),
47              numKnobs(numParameters),
48              si(s),
49              osimModel(aModel){}
50 
objectiveFunc(const Vector & newControls,bool new_coefficients,Real & f) const51     int objectiveFunc(  const Vector &newControls, bool new_coefficients, Real& f ) const override {
52 
53         // make a copy of out initial states
54         State s = si;
55 
56         // Update the coordinate value of r_elbow_flex
57         OpenSim::Coordinate& elbowFlexCoord = osimModel.updCoordinateSet().get("r_elbow_flex");
58         elbowFlexCoord.setValue(s, newControls[0]);
59         // Now equilibrate muscles at this configuration
60         const Set<Muscle> &muscleSet = osimModel.getMuscles();
61         // Make sure other muscle states are initialized the same with 1.0 activation, 0.1 fiberLength followed by equilibrium computation
62         for(int i=0; i< muscleSet.getSize(); i++ ){
63             muscleSet[i].setActivation(s, 1.0);
64             const ActivationFiberLengthMuscle* afl = ActivationFiberLengthMuscle::safeDownCast(&muscleSet[i]);
65             if (afl) afl->setFiberLength(s, .1);
66         }
67         // Make sure the muscles states are in equilibrium
68         osimModel.equilibrateMuscles(s);
69 
70         const OpenSim::Muscle& bicShort = osimModel.getMuscles().get("BICshort");
71         // Compute moment arm of BICshort, flip sign since the optimizer tries to minimize rather than maximize
72         f = -bicShort.computeMomentArm(s, elbowFlexCoord);
73 
74         stepCount++;
75 
76         if( f < bestSoFar){
77             bestSoFar = f;
78             cout << "\nobjective evaluation #: " << stepCount << " elbow flexion angle = " << newControls[0]*SimTK_RADIAN_TO_DEGREE <<  " BICshort moment arm  = " << -f << std::endl;
79         }
80 
81       return(0);
82 
83    }
84 
85 private:
86     int numKnobs;
87     State& si;
88     Model& osimModel;
89  };
90 
91 //______________________________________________________________________________
92 /**
93  * Define an optimization problem that finds a set of muscle controls to maximize
94  * the forward velocity of the forearm/hand segment mass center.
95  */
main()96 int main()
97 {
98     try {
99         std::clock_t startTime = std::clock();
100 
101         // Create a new OpenSim model
102         // Similar to arm26 model but without wrapping surfaces for better performance
103         Model osimModel("Arm26_Optimize.osim");
104 
105         // Initialize the system and get the state representing the state system
106         State& si = osimModel.initSystem();
107 
108         // initialize the starting shoulder angle
109         const CoordinateSet& coords = osimModel.getCoordinateSet();
110         coords.get("r_shoulder_elev").setValue(si, 0.0);
111 
112         // Set the initial muscle activations
113         const Set<Muscle> &muscleSet = osimModel.getMuscles();
114         for(int i=0; i< muscleSet.getSize(); i++ ){
115             muscleSet[i].setActivation(si, 1.0);
116             const ActivationFiberLengthMuscle* afl = ActivationFiberLengthMuscle::safeDownCast(&muscleSet[i]);
117             afl->setFiberLength(si, .1);
118         }
119         OpenSim::Coordinate& elbowFlexCoord = osimModel.updCoordinateSet().get("r_elbow_flex");
120         elbowFlexCoord.setValue(si, 1.0);
121         //osimModel.getMultibodySystem().realize(si, Stage::Velocity);
122         // Make sure the muscles states are in equilibrium
123         osimModel.equilibrateMuscles(si);
124 
125         // Initialize the optimizer system we've defined.
126         ExampleOptimizationSystem sys(1, si, osimModel);
127         Real f = NaN;
128 
129         /* Define initial values and bounds for the controls to optimize */
130 
131         Vector controls(1, 1.0); // 1 radian for default value
132         Vector lower_bounds(1, elbowFlexCoord.getRangeMin());
133         Vector upper_bounds(1, elbowFlexCoord.getRangeMax());
134 
135         sys.setParameterLimits( lower_bounds, upper_bounds );
136 
137         // Create an optimizer. Pass in our OptimizerSystem
138         // and the name of the optimization algorithm.
139         Optimizer opt(sys, SimTK::LBFGSB);
140 
141         // Specify settings for the optimizer
142         opt.setConvergenceTolerance(0.000001);
143         opt.useNumericalGradient(true);
144         opt.setMaxIterations(1000);
145         opt.setLimitedMemoryHistory(500);
146 
147         // Optimize it!
148         f = opt.optimize(controls);
149 
150         cout << "Elapsed time = " << (std::clock()-startTime)/CLOCKS_PER_SEC << "s" << endl;
151 
152         ASSERT_EQUAL(f, -0.049390, 1e-5);
153         cout << "OpenSim example completed successfully.\n";
154     }
155     catch (const std::exception& ex)
156     {
157         std::cout << ex.what() << std::endl;
158         return 1;
159     }
160 
161     // End of main() routine.
162     return 0;
163 }
164