/**
* 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);
}
