src/robolib/trajectory/ballistics: Basic impl of calculator base class
- BallisticCalculator.java: The base class for calculating solutions - BCResult.java: Holds computed solution and related values - BCEvalParams.java: Stores and computes values used in candidate evaluation - BallisticConstraint.java: evaluate() takes BCEvalParams
This commit is contained in:
Ray Healy
parent
d43b337f1e
commit
f1c80233f0
|
|
@ -0,0 +1,27 @@
|
|||
package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators;
|
||||
|
||||
import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
|
||||
|
||||
public class BCResult<P> {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<BallisticConstraint> 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 <M> The type of model to compute against
|
||||
* @param <P> 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 <M extends BallisticModel<P>, P> BCResult<P> computeSolution(
|
||||
M model, Velocity3d velRobot, Velocity3d velShooter,
|
||||
Angle robotHeading, boolean quickMode
|
||||
);
|
||||
|
||||
|
||||
protected <M extends BallisticModel<P>, 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <M> The type of BallisticModel
|
||||
* @param <P> 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 <M extends BallisticModel<P>, P> Double evaluate(M model, Velocity3d candidate);
|
||||
public abstract <M extends BallisticModel<P>, P> Double evaluate(
|
||||
M model, BCEvalParams evalParams
|
||||
);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue