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