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);
}
private void computeIntegralTerm() {
if (gains.getMaximumIntegralError() < 1e-5) {
integralTerm.setToZero(bodyFrame);
return;
}
double integratedErrorAngle = errorAngleAxis.getAngle() * dt;
double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle;
double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle;
double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle;
rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);
double errorMagnitude = rotationErrorCumulated.length();
if (errorMagnitude > gains.getMaximumIntegralError()) {
rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
}
rotationErrorCumulated.getFrameTuple(integralTerm);
integralGainMatrix.transform(integralTerm.getVector());
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSetToZero() throws Exception {
double epsilon = 1.0e-10;
Random random = new Random(21651016L);
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
ReferenceFrame expectedFrame = worldFrame;
double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
FrameOrientation expectedOrientation =
FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
FrameVector expectedAngularVelocity =
FrameVector.generateRandomFrameVector(random, expectedFrame);
String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
expectedNamePrefix,
expectedNameSuffix,
new YoVariableRegistry("schnoop"),
expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
expectedTime = 0.0;
expectedOrientation.setToZero();
expectedAngularVelocity.setToZero();
testedYoFrameSO3TrajectoryPoint.setToZero();
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
expectedFrame = ReferenceFrame.generateRandomReferenceFrame("blop", random, worldFrame);
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, worldFrame);
expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, worldFrame);
testedYoFrameSO3TrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(worldFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
expectedTime = 0.0;
expectedOrientation.setToZero(expectedFrame);
expectedAngularVelocity.setToZero(expectedFrame);
testedYoFrameSO3TrajectoryPoint.switchCurrentReferenceFrame(expectedFrame);
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
}
