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

lib/RoboLib/ballistics/models: Added unit test 

for ParabolicTimeOfFlightBM

- Added getters for internal state
- Fixed ReversableRadialBM.getTimeAt[XY]() (was
missing correction for launch position)
- Fixed
ParabolicTimeOfFlightBM.getCandidateVelocity()
(was adding gravity factor instead of subtracting)
This commit is contained in:
Ray Healy 2026-02-14 11:24:38 -05:00
parent e7a314c0eb
commit 9d8d9dd8f6
4 changed files with 242 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
View File

@ -14,19 +14,27 @@ public abstract class BallisticModel<P> {
this.posTarget = posTarget;
}
//
// Base getters
//
public Translation3d getPosLaunch() {
return this.posLaunch;
}
public Translation3d getPosTarget() {
return this.posTarget;
}
//
// Parameter Manipulation
//
/**
* @return Returns a recommended starting parameter
*/
public abstract P getInitParam();
/**
* Get an abstract velocity (in the same coordinate space as posLaunch and
* posTarget) which will pass thru
*
* @param param The parameters used to determine this candidate
* @return The computed result for the given parameter
*/
public abstract Velocity3d getCandidateVelocity(P param);
/**
* Get parameters that are "adjacent" to the input paramter (i.e. generate
@ -37,8 +45,25 @@ public abstract class BallisticModel<P> {
*/
public abstract P[] getNeighborParams(P param);
//
// Velocity modelling
//
/**
* Get the position of the projectile at a given time offset
* Get an initial velocity (in the same coordinate space as posLaunch and
* posTarget) which will pass thru posLaunch and posTarget with the given
* model configurations and parameters
*
* @param param The parameters used to determine this candidate
* @return The computed result for the given parameter
*/
public abstract Velocity3d getCandidateVelocity(P param);
/**
* Get the position of the projectile at a given time offset for a given
* starting velocity
*
* @param velocity The velocity to check
* @param time The time to check
@ -46,6 +71,10 @@ public abstract class BallisticModel<P> {
*/
public abstract Translation3d positionAtTime(Velocity3d velocity, Time time);
//
// Computed parameter check
//
/**
* Check if a given parameter has had a computed velocity
* @param param The param to check

67
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java
View File

@ -14,8 +14,7 @@ import edu.wpi.first.units.measure.Time;
public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
private final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
public static final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
protected final Time tofMin;
protected final Time tofMax;
@ -54,20 +53,41 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
this.computedSeconds = new HashSet<>();
}
//
// Base getters
//
public Time getInitParam() {
return this.tofMin.plus(this.tofMax).div(2);
public Time getTOFMin() {
return this.tofMin;
}
public Time getTOFMax() {
return this.tofMax;
}
public Time getTOFStep() {
return this.tofStep;
}
public Translation3d getTargetPosRelative() {
return this.targetPosRelative;
}
public Distance getRelativeHorizontalDistance() {
return this.relativeHorizontalDistance;
}
public Angle getAzimuth() {
return this.azimuth;
}
public Velocity3d getCandidateVelocity(Time param) {
this.computedSeconds.add(param.in(Units.Seconds));
return new Velocity3d(
this.relativeHorizontalDistance.div(param),
this.targetPosRelative.getMeasureZ().div(param)
.plus(k_gravity.times(param).times(0.5)),
azimuth
);
//
// Parameter manipulation
//
public Time getInitParam() {
return this.tofMin.plus(this.tofMax).div(2);
}
@ -84,6 +104,21 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
}
//
// Velocity modelling
//
public Velocity3d getCandidateVelocity(Time param) {
this.computedSeconds.add(param.in(Units.Seconds));
return new Velocity3d(
this.relativeHorizontalDistance.div(param),
this.targetPosRelative.getMeasureZ().div(param)
.minus(k_gravity.times(param).times(0.5)),
azimuth
);
}
public Translation3d positionAtTime(Velocity3d velocity, Time time) {
return new Translation3d(
this.posLaunch.getMeasureX()
@ -97,11 +132,19 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
}
//
// Reverse functions
//
public Time getTimeAtRadius(Velocity3d candidate, Distance radialDistance) {
return radialDistance.div(candidate.getHorizontalVelocity());
}
//
// Computed parameter check
//
public boolean paramComputed(Time param) {
return this.computedSeconds.contains(param.in(Units.Seconds));
}

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

@ -16,6 +16,11 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
super(posLaunch, posTarget);
}
//
// Reverse functions
//
/**
* Get the time taken to travel a given radial distance from posLaunch towards posTarget
*
@ -36,7 +41,8 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
*/
public Time getTimeAtX(Velocity3d candidate, Distance xDistance) {
Distance radialDistance = Units.Meters.of(
xDistance.in(Units.Meters)/Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
xDistance.minus(posLaunch.getMeasureX()).in(Units.Meters)
/ Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
);
return this.getTimeAtRadius(candidate, radialDistance);
}
@ -52,11 +58,13 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
*/
public Time getTimeAtY(Velocity3d candidate, Distance yDistance) {
Distance radialDistance = Units.Meters.of(
yDistance.in(Units.Meters)/Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
yDistance.minus(posLaunch.getMeasureY()).in(Units.Meters)
/ Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
);
return this.getTimeAtRadius(candidate, radialDistance);
}
/**
* Get the time to target on a given candidate trajectory
*

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

@ -0,0 +1,139 @@
package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
import org.junit.jupiter.api.Test;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Time;
import static org.junit.jupiter.api.Assertions.*;
import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
public class ParabolicTimeOfFlightBMTest {
private ParabolicTimeOfFlightBM getModel() {
return new ParabolicTimeOfFlightBM(
new Translation3d(Units.Meters.of(1), Units.Meters.of(1), Units.Meters.of(1)),
new Translation3d(Units.Meters.of(4), Units.Meters.of(5), Units.Meters.of(2)),
Units.Seconds.of(1.0), Units.Seconds.of(3), Units.Seconds.of(1.0)
);
}
@Test void testParamIterator() {
ParabolicTimeOfFlightBM model = getModel();
// Define the initial param
Time initParam = model.getInitParam();
assertEquals(2.0, initParam.in(Units.Seconds), "Init param");
assertArrayEquals(
new Time[]{Units.Seconds.of(1.0), Units.Seconds.of(3.0)},
model.getNeighborParams(initParam), "Neighbors"
);
}
@Test void testSolutionVelocity() {
ParabolicTimeOfFlightBM model = getModel();
Time tof = Units.Seconds.of(1.0);
// Compute the solution
Velocity3d result = model.getCandidateVelocity(tof);
assertEquals(new Velocity3d(
Units.MetersPerSecond.of(5),
Units.MetersPerSecond.of(1).minus(
ParabolicTimeOfFlightBM.k_gravity.times(0.5).times(tof)
),
Units.Radians.of(Math.atan2(4, 3))
), result, "Compute solution");
// Check that positions match expected values
assertEquals(
model.getPosLaunch(),
model.positionAtTime(result, Units.Seconds.of(0)),
"Launch position"
);
assertEquals(
model.getPosTarget(),
model.positionAtTime(result, tof),
"Target position check"
);
Time midTime = Units.Seconds.of(0.5);
assertEquals(
new Translation3d(
Units.Meters.of(2.5), // 1+3*0.5
Units.Meters.of(3), // 1+4*0.5
Units.Meters.of(1.0)
.plus(result.getZVelocity().times(midTime))
.plus(
ParabolicTimeOfFlightBM.k_gravity
.times(0.5).times(midTime).times(midTime)
)
),
model.positionAtTime(result, midTime),
"Mid-flight position check"
);
}
@Test void testReverseFunctions() {
ParabolicTimeOfFlightBM model = getModel();
Time tof = Units.Seconds.of(1.0);
Velocity3d solution = model.getCandidateVelocity(tof);
// 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)");
// 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.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)));
}
@Test void testCandidateTracking() {
ParabolicTimeOfFlightBM model = getModel();
Time[] candidates = {
Units.Seconds.of(1.0),
Units.Seconds.of(1.5),
Units.Seconds.of(2.0),
Units.Seconds.of(2.5),
Units.Seconds.of(3.0)
};
for(int cur_candidate = 0; cur_candidate < candidates.length; ++cur_candidate) {
// Check that existing candidates have been checked
for(int seen_candidate = 0; seen_candidate < cur_candidate; ++seen_candidate) {
assertTrue(
model.paramComputed(candidates[seen_candidate]),
String.format(
"Cur Candidate: %d, Seen Candidate: %d",
cur_candidate, seen_candidate
)
);
}
// Check that unseen candidates haven't been checked
for(int unseen_candidate = cur_candidate; unseen_candidate < candidates.length; ++unseen_candidate) {
assertFalse(
model.paramComputed(candidates[unseen_candidate]),
String.format(
"Cur Candidate: %d, Unseen Candidate: %d",
cur_candidate, unseen_candidate
)
);
}
// Compute for the candidate
model.getCandidateVelocity(candidates[cur_candidate]);
}
}
}