From f49d8434aa5bc50afa336228bd224c46e99ad29d Mon Sep 17 00:00:00 2001 From: Ray Healy Date: Tue, 10 Feb 2026 19:45:26 -0500 Subject: [PATCH] src/robolib/trajectory/ballistics: Define GreedyballisticCalculator Takes the first valid solution (if any) --- .../calculators/BallisticCalculator.java | 56 ++++++++++++++++--- .../GreedyBallisticCalculator.java | 50 +++++++++++++++++ 2 files changed, 97 insertions(+), 9 deletions(-) create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java index 62a5937..e45b2e9 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java @@ -37,29 +37,67 @@ public abstract class BallisticCalculator { // /** - * Public solution + * Compute the best solution for given initial conditions (pulling parameter + * from model.getInitParam()) + * + * @param The type of model to compute against + * @param

The candidate parameter type of the model + * @param model The model to compute against + * @param robotVelocity The velocity of the robot (in field coordinates) + * @param robotAngle The robot's current heading (in field coordinates) + * @param shooterVelocity The shooter's current launch velocity (in field coordinates) + * @return The generated BCResult + */ + public , P> BCResult computeSolution( + M model, + Velocity3d robotVelocity, + Angle robotAngle, + Velocity3d shooterVelocity + ) { + return this.computeSolution( + model, model.getInitParam(), robotVelocity, robotAngle, shooterVelocity + ); + } + + + /** + * Compute the best solution for given initial conditions + * * @param The type of model to compute against * @param

The candidate parameter type of the model * @param model The model to compute against * @param initParam The seed parameter (to start computation) - * @param velRobot The velocity of the robot (in the model's coordinate system) - * @param velShooter The velocity currently imparted by the shooter (in the model's coordinate system) - * @param robotHeading The heading of the robot (in the model's coordinate system) + * @param robotVelocity The velocity of the robot (in field coordinates) + * @param robotAngle The robot's current heading (in field coordinates) + * @param shooterVelocity The shooter's current launch velocity (in field coordinates) * @return The generated BCResult */ - public abstract , P> BCResult

computeSolution( - M model, Velocity3d velRobot, Velocity3d velShooter, - Angle robotHeading, boolean quickMode + public abstract , P> BCResult computeSolution( + M model, + P initParam, + Velocity3d robotVelocity, + Angle robotAngle, + Velocity3d shooterVelocity ); + /** + * Get the average penalty for a given set of params + * + * @param The type of model to compute against + * @param

The candidate parameter type of the model + * @param model The model to compute against + * @param evalParams The eval function param block + * @return The weighted average of penalties from velocity diferences and + * constraints (null if evalParams fail any constraints) + */ protected , P> Double evaluateCandidate( M model, BCEvalParams evalParams ) { // Get the weight component for velocity difference Velocity3d velShooterDiff = - evalParams.getCandidateGlobalShooterVelocity() - .minus(evalParams.getCurrentGlobalShooterVelocity()); + evalParams.getCandidateShooterVelocity().minus(evalParams.getCurrentShooterVelocity()); + double numerator = velShooterDiff.getSquareMagnitude() * this.velocityDiffWeight; double denominator = this.velocityDiffWeight; diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java new file mode 100644 index 0000000..f15fb8b --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java @@ -0,0 +1,50 @@ +package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators; + +import java.util.Arrays; +import java.util.LinkedList; + +import org.fairportrobotics.frc.robolib.trajectory.Velocity3d; +import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEvalParams; +import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel; + +import edu.wpi.first.units.measure.Angle; + +public class GreedyBallisticCalculator extends BallisticCalculator { + public GreedyBallisticCalculator() { + super(); + } + + + public , P> BCResult computeSolution( + M model, + P initParam, + Velocity3d robotVelocity, + Angle robotAngle, + Velocity3d shooterVelocity + ) { + if(initParam == null) { + initParam = model.getInitParam(); + } + + LinkedList

candidate_params = new LinkedList<>(); + candidate_params.addFirst(initParam); + + while(!candidate_params.isEmpty()) { + // Evaluate the candidates + P param = candidate_params.removeFirst(); + Velocity3d candidate = model.getCandidateVelocity(param); + + if(candidate != null) { + BCEvalParams evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidate); + Double penalty = this.evaluateCandidate(model, evalParams); + if(penalty != null) { + return new BCResult(penalty, evalParams); + } + } + + // Update the candidate list + candidate_params.addAll(Arrays.asList(model.getNeighborParams(param))); + } + return new BCResult(null, null); + } +}