/**
* Apply the actuator force to BodyA and BodyB.
*/
void TorqueActuator::computeForce(const State& s,
Vector_<SpatialVec>& bodyForces,
Vector& generalizedForces) const
{
if(_model==NULL) return;
const SimbodyEngine& engine = getModel().getSimbodyEngine();
const bool torqueIsGlobal = getTorqueIsGlobal();
const Vec3& axis = getAxis();
double force = 0;
if( isForceOverriden(s) ) {
force = computeOverrideForce(s);
} else {
force = computeActuation(s);
}
setForce(s, force );
if(!_bodyA)
return;
setForce(s, force );
Vec3 torque = force*UnitVec3(axis);
if (!torqueIsGlobal)
engine.transform(s, *_bodyA, torque, engine.getGroundBody(), torque);
applyTorque(s, *_bodyA, torque, bodyForces);
// if bodyB is not specified, use the ground body by default
if(_bodyB)
applyTorque(s, *_bodyB, -torque, bodyForces);
// get the angular velocity of the body in ground
Vec3 omegaA(0), omegaB(0);
engine.getAngularVelocity(s, *_bodyA, omegaA);
engine.getAngularVelocity(s, *_bodyB, omegaB);
// the "speed" is the relative angular velocity of the bodies
// projected onto the torque axis.
setSpeed(s, ~(omegaA-omegaB)*axis);
}
