diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java new file mode 100644 index 0000000..bbe047c --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java @@ -0,0 +1,27 @@ +package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators; + +import org.fairportrobotics.frc.robolib.trajectory.Velocity3d; + +public class BCResult

{ + public final Double velPenalty; /// The penalty for the computed launcher velocity (null if invalid) + public final Velocity3d velGlobal; /// The computed launcher velocity (in the global coorinate space) + public final Velocity3d velRelative; /// The computed launcher velocity (in the robot-relative coordinate space) + public final P velParam; /// The model parameter that yielded velGlobal + + + public BCResult( + Double velPenalty, + Velocity3d velGlobal, + Velocity3d velRelative, + P velParam + ) { + this.velPenalty = velPenalty; + this.velGlobal = velGlobal; + this.velRelative = velRelative; + this.velParam = velParam; + } + + public boolean isValid() { + return this.velPenalty != null; + } +} diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java new file mode 100644 index 0000000..62a5937 --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java @@ -0,0 +1,78 @@ +package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators; + +import java.util.ArrayList; + +import org.fairportrobotics.frc.robolib.trajectory.Velocity3d; +import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEvalParams; +import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BallisticConstraint; +import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel; + +import edu.wpi.first.units.measure.Angle; + +public abstract class BallisticCalculator { + private ArrayList constraints; + private double velocityDiffWeight; + + // + // Setup code + // + + public BallisticCalculator() { + constraints = new ArrayList<>(); + this.velocityDiffWeight = 1.0; + } + + public BallisticCalculator addConstraint(BallisticConstraint constraint) { + constraints.add(constraint); + return this; + } + + public BallisticCalculator setVelocityDiffWeight(double weight) { + this.velocityDiffWeight = weight; + return this; + } + + // + // Computation + // + + /** + * Public solution + * @param The type of model to compute against + * @param

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 velShooter The velocity currently imparted by the shooter (in the model's coordinate system) + * @param robotHeading The heading of the robot (in the model's coordinate system) + * @return The generated BCResult + */ + public abstract , P> BCResult

computeSolution( + M model, Velocity3d velRobot, Velocity3d velShooter, + Angle robotHeading, boolean quickMode + ); + + + protected , P> Double evaluateCandidate( + M model, BCEvalParams evalParams + ) { + // Get the weight component for velocity difference + Velocity3d velShooterDiff = + evalParams.getCandidateGlobalShooterVelocity() + .minus(evalParams.getCurrentGlobalShooterVelocity()); + double numerator = velShooterDiff.getSquareMagnitude() * this.velocityDiffWeight; + double denominator = this.velocityDiffWeight; + + // Evaluate against all constraints + for (BallisticConstraint constraint : this.constraints) { + Double penaltyPercent = constraint.evaluate(model, evalParams); + if(penaltyPercent == null) return null; + + numerator += penaltyPercent * constraint.getWeight(); + denominator += constraint.getWeight(); + } + + // Get the final sum + return numerator/denominator; + } +} 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 new file mode 100644 index 0000000..c1e4497 --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java @@ -0,0 +1,115 @@ +package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints; + +import org.fairportrobotics.frc.robolib.trajectory.Velocity3d; + +import edu.wpi.first.units.measure.Angle; + +/** + * Param block for ballistic constraint and calculator evaluation + */ +public class BCEvalParams { + // + // Members + // + + // Robot motion params + private final Velocity3d currentGlobalRobotVelocity; + private final Angle currentGlobalRobotAngle; + private final Velocity3d currentGlobalShooterVelocity; + + // Candidate params + private final Velocity3d candidateGlobalTotalVelocity; + + // Composite value cache + private Velocity3d cache_candidateGlobalShooterVelocity; + private Velocity3d cache_candidateRelativeShooterVelocity; + + // + // Setup + // + + public BCEvalParams( + Velocity3d currentGlobalRobotVelocity, + Angle currentGlobalRobotAngle, + Velocity3d currentGlobalShooterVelocity, + Velocity3d candidateGlobalTotalVelocity + ) { + this.currentGlobalRobotVelocity = currentGlobalRobotVelocity; + this.currentGlobalRobotAngle = currentGlobalRobotAngle; + this.currentGlobalShooterVelocity = currentGlobalShooterVelocity; + this.candidateGlobalTotalVelocity = candidateGlobalTotalVelocity; + + this.cache_candidateGlobalShooterVelocity = null; + this.cache_candidateRelativeShooterVelocity = null; + } + + + // + // Base attribute getters + // + + /** + * @return The robot's current velocity (in field coordinates) + */ + public Velocity3d getCurrentGlobalRobotVelocity() { + return this.currentGlobalRobotVelocity; + } + + + /** + * @return The robot's current heading angle (in field coordinates) + */ + public Angle getCurrentGlobalRobotAngle() { + return this.currentGlobalRobotAngle; + } + + + /** + * @return The shooter's current launch velocity (in field coordinates) + */ + public Velocity3d getCurrentGlobalShooterVelocity() { + return this.currentGlobalShooterVelocity; + } + + /** + * @return The candidate total (robot + shooter) velocity (in field coordinates) + */ + public Velocity3d getCandidateGlobalTotalVelocity() { + return this.candidateGlobalTotalVelocity; + } + + // + // Composite value getters + // + + /** + * @return The candidate shooter velocity (in field coordinates) at current + * robot velocity + */ + public Velocity3d getCandidateGlobalShooterVelocity() { + if(this.cache_candidateGlobalShooterVelocity == null) { + this.cache_candidateGlobalShooterVelocity = + this.candidateGlobalTotalVelocity + .minus(this.currentGlobalRobotVelocity); + } + return this.cache_candidateGlobalShooterVelocity; + } + + /** + * @return The candidate shooter velocity (in robot coordinates) at current + * robot velocity and heading + */ + public Velocity3d getCandidateRelativeShooterVelocity() { + if(this.cache_candidateRelativeShooterVelocity == null) { + this.cache_candidateRelativeShooterVelocity = + new Velocity3d( + this.getCandidateGlobalShooterVelocity().getHorizontalVelocity(), + this.getCandidateGlobalShooterVelocity().getVeritcalVelocity(), + this.getCandidateGlobalShooterVelocity().getAzimuthAngle() + .minus(this.getCurrentGlobalRobotAngle()) + ); + } + return this.cache_candidateRelativeShooterVelocity; + } + +} 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 c473446..b53150d 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 @@ -1,8 +1,8 @@ package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints; -import org.fairportrobotics.frc.robolib.trajectory.Velocity3d; import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel; + public abstract class BallisticConstraint { protected double weight; @@ -42,14 +42,14 @@ public abstract class BallisticConstraint { /** * Evaluate a given solution's fitness against this constraint * - * TODO: Expand this with additional parameters - * * @param The type of BallisticModel * @param

The parameter type of M - * @param model The model used to compute candidate - * @param candidate The candidate velocity computed by model + * @param model The model used to compute the candidate + * @param evalParams The candidate parameters used in evaluation * @return A penalty value (from 0 to 1.0) for this match (null if candidate * is invalid). Used with weight to determine candidate fitness */ - public abstract , P> Double evaluate(M model, Velocity3d candidate); + public abstract , P> Double evaluate( + M model, BCEvalParams evalParams + ); }