/**
* 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->getForce(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->getForce(getCMCActSubsys()->getCompleteState()) - _f[j];
j++;
}
}
