trajectory/ballistics/constraints: Update documentation

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java

@@ -6,7 +6,7 @@ import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 
 /**
- * Param block for ballistic constraint and calculator evaluation
+ * Parameter block for Ballistic Constraint and Ballistic Caluclator evaluation
  */
 public class BCEvalParams {
     //
@@ -29,15 +29,31 @@ public class BCEvalParams {
     //  Setup
     //
 
+    /**
+     * Generate a BCEvalParams instance for a zero velocity, zero angle
+     * robot and shooter with a given shooter velocity.
+     *
+     * @param candidateShooterVelocity The targeted shooter velocity
+     * @return The generated BCEvalParams instance
+     */
     public static BCEvalParams genSetShooterVelocity(
         Velocity3d  candidateShooterVelocity
     ) {
         return genSetShooterVelocity(
-            Velocity3d.zero, Units.Degrees.zero(), Velocity3d.zero,
+            Velocity3d.kZero, Units.Degrees.zero(), Velocity3d.kZero,
             candidateShooterVelocity
         );
     }
 
+    /**
+     * Generate a robotVelocity instance with a given shooter velocity
+     *
+     * @param robotVelocity The robot's velocity
+     * @param robotAngle The current robot angle
+     * @param currentShooterVelocity The current velocity imparted by the shooter
+     * @param candidateShooterVelocity The ideal velocity imparted by the shooter
+     * @return The generated BCEvalParams instance
+     */
     public static BCEvalParams genSetShooterVelocity(
         Velocity3d  robotVelocity,
         Angle       robotAngle,
@@ -53,15 +69,34 @@ public class BCEvalParams {
 
     }
 
+    /**
+     * Generate an instance with a given shooter relative velocity (from a 
+     * zero-velocity, zero-angle shooter and robot).
+     *
+     * @param candidateShooterVelocityRelative The targeted relative shooter
+     * velocity
+     * @return The generated BCEvalParams instance
+     */
     public static BCEvalParams genSetShooterRelativeVelocity(
         Velocity3d candidateShooterVelocityRelative
     ) {
         return genSetShooterRelativeVelocity(
-            Velocity3d.zero, Units.Degrees.zero(), Velocity3d.zero,
+            Velocity3d.kZero, Units.Degrees.zero(), Velocity3d.kZero,
             candidateShooterVelocityRelative
         );
     }
 
+    /**
+     * Generate an instance with a given shooter relative velocity (from a 
+     * zero-velocity, zero-angle shooter and robot).
+     *
+     * @param robotVelocity The robot velocity
+     * @param robotAngle The robot angle
+     * @param currentShooterVelocity The current shooter velocity
+     * @param candidateShooterVelocityRelative The targeted relative shooter
+     * velocity
+     * @return The generated BCEvalParams instance
+     */
     public static BCEvalParams genSetShooterRelativeVelocity(
         Velocity3d robotVelocity,
         Angle robotAngle,
@@ -77,6 +112,15 @@ public class BCEvalParams {
         return toRet;
     }
 
+    /**
+     * Main constructor (all params in field-relative coordinates)
+     *
+     * @param robotVelocity The robot's current velocity
+     * @param robotAngle The robot's current angle (counter clockwise from +X)
+     * @param currentShooterVelocity The current velocity imparted by the shooter
+     * @param candidateVelocity The candidate total (robot + shooter) velocity
+     * to evaluate
+     */
     public BCEvalParams(
         Velocity3d robotVelocity,
         Angle      robotAngle,
@@ -99,7 +143,9 @@ public class BCEvalParams {
     //
 
     /**
-     * @return The robot's current velocity (in field coordinates)
+     * Get the robot's current velocity (in field-relative coordinates)
+     *
+     * @return The robot's current velocity
      */
     public Velocity3d getRobotVelocity() {
         return this.robotVelocity;
@@ -107,7 +153,9 @@ public class BCEvalParams {
 
 
     /**
-     * @return The robot's current heading angle (in field coordinates)
+     * Get the robot's current heading angle (in field-relative coordinates)
+     *
+     * @return The robot's current heading angle
      */
     public Angle      getRobotAngle() {
         return this.robotAngle;
@@ -115,7 +163,9 @@ public class BCEvalParams {
 
 
     /**
-     * @return The shooter's current launch velocity (in field coordinates)
+     * Get the shooter's current launch velocity (in field-relative coordinates).
+     *
+     * @return The shooter's current launch velocity
      */
     public Velocity3d getCurrentShooterVelocity() {
         return this.currentShooterVelocity;
@@ -123,7 +173,9 @@ public class BCEvalParams {
 
 
     /**
-     * @return The candidate total (robot + shooter) velocity (in field coordinates)
+     * Get the total (robot + shooter) candidate velocity (in field-relative coordinates).
+     * 
+     * @return The total candidate velocity
      */
     public Velocity3d getCandidateVelocity() {
         return this.candidateVelocity;
@@ -134,8 +186,10 @@ public class BCEvalParams {
     //
 
     /**
-     * @return The candidate shooter velocity (in field coordinates) at current
+     * Get the candidate shooter velocity (in field-relative coordinates) at the
-     *  robot velocity
+     * current robot velocity.
+     *
+     * @return The candidate shooter velocity
      */
     public Velocity3d getCandidateShooterVelocity() {
         if(this.cache_candidateShooterVelocity == null) {
@@ -147,8 +201,10 @@ public class BCEvalParams {
     }
 
     /**
-     * @return The candidate shooter velocity (in robot coordinates) at current
+     * Get the candidate shooter velocity (in robot-relative coordinates) at the
-     *  robot velocity and heading
+     * current robot velocity and heading
+     *
+     * @return The candidate shooter velocity (in robot-relative coordinates)
      */
     public Velocity3d getCandidateShooterVelocityRelative() {
         if(this.cache_candidateShooterVelocityRelative == null) {
@@ -158,6 +214,7 @@ public class BCEvalParams {
         return this.cache_candidateShooterVelocityRelative;
     }
 
+    @Override
     public int hashCode() {
         return  this.robotVelocity.hashCode() ^
                 Double.hashCode(this.robotAngle.in(Units.Degrees)) ^
@@ -165,6 +222,7 @@ public class BCEvalParams {
                 this.candidateVelocity.hashCode();
     }
 
+    @Override
     public String toString() {
         return String.format(
             "BCEvalParams(robotVelocity: %s, robotAngle: %s, currentShooterVelocity: %s, candidateVelocity: %s)",
@@ -183,7 +241,11 @@ public class BCEvalParams {
             this.candidateVelocity.equals(other.candidateVelocity);
     }
 
+    @Override
     public boolean equals(Object other) {
+        if(other instanceof BCEvalParams) {
+            return this.equals((BCEvalParams) other);
+        }
         return false;
     }
 

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BallisticConstraint.java

@@ -3,7 +3,11 @@ package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints;
 import org.fairportrobotics.frc.robolib.trajectory.ballistics.exceptions.BallisticException;
 import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
 
+/**
+ * Abstract base class for a ballistic constraints
+ */
 public abstract class BallisticConstraint {
+    /** The weight given to this constraint in the evaluation function */
     protected double weight;
 
     /**
@@ -15,6 +19,7 @@ public abstract class BallisticConstraint {
 
     /**
      * Generate the constraint with a given weight value
+     *
      * @param weight The weight of this constraint's penalty
      */
     public BallisticConstraint(double weight) {
@@ -32,6 +37,8 @@ public abstract class BallisticConstraint {
     }
 
     /**
+     * Get the weight of this constraint
+     *
      * @return The weight of this constraint
      */
     public double getWeight() {

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java

@@ -15,11 +15,19 @@ import edu.wpi.first.units.measure.Time;
  */
 public class HeightAtPositionBC extends BallisticConstraint {
 
-    enum PositionType {
+    /**
+     * The types of position to evaluate forf
+     */
+    public enum PositionType {
+        /** Evaluate with the radial distance away from the target */
         TargetRadius,
+        /** Evaluate against an absolute X position */
         AbsoluteX,
+        /** Evaluate against an absolute Y position */
         AbsoluteY,
+        /** Evaluate against an X position relative to launch position */
         RelativeX,
+        /** Evaluate against a Y position relative to the launch position */
         RelativeY
     };
 
@@ -29,23 +37,33 @@ public class HeightAtPositionBC extends BallisticConstraint {
     private final PositionType positionType;
 
     /**
+     * Main constructor
+     *
      * @param minHeight The minimum passing height
      * @param safeHeight The height above which no penalty is applied
      * @param testPosition The position to test at
      * @param positionType The type of position check to run
      */
     public HeightAtPositionBC(
-        Distance minHeight, Distance safeHeight, Distance testRadius, PositionType positionType
+        Distance minHeight, Distance safeHeight, Distance testPosition, PositionType positionType
     ) {
         this.minHeightMeters    = minHeight.in(Units.Meters);
         this.safeHeightMeters   = safeHeight.in(Units.Meters);
-        this.testPosition       = testRadius;
+        this.testPosition       = testPosition;
         this.positionType       = positionType;
     }
 
 
-    public <M extends ReversableRadialBM<?>> Double evaluate_reversable(
+    /**
-        M model, BCEvalParams evalParams
+     * Evaluation function for a reversable, radial ballistic model
+     *
+     * @param model The model to evaluate against
+     * @param evalParams The params to evaluate
+     * @return The penalty percentage (from 0 to 1, inclusive). null for constraint failure
+     * @throws UnhandledEnumException If this.positionType is not valid
+     */
+    private Double evaluate_reversable(
+        ReversableRadialBM<?> model, BCEvalParams evalParams
     ) throws UnhandledEnumException{
         Time timeAtTarget;
         switch(this.positionType) {
@@ -84,7 +102,16 @@ public class HeightAtPositionBC extends BallisticConstraint {
         }
     }
 
-
+    /**
+     * General evaluation function
+     *
+     * @param <M> The type of model to evaluate for
+     * @param <P> The type of model parameter to evaluate for
+     * @param model The model parameter to evaluate for
+     * @param evalParams The params to evaluate
+     * @return The penalty percentage (from 0 to 1, inclusive). null for constraint failure
+     * @throws UnhandledEnumException If this.positionType is not valid
+     */
     public <M extends BallisticModel<P>, P> Double evaluate(
         M model, BCEvalParams evalParams
     ) throws UnhandledEnumException {

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/RobotLauncherVelBC.java

@@ -12,9 +12,9 @@ import edu.wpi.first.units.measure.LinearVelocity;
  * robot-relative launcher velocity (in spherical coordinates)
  */
 public class RobotLauncherVelBC extends BallisticConstraint {
-    private final Double    minVelocityMPS;
+    private final Double    minSpeedMPS;
-    private final Double    maxVelocityMPS;
+    private final Double    maxSpeedMPS;
-    private final Double    targetVelocityPercent;
+    private final Double    targetSpeedPercent;
 
     private final Double    minAzimuthDegrees;
     private final Double    maxAzimuthDegrees;
@@ -27,15 +27,20 @@ public class RobotLauncherVelBC extends BallisticConstraint {
     class BoundViolated extends Exception {}
 
     /**
-     * Full constructor for the constraint
+     * <p>Full constructor for the constraint</p>
      *
-     * Percent variables are a percentage (0.0-1.0) of the range between the min
+     * <p>Percent variables are a percentage (0.0-1.0) of the range between the
-     * and max values for the given value where no penalty is applied. Range is
+     * min and max values for the given value where no penalty is applied. Range
-     * always centered on the midpoint of the values
+     * is always centered on the midpoint of the values</p>
+     * 
+     * <p>min, max, and target percent values must all be non-null to allow
+     * evaluation on component. If any component is null, penalty will not be
+     * considered (pass-fail check will still be performed). If no component
+     * provides a penalty, 0.0 will be returned by default.</p>
      *
-     * @param minVelocity Minimum radial velocity (nullable)
+     * @param minSpeed Minimum radial velocity (nullable)
-     * @param maxVelocity Maximum radial velocity (nullable)
+     * @param maxSpeed Maximum radial velocity (nullable)
-     * @param targetVelocityPercent Ideal percent range for radial velocity (nullable)
+     * @param targetSpeedPercent Ideal percent range for radial velocity (nullable)
      * @param minAzimuth Minimum azimuth angle (nullable)
      * @param maxAzimuth Maximum azimuth angle (nullable)
      * @param targetAzimuthPercent Ideal percent range for azimuth (nullable)
@@ -44,13 +49,13 @@ public class RobotLauncherVelBC extends BallisticConstraint {
      * @param targetElevationPercent Ideal percent range for elevation (nullable)
      */
     public RobotLauncherVelBC(
-        LinearVelocity minVelocity, LinearVelocity maxVelocity, Double targetVelocityPercent,
+        LinearVelocity minSpeed, LinearVelocity maxSpeed, Double targetSpeedPercent,
         Angle minAzimuth, Angle maxAzimuth, Double targetAzimuthPercent,
         Angle minElevation, Angle maxElevation, Double targetElevationPercent
     ) {
-        this.minVelocityMPS         = minVelocity.in(Units.MetersPerSecond);
+        this.minSpeedMPS            = minSpeed.in(Units.MetersPerSecond);
-        this.maxVelocityMPS         = maxVelocity.in(Units.MetersPerSecond);
+        this.maxSpeedMPS            = maxSpeed.in(Units.MetersPerSecond);
-        this.targetVelocityPercent  = targetVelocityPercent;
+        this.targetSpeedPercent     = targetSpeedPercent;
 
         this.minAzimuthDegrees      = minAzimuth.in(Units.Degrees);
         this.maxAzimuthDegrees      = maxAzimuth.in(Units.Degrees);
@@ -62,6 +67,17 @@ public class RobotLauncherVelBC extends BallisticConstraint {
     }
 
 
+    /**
+     * Get the weight contribution of a single component
+     *
+     * @param lowerBound The lower bound value to compute for
+     * @param upperBound The upper bound value to compute for
+     * @param idealPercentRange The ideal (0 penalty) percent range
+     * @param value The value to check for
+     * @return The partial penalty value (from 0 to 1, inclusive, null if unable
+     * to compute penalty)
+     * @throws BoundViolated Thrown if the constraint is violated
+     */
     private Double getPartialWeight(
         Double lowerBound, Double upperBound, Double idealPercentRange, double value
     ) throws BoundViolated {
@@ -93,17 +109,27 @@ public class RobotLauncherVelBC extends BallisticConstraint {
         }
     }
 
+    /**
+     * Evaluate the constraint.
+     * 
+     * @param <M> The type of model to evaluate for
+     * @param <P> The type of parameter to evaluate for
+     * @param model The model to evaluate with
+     * @param evalParams The evaluation params
+     * @return The max penalty from each of the speed, azimuth, and elevation
+     * components and 0.0 (null if constraint violated).
+     */
     public <M extends BallisticModel<P>, P> Double evaluate(
         M model, BCEvalParams evalParams
     ) {
-        Double velocityWeight   = null;
+        Double speedWeight   = null;
         Double azimuthWeight    = null;
         Double elevationWeight  = null;
 
         // Compute the partial penalty for velocity
         try {
-            velocityWeight = this.getPartialWeight(
+            speedWeight = this.getPartialWeight(
-                minVelocityMPS, maxVelocityMPS, targetVelocityPercent,
+                minSpeedMPS, maxSpeedMPS, targetSpeedPercent,
                 evalParams.getCandidateShooterVelocityRelative().getSpeed().in(Units.MetersPerSecond)
             );
             azimuthWeight = this.getPartialWeight(
@@ -119,7 +145,7 @@ public class RobotLauncherVelBC extends BallisticConstraint {
         }
 
         double maxWeight = 0.0;
-        for (Double partialWeight : new Double[]{velocityWeight, azimuthWeight, elevationWeight}) {
+        for (Double partialWeight : new Double[]{speedWeight, azimuthWeight, elevationWeight}) {
             if(partialWeight != null) {
                 maxWeight = Math.max(partialWeight, maxWeight);
             }