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