RateLimitedYoFrameQuaternion rateLimitedQuaternion = new RateLimitedYoFrameQuaternion("blop", "", registry, (DoubleProvider) () -> maxRate.doubleValue(),
dt, ReferenceFrame.getWorldFrame());
rateLimitedQuaternion.update(new Quaternion());
if (distanceToGoal / dt < maxRate.doubleValue())
rateLimitedQuaternion.update(goalQuaternion);
EuclidCoreTestTools.assertQuaternionGeometricallyEquals(goalQuaternion, rateLimitedQuaternion, EPSILON);
rateLimitedQuaternion.update(goalQuaternion);
double distance = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedQuaternion.distance(goalQuaternion)));
assertTrue(distance < previousDistance);
rateLimitedQuaternion.update(goalQuaternion);
EuclidCoreTestTools.assertQuaternionGeometricallyEquals(goalQuaternion, rateLimitedQuaternion, EPSILON);