FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

trajectory/ballistics/constraints: Update documentation

This commit is contained in:
Ray Healy 2026-03-03 20:36:46 -05:00
parent df022beb0e
commit ef977386d9
4 changed files with 157 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
}

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

@ -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() {

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

@ -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 {

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

@ -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
* <p>Full constructor for the constraint</p>
*
* 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
* <p>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</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 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 <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(
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);
}