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