diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index eea10e40..7fa8f718 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -399,6 +399,10 @@ public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState l */ public static Translation2d cubeTranslation(Translation2d translation) { + if (Math.hypot(translation.getX(), translation.getY()) <= 1.0E-6) + { + return translation; + } return new Translation2d(Math.pow(translation.getNorm(), 3), translation.getAngle()); }