public void compute(
FrameVector output,
FrameOrientation desiredOrientation,
FrameVector desiredAngularVelocity,
FrameVector currentAngularVelocity,
FrameVector feedForward) {
computeProportionalTerm(desiredOrientation);
if (currentAngularVelocity != null)
computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity);
computeIntegralTerm();
output.setToZero(proportionalTerm.getReferenceFrame());
output.add(proportionalTerm);
output.add(derivativeTerm);
output.add(integralTerm);
// Limit the max acceleration of the feedback, but not of the feedforward...
// JEP changed 150430 based on Atlas hitting limit stops.
double feedbackAngularActionMagnitude = output.length();
double maximumAction = gains.getMaximumFeedback();
if (feedbackAngularActionMagnitude > maximumAction) {
output.scale(maximumAction / feedbackAngularActionMagnitude);
}
feedbackAngularAction.set(output);
rateLimitedFeedbackAngularAction.update();
rateLimitedFeedbackAngularAction.getFrameTuple(output);
feedForward.changeFrame(bodyFrame);
output.add(feedForward);
}
public void getCoMAcceleration(FrameVector comAccelerationToPack) {
boolean firstIteration = true;
double totalMass = 0.0;
for (RigidBody rigidBody : rigidBodies) {
double mass = rigidBody.getInertia().getMass();
rigidBody.getCoMOffset(comLocation);
spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(
linkLinearMomentumDot, base, rigidBody, comLocation);
linkLinearMomentumDot.scale(mass);
if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot);
else comAccelerationToPack.add(linkLinearMomentumDot);
totalMass += mass;
firstIteration = false;
}
comAccelerationToPack.scale(1.0 / totalMass);
}