//==============================================================================
// 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;
}
/**
* Constructor.
*
* @param aNX Number of controls.
* @param aController Parent controller.
*/
ActuatorForceTargetFast::
ActuatorForceTargetFast(SimTK::State& s, int aNX,CMC *aController):
OptimizationTarget(aNX), _controller(aController)
{
// NUMBER OF CONTROLS
if(getNumParameters()<=0) {
throw(Exception("ActuatorForceTargetFast: ERROR- no controls.\n"));
}
// ALLOCATE STATE ARRAYS
int ny = _controller->getModel().getNumStateVariables();
int nq = _controller->getModel().getNumCoordinates();
int nu = _controller->getModel().getNumSpeeds();
int na = _controller->getActuatorSet().getSize();
_y.setSize(ny);
_dydt.setSize(ny);
_dqdt.setSize(nq);
_dudt.setSize(nu);
_recipAreaSquared.setSize(na);
_recipOptForceSquared.setSize(na);
_recipAvgActForceRangeSquared.setSize(na);
int nConstraints = _controller->getTaskSet().getNumActiveTaskFunctions();
// NUMBERS OF CONSTRAINTS
// There are only linear equality constraints.
setNumEqualityConstraints(nConstraints);
setNumLinearEqualityConstraints(nConstraints);
// DERIVATIVE PERTURBATION SIZES;
setDX(1.0e-6);
// COMPUTE ACTUATOR AREAS
Array<double> f(1.0,na);
const Set<Actuator>& fSet = _controller->getActuatorSet();
for(int i=0,j=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
Muscle* musc = dynamic_cast<Muscle *>(act);
if(musc)
_recipAreaSquared[j] = f[j]/musc->getMaxIsometricForce();
else
_recipAreaSquared[j] = f[j]/act->getOptimalForce();
_recipAreaSquared[j] *= _recipAreaSquared[j];
j++;
}
}
/**
* This function is called at every time step for every actuator.
*
* @param s Current state of the system
* @param controls Controls being calculated
*/
void computeControls(const SimTK::State& s, SimTK::Vector &controls) const
{
// Get the current time in the simulation.
double t = s.getTime();
// Read the mass of the block.
double blockMass = getModel().getBodySet().get( "block" ).getMass();
// Get pointers to each of the muscles in the model.
Muscle* leftMuscle = dynamic_cast<Muscle*> ( &getActuatorSet().get(0) );
Muscle* rightMuscle = dynamic_cast<Muscle*> ( &getActuatorSet().get(1) );
// Compute the desired position of the block in the tug-of-war
// model.
double zdes = desiredModelZPosition(t);
// Compute the desired velocity of the block in the tug-of-war
// model.
double zdesv = desiredModelZVelocity(t);
// Compute the desired acceleration of the block in the tug-
// of-war model.
double zdesa = desiredModelZAcceleration(t);
// Get the z translation coordinate in the model.
const Coordinate& zCoord = _model->getCoordinateSet().
get( "blockToGround_zTranslation" );
// Get the current position of the block in the tug-of-war
// model.
double z = zCoord.getValue(s);
// Get the current velocity of the block in the tug-of-war
// model.
double zv = zCoord.getSpeedValue(s);
// Compute the correction to the desired acceleration arising
// from the deviation of the block's current position from its
// desired position (this deviation is the "position error").
double pErrTerm = kp * ( zdes - z );
// Compute the total desired velocity based on the initial
// desired velocity plus the position error term we
// computed above.
double vErrTerm = kv * ( zdesv - zv );
// Compute the total desired acceleration based on the initial
// desired acceleration plus the position error term we
// computed above.
double desAcc = zdesa + vErrTerm + pErrTerm;
// Compute the desired force on the block as the mass of the
// block times the total desired acceleration of the block.
double desFrc = desAcc * blockMass;
// Get the maximum isometric force for the left muscle.
double FoptL = leftMuscle->getMaxIsometricForce();
// Get the maximum isometric force for the right muscle.
double FoptR = rightMuscle->getMaxIsometricForce();
// If desired force is in direction of one muscle's pull
// direction, then set that muscle's control based on desired
// force. Otherwise, set the muscle's control to zero.
double leftControl = 0.0, rightControl = 0.0;
if( desFrc < 0 ) {
leftControl = abs( desFrc ) / FoptL;
rightControl = 0.0;
}
else if( desFrc > 0 ) {
leftControl = 0.0;
rightControl = abs( desFrc ) / FoptR;
}
// Don't allow any control value to be greater than one.
if( leftControl > 1.0 ) leftControl = 1.0;
if( rightControl > 1.0 ) rightControl = 1.0;
// Thelen muscle has only one control
Vector muscleControl(1, leftControl);
// Add in the controls computed for this muscle to the set of all model controls
leftMuscle->addInControls(muscleControl, controls);
// Specify control for other actuator (muscle) controlled by this controller
muscleControl[0] = rightControl;
rightMuscle->addInControls(muscleControl, controls);
}
