/**
* Get an optimal force.
*/
void StaticOptimizationTarget::
getActuation(SimTK::State& s, const SimTK::Vector ¶meters, SimTK::Vector &forces)
{
//return(_optimalForce[aIndex]);
const ForceSet& fs = _model->getForceSet();
SimTK::Vector tempAccel(getNumConstraints());
computeAcceleration(s, parameters, tempAccel);
for(int i=0,j=0;i<fs.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
if( act )forces(j++) = act->getActuation(s);
}
}
/**
* Compute the Force.
*/
SimTK::Vector ActuatorForceProbe::computeProbeInputs(const State& s) const
{
int nA = getActuatorNames().size();
SimTK::Vector TotalF(getNumProbeInputs());
TotalF = 0;
// Loop through each actuator in the list of actuator_names.
for (int i = 0; i<nA; ++i)
{
// Get the Actuator force.
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_model->getActuators()[_actuatorIndex[i]]);
const double Ftmp = act->getActuation(s);
// Append to output vector.
if (getSumForcesTogether())
TotalF(0) += std::pow(Ftmp, getExponent());
else
TotalF(i) = std::pow(Ftmp, getExponent());
}
return TotalF;
}
/**
* Evaluate the vector function.
*
* @param s SimTK::State.
* @param aF Array of actuator force differences.
*/
void VectorFunctionForActuators::
evaluate( const SimTK::State& s, double *aX, double *rF)
{
int i;
int N = getNX();
CMC& controller= dynamic_cast<CMC&>(_model->updControllerSet().get("CMC" ));
controller.updControlSet().setControlValues(_tf, aX);
// create a Manager that will integrate just the actuator subsystem and use only the
// CMC controller
Manager manager(*_model, *_integrator);
manager.setInitialTime(_ti);
manager.setFinalTime(_tf);
manager.setSystem( _CMCActuatorSystem );
// tell the mangager to not call the analayses or write to storage
// while the CMCSubsystem is being integrated.
manager.setPerformAnalyses(false);
manager.setWriteToStorage(false);
SimTK::State& actSysState = _CMCActuatorSystem->updDefaultState();
getCMCActSubsys()->updZ(actSysState) = _model->getMultibodySystem()
.getDefaultSubsystem().getZ(s);
actSysState.setTime(_ti);
// Integration
manager.integrate(actSysState, 0.000001);
const Set<Actuator>& forceSet = controller.getActuatorSet();
// Vector function values
int j = 0;
for(i=0;i<N;i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&forceSet[i]);
rF[j] = act->getActuation(getCMCActSubsys()->getCompleteState()) - _f[j];
j++;
}
}
/**
* Record the actuation quantities.
*/
int Actuation::
record(const SimTK::State& s)
{
if (_model == NULL) return(-1);
// MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);
// TIME NORMALIZATION
double tReal = s.getTime();
// FORCE
const Set<Actuator>& fSet = _model->getActuators();
for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
if (!fSet.get(i).get_isDisabled())
_fsp[iact++] = act->getActuation(s);
}
_forceStore->append(tReal, _na, _fsp);
// SPEED
for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
if (!fSet.get(i).get_isDisabled())
_fsp[iact++] = act->getSpeed(s);
}
_speedStore->append(tReal, _na, _fsp);
// POWER
for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
if (!fSet.get(i).get_isDisabled())
_fsp[iact++] = fSet.get(i).getPower(s);
}
_powerStore->append(tReal, _na, _fsp);
return(0);
}
