src/robolib/trajectory/ballistics: Define GreedyballisticCalculator

Takes the first valid solution (if any)

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 <M> The type of model to compute against
+     * @param <P> 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 <M extends BallisticModel<P>, 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 <M> The type of model to compute against
      * @param <P> 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 robotVelocity The velocity of the robot (in field coordinates)
-     * @param velShooter The velocity currently imparted by the shooter (in the model's coordinate system)
+     * @param robotAngle The robot's current heading (in field coordinates)
-     * @param robotHeading The heading of the robot (in the model's coordinate system)
+     * @param shooterVelocity The shooter's current launch velocity (in field coordinates)
      * @return The generated BCResult
      */
-    public abstract <M extends BallisticModel<P>, P> BCResult<P> computeSolution(
+    public abstract <M extends BallisticModel<P>, P> BCResult computeSolution(
-        M model, Velocity3d velRobot, Velocity3d velShooter,
+        M           model,
-        Angle robotHeading, boolean quickMode
+        P           initParam,
+        Velocity3d  robotVelocity,
+        Angle       robotAngle,
+        Velocity3d  shooterVelocity
     );
 
 
+    /**
+     * Get the average penalty for a given set of params
+     *
+     * @param <M> The type of model to compute against
+     * @param <P> 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 <M extends BallisticModel<P>, P> Double evaluateCandidate(
         M model, BCEvalParams evalParams
     ) {
         // Get the weight component for velocity difference
         Velocity3d velShooterDiff =
-            evalParams.getCandidateGlobalShooterVelocity()
+            evalParams.getCandidateShooterVelocity().minus(evalParams.getCurrentShooterVelocity());
-            .minus(evalParams.getCurrentGlobalShooterVelocity());
+
         double numerator    = velShooterDiff.getSquareMagnitude() * this.velocityDiffWeight;
         double denominator  = this.velocityDiffWeight;
 

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 <M extends BallisticModel<P>, P> BCResult computeSolution(
+        M           model,
+        P           initParam,
+        Velocity3d  robotVelocity,
+        Angle       robotAngle,
+        Velocity3d  shooterVelocity
+    ) {
+        if(initParam == null) {
+            initParam = model.getInitParam();
+        }
+
+        LinkedList<P> 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);
+    }
+}