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