From: Arnav495 Date: Fri, 6 Feb 2026 17:58:37 +0000 (-0800) Subject: Fix floating point issues. X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=42cd222ff79efc664b7a4b9b4514c199d82cf9b4;p=FRC2026.git Fix floating point issues. --- diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 5316e3e..d79a19d 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -94,7 +94,8 @@ public class ShooterPhysics { double avgHeight = (newRange.getFirst().height() + newRange.getSecond().height()) / 2; TurretState guess = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, avgHeight), avgHeight); if (guess.satisfies(constraints)) { - if (guess.height() < lastValid.height()) lastValid = guess; + if (guess.height() < lastValid.height()) + lastValid = guess; newRange = new Pair(guess, newRange.getSecond()); } else { newRange = new Pair(newRange.getFirst(), guess); @@ -142,7 +143,10 @@ public class ShooterPhysics { // peakZ = .5 * v_z_exit_vel² / g // v_z_exit_vel² = 2 * peakZ * g // v_z_exit_vel = √(2 * peakZ * g) - double zExitVel = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION); + // keep a second variable to avoid floating point precision issues where the + // sqrt for t ends up being NaN sometimes when peakZ = target.getZ() + double zExitVelSquared = 2 * peakZ * Constants.GRAVITY_ACCELERATION; + double zExitVel = Math.sqrt(zExitVelSquared); // now we need time to hit target // z_target = v_z_exit_vel * t - .5 * g * t² @@ -153,10 +157,11 @@ public class ShooterPhysics { // t = (-v_z_exit_vel ± √(v_z_exit_vel² - 2 * g * z_target)) / -g // onlz use - because we only want the part where it's coming down, and that // gives the longer time - double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) + double t = (-zExitVel - Math.sqrt(zExitVelSquared - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION; - if (t < 0) + // this is not equivalent to t <= 0 because of NaNs + if (!(t > 0)) throw new RuntimeException("Time should never be negative (got t=" + t + ")."); // calculate x and z exit_vel @@ -194,7 +199,8 @@ public class ShooterPhysics { Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); Translation3d guessVelocityMore = getRequiredExitVelocity(initialVelocity, target, guess + 0.1); double derivative = (guessVelocityMore.getNorm() - guessVelocity.getNorm()) / 0.1; - // System.out.println(guess + "\t\t" + guessVelocity.getNorm() + "\t\t" + derivative); + // System.out.println(guess + "\t\t" + guessVelocity.getNorm() + "\t\t" + + // derivative); // we've already hit minimum height and are trying to go lower if (guess <= effectiveMinHeight && derivative > 0) @@ -217,7 +223,6 @@ public class ShooterPhysics { public static Optional withAngle(Translation2d initialVelocity, Translation3d target, double pitch, double tolerance) { - // guess a peak height double guess = target.getZ() + 2; int maxIters = 50; @@ -232,7 +237,8 @@ public class ShooterPhysics { Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); TurretState polar = cvtShot(guessVelocity, guess); double difference = pitch - polar.pitch(); - // System.out.println(guess + "\t\t" + difference); + // System.out.println(guess + "\t\t" + guessVelocity + "\t\t" + polar.pitch() + + // "\t\t" + difference); // we've already hit minimum height and are trying to go lower if (guess <= target.getZ() && difference < 0) diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index 7add5dd..a303229 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -9,7 +9,6 @@ import java.util.Random; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; import edu.wpi.first.math.geometry.Rotation2d; @@ -152,7 +151,7 @@ class ShooterPhysicsTest { state1.height() - 0.1); assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString()); assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString()); - assertEquals(Math.PI/4, state1.pitch(), epsilon); + assertEquals(Math.PI / 4, state1.pitch(), epsilon); var t2 = new Translation3d(1, 1, 100); var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2); @@ -175,8 +174,8 @@ class ShooterPhysicsTest { @Test public void angleTest() { - // var t1 = new Translation3d(100, 0, 0); - // for (int i = 0; i < 1000; i++) { + // var t1 = new Translation3d(1, 2, 3); + // for (int i = 31; i < 1000; i++) { // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.); // System.out.println(i / 10. + ", " + x.pitch()); // } @@ -187,8 +186,10 @@ class ShooterPhysicsTest { assertTrue(state1.isPresent()); assertEquals(state1.get().pitch(), Units.degreesToRadians(30), epsilon); // get this as a velocity vector - Translation3d v1 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, t1, state1.get().height()); - assertEquals(ShooterPhysics.cvtShot(v1, state1.get().height()), state1.get()); + Translation3d v1 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, t1, + state1.get().height()); + assertEquals(ShooterPhysics.cvtShot(v1, state1.get().height()), + state1.get()); checkTrajectory(Translation3d.kZero, v1, t1, state1.get().height()); var t2 = new Translation3d(1, -1, 100); @@ -203,20 +204,30 @@ class ShooterPhysicsTest { assertTrue(state3.isPresent()); assertEquals(state3.get().pitch(), Units.degreesToRadians(45), epsilon); // get this as a velocity vector - Translation3d v3 = ShooterPhysics.getRequiredExitVelocity(iv3, t3, state3.get().height()); - assertEquals(ShooterPhysics.cvtShot(v3, state3.get().height()), state3.get()); - checkTrajectory(Translation3d.kZero, v3.plus(new Translation3d(iv3)), t3, state3.get().height()); + Translation3d v3 = ShooterPhysics.getRequiredExitVelocity(iv3, t3, + state3.get().height()); + assertEquals(ShooterPhysics.cvtShot(v3, state3.get().height()), + state3.get()); + checkTrajectory(Translation3d.kZero, v3.plus(new Translation3d(iv3)), t3, + state3.get().height()); } - @Disabled @Test public void simpleConstraintsTest() { - Constraints constraints = new Constraints(3, 20, .1, Math.PI - .1); - var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints); + // a test where the optimal shot is just plain height + Constraints constraints1 = new Constraints(3, 20, .1, Math.PI - .1); + var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints1); assertTrue(val1.isPresent()); var direct1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 2, 3), - constraints.height()); - assertEquals(direct1, val1); + constraints1.height()); + assertEquals(direct1.pitch(), val1.get().pitch(), epsilon); + assertEquals(direct1.yaw().getRadians(), val1.get().yaw().getRadians(), epsilon); + assertEquals(direct1.exitVel(), val1.get().exitVel(), epsilon); + + Constraints constraints2 = new Constraints(3, 20, Units.degreesToRadians(45), Math.PI - .2); + var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(2, 2, 3), constraints2); + assertTrue(val2.isPresent()); + assertEquals(Units.degreesToRadians(45), val2.get().pitch(), epsilon); } // test using a simple physics simulation