trajectory/ballistics/calculators: Docs + reorganization

- BCResult: Define a static no solution record
- Refactor evalSingleCandidate into BallisticCalculator
    - getCandidatePenalty() made private

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

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<BallisticConstraint> 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 <M> The type of model to compute against
      * @param <P> 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 <M> The type of model to evaluate for
+     * @param <P> 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 <M extends BallisticModel<P>, 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 <M extends BallisticModel<P>, P> Double evaluateCandidate(
+    private <M extends BallisticModel<P>, P> Double getCandidatePenalty(
         M model, BCEvalParams evalParams
     ) throws BallisticException {
         // Get the weight component for velocity difference

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 <M extends BallisticModel<P>, 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);
+            BCResult candidate = this.evaluateCandidate(
-            if(candidate != null) {
+                model, param, robotVelocity, robotAngle, shooterVelocity
-                BCEvalParams    evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidate);
+            );
-                Double          penalty = this.evaluateCandidate(model, evalParams);
+            if(candidate.isValid()) {
-                if(penalty != null) {
+                return candidate;
-                    return new BCResult(penalty, evalParams);
-                }
             }
 
             // Update the candidate list
             candidate_params.addAll(Arrays.asList(model.getNeighborParams(param)));
         }
-        return new BCResult(null, null);
+        return BCResult.kNoSolution;
     }
 }

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 <M extends BallisticModel<P>, 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 <M extends BallisticModel<P>, 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;