//==============================================================================
// CONSTRUCTION AND DESTRUCTION
//==============================================================================
bool ActuatorForceTargetFast::
prepareToOptimize(SimTK::State& s, double *x)
{
// Keep around a "copy" of the state so we can use it in objective function
// in cases where we're tracking states
_saveState = s;
#ifdef USE_LINEAR_CONSTRAINT_MATRIX
int nf = _controller->getActuatorSet().getSize();
int nc = getNumConstraints();
_constraintMatrix.resize(nc,nf);
_constraintVector.resize(nc);
Vector f(nf), c(nc);
// Build linear constraint matrix and constant constraint vector
f = 0;
computeConstraintVector(s, f, _constraintVector);
for(int j=0; j<nf; j++) {
f[j] = 1;
computeConstraintVector(s, f, c);
_constraintMatrix(j) = (c - _constraintVector);
f[j] = 0;
}
#endif
// use temporary copy of state because computeIsokineticForceAssumingInfinitelyStiffTendon
// will change the muscle states. This is necessary ONLY in the case of deprecated muscles
SimTK::State tempState = s;
double activation = 1.0;
getController()->getModel().getMultibodySystem().realize( tempState, SimTK::Stage::Dynamics );
// COMPUTE MAX ISOMETRIC FORCE
const Set<Actuator>& fSet = _controller->getActuatorSet();
double fOpt = SimTK::NaN;
getController()->getModel().getMultibodySystem().realize(tempState, SimTK::Stage::Dynamics );
for(int i=0 ; i<fSet.getSize(); ++i) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
Muscle* mus = dynamic_cast<Muscle*>(act);
if(mus==NULL) {
fOpt = act->getOptimalForce();
}
else{
fOpt = mus->calcInextensibleTendonActiveFiberForce(tempState,
activation);
}
if( std::fabs(fOpt) < SimTK::TinyReal )
fOpt = SimTK::TinyReal;
_recipOptForceSquared[i] = 1.0 / (fOpt*fOpt);
}
// return false to indicate that we still need to proceed with optimization (did not do a lapack direct solve)
return false;
}
//==============================================================================
// CONSTRUCTION
//==============================================================================
bool StaticOptimizationTarget::
prepareToOptimize(SimTK::State& s, double *x)
{
// COMPUTE MAX ISOMETRIC FORCE
const ForceSet& fSet = _model->getForceSet();
for(int i=0, j=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet.get(i));
if( act ) {
double fOpt;
Muscle *mus = dynamic_cast<Muscle*>(&fSet.get(i));
if( mus ) {
//ActivationFiberLengthMuscle *aflmus = dynamic_cast<ActivationFiberLengthMuscle*>(mus);
if(mus && _useMusclePhysiology) {
_model->setAllControllersEnabled(true);
fOpt = mus->calcInextensibleTendonActiveFiberForce(s, 1.0);
_model->setAllControllersEnabled(false);
} else {
fOpt = mus->getMaxIsometricForce();
}
} else {
fOpt = act->getOptimalForce();
}
_optimalForce[j++] = fOpt;
}
}
#ifdef USE_LINEAR_CONSTRAINT_MATRIX
//cout<<"Computing linear constraint matrix..."<<endl;
int np = getNumParameters();
int nc = getNumConstraints();
_constraintMatrix.resize(nc,np);
_constraintVector.resize(nc);
Vector pVector(np), cVector(nc);
// Build linear constraint matrix and constant constraint vector
pVector = 0;
computeConstraintVector(s, pVector,_constraintVector);
for(int p=0; p<np; p++) {
pVector[p] = 1;
computeConstraintVector(s, pVector, cVector);
for(int c=0; c<nc; c++) _constraintMatrix(c,p) = (cVector[c] - _constraintVector[c]);
pVector[p] = 0;
}
#endif
// return false to indicate that we still need to proceed with optimization
return false;
}
