/**
* Compute all constraints given x.
*/
void ActuatorForceTargetFast::
computeConstraintVector(SimTK::State& s, const Vector &x,Vector &c) const
{
CMC_TaskSet& taskSet = _controller->updTaskSet();
const Set<Actuator>& fSet = _controller->getActuatorSet();
int nf = fSet.getSize();
// Now override the actuator forces with computed active force
// (from static optimization) but also include the passive force
// contribution of muscles when applying forces to the model
for(int i=0;i<nf;i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideForce(s,true);
act->setOverrideForce(s, x[i]);
}
_controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );
taskSet.computeAccelerations(s);
Array<double> &w = taskSet.getWeights();
Array<double> &aDes = taskSet.getDesiredAccelerations();
Array<double> &a = taskSet.getAccelerations();
// CONSTRAINTS
for(int i=0; i<getNumConstraints(); i++)
c[i]=w[i]*(aDes[i]-a[i]);
// reset the actuator control
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideForce(s,false);
}
_controller->getModel().getMultibodySystem().realizeModel(s);
}
