diff --git a/Applications/Analyze/test/testStaticOptimization.cpp b/Applications/Analyze/test/testStaticOptimization.cpp index d82a88b668..5ff01d40b2 100644 --- a/Applications/Analyze/test/testStaticOptimization.cpp +++ b/Applications/Analyze/test/testStaticOptimization.cpp @@ -265,4 +265,14 @@ void testArm26DisabledMuscles() { ASSERT_EQUAL(forces.getColumnLabels().findIndex("TRIlat"), -1); ASSERT_EQUAL(forces.getColumnLabels().findIndex("TRImed"), -1); + StaticOptimization& statOpt =(StaticOptimization&)model.getAnalysisSet().get("StaticOptimization"); + Storage* statesDerivativeStore = statOpt.getStatesDerivativeStore(); + const Storage* statesStore = statOpt._statesStore; + Array time; + Array time_d; + int nt = statesStore->getTimeColumn(time); + int nt_d = statesDerivativeStore->getTimeColumn(time_d); + ASSERT_EQUAL(nt, nt_d); + ASSERT_EQUAL>(time, time_d, std::numeric_limits::epsilon()); + } diff --git a/CHANGELOG.md b/CHANGELOG.md index af3ed34c1f..3d16ae224f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -89,6 +89,7 @@ v4.6 - Fixed a bug where `DeGrooteFregly2016Muscle::getBoundsNormalizedFiberLength()` was returning tendon force bounds rather than fiber length bounds. (#4040) - Fixed bugs in `PolynomialPathFitter` when too few coordinate samples were provided. (#4039) +- In `StaticOptimization`, state derivatives are now pre-calculated at the time points of the original data, rather than calculated during optimization from splines. This change reduces computational time by as much as 25%. (#4037) - Exposed the "dissipated energy" state variable allocated by the `SimTK::Force::LinearBushing` that is internal to `BushingForce`. This change fixed a bug in Moco where adding a `BushingForce` led to a segfault due to a mismatch between the size of the auxiliary state vector reserved by Moco and `SimTK::State::getZ()`. (#4054) diff --git a/OpenSim/Analyses/StaticOptimization.cpp b/OpenSim/Analyses/StaticOptimization.cpp index 9bf31fe7bd..d619cf4b8d 100644 --- a/OpenSim/Analyses/StaticOptimization.cpp +++ b/OpenSim/Analyses/StaticOptimization.cpp @@ -21,18 +21,18 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ - //============================================================================= // INCLUDES //============================================================================= -#include -#include -#include -#include #include "StaticOptimization.h" + #include "StaticOptimizationTarget.h" -#include +#include +#include +#include +#include +#include using namespace OpenSim; using namespace std; @@ -44,46 +44,45 @@ using namespace std; /** * Destructor. */ -StaticOptimization::~StaticOptimization() -{ +StaticOptimization::~StaticOptimization() { deleteStorage(); delete _modelWorkingCopy; - if(_ownsForceSet) delete _forceSet; + if (_ownsForceSet) delete _forceSet; } //_____________________________________________________________________________ /** */ -StaticOptimization::StaticOptimization(Model *aModel) : - Analysis(aModel), - _numCoordinateActuators(0), - _useModelForceSet(_useModelForceSetProp.getValueBool()), - _activationExponent(_activationExponentProp.getValueDbl()), - _useMusclePhysiology(_useMusclePhysiologyProp.getValueBool()), - _convergenceCriterion(_convergenceCriterionProp.getValueDbl()), - _maximumIterations(_maximumIterationsProp.getValueInt()), - _modelWorkingCopy(NULL) -{ +StaticOptimization::StaticOptimization(Model* aModel) + : Analysis(aModel), _numCoordinateActuators(0), + _useModelForceSet(_useModelForceSetProp.getValueBool()), + _activationExponent(_activationExponentProp.getValueDbl()), + _useMusclePhysiology(_useMusclePhysiologyProp.getValueBool()), + _convergenceCriterion(_convergenceCriterionProp.getValueDbl()), + _maximumIterations(_maximumIterationsProp.getValueInt()), + _modelWorkingCopy(NULL) { setNull(); - if(aModel) setModel(*aModel); - else allocateStorage(); + if (aModel) + setModel(*aModel); + else + allocateStorage(); } -// Copy constructor and virtual copy +// Copy constructor and virtual copy //_____________________________________________________________________________ /** * Copy constructor. * */ -StaticOptimization::StaticOptimization(const StaticOptimization &aStaticOptimization): - Analysis(aStaticOptimization), - _numCoordinateActuators(aStaticOptimization._numCoordinateActuators), - _useModelForceSet(_useModelForceSetProp.getValueBool()), - _activationExponent(_activationExponentProp.getValueDbl()), - _useMusclePhysiology(_useMusclePhysiologyProp.getValueBool()), - _convergenceCriterion(_convergenceCriterionProp.getValueDbl()), - _maximumIterations(_maximumIterationsProp.getValueInt()), - _modelWorkingCopy(NULL) -{ +StaticOptimization::StaticOptimization( + const StaticOptimization& aStaticOptimization) + : Analysis(aStaticOptimization), + _numCoordinateActuators(aStaticOptimization._numCoordinateActuators), + _useModelForceSet(_useModelForceSetProp.getValueBool()), + _activationExponent(_activationExponentProp.getValueDbl()), + _useMusclePhysiology(_useMusclePhysiologyProp.getValueBool()), + _convergenceCriterion(_convergenceCriterionProp.getValueDbl()), + _maximumIterations(_maximumIterationsProp.getValueInt()), + _modelWorkingCopy(NULL) { setNull(); // COPY TYPE AND NAME *this = aStaticOptimization; @@ -102,29 +101,27 @@ StaticOptimization::StaticOptimization(const StaticOptimization &aStaticOptimiza * * @return Reference to this object. */ -StaticOptimization& StaticOptimization:: -operator=(const StaticOptimization &aStaticOptimization) -{ +StaticOptimization& StaticOptimization::operator=( + const StaticOptimization& aStaticOptimization) { // BASE CLASS Analysis::operator=(aStaticOptimization); _modelWorkingCopy = aStaticOptimization._modelWorkingCopy; _numCoordinateActuators = aStaticOptimization._numCoordinateActuators; _useModelForceSet = aStaticOptimization._useModelForceSet; - _activationExponent=aStaticOptimization._activationExponent; - _convergenceCriterion=aStaticOptimization._convergenceCriterion; - _maximumIterations=aStaticOptimization._maximumIterations; + _activationExponent = aStaticOptimization._activationExponent; + _convergenceCriterion = aStaticOptimization._convergenceCriterion; + _maximumIterations = aStaticOptimization._maximumIterations; _forceReporter = nullptr; - _useMusclePhysiology=aStaticOptimization._useMusclePhysiology; - return(*this); + _useMusclePhysiology = aStaticOptimization._useMusclePhysiology; + return (*this); } //_____________________________________________________________________________ /** * SetNull(). */ -void StaticOptimization::setNull() -{ +void StaticOptimization::setNull() { setAuthors("Jeffrey A. Reinbolt"); setupProperties(); @@ -133,8 +130,8 @@ void StaticOptimization::setNull() _activationStorage = NULL; _ownsForceSet = false; _forceSet = NULL; - _activationExponent=2; - _useMusclePhysiology=true; + _activationExponent = 2; + _useMusclePhysiology = true; _numCoordinateActuators = 0; _convergenceCriterion = 1e-4; _maximumIterations = 100; @@ -145,32 +142,34 @@ void StaticOptimization::setNull() /** * Connect properties to local pointers. */ -void StaticOptimization:: -setupProperties() -{ - _useModelForceSetProp.setComment("If true, the model's own force set will be used in the static optimization computation. " - "Otherwise, inverse dynamics for coordinate actuators will be computed for all unconstrained degrees of freedom."); +void StaticOptimization::setupProperties() { + _useModelForceSetProp.setComment( + "If true, the model's own force set will be used in the static " + "optimization computation. " + "Otherwise, inverse dynamics for coordinate actuators will be " + "computed for all unconstrained degrees of freedom."); _useModelForceSetProp.setName("use_model_force_set"); _propertySet.append(&_useModelForceSetProp); _activationExponentProp.setComment( - "A double indicating the exponent to raise activations to when solving static optimization. "); + "A double indicating the exponent to raise activations to when " + "solving static optimization. "); _activationExponentProp.setName("activation_exponent"); _propertySet.append(&_activationExponentProp); - - _useMusclePhysiologyProp.setComment( - "If true muscle force-length curve is observed while running optimization."); + _useMusclePhysiologyProp.setComment("If true muscle force-length curve is " + "observed while running optimization."); _useMusclePhysiologyProp.setName("use_muscle_physiology"); _propertySet.append(&_useMusclePhysiologyProp); - _convergenceCriterionProp.setComment( - "Value used to determine when the optimization solution has converged"); + _convergenceCriterionProp.setComment("Value used to determine when the " + "optimization solution has converged"); _convergenceCriterionProp.setName("optimizer_convergence_criterion"); _propertySet.append(&_convergenceCriterionProp); _maximumIterationsProp.setComment( - "An integer for setting the maximum number of iterations the optimizer can use at each time. "); + "An integer for setting the maximum number of iterations the " + "optimizer can use at each time. "); _maximumIterationsProp.setName("optimizer_max_iterations"); _propertySet.append(&_maximumIterationsProp); } @@ -182,9 +181,7 @@ setupProperties() /** * Construct a description for the static optimization files. */ -void StaticOptimization:: -constructDescription() -{ +void StaticOptimization::constructDescription() { string descrip = "This file contains static optimization results.\n\n"; setDescription(descrip); } @@ -193,16 +190,14 @@ constructDescription() /** * Construct column labels for the static optimization files. */ -void StaticOptimization:: -constructColumnLabels() -{ +void StaticOptimization::constructColumnLabels() { Array labels; labels.append("time"); - if(_model) + if (_model) for (int i = 0; i < _forceSet->getActuators().getSize(); i++) { - if (ScalarActuator* act = dynamic_cast(&_forceSet->getActuators().get(i))) { - if (act->get_appliesForce()) - labels.append(act->getName()); + if (ScalarActuator* act = dynamic_cast( + &_forceSet->getActuators().get(i))) { + if (act->get_appliesForce()) labels.append(act->getName()); } } setColumnLabels(labels); @@ -212,16 +207,12 @@ constructColumnLabels() /** * Allocate storage for the static optimization. */ -void StaticOptimization:: -allocateStorage() -{ - _activationStorage = new Storage(1000,"Static Optimization"); +void StaticOptimization::allocateStorage() { + _activationStorage = new Storage(1000, "Static Optimization"); _activationStorage->setDescription(getDescription()); _activationStorage->setColumnLabels(getColumnLabels()); - } - //============================================================================= // DESTRUCTION METHODS //============================================================================= @@ -229,10 +220,9 @@ allocateStorage() /** * Delete storage objects. */ -void StaticOptimization:: -deleteStorage() -{ - delete _activationStorage; _activationStorage = NULL; +void StaticOptimization::deleteStorage() { + delete _activationStorage; + _activationStorage = NULL; } //============================================================================= @@ -244,11 +234,7 @@ deleteStorage() * * @param aModel Model pointer */ -void StaticOptimization:: -setModel(Model& aModel) -{ - Analysis::setModel(aModel); -} +void StaticOptimization::setModel(Model& aModel) { Analysis::setModel(aModel); } //----------------------------------------------------------------------------- // STORAGE @@ -259,10 +245,8 @@ setModel(Model& aModel) * * @return Activation storage. */ -Storage* StaticOptimization:: -getActivationStorage() -{ - return(_activationStorage); +Storage* StaticOptimization::getActivationStorage() { + return _activationStorage; } //_____________________________________________________________________________ /** @@ -270,11 +254,9 @@ getActivationStorage() * * @return Force storage. */ -Storage* StaticOptimization:: -getForceStorage() -{ +Storage* StaticOptimization::getForceStorage() { if (_forceReporter) - return(&_forceReporter->updForceStorage()); + return &_forceReporter->updForceStorage(); else return nullptr; } @@ -286,21 +268,21 @@ getForceStorage() /** * Record the results. */ -int StaticOptimization:: -record(const SimTK::State& s) -{ - if(!_modelWorkingCopy) return -1; +int StaticOptimization::record(const SimTK::State& s) { + if (!_modelWorkingCopy) return -1; - // Set model to whatever defaults have been updated to from the last iteration + // Set model to whatever defaults have been updated to from the last + // iteration SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState(); sWorkingCopy.setTime(s.getTime()); - _modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy); + _modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy); // update Q's and U's sWorkingCopy.setQ(s.getQ()); sWorkingCopy.setU(s.getU()); - _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity); + _modelWorkingCopy->getMultibodySystem().realize( + sWorkingCopy, SimTK::Stage::Velocity); //_modelWorkingCopy->equilibrateMuscles(sWorkingCopy); const Set& fs = _modelWorkingCopy->getActuators(); @@ -317,39 +299,43 @@ record(const SimTK::State& s) // Optimization target _modelWorkingCopy->setAllControllersEnabled(false); - StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology); + StaticOptimizationTarget target( + sWorkingCopy, _modelWorkingCopy, na, nacc, _useMusclePhysiology); target.setStatesStore(_statesStore); target.setStatesSplineSet(_statesSplineSet); + target.setStatesDerivativeStore(_statesDerivativeStore); target.setActivationExponent(_activationExponent); target.setDX(_numericalDerivativeStepSize); // Pick optimizer algorithm SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint; - //SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP; + // SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP; // Optimizer - SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm); + SimTK::Optimizer* optimizer = new SimTK::Optimizer(target, algorithm); // Optimizer options - //cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n"; + // cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n"; optimizer->setDiagnosticsLevel(_printLevel); - //cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n"; + // cout<<"Setting optimizer convergence criterion to + // "<<_convergenceCriterion<<".\n"; optimizer->setConvergenceTolerance(_convergenceCriterion); - //cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n"; + // cout<<"Setting optimizer maximum iterations to + // "<<_maximumIterations<<".\n"; optimizer->setMaxIterations(_maximumIterations); optimizer->useNumericalGradient(false); optimizer->useNumericalJacobian(false); - if(algorithm == SimTK::InteriorPoint) { + if (algorithm == SimTK::InteriorPoint) { // Some IPOPT-specific settings - optimizer->setLimitedMemoryHistory(500); // works well for our small systems - optimizer->setAdvancedBoolOption("warm_start",true); - optimizer->setAdvancedRealOption("obj_scaling_factor",1); - optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1); + optimizer->setLimitedMemoryHistory(500); + optimizer->setAdvancedBoolOption("warm_start", true); + optimizer->setAdvancedRealOption("obj_scaling_factor", 1); + optimizer->setAdvancedRealOption("nlp_scaling_max_gradient", 1); } // Parameter bounds SimTK::Vector lowerBounds(na), upperBounds(na); - for(int i=0,j=0;i(&fs.get(i)); if (act) { lowerBounds(j) = act->getMinControl(); @@ -357,27 +343,27 @@ record(const SimTK::State& s) j++; } } - + target.setParameterLimits(lowerBounds, upperBounds); _parameters = 0; // Set initial guess to zeros // Static optimization - _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity); + _modelWorkingCopy->getMultibodySystem().realize( + sWorkingCopy, SimTK::Stage::Velocity); target.prepareToOptimize(sWorkingCopy, &_parameters[0]); - //LARGE_INTEGER start; - //LARGE_INTEGER stop; - //LARGE_INTEGER frequency; + // LARGE_INTEGER start; + // LARGE_INTEGER stop; + // LARGE_INTEGER frequency; - //QueryPerformanceFrequency(&frequency); - //QueryPerformanceCounter(&start); + // QueryPerformanceFrequency(&frequency); + // QueryPerformanceCounter(&start); try { - target.setCurrentState( &sWorkingCopy ); + target.setCurrentState(&sWorkingCopy); optimizer->optimize(_parameters); - } - catch (const SimTK::Exception::Base& ex) { + } catch (const SimTK::Exception::Base& ex) { log_warn(ex.getMessage()); log_warn("OPTIMIZATION FAILED..."); log_warn("StaticOptimization.record: The optimizer could not find a " @@ -386,13 +372,15 @@ record(const SimTK::State& s) double tolBounds = 1e-1; bool weakModel = false; - string msgWeak = "The model appears too weak for static optimization.\nTry increasing the strength and/or range of the following force(s):\n"; - for(int a=0;a(&_forceSet->get(a)); - if( act ) { - Muscle* mus = dynamic_cast(&_forceSet->get(a)); - if(mus==NULL) { - if(_parameters(a) < (lowerBounds(a)+tolBounds)) { + if (act) { + Muscle* mus = dynamic_cast(&_forceSet->get(a)); + if (mus == NULL) { + if (_parameters(a) < (lowerBounds(a) + tolBounds)) { msgWeak += " "; msgWeak += act->getName(); msgWeak += " approaching lower bound of "; @@ -401,7 +389,7 @@ record(const SimTK::State& s) msgWeak += oLower.str(); msgWeak += "\n"; weakModel = true; - } else if(_parameters(a) > (upperBounds(a)-tolBounds)) { + } else if (_parameters(a) > (upperBounds(a) - tolBounds)) { msgWeak += " "; msgWeak += act->getName(); msgWeak += " approaching upper bound of "; @@ -410,9 +398,9 @@ record(const SimTK::State& s) msgWeak += oUpper.str(); msgWeak += "\n"; weakModel = true; - } + } } else { - if(_parameters(a) > (upperBounds(a)-tolBounds)) { + if (_parameters(a) > (upperBounds(a) - tolBounds)) { msgWeak += " "; msgWeak += mus->getName(); msgWeak += " approaching upper bound of "; @@ -425,20 +413,26 @@ record(const SimTK::State& s) } } } - if(weakModel) log_warn(msgWeak); + if (weakModel) log_warn(msgWeak); - if(!weakModel) { + if (!weakModel) { double tolConstraints = 1e-6; bool incompleteModel = false; - 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"; + 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"; SimTK::Vector constraints; - target.constraintFunc(_parameters,true,constraints); + target.constraintFunc(_parameters, true, constraints); - auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder(); + auto coordinates = + _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder(); - for(int acc=0;acc tolConstraints) { - const Coordinate& coord = *coordinates[_accelerationIndices[acc]]; + for (int acc = 0; acc < nacc; acc++) { + if (fabs(constraints(acc)) > tolConstraints) { + const Coordinate& coord = + *coordinates[_accelerationIndices[acc]]; msgIncomplete += " "; msgIncomplete += coord.getName(); msgIncomplete += ": constraint violation = "; @@ -450,32 +444,33 @@ record(const SimTK::State& s) } } _forceReporter->step(sWorkingCopy, 1); - if(incompleteModel) log_warn(msgIncomplete); + if (incompleteModel) log_warn(msgIncomplete); } } - //QueryPerformanceCounter(&stop); - //double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart; - //cout << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl; + // QueryPerformanceCounter(&stop); + // double duration = + // (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart; cout + // << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl; if (Logger::shouldLog(Logger::Level::Info)) { target.printPerformance(sWorkingCopy, &_parameters[0]); } - //update defaults for use in the next step + // update defaults for use in the next step const Set& actuators = _modelWorkingCopy->getActuators(); - for(int k=0; k < actuators.getSize(); ++k){ - ActivationFiberLengthMuscle *mus = dynamic_cast(&actuators[k]); - if(mus){ - mus->setDefaultActivation(_parameters[k]); - } + for (int k = 0; k < actuators.getSize(); ++k) { + ActivationFiberLengthMuscle* mus = + dynamic_cast(&actuators[k]); + if (mus) { mus->setDefaultActivation(_parameters[k]); } } - _activationStorage->append(sWorkingCopy.getTime(),na,&_parameters[0]); + _activationStorage->append(sWorkingCopy.getTime(), na, &_parameters[0]); SimTK::Vector forces(na); - target.getActuation(const_cast(sWorkingCopy), _parameters,forces); + target.getActuation( + const_cast(sWorkingCopy), _parameters, forces); _forceReporter->step(sWorkingCopy, 1); @@ -486,21 +481,20 @@ record(const SimTK::State& s) * This method is called at the beginning of an analysis so that any * necessary initializations may be performed. * - * This method is meant to be called at the beginning of an integration + * This method is meant to be called at the beginning of an integration * * @param s Current state . * * @return -1 on error, 0 otherwise. */ -int StaticOptimization::begin(const SimTK::State& s ) -{ - if(!proceed()) return(0); +int StaticOptimization::begin(const SimTK::State& s) { + if (!proceed()) return 0; // Make a working copy of the model delete _modelWorkingCopy; _modelWorkingCopy = _model->clone(); // Remove disabled Actuators so we don't use them downstream (issue #2438) - const Set& actuators= _modelWorkingCopy->getActuators(); + const Set& actuators = _modelWorkingCopy->getActuators(); for (int i = actuators.getSize() - 1; i >= 0; i--) { if (!actuators.get(i).get_appliesForce()) { _modelWorkingCopy->updForceSet().remove(i); @@ -509,11 +503,11 @@ int StaticOptimization::begin(const SimTK::State& s ) _modelWorkingCopy->initSystem(); // Replace model force set with only generalized forces - if(_model) { + if (_model) { SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState(); // Update the _forceSet we'll be computing inverse dynamics for - if(_ownsForceSet) delete _forceSet; - if(_useModelForceSet) { + if (_ownsForceSet) delete _forceSet; + if (_useModelForceSet) { // Set pointer to model's internal force set _forceSet = &_modelWorkingCopy->updForceSet(); _ownsForceSet = false; @@ -521,42 +515,46 @@ int StaticOptimization::begin(const SimTK::State& s ) ForceSet& as = _modelWorkingCopy->updForceSet(); // Keep a copy of forces that are not muscles to restore them back. ForceSet* saveForces = as.clone(); - // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom - _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false); + // Generate an force set consisting of a coordinate actuator for + // every unconstrained degree of freedom + _forceSet = CoordinateActuator:: + CreateForceSetOfCoordinateActuatorsForModel( + sWorkingCopyTemp, *_modelWorkingCopy, 1, false); _ownsForceSet = false; _modelWorkingCopy->setAllControllersEnabled(false); _numCoordinateActuators = _forceSet->getSize(); // Copy whatever forces that are not muscles back into the model - - for(int i=0; igetSize(); i++){ + + for (int i = 0; i < saveForces->getSize(); i++) { // const Force& f=saveForces->get(i); - if ((dynamic_cast(&saveForces->get(i)))==NULL) + if ((dynamic_cast(&saveForces->get(i))) == NULL) as.append(saveForces->get(i).clone()); } } SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem(); // Set modeling options for Actuators to be overridden - for(int i=0; i<_forceSet->getSize(); i++) { - ScalarActuator* act = dynamic_cast(&_forceSet->get(i)); - if( act ) { - act->overrideActuation(sWorkingCopy, true); - } + for (int i = 0; i < _forceSet->getSize(); i++) { + ScalarActuator* act = + dynamic_cast(&_forceSet->get(i)); + if (act) { act->overrideActuation(sWorkingCopy, true); } } sWorkingCopy.setTime(s.getTime()); sWorkingCopy.setQ(s.getQ()); sWorkingCopy.setU(s.getU()); - // No need to copy Zs to be consistent with record method below - _modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity); + // No need to copy Zs to be consistent with record method below + _modelWorkingCopy->getMultibodySystem().realize( + s, SimTK::Stage::Velocity); _modelWorkingCopy->equilibrateMuscles(sWorkingCopy); - // Gather indices into speed set corresponding to the unconstrained degrees of freedom - // (for which we will set acceleration constraints) + // Gather indices into speed set corresponding to the unconstrained + // degrees of freedom (for which we will set acceleration constraints) _accelerationIndices.setSize(0); - auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder(); - for(size_t i=0u; igetCoordinatesInMultibodyTreeOrder(); + for (size_t i = 0u; i < coordinates.size(); ++i) { const Coordinate& coord = *coordinates[i]; - if(!coord.isConstrained(sWorkingCopy)) { + if (!coord.isConstrained(sWorkingCopy)) { _accelerationIndices.append(static_cast(i)); } } @@ -564,9 +562,10 @@ int StaticOptimization::begin(const SimTK::State& s ) int na = _forceSet->getSize(); int nacc = _accelerationIndices.getSize(); - if(na < nacc) + if (na < nacc) throw(Exception("StaticOptimization: ERROR- over-constrained " - "system -- need at least as many forces as there are degrees of freedom.\n") ); + "system -- need at least as many forces as there " + "are degrees of freedom.\n")); _forceReporter.reset(new ForceReporter(_modelWorkingCopy)); _forceReporter->begin(sWorkingCopy); @@ -576,7 +575,10 @@ int StaticOptimization::begin(const SimTK::State& s ) _parameters = 0; } - _statesSplineSet=GCVSplineSet(5,_statesStore); + _statesSplineSet = GCVSplineSet(5, _statesStore); + + // Calculate and store state derivatives + _statesDerivativeStore = _statesSplineSet.constructStorage(1); // DESCRIPTION AND LABELS constructDescription(); @@ -591,16 +593,15 @@ int StaticOptimization::begin(const SimTK::State& s ) // RECORD int status = 0; - if(_activationStorage->getSize()<=0) { + if (_activationStorage->getSize() <= 0) { status = record(s); const Set& fs = _modelWorkingCopy->getActuators(); - for(int k=0;k(&fs[k]); - if (act){ + for (int k = 0; k < fs.getSize(); k++) { + ScalarActuator* act = dynamic_cast(&fs[k]); + if (act) { log_info("Bounds for '{}': {} to {}.", act->getName(), act->getMinControl(), act->getMaxControl()); - } - else{ + } else { std::string msg = getConcreteClassName(); msg += "::can only process scalar Actuator types."; throw Exception(msg); @@ -608,7 +609,7 @@ int StaticOptimization::begin(const SimTK::State& s ) } } - return(status); + return status; } //_____________________________________________________________________________ /** @@ -624,40 +625,37 @@ int StaticOptimization::begin(const SimTK::State& s ) * * @return -1 on error, 0 otherwise. */ -int StaticOptimization::step(const SimTK::State& s, int stepNumber ) -{ - if(!proceed(stepNumber)) return(0); +int StaticOptimization::step(const SimTK::State& s, int stepNumber) { + if (!proceed(stepNumber)) return 0; record(s); - return(0); + return 0; } //_____________________________________________________________________________ /** * This method is called at the end of an analysis so that any * necessary finalizations may be performed. * - * @param s Current state + * @param s Current state * * @return -1 on error, 0 otherwise. */ -int StaticOptimization::end( const SimTK::State& s ) -{ - if(!proceed()) return(0); +int StaticOptimization::end(const SimTK::State& s) { + if (!proceed()) return 0; record(s); - return(0); + return 0; } - //============================================================================= // IO //============================================================================= //_____________________________________________________________________________ /** * Print results. - * + * * The file names are constructed as * aDir + "/" + aBaseName + "_" + ComponentName + aExtension * @@ -669,20 +667,21 @@ int StaticOptimization::end( const SimTK::State& s ) * * @return 0 on success, -1 on error. */ -int StaticOptimization:: -printResults(const string &aBaseName,const string &aDir,double aDT, - const string &aExtension) -{ +int StaticOptimization::printResults(const string& aBaseName, + const string& aDir, double aDT, const string& aExtension) { // ACTIVATIONS - Storage::printResult(_activationStorage,aBaseName+"_"+getName()+"_activation",aDir,aDT,aExtension); + Storage::printResult(_activationStorage, + aBaseName + "_" + getName() + "_activation", aDir, aDT, aExtension); // FORCES - Storage::printResult(getForceStorage(),aBaseName+"_"+getName()+"_force",aDir,aDT,aExtension); + Storage::printResult(getForceStorage(), + aBaseName + "_" + getName() + "_force", aDir, aDT, aExtension); // Make a ControlSet out of activations for use in forward dynamics ControlSet cs(*_activationStorage); - std::string path = (aDir=="") ? "." : aDir; - std::string name = path + "/" + aBaseName+"_"+getName()+"_controls.xml"; + std::string path = (aDir == "") ? "." : aDir; + std::string name = + path + "/" + aBaseName + "_" + getName() + "_controls.xml"; cs.print(name); - return(0); + return 0; } diff --git a/OpenSim/Analyses/StaticOptimization.h b/OpenSim/Analyses/StaticOptimization.h index ee3e4e86fc..1c815bd37a 100644 --- a/OpenSim/Analyses/StaticOptimization.h +++ b/OpenSim/Analyses/StaticOptimization.h @@ -79,6 +79,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(StaticOptimization, Analysis); Storage *_activationStorage; Storage *_forceStorage; GCVSplineSet _statesSplineSet; + Storage *_statesDerivativeStore; Array _accelerationIndices; @@ -123,6 +124,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(StaticOptimization, Analysis); void setStorageCapacityIncrements(int) {} Storage* getActivationStorage(); Storage* getForceStorage(); + Storage* getStatesDerivativeStore() {return _statesDerivativeStore; } bool getUseModelForceSet() { return _useModelForceSet; } void setUseModelForceSet(bool aUseModelActuatorSet) { _useModelForceSet = aUseModelActuatorSet; } @@ -136,6 +138,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(StaticOptimization, Analysis); double getConvergenceCriterion() { return _convergenceCriterion; } void setMaxIterations( const int maxIt) { _maximumIterations = maxIt; } int getMaxIterations() {return _maximumIterations; } + //-------------------------------------------------------------------------- // ANALYSIS //-------------------------------------------------------------------------- diff --git a/OpenSim/Analyses/StaticOptimizationTarget.cpp b/OpenSim/Analyses/StaticOptimizationTarget.cpp index 7383ed5810..889d9db0a5 100644 --- a/OpenSim/Analyses/StaticOptimizationTarget.cpp +++ b/OpenSim/Analyses/StaticOptimizationTarget.cpp @@ -21,50 +21,39 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -/* Note: This code was originally developed by Realistic Dynamics Inc. - * Author: Frank C. Anderson +/* Note: This code was originally developed by Realistic Dynamics Inc. + * Author: Frank C. Anderson */ - //============================================================================= // INCLUDES //============================================================================= -#include #include "StaticOptimizationTarget.h" +#include + using namespace OpenSim; using namespace std; -using SimTK::Vector; using SimTK::Matrix; using SimTK::Real; +using SimTK::Vector; #define USE_LINEAR_CONSTRAINT_MATRIX const double StaticOptimizationTarget::SMALLDX = 1.0e-14; -//const double StaticOptimizationTarget::_activationExponent = 2.0; - +// const double StaticOptimizationTarget::_activationExponent = 2.0; + //============================================================================== // CONSTRUCTOR //============================================================================== //______________________________________________________________________________ -/** - * Construct an optimization target. - * - * @param aNP The number of parameters. - * @param aNC The number of constraints. - * @param aT Current time in the integration. - * @param aX Current control values. - * @param aY Current states. - * @param aDYDT Current state derivatives. - */ -StaticOptimizationTarget:: -StaticOptimizationTarget(const SimTK::State& s, Model *aModel,int aNP,int aNC, bool useMusclePhysiology) -{ +StaticOptimizationTarget::StaticOptimizationTarget(const SimTK::State& s, + Model* aModel, int aNP, int aNC, bool useMusclePhysiology) { // ALLOCATE STATE ARRAYS _recipAreaSquared.setSize(aNP); _recipOptForceSquared.setSize(aNP); _optimalForce.setSize(aNP); - _useMusclePhysiology=useMusclePhysiology; + _useMusclePhysiology = useMusclePhysiology; setModel(*aModel); setNumParams(aNP); @@ -72,12 +61,13 @@ StaticOptimizationTarget(const SimTK::State& s, Model *aModel,int aNP,int aNC, b setActivationExponent(2.0); computeActuatorAreas(s); - // Gather indices into speed set corresponding to the unconstrained degrees of freedom (for which we will set acceleration constraints) + // Gather indices into speed set corresponding to the unconstrained degrees + // of freedom (for which we will set acceleration constraints) _accelerationIndices.setSize(0); auto coordinates = aModel->getCoordinatesInMultibodyTreeOrder(); for (size_t i = 0u; i < coordinates.size(); ++i) { const Coordinate& coord = *coordinates[i]; - if(!coord.isConstrained(s)) { + if (!coord.isConstrained(s)) { _accelerationIndices.append(static_cast(i)); } } @@ -86,51 +76,51 @@ StaticOptimizationTarget(const SimTK::State& s, Model *aModel,int aNP,int aNC, b //============================================================================== // CONSTRUCTION //============================================================================== -bool StaticOptimizationTarget:: -prepareToOptimize(SimTK::State& s, double *x) -{ +bool StaticOptimizationTarget::prepareToOptimize(SimTK::State& s, double* x) { // COMPUTE MAX ISOMETRIC FORCE const ForceSet& fSet = _model->getForceSet(); - - for(int i=0, j=0;i(&fSet.get(i)); - if( act ) { - double fOpt; - Muscle *mus = dynamic_cast(&fSet.get(i)); - if( mus ) { - //ActivationFiberLengthMuscle *aflmus = dynamic_cast(mus); - if(mus && _useMusclePhysiology) { + if (act) { + double fOpt; + Muscle* mus = dynamic_cast(&fSet.get(i)); + if (mus) { + // ActivationFiberLengthMuscle *aflmus = + // dynamic_cast(mus); + if (mus && _useMusclePhysiology) { _model->setAllControllersEnabled(true); fOpt = mus->calcInextensibleTendonActiveFiberForce(s, 1.0); _model->setAllControllersEnabled(false); } else { fOpt = mus->getMaxIsometricForce(); } - } else { - fOpt = act->getOptimalForce(); - } + } else { + fOpt = act->getOptimalForce(); + } _optimalForce[j++] = fOpt; - } + } } #ifdef USE_LINEAR_CONSTRAINT_MATRIX - //cout<<"Computing linear constraint matrix..."<getForceSet(); SimTK::Vector tempAccel(getNumConstraints()); computeAcceleration(s, parameters, tempAccel); - for(int i=0,j=0;i(&fs.get(i)); - if( act )forces(j++) = act->getActuation(s); + if (act) forces(j++) = act->getActuation(s); } } //============================================================================== // UTILITY //============================================================================== //______________________________________________________________________________ -/** - * Ensure that a derivative perturbation is a valid size - */ -void StaticOptimizationTarget:: -validatePerturbationSize(double &aSize) -{ - if(aSizeupdForceSet(); - for(int i=0, j=0;i(&forceSet.get(i)); - if( act ) { - act->setActuation(s, 1.0); - _recipAreaSquared[j] = act->getStress(s); - _recipAreaSquared[j] *= _recipAreaSquared[j]; - j++; + for (int i = 0, j = 0; i < forceSet.getSize(); i++) { + ScalarActuator* act = dynamic_cast(&forceSet.get(i)); + if (act) { + act->setActuation(s, 1.0); + _recipAreaSquared[j] = act->getStress(s); + _recipAreaSquared[j] *= _recipAreaSquared[j]; + j++; } } } @@ -340,105 +266,86 @@ computeActuatorAreas(const SimTK::State& s ) // STATIC DERIVATIVES //============================================================================= //_____________________________________________________________________________ -/** - * Compute derivatives of a constraint with respect to the - * controls by central differences. - * - * @param dx An array of control perturbation values. - * @param x Values of the controls at time t. - * @param ic Index of the constraint. - * @param dcdx The derivatives of the constraints. - * - * @return -1 if an error is encountered, 0 otherwise. - */ -int StaticOptimizationTarget:: -CentralDifferencesConstraint(const StaticOptimizationTarget *aTarget, - double *dx,const Vector &x,Matrix &jacobian) -{ - if(aTarget==NULL) return(-1); + +int StaticOptimizationTarget::CentralDifferencesConstraint( + const StaticOptimizationTarget* aTarget, double* dx, const Vector& x, + Matrix& jacobian) { + if (aTarget == NULL) return (-1); // INITIALIZE CONTROLS - int nx = aTarget->getNumParameters(); if(nx<=0) return(-1); - int nc = aTarget->getNumConstraints(); if(nc<=0) return(-1); - Vector xp=x; - Vector cf(nc),cb(nc); + int nx = aTarget->getNumParameters(); + if (nx <= 0) return (-1); + int nc = aTarget->getNumConstraints(); + if (nc <= 0) return (-1); + Vector xp = x; + Vector cf(nc), cb(nc); // INITIALIZE STATUS int status = -1; // LOOP OVER CONTROLS - for(int i=0;iconstraintFunc(xp,true,cf); - if(status<0) return(status); + status = aTarget->constraintFunc(xp, true, cf); + if (status < 0) return (status); // PERTURB BACKWARD xp[i] = x[i] - dx[i]; - status = aTarget->constraintFunc(xp,true,cb); - if(status<0) return(status); + status = aTarget->constraintFunc(xp, true, cb); + if (status < 0) return (status); // DERIVATIVES OF CONSTRAINTS double rdx = 0.5 / dx[i]; - for(int j=0;jgetNumParameters(); if(nx<=0) return(-1); - Vector xp=x; + int nx = aTarget->getNumParameters(); + if (nx <= 0) return -1; + Vector xp = x; // PERFORMANCE - double pf,pb; + double pf, pb; // INITIALIZE STATUS int status = -1; // LOOP OVER CONTROLS - for(int i=0;iobjectiveFunc(xp,true,pf); - if(status<0) return(status); + status = aTarget->objectiveFunc(xp, true, pf); + if (status < 0) return status; // PERTURB BACKWARD xp[i] = x[i] - dx[i]; - status = aTarget->objectiveFunc(xp,true,pb); - if(status<0) return(status); + status = aTarget->objectiveFunc(xp, true, pb); + if (status < 0) return status; // DERIVATIVES OF PERFORMANCE double rdx = 0.5 / dx[i]; - dpdx[i] = rdx*(pf-pb); + dpdx[i] = rdx * (pf - pb); // RESTORE CONTROLS xp[i] = x[i]; } - return(status); + return status; } //============================================================================== @@ -448,134 +355,118 @@ CentralDifferences(const StaticOptimizationTarget *aTarget, // PERFORMANCE //------------------------------------------------------------------------------ //______________________________________________________________________________ -/** - * Compute performance given parameters. - * - * @param parameters Vector of optimization parameters. - * @param performance Value of the performance criterion. - * @return Status (normal termination = 0, error < 0). - */ -int StaticOptimizationTarget:: -objectiveFunc(const Vector ¶meters, const bool new_parameters, Real &performance) const -{ - //LARGE_INTEGER start; - //LARGE_INTEGER stop; - //LARGE_INTEGER frequency; - //QueryPerformanceFrequency(&frequency); - //QueryPerformanceCounter(&start); +int StaticOptimizationTarget::objectiveFunc(const Vector& parameters, + const bool new_parameters, Real& performance) const { + // LARGE_INTEGER start; + // LARGE_INTEGER stop; + // LARGE_INTEGER frequency; + + // QueryPerformanceFrequency(&frequency); + // QueryPerformanceCounter(&start); int na = _model->getActuators().getSize(); double p = 0.0; - for(int i=0;igetActuators().getSize(); - for(int i=0;igetCoordinatesInMultibodyTreeOrder(); // CONSTRAINTS - for(int i=0; igetStateIndex(coord.getSpeedName(), 0); - if (ind < 0){ + if (ind < 0) { // get the full coordinate speed state variable path name string fullname = coord.getStateVariableNames()[1]; ind = _statesStore->getStateIndex(fullname, 0); - if (ind < 0){ - string msg = "StaticOptimizationTarget::computeConstraintVector: \n"; - msg+= "target motion for coordinate '"; + if (ind < 0) { + string msg = + "StaticOptimizationTarget::computeConstraintVector: \n"; + msg += "target motion for coordinate '"; msg += coord.getName() + "' not found."; throw Exception(msg); } } - Function& targetFunc = _statesSplineSet.get(ind); - std::vector derivComponents(1,0); //take first derivative - double targetAcceleration = targetFunc.calcDerivative(derivComponents, SimTK::Vector(1, s.getTime())); - //std::cout << "computeConstraintVector:" << targetAcceleration << " - " << actualAcceleration[i] << endl; + + double t = s.getTime(); + int nq = _model->getNumCoordinates(); + int nu = _model->getNumSpeeds(); + Array targetStateArray(0.0, nq + nu); + + //Ensure state time matches stored time associated with derivative + int idx = _statesDerivativeStore->findIndex(t); + double tstore; + _statesDerivativeStore->getTime(idx, tstore); + OPENSIM_THROW_IF( + abs(t - tstore) > FLT_EPSILON, + OpenSim::Exception, + "State time " + std::to_string(t) + " does not match storage time" + + std::to_string(tstore) + ) + + _statesDerivativeStore->getDataAtTime(t, nq + nu, targetStateArray); + double targetAcceleration = targetStateArray[ind]; + + // std::cout << "computeConstraintVector:" << targetAcceleration << " - + // " << actualAcceleration[i] << endl; constraints[i] = targetAcceleration - actualAcceleration[i]; } - //QueryPerformanceCounter(&stop); - //double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart; - //std::cout << "computeConstraintVector time = " << (duration*1.0e3) << " milliseconds" << std::endl; + // QueryPerformanceCounter(&stop); + // double duration = + // (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart; + // std::cout << "computeConstraintVector time = " << (duration*1.0e3) << " + // milliseconds" << std::endl; // 1.5 ms } //______________________________________________________________________________ -/** - * Compute the gradient of constraint given parameters. - * - * @param parameters Vector of parameters. - * @param jac Derivative of constraint with respect to the parameters. - * @return Status (normal termination = 0, error < 0). - */ -int StaticOptimizationTarget:: -constraintJacobian(const SimTK::Vector ¶meters, const bool new_parameters, SimTK::Matrix &jac) const -{ - //LARGE_INTEGER start; - //LARGE_INTEGER stop; - //LARGE_INTEGER frequency; - //QueryPerformanceFrequency(&frequency); - //QueryPerformanceCounter(&start); +int StaticOptimizationTarget::constraintJacobian( + const SimTK::Vector& parameters, const bool new_parameters, + SimTK::Matrix& jac) const { + // LARGE_INTEGER start; + // LARGE_INTEGER stop; + // LARGE_INTEGER frequency; + + // QueryPerformanceFrequency(&frequency); + // QueryPerformanceCounter(&start); #ifndef USE_LINEAR_CONSTRAINT_MATRIX - // Compute gradient - StaticOptimizationTarget::CentralDifferencesConstraint(this,&_dx[0],parameters,jac); + // Compute gradient + StaticOptimizationTarget::CentralDifferencesConstraint( + this, &_dx[0], parameters, jac); #else // Use precomputed constraint matrix (works if constraint is linear) - //cout<<"Computing constraint gradient assuming linear dependence..."<getForceSet(); - for(int i=0,j=0;i(&fs.get(i)); - if( act ) { - act->setOverrideActuation(s, parameters[j] * _optimalForce[j]); - j++; - } + for (int i = 0, j = 0; i < fs.getSize(); i++) { + ScalarActuator* act = dynamic_cast(&fs.get(i)); + if (act) { + act->setOverrideActuation(s, parameters[j] * _optimalForce[j]); + j++; + } } - _model->getMultibodySystem().realize(s,SimTK::Stage::Acceleration); + _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration); SimTK::Vector udot = _model->getMatterSubsystem().getUDot(s); - for(int i=0; i<_accelerationIndices.getSize(); i++) + for (int i = 0; i < _accelerationIndices.getSize(); i++) rAccel[i] = udot[_accelerationIndices[i]]; - //QueryPerformanceCounter(&stop); - //double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart; - //std::cout << "computeAcceleration time = " << (duration*1.0e3) << " milliseconds" << std::endl; + // QueryPerformanceCounter(&stop); + // double duration = + // (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart; + // std::cout << "computeAcceleration time = " << (duration*1.0e3) << " + // milliseconds" << std::endl; // 1.45 ms } diff --git a/OpenSim/Analyses/StaticOptimizationTarget.h b/OpenSim/Analyses/StaticOptimizationTarget.h index 0f1f5d75d0..b71103b43e 100644 --- a/OpenSim/Analyses/StaticOptimizationTarget.h +++ b/OpenSim/Analyses/StaticOptimizationTarget.h @@ -26,107 +26,263 @@ //============================================================================= // INCLUDES //============================================================================= -#include "osimAnalysesDLL.h" #include "OpenSim/Common/Array.h" -#include +#include "osimAnalysesDLL.h" #include +#include + //============================================================================= //============================================================================= -namespace OpenSim { +namespace OpenSim { + +class Model; /** - * This class provides an interface specification for static optimization Objective Function. - * + * This class provides an interface specification for static optimization + * Objective Function. * @author Jeff Reinbolt */ -class OSIMANALYSES_API StaticOptimizationTarget : public SimTK::OptimizerSystem -{ -//============================================================================= -// DATA -//============================================================================= +class OSIMANALYSES_API StaticOptimizationTarget + : public SimTK::OptimizerSystem { + //============================================================================= + // DATA + //============================================================================= public: /** Smallest allowable perturbation size for computing derivatives. */ static const double SMALLDX; private: - /** Work model. */ - Model *_model; + Model* _model; /** Current state */ const SimTK::State* _currentState; /** Reciprocal of actuator area squared. */ Array _recipAreaSquared; - /** Reciprocal of optimal force squared accounting for force-length curve if actuator is a muscle. */ + /** Reciprocal of optimal force squared accounting for force-length curve if + * actuator is a muscle. */ Array _recipOptForceSquared; - /** Optimal force accounting for force-length curve if desired and if actuator is a muscle. */ + /** Optimal force accounting for force-length curve if desired and if + * actuator is a muscle. */ Array _optimalForce; - + SimTK::Matrix _constraintMatrix; SimTK::Vector _constraintVector; - const Storage *_statesStore; + const Storage* _statesStore; GCVSplineSet _statesSplineSet; + const Storage* _statesDerivativeStore; protected: double _activationExponent; - bool _useMusclePhysiology; + bool _useMusclePhysiology; /** Perturbation size for computing numerical derivatives. */ Array _dx; Array _accelerationIndices; -//============================================================================= -// METHODS -//============================================================================= + //============================================================================= + // METHODS + //============================================================================= public: - StaticOptimizationTarget(const SimTK::State& s, Model *aModel,int aNX,int aNC, const bool useMusclePhysiology=true); + /** + * Construct an optimization target. + * + * @param s The current model state. + * @param aModel The model to use in optimization. + * @param aNP The number of parameters. + * @param aNC The number of constraints. + * @param useMusclePhysiology If false, ignores muscle force-length and + * force-velocity relationships as well as pennation angle + */ + StaticOptimizationTarget(const SimTK::State& s, Model* aModel, int aNP, + int aNC, const bool useMusclePhysiology = true); // SET AND GET + + /** + * Set the model. + * + * @param aModel Model. + */ void setModel(Model& aModel); - void setStatesStore(const Storage *aStatesStore); + + /** + * Set the states storage. + * + * @param aStatesStore States storage. + */ + void setStatesStore(const Storage* aStatesStore); + + /** + * Set the states derivative storage. + * + * @param aStatesDerivativeStore States derivative storage. + */ + void setStatesDerivativeStore(const Storage* aStatesDerivativeStore); + + /** + * Set the states spline set. + * + * @param aStatesSplineSet States spline set. + */ void setStatesSplineSet(GCVSplineSet aStatesSplineSet); + + /** + * Set the number of parameters. + * + * The number of parameters can be set at any time. However, the + * perturbation sizes for the parameters (i.e., _dx) is destroyed. + * Therefore, the perturbation sizes must be reset. + * + * @param aNP Number of parameters. + * @see setDX() + */ void setNumParams(const int aNP); + + /** + * Set the number of constraints. + * + * @param aNC Number of constraints. + */ void setNumConstraints(const int aNC); + + /** + * Set the derivative perturbation size. + */ void setDX(double aVal); - void setDX(int aIndex,double aVal); + + /** + * Set the derivative perturbation size for all controls. + */ + void setDX(int aIndex, double aVal); + + /** + * Get the derivative perturbation size. + */ double getDX(int aIndex); + + /** + * Get a pointer to the vector of derivative perturbation sizes. + */ double* getDXArray(); - void getActuation(SimTK::State& s, const SimTK::Vector ¶meters, SimTK::Vector &forces); - void setActivationExponent(double aActivationExponent) { _activationExponent=aActivationExponent; } + + /** + * Get an optimal force. + */ + void getActuation(SimTK::State& s, const SimTK::Vector& parameters, + SimTK::Vector& forces); + + void setActivationExponent(double aActivationExponent) { + _activationExponent = aActivationExponent; + } double getActivationExponent() const { return _activationExponent; } - void setCurrentState( const SimTK::State* state) { _currentState = state; } + void setCurrentState(const SimTK::State* state) { _currentState = state; } const SimTK::State* getCurrentState() const { return _currentState; } // UTILITY - void validatePerturbationSize(double &aSize); - virtual void printPerformance(const SimTK::State& s, double *x); + /** + * Ensure that a derivative perturbation is a valid size + */ + void validatePerturbationSize(double& aSize); + + virtual void printPerformance(const SimTK::State& s, double* x); void computeActuatorAreas(const SimTK::State& s); - static int - CentralDifferencesConstraint(const StaticOptimizationTarget *aTarget, - double *dx,const SimTK::Vector &x,SimTK::Matrix &jacobian); - static int - CentralDifferences(const StaticOptimizationTarget *aTarget, - double *dx,const SimTK::Vector &x,SimTK::Vector &dpdx); + /** + * Compute derivatives of a constraint with respect to the + * controls by central differences. + * + * @param aTarget Optimization target. + * @param dx An array of control perturbation values. + * @param x Values of the controls at time t. + * @param jacobian System Jacobian matrix. + * + * @return -1 if an error is encountered, 0 otherwise. + */ + static int CentralDifferencesConstraint( + const StaticOptimizationTarget* aTarget, double* dx, + const SimTK::Vector& x, SimTK::Matrix& jacobian); - bool prepareToOptimize(SimTK::State& s, double *x); + /** + * Compute derivatives of performance with respect to the + * controls by central differences. Note that the gradient array should + * be allocated as dpdx[nx]. + * + * @param aTarget Optimization target. + * @param dx An array of control perturbation values. + * @param x Values of the controls at time t. + * @param dpdx The derivatives of the performance criterion. + * + * @return -1 if an error is encountered, 0 otherwise. + */ + static int CentralDifferences(const StaticOptimizationTarget* aTarget, + double* dx, const SimTK::Vector& x, SimTK::Vector& dpdx); + + bool prepareToOptimize(SimTK::State& s, double* x); //-------------------------------------------------------------------------- // REQUIRED OPTIMIZATION TARGET METHODS //-------------------------------------------------------------------------- - int objectiveFunc(const SimTK::Vector &x, bool new_coefficients, SimTK::Real& rP) const override; - int gradientFunc(const SimTK::Vector &x, bool new_coefficients, SimTK::Vector &gradient) const override; - int constraintFunc(const SimTK::Vector &x, bool new_coefficients, SimTK::Vector &constraints) const override; - int constraintJacobian(const SimTK::Vector &x, bool new_coefficients, SimTK::Matrix &jac) const override; + + /** + * Compute the objective function given the optimization parameters. + * + * @param parameters Vector of optimization parameters. + * @param new_parameters Flag indicating if the parameters have changed. + * @param performance Value of the objective function. + * @return Status (normal termination = 0, error < 0). + */ + int objectiveFunc(const SimTK::Vector& parameters, bool new_parameters, + SimTK::Real& performance) const override; + + /** + * Compute the gradient of the objective function given the optimization + * parameters. + * + * @param parameters Vector of optimization parameters. + * @param new_parameters Flag indicating if the parameters have changed. + * @param gradient Derivatives of the objective function with respect to the + * optimization parameters. + * @return Status (normal termination = 0, error < 0). + */ + int gradientFunc(const SimTK::Vector& parameters, bool new_parameters, + SimTK::Vector& gradient) const override; + + /** + * Compute acceleration constraints given the optimization parameters. + * + * @param parameters Vector of optimization parameters. + * @param new_parameters Flag indicating if the parameters have changed. + * @param constraints Vector of optimization constraints. + * @return Status (normal termination = 0, error < 0). + */ + int constraintFunc(const SimTK::Vector& parameters, bool new_parameters, + SimTK::Vector& constraints) const override; + + /** + * Compute the constraint Jacobian given the optimization parameters. + * + * @param parameters Vector of parameters. + * @param new_parameters Flag indicating if the parameters have changed. + * @param jac The constraint Jacobian with respect to the optimization + * parameters. + * @return Status (normal termination = 0, error < 0). + */ + int constraintJacobian(const SimTK::Vector& parameters, bool new_parameters, + SimTK::Matrix& jac) const override; private: - void computeConstraintVector(SimTK::State& s, const SimTK::Vector &x, SimTK::Vector &c) const; - void computeAcceleration(SimTK::State& s, const SimTK::Vector &aF,SimTK::Vector &rAccel) const; - void cumulativeTime(double &aTime, double aIncrement); + /** + * Compute all constraints given the optimization parameters. + */ + void computeConstraintVector(SimTK::State& s, + const SimTK::Vector& parameters, SimTK::Vector& constraints) const; + void computeAcceleration(SimTK::State& s, const SimTK::Vector& parameters, + SimTK::Vector& rAccel) const; }; -}; //namespace +}; // namespace OpenSim #endif // _StaticOptimizationTarget_h_