1 /* -------------------------------------------------------------------------- *
2 * OpenSim: StaticOptimization.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): Jeffrey A. Reinbolt *
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 //=============================================================================
26 // INCLUDES
27 //=============================================================================
28 #include <OpenSim/Common/IO.h>
29 #include <OpenSim/Simulation/Model/Model.h>
30 #include <OpenSim/Actuators/CoordinateActuator.h>
31 #include <OpenSim/Simulation/Control/ControlSet.h>
32 #include "StaticOptimization.h"
33 #include "StaticOptimizationTarget.h"
34 #include <OpenSim/Simulation/Model/ActivationFiberLengthMuscle.h>
35
36
37 using namespace OpenSim;
38 using namespace std;
39
40 //=============================================================================
41 // CONSTRUCTOR(S) AND DESTRUCTOR
42 //=============================================================================
43 //_____________________________________________________________________________
44 /**
45 * Destructor.
46 */
~StaticOptimization()47 StaticOptimization::~StaticOptimization()
48 {
49 deleteStorage();
50 delete _modelWorkingCopy;
51 if(_ownsForceSet) delete _forceSet;
52 }
53 //_____________________________________________________________________________
54 /**
55 */
StaticOptimization(Model * aModel)56 StaticOptimization::StaticOptimization(Model *aModel) :
57 Analysis(aModel),
58 _numCoordinateActuators(0),
59 _useModelForceSet(_useModelForceSetProp.getValueBool()),
60 _activationExponent(_activationExponentProp.getValueDbl()),
61 _useMusclePhysiology(_useMusclePhysiologyProp.getValueBool()),
62 _convergenceCriterion(_convergenceCriterionProp.getValueDbl()),
63 _maximumIterations(_maximumIterationsProp.getValueInt()),
64 _modelWorkingCopy(NULL)
65 {
66 setNull();
67
68 if(aModel) setModel(*aModel);
69 else allocateStorage();
70 }
71 // Copy constructor and virtual copy
72 //_____________________________________________________________________________
73 /**
74 * Copy constructor.
75 *
76 */
StaticOptimization(const StaticOptimization & aStaticOptimization)77 StaticOptimization::StaticOptimization(const StaticOptimization &aStaticOptimization):
78 Analysis(aStaticOptimization),
79 _numCoordinateActuators(aStaticOptimization._numCoordinateActuators),
80 _useModelForceSet(_useModelForceSetProp.getValueBool()),
81 _activationExponent(_activationExponentProp.getValueDbl()),
82 _useMusclePhysiology(_useMusclePhysiologyProp.getValueBool()),
83 _convergenceCriterion(_convergenceCriterionProp.getValueDbl()),
84 _maximumIterations(_maximumIterationsProp.getValueInt()),
85 _modelWorkingCopy(NULL)
86 {
87 setNull();
88 // COPY TYPE AND NAME
89 *this = aStaticOptimization;
90 _forceReporter = nullptr;
91 }
92
93 //=============================================================================
94 // OPERATORS
95 //=============================================================================
96 //-----------------------------------------------------------------------------
97 // ASSIGNMENT
98 //-----------------------------------------------------------------------------
99 //_____________________________________________________________________________
100 /**
101 * Assign this object to the values of another.
102 *
103 * @return Reference to this object.
104 */
105 StaticOptimization& StaticOptimization::
operator =(const StaticOptimization & aStaticOptimization)106 operator=(const StaticOptimization &aStaticOptimization)
107 {
108 // BASE CLASS
109 Analysis::operator=(aStaticOptimization);
110
111 _modelWorkingCopy = aStaticOptimization._modelWorkingCopy;
112 _numCoordinateActuators = aStaticOptimization._numCoordinateActuators;
113 _useModelForceSet = aStaticOptimization._useModelForceSet;
114 _activationExponent=aStaticOptimization._activationExponent;
115 _convergenceCriterion=aStaticOptimization._convergenceCriterion;
116 _maximumIterations=aStaticOptimization._maximumIterations;
117 _forceReporter = nullptr;
118 _useMusclePhysiology=aStaticOptimization._useMusclePhysiology;
119 return(*this);
120 }
121
122 //_____________________________________________________________________________
123 /**
124 * SetNull().
125 */
setNull()126 void StaticOptimization::setNull()
127 {
128 setAuthors("Jeffrey A. Reinbolt");
129 setupProperties();
130
131 // OTHER VARIABLES
132 _useModelForceSet = true;
133 _activationStorage = NULL;
134 _ownsForceSet = false;
135 _forceSet = NULL;
136 _activationExponent=2;
137 _useMusclePhysiology=true;
138 _numCoordinateActuators = 0;
139 _convergenceCriterion = 1e-4;
140 _maximumIterations = 100;
141 _forceReporter = nullptr;
142 setName("StaticOptimization");
143 }
144 //_____________________________________________________________________________
145 /**
146 * Connect properties to local pointers.
147 */
148 void StaticOptimization::
setupProperties()149 setupProperties()
150 {
151 _useModelForceSetProp.setComment("If true, the model's own force set will be used in the static optimization computation. "
152 "Otherwise, inverse dynamics for coordinate actuators will be computed for all unconstrained degrees of freedom.");
153 _useModelForceSetProp.setName("use_model_force_set");
154 _propertySet.append(&_useModelForceSetProp);
155
156 _activationExponentProp.setComment(
157 "A double indicating the exponent to raise activations to when solving static optimization. ");
158 _activationExponentProp.setName("activation_exponent");
159 _propertySet.append(&_activationExponentProp);
160
161
162 _useMusclePhysiologyProp.setComment(
163 "If true muscle force-length curve is observed while running optimization.");
164 _useMusclePhysiologyProp.setName("use_muscle_physiology");
165 _propertySet.append(&_useMusclePhysiologyProp);
166
167 _convergenceCriterionProp.setComment(
168 "Value used to determine when the optimization solution has converged");
169 _convergenceCriterionProp.setName("optimizer_convergence_criterion");
170 _propertySet.append(&_convergenceCriterionProp);
171
172 _maximumIterationsProp.setComment(
173 "An integer for setting the maximum number of iterations the optimizer can use at each time. ");
174 _maximumIterationsProp.setName("optimizer_max_iterations");
175 _propertySet.append(&_maximumIterationsProp);
176 }
177
178 //=============================================================================
179 // CONSTRUCTION METHODS
180 //=============================================================================
181 //_____________________________________________________________________________
182 /**
183 * Construct a description for the static optimization files.
184 */
185 void StaticOptimization::
constructDescription()186 constructDescription()
187 {
188 string descrip = "This file contains static optimization results.\n\n";
189 setDescription(descrip);
190 }
191
192 //_____________________________________________________________________________
193 /**
194 * Construct column labels for the static optimization files.
195 */
196 void StaticOptimization::
constructColumnLabels()197 constructColumnLabels()
198 {
199 Array<string> labels;
200 labels.append("time");
201 if(_model)
202 for (int i = 0; i < _forceSet->getActuators().getSize(); i++) {
203 if (ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->getActuators().get(i))) {
204 //std::cout << act->dump() << std::endl;
205 if (act->get_appliesForce())
206 labels.append(act->getName());
207 }
208 }
209 setColumnLabels(labels);
210 }
211
212 //_____________________________________________________________________________
213 /**
214 * Allocate storage for the static optimization.
215 */
216 void StaticOptimization::
allocateStorage()217 allocateStorage()
218 {
219 _activationStorage = new Storage(1000,"Static Optimization");
220 _activationStorage->setDescription(getDescription());
221 _activationStorage->setColumnLabels(getColumnLabels());
222
223 }
224
225
226 //=============================================================================
227 // DESTRUCTION METHODS
228 //=============================================================================
229 //_____________________________________________________________________________
230 /**
231 * Delete storage objects.
232 */
233 void StaticOptimization::
deleteStorage()234 deleteStorage()
235 {
236 delete _activationStorage; _activationStorage = NULL;
237 }
238
239 //=============================================================================
240 // GET AND SET
241 //=============================================================================
242 //_____________________________________________________________________________
243 /**
244 * Set the model for which the static optimization is to be computed.
245 *
246 * @param aModel Model pointer
247 */
248 void StaticOptimization::
setModel(Model & aModel)249 setModel(Model& aModel)
250 {
251 Analysis::setModel(aModel);
252 }
253
254 //-----------------------------------------------------------------------------
255 // STORAGE
256 //-----------------------------------------------------------------------------
257 //_____________________________________________________________________________
258 /**
259 * Get the activation storage.
260 *
261 * @return Activation storage.
262 */
263 Storage* StaticOptimization::
getActivationStorage()264 getActivationStorage()
265 {
266 return(_activationStorage);
267 }
268 //_____________________________________________________________________________
269 /**
270 * Get the force storage.
271 *
272 * @return Force storage.
273 */
274 Storage* StaticOptimization::
getForceStorage()275 getForceStorage()
276 {
277 if (_forceReporter)
278 return(&_forceReporter->updForceStorage());
279 else
280 return nullptr;
281 }
282
283 //-----------------------------------------------------------------------------
284 // STORAGE CAPACITY
285 //-----------------------------------------------------------------------------
286 //_____________________________________________________________________________
287 /**
288 * Set the capacity increments of all storage instances.
289 *
290 * @param aIncrement Increment by which storage capacities will be increased
291 * when storage capacities run out.
292 */
293 void StaticOptimization::
setStorageCapacityIncrements(int aIncrement)294 setStorageCapacityIncrements(int aIncrement)
295 {
296 _activationStorage->setCapacityIncrement(aIncrement);
297 }
298
299 //=============================================================================
300 // ANALYSIS
301 //=============================================================================
302 //_____________________________________________________________________________
303 /**
304 * Record the results.
305 */
306 int StaticOptimization::
record(const SimTK::State & s)307 record(const SimTK::State& s)
308 {
309 if(!_modelWorkingCopy) return -1;
310
311 // Set model to whatever defaults have been updated to from the last iteration
312 SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState();
313 sWorkingCopy.setTime(s.getTime());
314 _modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy);
315
316 // update Q's and U's
317 sWorkingCopy.setQ(s.getQ());
318 sWorkingCopy.setU(s.getU());
319
320 _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
321 //_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
322
323 const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
324
325 int na = fs.getSize();
326 int nacc = _accelerationIndices.getSize();
327
328 // IPOPT
329 _numericalDerivativeStepSize = 0.0001;
330 _optimizerAlgorithm = "ipopt";
331 _printLevel = 0;
332 //_optimizationConvergenceTolerance = 1e-004;
333 //_maxIterations = 2000;
334
335 // Optimization target
336 _modelWorkingCopy->setAllControllersEnabled(false);
337 StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
338 target.setStatesStore(_statesStore);
339 target.setStatesSplineSet(_statesSplineSet);
340 target.setActivationExponent(_activationExponent);
341 target.setDX(_numericalDerivativeStepSize);
342
343 // Pick optimizer algorithm
344 SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
345 //SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;
346
347 // Optimizer
348 SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);
349
350 // Optimizer options
351 //cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
352 optimizer->setDiagnosticsLevel(_printLevel);
353 //cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
354 optimizer->setConvergenceTolerance(_convergenceCriterion);
355 //cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
356 optimizer->setMaxIterations(_maximumIterations);
357 optimizer->useNumericalGradient(false);
358 optimizer->useNumericalJacobian(false);
359 if(algorithm == SimTK::InteriorPoint) {
360 // Some IPOPT-specific settings
361 optimizer->setLimitedMemoryHistory(500); // works well for our small systems
362 optimizer->setAdvancedBoolOption("warm_start",true);
363 optimizer->setAdvancedRealOption("obj_scaling_factor",1);
364 optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1);
365 }
366
367 // Parameter bounds
368 SimTK::Vector lowerBounds(na), upperBounds(na);
369 for(int i=0,j=0;i<fs.getSize();i++) {
370 ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
371 if (act) {
372 lowerBounds(j) = act->getMinControl();
373 upperBounds(j) = act->getMaxControl();
374 j++;
375 }
376 }
377
378 target.setParameterLimits(lowerBounds, upperBounds);
379
380 _parameters = 0; // Set initial guess to zeros
381
382 // Static optimization
383 _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
384 target.prepareToOptimize(sWorkingCopy, &_parameters[0]);
385
386 //LARGE_INTEGER start;
387 //LARGE_INTEGER stop;
388 //LARGE_INTEGER frequency;
389
390 //QueryPerformanceFrequency(&frequency);
391 //QueryPerformanceCounter(&start);
392
393 try {
394 target.setCurrentState( &sWorkingCopy );
395 optimizer->optimize(_parameters);
396 }
397 catch (const SimTK::Exception::Base& ex) {
398 cout << ex.getMessage() << endl;
399 cout << "OPTIMIZATION FAILED..." << endl;
400 cout << endl;
401 cout << "StaticOptimization.record: WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
402 cout << endl;
403
404 double tolBounds = 1e-1;
405 bool weakModel = false;
406 string msgWeak = "The model appears too weak for static optimization.\nTry increasing the strength and/or range of the following force(s):\n";
407 for(int a=0;a<na;a++) {
408 Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(a));
409 if( act ) {
410 Muscle* mus = dynamic_cast<Muscle*>(&_forceSet->get(a));
411 if(mus==NULL) {
412 if(_parameters(a) < (lowerBounds(a)+tolBounds)) {
413 msgWeak += " ";
414 msgWeak += act->getName();
415 msgWeak += " approaching lower bound of ";
416 ostringstream oLower;
417 oLower << lowerBounds(a);
418 msgWeak += oLower.str();
419 msgWeak += "\n";
420 weakModel = true;
421 } else if(_parameters(a) > (upperBounds(a)-tolBounds)) {
422 msgWeak += " ";
423 msgWeak += act->getName();
424 msgWeak += " approaching upper bound of ";
425 ostringstream oUpper;
426 oUpper << upperBounds(a);
427 msgWeak += oUpper.str();
428 msgWeak += "\n";
429 weakModel = true;
430 }
431 } else {
432 if(_parameters(a) > (upperBounds(a)-tolBounds)) {
433 msgWeak += " ";
434 msgWeak += mus->getName();
435 msgWeak += " approaching upper bound of ";
436 ostringstream o;
437 o << upperBounds(a);
438 msgWeak += o.str();
439 msgWeak += "\n";
440 weakModel = true;
441 }
442 }
443 }
444 }
445 if(weakModel) cout << msgWeak << endl;
446
447 if(!weakModel) {
448 double tolConstraints = 1e-6;
449 bool incompleteModel = false;
450 string msgIncomplete = "The model appears unsuitable for static optimization.\nTry appending the model with additional force(s) or locking joint(s) to reduce the following acceleration constraint violation(s):\n";
451 SimTK::Vector constraints;
452 target.constraintFunc(_parameters,true,constraints);
453
454 auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
455
456 for(int acc=0;acc<nacc;acc++) {
457 if(fabs(constraints(acc)) > tolConstraints) {
458 const Coordinate& coord = *coordinates[_accelerationIndices[acc]];
459 msgIncomplete += " ";
460 msgIncomplete += coord.getName();
461 msgIncomplete += ": constraint violation = ";
462 ostringstream o;
463 o << constraints(acc);
464 msgIncomplete += o.str();
465 msgIncomplete += "\n";
466 incompleteModel = true;
467 }
468 }
469 _forceReporter->step(sWorkingCopy, 1);
470 if(incompleteModel) cout << msgIncomplete << endl;
471 }
472 }
473
474 //QueryPerformanceCounter(&stop);
475 //double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
476 //cout << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl;
477
478 target.printPerformance(sWorkingCopy, &_parameters[0]);
479
480 //update defaults for use in the next step
481
482 const Set<Actuator>& actuators = _modelWorkingCopy->getActuators();
483 for(int k=0; k < actuators.getSize(); ++k){
484 ActivationFiberLengthMuscle *mus = dynamic_cast<ActivationFiberLengthMuscle*>(&actuators[k]);
485 if(mus){
486 mus->setDefaultActivation(_parameters[k]);
487 }
488 }
489
490 _activationStorage->append(sWorkingCopy.getTime(),na,&_parameters[0]);
491
492 SimTK::Vector forces(na);
493 target.getActuation(const_cast<SimTK::State&>(sWorkingCopy), _parameters,forces);
494
495 _forceReporter->step(sWorkingCopy, 1);
496
497 return 0;
498 }
499 //_____________________________________________________________________________
500 /**
501 * This method is called at the beginning of an analysis so that any
502 * necessary initializations may be performed.
503 *
504 * This method is meant to be called at the beginning of an integration
505 *
506 * @param s Current state .
507 *
508 * @return -1 on error, 0 otherwise.
509 */
begin(const SimTK::State & s)510 int StaticOptimization::begin(const SimTK::State& s )
511 {
512 if(!proceed()) return(0);
513
514 // Make a working copy of the model
515 delete _modelWorkingCopy;
516 _modelWorkingCopy = _model->clone();
517 // Remove disabled Actuators so we don't use them downstream (issue #2438)
518 const Set<Actuator>& actuators= _modelWorkingCopy->getActuators();
519 for (int i = actuators.getSize() - 1; i >= 0; i--) {
520 if (!actuators.get(i).get_appliesForce()) {
521 _modelWorkingCopy->updForceSet().remove(i);
522 }
523 }
524 _modelWorkingCopy->initSystem();
525
526 // Replace model force set with only generalized forces
527 if(_model) {
528 SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
529 // Update the _forceSet we'll be computing inverse dynamics for
530 if(_ownsForceSet) delete _forceSet;
531 if(_useModelForceSet) {
532 // Set pointer to model's internal force set
533 _forceSet = &_modelWorkingCopy->updForceSet();
534 _ownsForceSet = false;
535 } else {
536 ForceSet& as = _modelWorkingCopy->updForceSet();
537 // Keep a copy of forces that are not muscles to restore them back.
538 ForceSet* saveForces = as.clone();
539 // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
540 _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
541 _ownsForceSet = false;
542 _modelWorkingCopy->setAllControllersEnabled(false);
543 _numCoordinateActuators = _forceSet->getSize();
544 // Copy whatever forces that are not muscles back into the model
545
546 for(int i=0; i<saveForces->getSize(); i++){
547 // const Force& f=saveForces->get(i);
548 if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
549 as.append(saveForces->get(i).clone());
550 }
551 }
552
553 SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
554 // Set modeling options for Actuators to be overridden
555 for(int i=0; i<_forceSet->getSize(); i++) {
556 ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
557 if( act ) {
558 act->overrideActuation(sWorkingCopy, true);
559 }
560 }
561
562 sWorkingCopy.setTime(s.getTime());
563 sWorkingCopy.setQ(s.getQ());
564 sWorkingCopy.setU(s.getU());
565 // No need to copy Zs to be consistent with record method below
566 _modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
567 _modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
568 // Gather indices into speed set corresponding to the unconstrained degrees of freedom
569 // (for which we will set acceleration constraints)
570 _accelerationIndices.setSize(0);
571 auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
572 for(size_t i=0u; i<coordinates.size(); ++i) {
573 const Coordinate& coord = *coordinates[i];
574 if(!coord.isConstrained(sWorkingCopy)) {
575 Array<int> inds = _statesStore->
576 getColumnIndicesForIdentifier(coord.getName()) ;
577 _accelerationIndices.append(inds[0]);
578 }
579 }
580
581 int na = _forceSet->getSize();
582 int nacc = _accelerationIndices.getSize();
583
584 if(na < nacc)
585 throw(Exception("StaticOptimization: ERROR- over-constrained "
586 "system -- need at least as many forces as there are degrees of freedom.\n") );
587
588 _forceReporter.reset(new ForceReporter(_modelWorkingCopy));
589 _forceReporter->begin(sWorkingCopy);
590 _forceReporter->updForceStorage().reset();
591
592 _parameters.resize(_modelWorkingCopy->getNumControls());
593 _parameters = 0;
594 }
595
596 _statesSplineSet=GCVSplineSet(5,_statesStore);
597
598 // DESCRIPTION AND LABELS
599 constructDescription();
600 constructColumnLabels();
601
602 deleteStorage();
603 allocateStorage();
604
605 // RESET STORAGE
606 _activationStorage->reset(s.getTime());
607 _forceReporter->updForceStorage().reset(s.getTime());
608
609 // RECORD
610 int status = 0;
611 if(_activationStorage->getSize()<=0) {
612 status = record(s);
613 const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
614 for(int k=0;k<fs.getSize();k++) {
615 ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]);
616 if (act){
617 cout << "Bounds for " << act->getName() << ": "
618 << act->getMinControl() << " to "
619 << act->getMaxControl() << endl;
620 }
621 else{
622 std::string msg = getConcreteClassName();
623 msg += "::can only process scalar Actuator types.";
624 throw Exception(msg);
625 }
626 }
627 }
628
629 return(status);
630 }
631 //_____________________________________________________________________________
632 /**
633 * This method is called to perform the analysis. It can be called during
634 * the execution of a forward integrations or after the integration by
635 * feeding it the necessary data.
636 *
637 * This method should be overridden in derived classes. It is
638 * included here so that the derived class will not have to implement it if
639 * it is not necessary.
640 *
641 * @param s Current state .
642 *
643 * @return -1 on error, 0 otherwise.
644 */
step(const SimTK::State & s,int stepNumber)645 int StaticOptimization::step(const SimTK::State& s, int stepNumber )
646 {
647 if(!proceed(stepNumber)) return(0);
648
649 record(s);
650
651 return(0);
652 }
653 //_____________________________________________________________________________
654 /**
655 * This method is called at the end of an analysis so that any
656 * necessary finalizations may be performed.
657 *
658 * @param s Current state
659 *
660 * @return -1 on error, 0 otherwise.
661 */
end(const SimTK::State & s)662 int StaticOptimization::end( const SimTK::State& s )
663 {
664 if(!proceed()) return(0);
665
666 record(s);
667
668 return(0);
669 }
670
671
672 //=============================================================================
673 // IO
674 //=============================================================================
675 //_____________________________________________________________________________
676 /**
677 * Print results.
678 *
679 * The file names are constructed as
680 * aDir + "/" + aBaseName + "_" + ComponentName + aExtension
681 *
682 * @param aDir Directory in which the results reside.
683 * @param aBaseName Base file name.
684 * @param aDT Desired time interval between adjacent storage vectors. Linear
685 * interpolation is used to print the data out at the desired interval.
686 * @param aExtension File extension.
687 *
688 * @return 0 on success, -1 on error.
689 */
690 int StaticOptimization::
printResults(const string & aBaseName,const string & aDir,double aDT,const string & aExtension)691 printResults(const string &aBaseName,const string &aDir,double aDT,
692 const string &aExtension)
693 {
694 // ACTIVATIONS
695 Storage::printResult(_activationStorage,aBaseName+"_"+getName()+"_activation",aDir,aDT,aExtension);
696
697 // FORCES
698 Storage::printResult(getForceStorage(),aBaseName+"_"+getName()+"_force",aDir,aDT,aExtension);
699
700 // Make a ControlSet out of activations for use in forward dynamics
701 ControlSet cs(*_activationStorage);
702 std::string path = (aDir=="") ? "." : aDir;
703 std::string name = path + "/" + aBaseName+"_"+getName()+"_controls.xml";
704 cs.print(name);
705 return(0);
706 }
707