diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java index 1db05af..21bc731 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java @@ -7,6 +7,8 @@ import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEval */ public record BCResult(Double penalty, BCEvalParams evalParams) { + public static final BCResult kNoSolution = new BCResult(null, null); + /** * Checks if the result is valid (i.e. checks if penalty != null) * @return True if the evalParams evaluates as valid, false otherwise 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 f5b09eb..f654606 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 @@ -10,24 +10,43 @@ import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticMo import edu.wpi.first.units.measure.Angle; +/** + * Base cass for ballistics calculators + */ public abstract class BallisticCalculator { + /** The list of constraints used to evaluate solutions */ private ArrayList constraints; + /** The weight to assign to difference in velocity while evaluating */ private double velocityDiffWeight; // // Setup code // + /** + * The default constructor. Sets velocity diff weight to 1.0 + */ public BallisticCalculator() { constraints = new ArrayList<>(); this.velocityDiffWeight = 1.0; } + + /** + * Add a constraint to the calculator + * @param constraint The constraint to add to the calculator + * @return this + */ public BallisticCalculator addConstraint(BallisticConstraint constraint) { constraints.add(constraint); return this; } + /** + * Set the weight for the difference in velocity + * @param weight The weight to assign to difference in velocity + * @return this + */ public BallisticCalculator setVelocityDiffWeight(double weight) { this.velocityDiffWeight = weight; return this; @@ -62,7 +81,9 @@ public abstract class BallisticCalculator { /** - * Compute the best solution for given initial conditions + * Compute the best solution for given initial conditions. + * + * NOTE: May reset seen candidates * * @param The type of model to compute against * @param

The candidate parameter type of the model @@ -81,6 +102,30 @@ public abstract class BallisticCalculator { Velocity3d shooterVelocity ) throws BallisticException; + /** + * Generate and evaluate a solution for a given model, param, and robot state + * + * @param The type of model to evaluate for + * @param

The type of param to model M + * @param model The model to evaluate for + * @param modelParam The parameter to evaluate for + * @param robotVelocity The robot's current velocity + * @param robotAngle The robot's current angle + * @param shooterVelocity The current velocity provided by the shooter + * @return The BCResult instance for the given params + * @throws BallisticException If thrown by registered constraints + */ + protected , P> BCResult evaluateCandidate( + M model, + P modelParam, + Velocity3d robotVelocity, + Angle robotAngle, + Velocity3d shooterVelocity + ) throws BallisticException { + Velocity3d candidateVelocity = model.getCandidateVelocity(modelParam); + BCEvalParams evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidateVelocity); + return new BCResult(this.getCandidatePenalty(model, evalParams), evalParams); + } /** * Get the average penalty for a given set of params @@ -91,8 +136,9 @@ public abstract class BallisticCalculator { * @param evalParams The eval function param block * @return The weighted average of penalties from velocity diferences and * constraints (null if evalParams fail any constraints) + * @throws BallisicsException If thrown by registered constraints */ - protected , P> Double evaluateCandidate( + private , P> Double getCandidatePenalty( M model, BCEvalParams evalParams ) throws BallisticException { // Get the weight component for velocity difference 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 index bded2fd..39e6bde 100644 --- 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 @@ -4,18 +4,23 @@ 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.exceptions.BallisticException; import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel; import edu.wpi.first.units.measure.Angle; +/** + * Simple greedy calculator (i.e. returns the first valid result) + */ public class GreedyBallisticCalculator extends BallisticCalculator { + /** + * Default constructor for the calculator + */ public GreedyBallisticCalculator() { super(); } - + @Override public , P> BCResult computeSolution( M model, P initParam, @@ -33,18 +38,16 @@ public class GreedyBallisticCalculator extends BallisticCalculator { P param = candidate_params.removeFirst(); if(model.paramComputed(param)) continue; - 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); - } + BCResult candidate = this.evaluateCandidate( + model, param, robotVelocity, robotAngle, shooterVelocity + ); + if(candidate.isValid()) { + return candidate; } // Update the candidate list candidate_params.addAll(Arrays.asList(model.getNeighborParams(param))); } - return new BCResult(null, null); + return BCResult.kNoSolution; } } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java index 9c49f4a..da77634 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java @@ -1,30 +1,25 @@ package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators; import org.fairportrobotics.frc.robolib.trajectory.Velocity3d; -import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEvalParams; import org.fairportrobotics.frc.robolib.trajectory.ballistics.exceptions.BallisticException; import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel; import edu.wpi.first.units.measure.Angle; +/** + * Solution calculator implementing a gradient descent algorithm to find a + * solution with a local minimum penalty (not guaranteed to find a global + * minimum penalty) + */ public class HillDescentBallisticCalculator extends BallisticCalculator { + + /** + * The default consttructor for the calculator + */ public HillDescentBallisticCalculator() { super(); } - - private , P> BCResult evalSingleCandidate( - M model, - P modelParam, - Velocity3d robotVelocity, - Angle robotAngle, - Velocity3d shooterVelocity - ) throws BallisticException { - Velocity3d candidateVelocity = model.getCandidateVelocity(modelParam); - BCEvalParams evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidateVelocity); - return new BCResult(this.evaluateCandidate(model, evalParams), evalParams); - } - public , P> BCResult computeSolution( M model, P initParam, @@ -35,7 +30,7 @@ public class HillDescentBallisticCalculator extends BallisticCalculator { model.paramComputedReset(); P bestParam = initParam; - BCResult bestResult = this.evalSingleCandidate(model, initParam, robotVelocity, robotAngle, shooterVelocity); + BCResult bestResult = this.evaluateCandidate(model, initParam, robotVelocity, robotAngle, shooterVelocity); boolean foundImprovement = true; while(bestResult.isValid() && foundImprovement) { @@ -44,7 +39,7 @@ public class HillDescentBallisticCalculator extends BallisticCalculator { for (P param : model.getNeighborParams(bestParam)) { if(model.paramComputed(param)) continue; - BCResult result = this.evalSingleCandidate(model, param, robotVelocity, robotAngle, shooterVelocity); + BCResult result = this.evaluateCandidate(model, param, robotVelocity, robotAngle, shooterVelocity); if(result.isValid() && result.penalty() < bestResult.penalty()) { bestParam = param; bestResult = result;