FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

trajectory/ballistics/models: Added launch and 

field relative ReversableRadialBM for
getTimeAt[XY]
This commit is contained in:
Ray Healy 2026-02-14 15:02:27 -05:00
parent e171339c5c
commit 21f4a1100e
2 changed files with 72 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java
View File

@ -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<P> extends BallisticModel<P> {
public ReversableRadialBM(Translation3d posLaunch, Translation3d posTarget) {
super(posLaunch, posTarget);
}
//
// Reverse functions
//
@ -32,44 +32,78 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
/**
* 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());

21
lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java
View File

@ -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() {