/** * 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; }