From: Arnav495 Date: Sat, 7 Feb 2026 21:36:33 +0000 (-0800) Subject: Fix withVelocity. X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=a0338456550c94733ae01636e886428c300ed04b;p=FRC2026.git Fix withVelocity. --- diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 9f6b30e..c9bd59c 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -185,11 +185,14 @@ public class ShooterPhysics { public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target, double tolerance) { + // System.out.println("!!! inv:" + initialVelocity + " tgt:" + target + " tlr:" + tolerance); // trying to calculate a shot for height=0 returns NaN - double effectiveMinHeight = Math.max(target.getZ(), 0.01); + double effectiveMinHeight = Math.max(target.getZ(), 0.001); // guess a peak height - double guess = target.getZ() + 2; + double guess = effectiveMinHeight + 5; + double lastDerivative = Double.NaN; + double lastGuess = Double.NaN; int maxIters = 50; while (maxIters >= 0) { maxIters--; @@ -201,9 +204,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); // we've already hit minimum height and are trying to go lower if (guess <= effectiveMinHeight && derivative > 0) @@ -212,8 +214,21 @@ public class ShooterPhysics { if (Math.abs(derivative) <= tolerance) return cvtShot(guessVelocity, guess); - // desmos guesstimation indicates this works quite well - guess -= derivative * (Math.pow(guess, 2) + 1) / 10; + double secondDerivative; + if (Double.isNaN(lastDerivative) || Double.isNaN(lastGuess)) + secondDerivative = 0; + else + secondDerivative = (lastDerivative - derivative) / (lastGuess - guess); + + assert Double.isFinite(secondDerivative); + lastGuess = guess; + lastDerivative = derivative; + + // System.out.println(guess + "\t\t" + "\t\t" + derivative + "\t\t" + secondDerivative); + if (secondDerivative == 0) + guess -= derivative * 2; + else + guess -= derivative / Math.abs(secondDerivative); } throw new RuntimeException("Failed to compute a trajectory for a minimum speed."); diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index 046004c..5c3572c 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -9,6 +9,7 @@ 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; @@ -137,7 +138,7 @@ class ShooterPhysicsTest { public void velocityTest() { // var t1 = new Translation3d(100, 0, 0); - // for (int i = 0; i < 1000; i++) { + // for (int i = 1; i < 1000; i++) { // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.); // System.out.println(i / 10. + ", " + x.exitVel()); // } @@ -146,9 +147,9 @@ class ShooterPhysicsTest { var state1 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t1); // check moving either way is higher velocity var state1Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t1, - state1.height() + 0.1); + state1.height() + 0.2); var state1Minus = ShooterPhysics.getShotParams(Translation2d.kZero, t1, - state1.height() - 0.1); + state1.height() - 0.2); assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString()); assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString()); assertEquals(Math.PI / 4, state1.pitch(), epsilon); @@ -156,7 +157,8 @@ class ShooterPhysicsTest { var t2 = new Translation3d(1, 1, 100); var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2); // this should get to the minimum height - var state2Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t2, state2.height() + 0.1); + var state2Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t2, + state2.height() + 0.1); assertTrue(state2.exitVel() < state2Plus.exitVel(), state2Plus.toString()); assertEquals(t2.getZ(), state2.height(), epsilon); @@ -165,8 +167,8 @@ class ShooterPhysicsTest { var v3 = new Translation2d(10, -20); var state3 = ShooterPhysics.withMinimumSpeed(v3, t3); // check moving either way is higher velocity - var state3Plus = ShooterPhysics.getShotParams(v3, t3, state3.height() + 0.1); - var state3Minus = ShooterPhysics.getShotParams(v3, t3, state3.height() - 0.1); + var state3Plus = ShooterPhysics.getShotParams(v3, t3, state3.height() + 0.2); + var state3Minus = ShooterPhysics.getShotParams(v3, t3, state3.height() - 0.2); assertTrue(state3.exitVel() < state3Plus.exitVel(), state3Plus.toString()); assertTrue(state3.exitVel() < state3Minus.exitVel(), state3Minus.toString()); } @@ -255,12 +257,12 @@ class ShooterPhysicsTest { // compute 1000 random shots and simulate them for (int i = 0; i < 1000; i++) { - Translation2d initPos = new Translation2d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10); - Translation3d target = new Translation3d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10, - rng.nextDouble() * 10 + 1); - Translation2d initVel = new Translation2d(rng.nextDouble() * 10 - 5, rng.nextDouble() * 10 - 5); + Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10)); + Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10), + rng.nextDouble(1, 11)); + Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5)); Translation3d robotToTarget = target.minus(new Translation3d(initPos)); - double arcHeight = target.getZ() + rng.nextDouble() * 10; + double arcHeight = target.getZ() + rng.nextDouble(10); Translation3d exitVel = ShooterPhysics.getRequiredExitVelocity(initVel, robotToTarget, arcHeight); @@ -269,6 +271,37 @@ class ShooterPhysicsTest { } } + @Disabled + @Test + public void simulatedConstraintsTest() { + Random rng = new Random(6328); + + for (int i = 0; i < 1000; i++) { + Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10)); + Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10), + rng.nextDouble(1, 11)); + Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5)); + Translation3d robotToTarget = target.minus(new Translation3d(initPos)); + + double minAngle = rng.nextDouble(.01, Math.PI / 2 - .03); + Constraints constraints = new Constraints(rng.nextDouble(20), rng.nextDouble(30), minAngle, + rng.nextDouble(minAngle + .01, Math.PI / 2 - .01)); + + var state = ShooterPhysics.getConstrainedParams(initVel, robotToTarget, constraints); + + if (state.isPresent()) { + assertTrue(state.get().satisfies(constraints)); + + // check going down breaks a constraint; we've found the minimum + var lower = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() - .1); + assertTrue(lower.isEmpty()); + // check going up is okay + var higher = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() + .1); + assertTrue(higher.isPresent()); + } + } + } + private class PhysicsObject { private Translation3d pos; private Translation3d vel;