/**
* Record the results.
*/
int StaticOptimization::
record(const SimTK::State& s)
{
if(!_modelWorkingCopy) return -1;
// 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);
// update Q's and U's
sWorkingCopy.setQ(s.getQ());
sWorkingCopy.setU(s.getU());
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
//_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
int na = fs.getSize();
int nacc = _accelerationIndices.getSize();
// IPOPT
_numericalDerivativeStepSize = 0.0001;
_optimizerAlgorithm = "ipopt";
_printLevel = 0;
//_optimizationConvergenceTolerance = 1e-004;
//_maxIterations = 2000;
// Optimization target
_modelWorkingCopy->setAllControllersEnabled(false);
StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
target.setStatesStore(_statesStore);
target.setStatesSplineSet(_statesSplineSet);
target.setActivationExponent(_activationExponent);
target.setDX(_numericalDerivativeStepSize);
// Pick optimizer algorithm
SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
//SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;
// Optimizer
SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);
// Optimizer options
//cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
optimizer->setDiagnosticsLevel(_printLevel);
//cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
optimizer->setConvergenceTolerance(_convergenceCriterion);
//cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
optimizer->setMaxIterations(_maximumIterations);
optimizer->useNumericalGradient(false);
optimizer->useNumericalJacobian(false);
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);
}
// Parameter bounds
SimTK::Vector lowerBounds(na), upperBounds(na);
for(int i=0,j=0;i<fs.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
if (act) {
lowerBounds(j) = act->getMinControl();
upperBounds(j) = act->getMaxControl();
j++;
}
}
target.setParameterLimits(lowerBounds, upperBounds);
_parameters = 0; // Set initial guess to zeros
// Static optimization
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
target.prepareToOptimize(sWorkingCopy, &_parameters[0]);
//LARGE_INTEGER start;
//LARGE_INTEGER stop;
//LARGE_INTEGER frequency;
//QueryPerformanceFrequency(&frequency);
//QueryPerformanceCounter(&start);
try {
target.setCurrentState( &sWorkingCopy );
optimizer->optimize(_parameters);
}
catch (const SimTK::Exception::Base& ex) {
cout << ex.getMessage() << endl;
cout << "OPTIMIZATION FAILED..." << endl;
cout << endl;
cout << "StaticOptimization.record: WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
cout << endl;
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<na;a++) {
Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(a));
if( act ) {
Muscle* mus = dynamic_cast<Muscle*>(&_forceSet->get(a));
if(mus==NULL) {
if(_parameters(a) < (lowerBounds(a)+tolBounds)) {
msgWeak += " ";
msgWeak += act->getName();
msgWeak += " approaching lower bound of ";
ostringstream oLower;
oLower << lowerBounds(a);
msgWeak += oLower.str();
msgWeak += "\n";
weakModel = true;
} else if(_parameters(a) > (upperBounds(a)-tolBounds)) {
msgWeak += " ";
msgWeak += act->getName();
msgWeak += " approaching upper bound of ";
ostringstream oUpper;
oUpper << upperBounds(a);
msgWeak += oUpper.str();
msgWeak += "\n";
weakModel = true;
}
} else {
if(_parameters(a) > (upperBounds(a)-tolBounds)) {
msgWeak += " ";
msgWeak += mus->getName();
msgWeak += " approaching upper bound of ";
ostringstream o;
o << upperBounds(a);
msgWeak += o.str();
msgWeak += "\n";
weakModel = true;
}
}
}
}
if(weakModel) cout << msgWeak << endl;
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";
SimTK::Vector constraints;
target.constraintFunc(_parameters,true,constraints);
const CoordinateSet& coordSet = _modelWorkingCopy->getCoordinateSet();
for(int acc=0;acc<nacc;acc++) {
if(fabs(constraints(acc)) > tolConstraints) {
const Coordinate& coord = coordSet.get(_accelerationIndices[acc]);
msgIncomplete += " ";
msgIncomplete += coord.getName();
msgIncomplete += ": constraint violation = ";
ostringstream o;
o << constraints(acc);
msgIncomplete += o.str();
msgIncomplete += "\n";
incompleteModel = true;
}
}
_forceReporter->step(sWorkingCopy, 1);
if(incompleteModel) cout << msgIncomplete << endl;
}
}
//QueryPerformanceCounter(&stop);
//double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
//cout << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl;
target.printPerformance(sWorkingCopy, &_parameters[0]);
//update defaults for use in the next step
const Set<Actuator>& actuators = _modelWorkingCopy->getActuators();
for(int k=0; k < actuators.getSize(); ++k){
ActivationFiberLengthMuscle *mus = dynamic_cast<ActivationFiberLengthMuscle*>(&actuators[k]);
if(mus){
mus->setDefaultActivation(_parameters[k]);
}
}
_activationStorage->append(sWorkingCopy.getTime(),na,&_parameters[0]);
SimTK::Vector forces(na);
target.getActuation(const_cast<SimTK::State&>(sWorkingCopy), _parameters,forces);
_forceReporter->step(sWorkingCopy, 1);
return 0;
}
// for adding any components to the model
void CMC::extendAddToSystem( SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
// add event handler for updating controls for next window
CMC* mutableThis = const_cast<CMC *>(this);
ComputeControlsEventHandler* computeControlsHandler =
new ComputeControlsEventHandler(mutableThis);
system.updDefaultSubsystem().addEventHandler(computeControlsHandler );
const Set<Actuator>& fSet = getActuatorSet();
int nActs = fSet.getSize();
mutableThis->_controlSetIndices.setSize(nActs);
// Create the control set that will hold the controls computed by CMC
mutableThis->_controlSet.setName(_model->getName());
mutableThis->_controlSet.setSize(0);
// Define the control set used to specify control bounds and to hold
// the computed control values from the CMC algorithm
double xmin =0, xmax=0;
std::string actName = "";
for(int i=0; i < nActs; ++i ) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
//Actuator& act = getActuatorSet().get(i);
ControlLinear *control = new ControlLinear();
control->setName(act->getName() + ".excitation" );
xmin = act->getMinControl();
if (xmin ==-SimTK::Infinity)
xmin =-MAX_CONTROLS_FOR_RRA;
xmax = act->getMaxControl();
if (xmax ==SimTK::Infinity)
xmax =MAX_CONTROLS_FOR_RRA;
Muscle *musc = dynamic_cast<Muscle *>(act);
// if controlling muscles, CMC requires that the control be constant (i.e. piecewise constant or use steps)
// since it uses this assumption to rootsolve for the required controls over the CMC time-window.
if(musc){
control->setUseSteps(true);
if(xmin < MIN_CMC_CONTROL_VALUE){
cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls < " << MIN_CMC_CONTROL_VALUE <<".\n" <<
"The minimum control limit for muscle '" << musc->getName() << "' has been reset to " << MIN_CMC_CONTROL_VALUE <<"." <<endl;
xmin = MIN_CMC_CONTROL_VALUE;
}
if(xmax < MAX_CMC_CONTROL_VALUE){
cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls > " << MAX_CMC_CONTROL_VALUE <<".\n" <<
"The maximum control limit for muscle '" << musc->getName() << "' has been reset to " << MAX_CMC_CONTROL_VALUE << "." << endl;
xmax = MAX_CMC_CONTROL_VALUE;
}
}
control->setDefaultParameterMin(xmin);
control->setDefaultParameterMax(xmax);
mutableThis->_controlSet.adoptAndAppend(control);
mutableThis->_controlSetIndices.set(i, i);
}
mutableThis->setNumControls(_controlSet.getSize());
}
/**
* Allocate storage for the muscle variables.
*/
void MuscleAnalysis::allocateStorageObjects()
{
if(_model==NULL) return;
// CLEAR EXISTING WORK ARRAYS
_storageList.setMemoryOwner(true);
_storageList.setSize(0);
_momentArmStorageArray.setMemoryOwner(true);
_momentArmStorageArray.setSize(0);
_muscleArray.setMemoryOwner(false);
_muscleArray.setSize(0);
// FOR MOMENT ARMS AND MOMENTS
if(_computeMoments) {
const CoordinateSet& qSet = _model->getCoordinateSet();
_coordinateList = _coordinateListProp.getValueStrArray();
if(IO::Lowercase(_coordinateList[0]) == "all"){
_coordinateList.setSize(0);
for(int i=0; i < qSet.getSize(); ++i) {
_coordinateList.append(qSet[i].getName());
}
}
else{
int i=0;
while(i <_coordinateList.getSize()){
int found = qSet.getIndex(_coordinateList[i]);
if(found < 0){
cout << "MuscleAnalysis: WARNING - coordinate ";
cout << _coordinateList[i] << " is not part of model." << endl;
_coordinateList.remove(i);
}
else{
++i;
}
}
}
int nq = _coordinateList.getSize();
Storage* store;
for(int i=0;i<nq;i++) {
string name = "MomentArm_" + _coordinateList[i];
store = new Storage(1000,name);
store->setDescription(getDescription());
_storageList.append(store);
}
for(int i=0;i<nq;i++) {
string name = "Moment_" + _coordinateList[i];
store = new Storage(1000,name);
store->setDescription(getDescription());
_storageList.append(store);
}
// POPULATE ACTIVE MOMENT ARM ARRAY
_momentArmStorageArray.setSize(0);
for(int i=0; i<nq; i++) {
int found = qSet.getIndex(_coordinateList[i]);
if(found >=0){
StorageCoordinatePair *pair = new StorageCoordinatePair();
pair->q = &qSet[found];
pair->momentArmStore = _storageList[i];
pair->momentStore = _storageList[i+nq];
_momentArmStorageArray.append(pair);
}
}
}
// EVERYTHING ELSE
//_storageList.setMemoryOwner(false);
_pennationAngleStore = new Storage(1000,"PennationAngle");
_pennationAngleStore->setDescription(getDescription());
_storageList.append(_pennationAngleStore );
_lengthStore = new Storage(1000,"Length");
_lengthStore->setDescription(getDescription());
_storageList.append(_lengthStore );
_fiberLengthStore = new Storage(1000,"FiberLength");
_fiberLengthStore->setDescription(getDescription());
_storageList.append(_fiberLengthStore );
_normalizedFiberLengthStore = new Storage(1000,"NormalizedFiberLength");
_normalizedFiberLengthStore->setDescription(getDescription());
_storageList.append(_normalizedFiberLengthStore );
_tendonLengthStore = new Storage(1000,"TendonLength");
_tendonLengthStore->setDescription(getDescription());
_storageList.append(_tendonLengthStore );
_fiberVelocityStore = new Storage(1000,"FiberVelocity");
_fiberVelocityStore->setDescription(getDescription());
_storageList.append(_fiberVelocityStore );
_normFiberVelocityStore = new Storage(1000,"NormFiberVelocity");
_normFiberVelocityStore->setDescription(getDescription());
_storageList.append(_normFiberVelocityStore );
_pennationAngularVelocityStore = new Storage(1000,
"PennationAngularVelocity");
_pennationAngularVelocityStore->setDescription(getDescription());
_storageList.append(_pennationAngularVelocityStore );
_forceStore = new Storage(1000,"TendonForce");
_forceStore->setDescription(getDescription());
_storageList.append(_forceStore );
_fiberForceStore = new Storage(1000,"FiberForce");
_fiberForceStore->setDescription(getDescription());
_storageList.append(_fiberForceStore );
_activeFiberForceStore = new Storage(1000,"ActiveFiberForce");
_activeFiberForceStore->setDescription(getDescription());
_storageList.append(_activeFiberForceStore );
_passiveFiberForceStore = new Storage(1000,"PassiveFiberForce");
_passiveFiberForceStore->setDescription(getDescription());
_storageList.append(_passiveFiberForceStore );
_activeFiberForceAlongTendonStore = new Storage(1000,
"ActiveFiberForceAlongTendon");
_activeFiberForceAlongTendonStore->setDescription(getDescription());
_storageList.append(_activeFiberForceAlongTendonStore );
_passiveFiberForceAlongTendonStore = new Storage(1000,
"PassiveFiberForceAlongTendon");
_passiveFiberForceAlongTendonStore->setDescription(getDescription());
_storageList.append(_passiveFiberForceAlongTendonStore );
_fiberActivePowerStore = new Storage(1000,"FiberActivePower");
_fiberActivePowerStore->setDescription(getDescription());
_storageList.append(_fiberActivePowerStore );
_fiberPassivePowerStore = new Storage(1000,"FiberPassivePower");
_fiberPassivePowerStore->setDescription(getDescription());
_storageList.append(_fiberPassivePowerStore );
_tendonPowerStore = new Storage(1000,"TendonPower");
_tendonPowerStore->setDescription(getDescription());
_storageList.append(_tendonPowerStore );
_musclePowerStore = new Storage(1000,"MuscleActuatorPower");
_musclePowerStore->setDescription(getDescription());
_storageList.append(_musclePowerStore );
// POPULATE MUSCLE LIST FOR "all"
ForceSet& fSet = _model->updForceSet();
_muscleList = _muscleListProp.getValueStrArray();
int nm = _muscleList.getSize();
if((nm==1) && (_muscleList.get(0)=="all")) {
_muscleList.setSize(0);
int nf = fSet.getSize();
for(int i=0;i<nf;i++) {
Muscle *m = dynamic_cast<Muscle*>(&fSet.get(i));
if( m ) _muscleList.append(m->getName());
}
}
// POPULATE ACTIVE MUSCLE ARRAY
Array<string> tmpMuscleList("");
nm = _muscleList.getSize();
_muscleArray.setSize(0);
for(int i=0; i<nm; i++) {
if(fSet.contains(_muscleList[i])) {
Muscle* mus = dynamic_cast<Muscle*>( &fSet.get(_muscleList[i]) );
if(mus){
_muscleArray.append(mus);
tmpMuscleList.append(mus->getName());
}
}
}
_muscleList = tmpMuscleList;
// CONSTRUCT AND SET COLUMN LABELS
constructColumnLabels();
int size = _storageList.getSize();
Storage* store;
for(int i=0;i<size;i++) {
store = _storageList[i];
if(store==NULL) continue;
store->setColumnLabels(getColumnLabels());
}
}
bool SimbodySimmModel::writeMuscle(Muscle& aMuscle, const ForceSet& aActuatorSet, ofstream& aStream)
{
aStream << "beginmuscle " << aMuscle.getName() << endl;
const PathPointSet& pts = aMuscle.getGeometryPath().getPathPointSet();
aStream << "beginpoints" << endl;
for (int i = 0; i < pts.getSize(); i++)
{
PathPoint& pt = pts.get(i);
if (pt.getConcreteClassName()==("ConditionalPathPoint")) {
ConditionalPathPoint* mvp = (ConditionalPathPoint*)(&pt);
Vec3& attachment = mvp->getLocation();
double range[]{ mvp->get_range(0), mvp->get_range(1) };
aStream << attachment[0] << " " << attachment[1] << " " << attachment[2] << " segment " << mvp->getBody().getName();
if (mvp->hasCoordinate()) {
const Coordinate& coord = mvp->getCoordinate();
if (coord.getMotionType() == Coordinate::Rotational)
aStream << " ranges 1 " << coord.getName() << " (" << range[0] * SimTK_RADIAN_TO_DEGREE << ", " << range[1] * SimTK_RADIAN_TO_DEGREE << ")" << endl;
else
aStream << " ranges 1 " << coord.getName() << " (" << range[0] << ", " << range[1] << ")" << endl;
} else {
aStream << " ranges 1 "
<< mvp->getConnector<Coordinate>("coordinate").getConnecteeName()
<< " (0.0, 1.0)" << endl;
}
} else if (pt.getConcreteClassName()==("MovingPathPoint")) {
MovingPathPoint* mpp = (MovingPathPoint*)(&pt);
const Vec3& attachment = mpp->get_location();
if (mpp->hasXCoordinate()) {
aStream << "f" << addMuscleFunction(&mpp->get_x_location(), mpp->getXCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getXCoordinate().getName() << ") ";
} else {
aStream << attachment[0] << " ";
}
if (mpp->hasYCoordinate()) {
aStream << "f" << addMuscleFunction(&mpp->get_y_location(), mpp->getYCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getYCoordinate().getName() << ") ";
} else {
aStream << attachment[1] << " ";
}
if (mpp->hasZCoordinate()) {
aStream << "f" << addMuscleFunction(&mpp->get_z_location(), mpp->getZCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getZCoordinate().getName() << ")";
} else {
aStream << attachment[2];
}
aStream << " segment " << mpp->getBody().getName() << endl;
} else {
Vec3& attachment = pt.getLocation();
aStream << attachment[0] << " " << attachment[1] << " " << attachment[2] << " segment " << pt.getBody().getName() << endl;
}
}
aStream << "endpoints" << endl;
Array<std::string> groupNames;
aActuatorSet.getGroupNamesContaining(aMuscle.getName(),groupNames);
if(groupNames.getSize()) {
aStream << "begingroups" << endl;
for(int i=0; i<groupNames.getSize(); i++)
aStream << " " << groupNames[i];
aStream << endl << "endgroups" << endl;
}
const PathWrapSet& wrapObjects = aMuscle.getGeometryPath().getWrapSet();
for (int i=0; i<wrapObjects.getSize(); i++)
aStream << "wrapobject " << wrapObjects.get(i).getWrapObjectName() << " " <<
(dynamic_cast<WrapEllipsoid*>(&wrapObjects.get(i)) ? (wrapObjects.get(i).getMethodName()+" ") : "") <<
"range " << wrapObjects.get(i).getStartPoint() << " " << wrapObjects.get(i).getEndPoint() << endl;
if (dynamic_cast<Schutte1993Muscle_Deprecated*>(&aMuscle))
{
Schutte1993Muscle_Deprecated *szh = dynamic_cast<Schutte1993Muscle_Deprecated*>(&aMuscle);
aStream << "max_force " << szh->getMaxIsometricForce() << endl;
aStream << "optimal_fiber_length " << szh->getOptimalFiberLength() << endl;
aStream << "tendon_slack_length " << szh->getTendonSlackLength() << endl;
aStream << "pennation_angle " << szh->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
aStream << "max_contraction_velocity " << szh->getMaxContractionVelocity() << endl;
aStream << "timescale " << szh->getTimeScale() << endl;
aStream << "muscle_model 4" << endl;
aStream << "active_force_length_curve f" << addMuscleFunction(&szh->getActiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
aStream << "passive_force_length_curve f" << addMuscleFunction(&szh->getPassiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
aStream << "tendon_force_length_curve f" << addMuscleFunction(&szh->getTendonForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
}
else if (dynamic_cast<Thelen2003Muscle_Deprecated*>(&aMuscle))
{
Thelen2003Muscle_Deprecated *sdm = dynamic_cast<Thelen2003Muscle_Deprecated*>(&aMuscle);
aStream << "max_force " << sdm->getMaxIsometricForce() << endl;
aStream << "optimal_fiber_length " << sdm->getOptimalFiberLength() << endl;
aStream << "tendon_slack_length " << sdm->getTendonSlackLength() << endl;
aStream << "pennation_angle " << sdm->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
aStream << "activation_time_constant " << sdm->getActivationTimeConstant() << endl;
aStream << "deactivation_time_constant " << sdm->getDeactivationTimeConstant() << endl;
aStream << "Vmax " << sdm->getVmax() << endl;
aStream << "Vmax0 " << sdm->getVmax0() << endl;
aStream << "FmaxTendonStrain " << sdm->getFmaxTendonStrain() << endl;
aStream << "FmaxMuscleStrain " << sdm->getFmaxMuscleStrain() << endl;
aStream << "KshapeActive " << sdm->getKshapeActive() << endl;
aStream << "KshapePassive " << sdm->getKshapePassive() << endl;
aStream << "damping " << sdm->getDamping() << endl;
aStream << "Af " << sdm->getAf() << endl;
aStream << "Flen " << sdm->getFlen() << endl;
aStream << "muscle_model 9" << endl;
}
else if (dynamic_cast<Delp1990Muscle_Deprecated*>(&aMuscle))
{
Delp1990Muscle_Deprecated *szh = dynamic_cast<Delp1990Muscle_Deprecated*>(&aMuscle);
aStream << "max_force " << szh->getMaxIsometricForce() << endl;
aStream << "optimal_fiber_length " << szh->getOptimalFiberLength() << endl;
aStream << "tendon_slack_length " << szh->getTendonSlackLength() << endl;
aStream << "pennation_angle " << szh->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
aStream << "max_contraction_velocity " << szh->getMaxContractionVelocity() << endl;
aStream << "timescale " << szh->getTimeScale() << endl;
aStream << "muscle_model 2" << endl;
if (szh->getActiveForceLengthCurve())
aStream << "active_force_length_curve f" << addMuscleFunction(szh->getActiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
if (szh->getPassiveForceLengthCurve())
aStream << "passive_force_length_curve f" << addMuscleFunction(szh->getPassiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
if (szh->getTendonForceLengthCurve())
aStream << "tendon_force_length_curve f" << addMuscleFunction(szh->getTendonForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
if (szh->getForceVelocityCurve())
aStream << "force_velocity_curve f" << addMuscleFunction(szh->getForceVelocityCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
}
aStream << "endmuscle" << endl << endl;
return true;
}
