From ef977386d9b0b73fba99c14d7ae1b10b8785718f Mon Sep 17 00:00:00 2001 From: Ray Healy Date: Tue, 3 Mar 2026 20:36:46 -0500 Subject: [PATCH] trajectory/ballistics/constraints: Update documentation --- .../ballistics/constraints/BCEvalParams.java | 84 ++++++++++++++++--- .../constraints/BallisticConstraint.java | 7 ++ .../constraints/HeightAtPositionBC.java | 39 +++++++-- .../constraints/RobotLauncherVelBC.java | 62 ++++++++++---- 4 files changed, 157 insertions(+), 35 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java index bdbc4a2..d2e6d4e 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java +++ b/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 - * robot velocity + * Get the candidate shooter velocity (in field-relative coordinates) at the + * 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 - * robot velocity and heading + * Get the candidate shooter velocity (in robot-relative coordinates) at the + * 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; } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BallisticConstraint.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BallisticConstraint.java index df0affb..c6f2905 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BallisticConstraint.java +++ b/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() { diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java index 3ef6d34..ea9ccc8 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java +++ b/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 > 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 The type of model to evaluate for + * @param

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 , P> Double evaluate( M model, BCEvalParams evalParams ) throws UnhandledEnumException { diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/RobotLauncherVelBC.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/RobotLauncherVelBC.java index 0bdb16d..558976a 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/RobotLauncherVelBC.java +++ b/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 maxVelocityMPS; - private final Double targetVelocityPercent; + private final Double minSpeedMPS; + private final Double maxSpeedMPS; + 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 + *

Full constructor for the constraint

* - * Percent variables are a percentage (0.0-1.0) of the range between the min - * and max values for the given value where no penalty is applied. Range is - * always centered on the midpoint of the values + *

Percent variables are a percentage (0.0-1.0) of the range between the + * min and max values for the given value where no penalty is applied. Range + * is always centered on the midpoint of the values

+ * + *

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.

* - * @param minVelocity Minimum radial velocity (nullable) - * @param maxVelocity Maximum radial velocity (nullable) - * @param targetVelocityPercent Ideal percent range for radial velocity (nullable) + * @param minSpeed Minimum radial velocity (nullable) + * @param maxSpeed Maximum 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.maxVelocityMPS = maxVelocity.in(Units.MetersPerSecond); - this.targetVelocityPercent = targetVelocityPercent; + this.minSpeedMPS = minSpeed.in(Units.MetersPerSecond); + this.maxSpeedMPS = maxSpeed.in(Units.MetersPerSecond); + 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 The type of model to evaluate for + * @param

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 , 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( - minVelocityMPS, maxVelocityMPS, targetVelocityPercent, + speedWeight = this.getPartialWeight( + 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); }