/** * 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; }
/** * 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 * * @param s Current state . * * @return -1 on error, 0 otherwise. */ int StaticOptimization:: begin(SimTK::State& s ) { if(!proceed()) return(0); // Make a working copy of the model delete _modelWorkingCopy; _modelWorkingCopy = _model->clone(); _modelWorkingCopy->initSystem(); // Replace model force set with only generalized forces if(_model) { SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState(); // Update the _forceSet we'll be computing inverse dynamics for if(_ownsForceSet) delete _forceSet; if(_useModelForceSet) { // Set pointer to model's internal force set _forceSet = &_modelWorkingCopy->updForceSet(); _ownsForceSet = false; } else { 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); _ownsForceSet = false; _modelWorkingCopy->setAllControllersEnabled(false); _numCoordinateActuators = _forceSet->getSize(); // Copy whatever forces that are not muscles back into the model for(int i=0; i<saveForces->getSize(); i++){ const Force& f=saveForces->get(i); if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL) as.append(saveForces->get(i).clone()); } } SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem(); // Set modeling options for Actuators to be overriden for(int i=0; i<_forceSet->getSize(); i++) { ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i)); if( act ) { act->overrideActuation(sWorkingCopy, true); } } sWorkingCopy.setTime(s.getTime()); sWorkingCopy.setQ(s.getQ()); sWorkingCopy.setU(s.getU()); sWorkingCopy.setZ(s.getZ()); _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) _accelerationIndices.setSize(0); const CoordinateSet& coordSet = _model->getCoordinateSet(); for(int i=0; i<coordSet.getSize(); i++) { const Coordinate& coord = coordSet.get(i); if(!coord.isConstrained(sWorkingCopy)) { Array<int> inds = _statesStore-> getColumnIndicesForIdentifier(coord.getName()) ; _accelerationIndices.append(inds[0]); } } int na = _forceSet->getSize(); int nacc = _accelerationIndices.getSize(); if(na < nacc) throw(Exception("StaticOptimization: ERROR- over-constrained " "system -- need at least as many forces as there are degrees of freedom.\n") ); _forceReporter.reset(new ForceReporter(_modelWorkingCopy)); _forceReporter->begin(sWorkingCopy); _forceReporter->updForceStorage().reset(); _parameters.resize(_modelWorkingCopy->getNumControls()); _parameters = 0; } _statesSplineSet=GCVSplineSet(5,_statesStore); // DESCRIPTION AND LABELS constructDescription(); constructColumnLabels(); deleteStorage(); allocateStorage(); // RESET STORAGE _activationStorage->reset(s.getTime()); _forceReporter->updForceStorage().reset(s.getTime()); // RECORD int status = 0; if(_activationStorage->getSize()<=0) { status = record(s); const Set<Actuator>& fs = _modelWorkingCopy->getActuators(); for(int k=0;k<fs.getSize();k++) { ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]); if (act){ cout << "Bounds for " << act->getName() << ": " << act->getMinControl() << " to " << act->getMaxControl() << endl; } else{ std::string msg = getConcreteClassName(); msg += "::can only process scalar Actuator types."; throw Exception(msg); } } } return(status); }
// 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()); }