From 21f4a1100eef8c6e418c71b1877e73b667ed67b9 Mon Sep 17 00:00:00 2001 From: Ray Healy Date: Sat, 14 Feb 2026 15:02:27 -0500 Subject: [PATCH] trajectory/ballistics/models: Added launch and field relative ReversableRadialBM for getTimeAt[XY] --- .../ballistics/models/ReversableRadialBM.java | 82 +++++++++++++------ .../models/ParabolicTimeOfFlightBMTest.java | 21 +++-- 2 files changed, 72 insertions(+), 31 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java index ae26cf0..e5bf120 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java @@ -12,11 +12,11 @@ import edu.wpi.first.units.measure.Time; * each radial position corresponds to exactly one time (e.g. a simple parabolic trajectory) */ public abstract class ReversableRadialBM

extends BallisticModel

{ + public ReversableRadialBM(Translation3d posLaunch, Translation3d posTarget) { super(posLaunch, posTarget); } - // // Reverse functions // @@ -32,44 +32,78 @@ public abstract class ReversableRadialBM

extends BallisticModel

{ /** - * Get the time taken to travel a given X distance (relative to posLaunch) on a given candidate - * trajectory + * Core function converting linear position (relative to posLaunch) to time + * using this.getTimeAtRadius * - * @param candidate The generated velocity to compute against - * @param xDistance The X distance relative to posLaunch to solve for - * @return The time it takes to travel a given X distance relative to posTarget + * @param candidate The candidate velocity to compute for + * @param distance Horizontal position (relative to posLaunch) to test for + * @param isXAxis If true, evaluate for the X axis + * @return The time to the given position + */ + private Time getTimeAtHoriAxis(Velocity3d candidate, Distance distance, boolean isXAxis) { + double azimuthRadians = candidate.getAzimuthAngle().in(Units.Radians); + Distance radialDistance = Units.Meters.of( + distance.in(Units.Meters) / ((isXAxis) ? Math.cos(azimuthRadians) : Math.sin(azimuthRadians)) + ); + return this.getTimeAtRadius(candidate, radialDistance); + } + + + /** + * Get the time taken to reach a given X position (relative to posLaunch) + * for a given launch velocity + * + * @param candidate The launch velocity + * @param xDistance The X position (relative to posLaunch) + * @return The time to the given position + */ + public Time getTimeAtXRelative(Velocity3d candidate, Distance xDistance) { + return getTimeAtHoriAxis(candidate, xDistance, true); + } + + /** + * Get the time taken to reach a given Y position (relative to posLaunch) + * for a given launch velocity + * + * @param candidate The launch velocity + * @param yDistance The Y position (relative to posLaunch) + * @return The time to the given position + */ + public Time getTimeAtYRelative(Velocity3d candidate, Distance yDistance) { + return getTimeAtHoriAxis(candidate, yDistance, false); + } + + /** + * Get the time taken to reach a given X position (relative to the field) + * for a given launch velocity + * + * @param candidate The launch velocity + * @param xDistance The X position (relative to the field) + * @return The time to the given position */ public Time getTimeAtX(Velocity3d candidate, Distance xDistance) { - Distance radialDistance = Units.Meters.of( - xDistance.minus(posLaunch.getMeasureX()).in(Units.Meters) - / Math.cos(candidate.getAzimuthAngle().in(Units.Radians)) - ); - return this.getTimeAtRadius(candidate, radialDistance); + return getTimeAtXRelative(candidate, xDistance.minus(posLaunch.getMeasureX())); } /** - * Get the time taken to travel a given Y distance (relative to posLaunch) on a given candidate - * trajectory + * Get the time taken to reach a given Y position (relative to the field) + * for a given launch velocity * - * @param candidate The generated velocity to compute against - * @param yDistance The Y distance relative to posLaunch to solve for - * @return The time it takes to travel a given Y distance relative to posTarget + * @param candidate The launch velocity + * @param yDistance The Y position (relative to the field) + * @return The time to the given position */ public Time getTimeAtY(Velocity3d candidate, Distance yDistance) { - Distance radialDistance = Units.Meters.of( - yDistance.minus(posLaunch.getMeasureY()).in(Units.Meters) - / Math.sin(candidate.getAzimuthAngle().in(Units.Radians)) - ); - return this.getTimeAtRadius(candidate, radialDistance); + return getTimeAtYRelative(candidate, yDistance.minus(posLaunch.getMeasureY())); } /** - * Get the time to target on a given candidate trajectory + * Get the time to target on a given launch velocity * - * @param candidate The generated velocity to compute against - * @return The time it takes to travel to the target position + * @param candidate The launch velocity + * @return The time to target */ public Time getTimeAtTarget(Velocity3d candidate) { return this.getTimeAtX(candidate, posTarget.getMeasureX()); diff --git a/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java index d743c56..c92988f 100644 --- a/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java +++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java @@ -32,6 +32,7 @@ public class ParabolicTimeOfFlightBMTest { ); } + @Test void testSolutionVelocity() { ParabolicTimeOfFlightBM model = getModel(); Time tof = Units.Seconds.of(1.0); @@ -83,20 +84,26 @@ public class ParabolicTimeOfFlightBMTest { // Test initial position Time t0 = Units.Seconds.of(0.0); assertEquals(t0, model.getTimeAtRadius(solution, Units.Meters.of(0.0)), "Init Position (Rad)"); - assertEquals(t0, model.getTimeAtX(solution, model.getPosLaunch().getMeasureX()), "Init Position (X)"); - assertEquals(t0, model.getTimeAtY(solution, model.getPosLaunch().getMeasureY()), "Init Position (Y)"); + assertEquals(t0, model.getTimeAtX(solution, model.getPosLaunch().getMeasureX()), "Init Position (X, abs)"); + assertEquals(t0, model.getTimeAtY(solution, model.getPosLaunch().getMeasureY()), "Init Position (Y, abs)"); + assertEquals(t0, model.getTimeAtXRelative(solution, Units.Meters.of(0.0)), "Init Position (X, rel)"); + assertEquals(t0, model.getTimeAtYRelative(solution, Units.Meters.of(0.0)), "Init Position (Y, rel)"); // Test target position assertEquals(tof, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance()), "Final Position (Rad)"); - assertEquals(tof, model.getTimeAtX(solution, model.getPosTarget().getMeasureX()), "Final Position (X)"); - assertEquals(tof, model.getTimeAtY(solution, model.getPosTarget().getMeasureY()), "Final Position (Y)"); + assertEquals(tof, model.getTimeAtX(solution, model.getPosTarget().getMeasureX()), "Final Position (X, Abs)"); + assertEquals(tof, model.getTimeAtY(solution, model.getPosTarget().getMeasureY()), "Final Position (Y, Abs)"); + assertEquals(tof, model.getTimeAtXRelative(solution, model.getTargetPosRelative().getMeasureX()), "Final Position (X, Rel)"); + assertEquals(tof, model.getTimeAtYRelative(solution, model.getTargetPosRelative().getMeasureY()), "Final Position (Y, Rel)"); assertEquals(tof, model.getTimeAtTarget(solution)); // Test mid-flight position Time tMid = Units.Seconds.of(0.5); - assertEquals(tMid, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance().div(2))); - assertEquals(tMid, model.getTimeAtX(solution, Units.Meters.of(2.5))); - assertEquals(tMid, model.getTimeAtY(solution, Units.Meters.of(3.0))); + assertEquals(tMid, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance().div(2)), "Mid Position (Rad)"); + assertEquals(tMid, model.getTimeAtX(solution, Units.Meters.of(2.5)), "Mid Position (X, Abs)"); + assertEquals(tMid, model.getTimeAtY(solution, Units.Meters.of(3.0)), "Mid Position (Y, Abs)"); + assertEquals(tMid, model.getTimeAtXRelative(solution, Units.Meters.of(1.5)), "Mid Position (X, REL)"); + assertEquals(tMid, model.getTimeAtYRelative(solution, Units.Meters.of(2.0)), "Mid Position (Y, REL)"); } @Test void testCandidateTracking() {