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

trajectory/ballistics/calculators: Docs + reorganization 

- BCResult: Define a static no solution record
- Refactor evalSingleCandidate into BallisticCalculator
    - getCandidatePenalty() made private
This commit is contained in:
Ray Healy 2026-03-14 12:08:27 -04:00
parent 138dfc5d7f
commit 93aa5b7430
4 changed files with 74 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java
View File

@ -7,6 +7,8 @@ import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEval
*/
public record BCResult(Double penalty, BCEvalParams evalParams) {
public static final BCResult kNoSolution = new BCResult(null, null);
/**
* Checks if the result is valid (i.e. checks if penalty != null)
* @return True if the evalParams evaluates as valid, false otherwise

50
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java
View File

@ -10,24 +10,43 @@ import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticMo
import edu.wpi.first.units.measure.Angle;
/**
* Base cass for ballistics calculators
*/
public abstract class BallisticCalculator {
/** The list of constraints used to evaluate solutions */
private ArrayList<BallisticConstraint> constraints;
/** The weight to assign to difference in velocity while evaluating */
private double velocityDiffWeight;
//
// Setup code
//
/**
* The default constructor. Sets velocity diff weight to 1.0
*/
public BallisticCalculator() {
constraints = new ArrayList<>();
this.velocityDiffWeight = 1.0;
}
/**
* Add a constraint to the calculator
* @param constraint The constraint to add to the calculator
* @return this
*/
public BallisticCalculator addConstraint(BallisticConstraint constraint) {
constraints.add(constraint);
return this;
}
/**
* Set the weight for the difference in velocity
* @param weight The weight to assign to difference in velocity
* @return this
*/
public BallisticCalculator setVelocityDiffWeight(double weight) {
this.velocityDiffWeight = weight;
return this;
@ -62,7 +81,9 @@ public abstract class BallisticCalculator {
/**
* Compute the best solution for given initial conditions
* Compute the best solution for given initial conditions.
*
* NOTE: May reset seen candidates
*
* @param <M> The type of model to compute against
* @param <P> The candidate parameter type of the model
@ -81,6 +102,30 @@ public abstract class BallisticCalculator {
Velocity3d shooterVelocity
) throws BallisticException;
/**
* Generate and evaluate a solution for a given model, param, and robot state
*
* @param <M> The type of model to evaluate for
* @param <P> The type of param to model M
* @param model The model to evaluate for
* @param modelParam The parameter to evaluate for
* @param robotVelocity The robot's current velocity
* @param robotAngle The robot's current angle
* @param shooterVelocity The current velocity provided by the shooter
* @return The BCResult instance for the given params
* @throws BallisticException If thrown by registered constraints
*/
protected <M extends BallisticModel<P>, P> BCResult evaluateCandidate(
M model,
P modelParam,
Velocity3d robotVelocity,
Angle robotAngle,
Velocity3d shooterVelocity
) throws BallisticException {
Velocity3d candidateVelocity = model.getCandidateVelocity(modelParam);
BCEvalParams evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidateVelocity);
return new BCResult(this.getCandidatePenalty(model, evalParams), evalParams);
}
/**
* Get the average penalty for a given set of params
@ -91,8 +136,9 @@ public abstract class BallisticCalculator {
* @param evalParams The eval function param block
* @return The weighted average of penalties from velocity diferences and
* constraints (null if evalParams fail any constraints)
* @throws BallisicsException If thrown by registered constraints
*/
protected <M extends BallisticModel<P>, P> Double evaluateCandidate(
private <M extends BallisticModel<P>, P> Double getCandidatePenalty(
M model, BCEvalParams evalParams
) throws BallisticException {
// Get the weight component for velocity difference

23
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java
View File

@ -4,18 +4,23 @@ 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.exceptions.BallisticException;
import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
import edu.wpi.first.units.measure.Angle;
/**
* Simple greedy calculator (i.e. returns the first valid result)
*/
public class GreedyBallisticCalculator extends BallisticCalculator {
/**
* Default constructor for the calculator
*/
public GreedyBallisticCalculator() {
super();
}
@Override
public <M extends BallisticModel<P>, P> BCResult computeSolution(
M model,
P initParam,
@ -33,18 +38,16 @@ public class GreedyBallisticCalculator extends BallisticCalculator {
P param = candidate_params.removeFirst();
if(model.paramComputed(param)) continue;
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);
}
BCResult candidate = this.evaluateCandidate(
model, param, robotVelocity, robotAngle, shooterVelocity
);
if(candidate.isValid()) {
return candidate;
}
// Update the candidate list
candidate_params.addAll(Arrays.asList(model.getNeighborParams(param)));
}
return new BCResult(null, null);
return BCResult.kNoSolution;
}
}

27
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java
View File

@ -1,30 +1,25 @@
package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators;
import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEvalParams;
import org.fairportrobotics.frc.robolib.trajectory.ballistics.exceptions.BallisticException;
import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
import edu.wpi.first.units.measure.Angle;
/**
* Solution calculator implementing a gradient descent algorithm to find a
* solution with a local minimum penalty (not guaranteed to find a global
* minimum penalty)
*/
public class HillDescentBallisticCalculator extends BallisticCalculator {
/**
* The default consttructor for the calculator
*/
public HillDescentBallisticCalculator() {
super();
}
private <M extends BallisticModel<P>, P> BCResult evalSingleCandidate(
M model,
P modelParam,
Velocity3d robotVelocity,
Angle robotAngle,
Velocity3d shooterVelocity
) throws BallisticException {
Velocity3d candidateVelocity = model.getCandidateVelocity(modelParam);
BCEvalParams evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidateVelocity);
return new BCResult(this.evaluateCandidate(model, evalParams), evalParams);
}
public <M extends BallisticModel<P>, P> BCResult computeSolution(
M model,
P initParam,
@ -35,7 +30,7 @@ public class HillDescentBallisticCalculator extends BallisticCalculator {
model.paramComputedReset();
P bestParam = initParam;
BCResult bestResult = this.evalSingleCandidate(model, initParam, robotVelocity, robotAngle, shooterVelocity);
BCResult bestResult = this.evaluateCandidate(model, initParam, robotVelocity, robotAngle, shooterVelocity);
boolean foundImprovement = true;
while(bestResult.isValid() && foundImprovement) {
@ -44,7 +39,7 @@ public class HillDescentBallisticCalculator extends BallisticCalculator {
for (P param : model.getNeighborParams(bestParam)) {
if(model.paramComputed(param)) continue;
BCResult result = this.evalSingleCandidate(model, param, robotVelocity, robotAngle, shooterVelocity);
BCResult result = this.evaluateCandidate(model, param, robotVelocity, robotAngle, shooterVelocity);
if(result.isValid() && result.penalty() < bestResult.penalty()) {
bestParam = param;
bestResult = result;