Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-04-25 23:45:43 -07:00
parent 2f0990e9d2 0695a4db89
commit 36811211be
79 changed files with 2093 additions and 415 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.math;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
/** Math utility functions. */
public final class MathUtil {
private MathUtil() {
@@ -44,50 +47,51 @@ public final class MathUtil {
* @return The value after the deadband is applied.
*/
public static double applyDeadband(double value, double deadband, double maxMagnitude) {
if (Math.abs(value) > deadband) {
if (maxMagnitude / deadband > 1.0e12) {
// If max magnitude is sufficiently large, the implementation encounters
// roundoff error. Implementing the limiting behavior directly avoids
// the problem.
return value > 0.0 ? value - deadband : value + deadband;
}
if (value > 0.0) {
// Map deadband to 0 and map max to max.
//
// y - y₁ = m(x - x₁)
// y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
// y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
//
// (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
// x₁ = deadband
// y₁ = 0
// x₂ = max
// y₂ = max
//
// y = (max - 0)/(max - deadband) (x - deadband) + 0
// y = max/(max - deadband) (x - deadband)
// y = max (x - deadband)/(max - deadband)
return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
} else {
// Map -deadband to 0 and map -max to -max.
//
// y - y₁ = m(x - x₁)
// y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
// y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
//
// (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
// x₁ = -deadband
// y₁ = 0
// x₂ = -max
// y₂ = -max
//
// y = (-max - 0)/(-max + deadband) (x + deadband) + 0
// y = max/(max - deadband) (x + deadband)
// y = max (x + deadband)/(max - deadband)
return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
}
if (Math.abs(value) < deadband) {
return 0;
}
if (value > 0.0) {
// Map deadband to 0 and map max to max with a linear relationship.
//
// y - y₁ = m(x - x₁)
// y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
// y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
//
// (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
//
// x₁ = deadband
// y₁ = 0
// x₂ = max
// y₂ = max
// y = (max - 0)/(max - deadband) (x - deadband) + 0
// y = max/(max - deadband) (x - deadband)
//
// To handle high values of max, rewrite so that max only appears on the denominator.
//
// y = ((max - deadband) + deadband)/(max - deadband) (x - deadband)
// y = (1 + deadband/(max - deadband)) (x - deadband)
return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband);
} else {
return 0.0;
// Map -deadband to 0 and map -max to -max with a linear relationship.
//
// y - y₁ = m(x - x₁)
// y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
// y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
//
// (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
//
// x₁ = -deadband
// y₁ = 0
// x₂ = -max
// y₂ = -max
// y = (-max - 0)/(-max + deadband) (x + deadband) + 0
// y = max/(max - deadband) (x + deadband)
//
// To handle high values of max, rewrite so that max only appears on the denominator.
//
// y = ((max - deadband) + deadband)/(max - deadband) (x + deadband)
// y = (1 + deadband/(max - deadband)) (x + deadband)
return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband);
}
}
@@ -209,4 +213,62 @@ public final class MathUtil {
double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
return Math.abs(error) < tolerance;
}
/**
* Limits translation velocity.
*
* @param current Translation at current timestep.
* @param next Translation at next timestep.
* @param dt Timestep duration.
* @param maxVelocity Maximum translation velocity.
* @return Returns the next Translation2d limited to maxVelocity
*/
public static Translation2d slewRateLimit(
Translation2d current, Translation2d next, double dt, double maxVelocity) {
if (maxVelocity < 0) {
Exception e = new IllegalArgumentException();
MathSharedStore.reportError(
"maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
return next;
}
Translation2d diff = next.minus(current);
double dist = diff.getNorm();
if (dist < 1e-9) {
return next;
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
return current.plus(diff.times(maxVelocity * dt / dist));
}
return next;
}
/**
* Limits translation velocity.
*
* @param current Translation at current timestep.
* @param next Translation at next timestep.
* @param dt Timestep duration.
* @param maxVelocity Maximum translation velocity.
* @return Returns the next Translation3d limited to maxVelocity
*/
public static Translation3d slewRateLimit(
Translation3d current, Translation3d next, double dt, double maxVelocity) {
if (maxVelocity < 0) {
Exception e = new IllegalArgumentException();
MathSharedStore.reportError(
"maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
return next;
}
Translation3d diff = next.minus(current);
double dist = diff.getNorm();
if (dist < 1e-9) {
return next;
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
return current.plus(diff.times(maxVelocity * dt / dist));
}
return next;
}
}

4
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -188,13 +188,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
*/
public double calculate(double currentVelocity, double nextVelocity) {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (ka == 0.0) {
if (ka < 1e-9) {
return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
} else {
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
return kg
+ ks * Math.signum(currentVelocity)
+ 1.0 / B_d * (nextVelocity - A_d * currentVelocity);

4
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -171,13 +171,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
*/
public double calculate(double currentVelocity, double nextVelocity) {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (ka == 0.0) {
if (ka < 1e-9) {
return ks * Math.signum(nextVelocity) + kv * nextVelocity;
} else {
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
}
}

6
wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
View File

@@ -69,8 +69,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
}
/**
* Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
* of the filter.
* Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
* covariance (s) of the filter.
*
* @param x An array of the means.
* @param s Square-root covariance of the filter.
@@ -86,6 +86,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
// 2 * states + 1 by states
Matrix<S, ?> sigmas =
new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
// equation (17)
sigmas.setColumn(0, x);
for (int k = 0; k < m_states.getNum(); k++) {
var xPlusU = x.plus(U.extractColumnVector(k));

117
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -35,9 +35,9 @@ import org.ejml.simple.SimpleMatrix;
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
* chapter 9 "Stochastic control theory".
*
* <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
* information about the SR-UKF, see <a
* href="https://www.researchgate.net/publication/3908304">https://www.researchgate.net/publication/3908304</a>.
* <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). The main reason for
* this is to guarantee that the covariance matrix remains positive definite. For more information
* about the SR-UKF, see https://www.researchgate.net/publication/3908304.
*
* @param <States> Number of states.
* @param <Inputs> Number of inputs.
@@ -105,7 +105,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
}
/**
* Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
* Constructs an Unscented Kalman filter with custom mean, residual, and addition functions. Using
* custom functions for arithmetic can be useful if you have angles in the state or measurements,
* because they allow you to correctly account for the modular nature of angle arithmetic.
*
@@ -193,12 +193,21 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
"Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
}
// New mean is usually just the sum of the sigmas * weight:
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
// New mean is usually just the sum of the sigmas * weights:
//
// 2n
// x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
// i=0
//
// equations (19) and (23) in the paper show this,
// but we allow a custom function, usually for angle wrapping
Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
// Form an intermediate matrix S⁻ as:
//
// [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
//
// the part of equations (20) and (24) within the "qr{}"
Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
for (int i = 0; i < 2 * s.getNum(); i++) {
Sbar.setColumn(
@@ -214,8 +223,24 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
}
Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
// Compute the square-root covariance of the sigma points
//
// We transpose S⁻ first because we formed it by horizontally
// concatenating each part; it should be vertical so we can take
// the QR decomposition as defined in the "QR Decomposition" passage
// of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
//
// The resulting matrix R is the square-root covariance S, but it
// is upper triangular, so we need to transpose it.
//
// equations (20) and (24)
Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)).transpose());
// Update or downdate the square-root covariance with (𝒳₀-x̂)
// depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
//
// equations (21) and (25)
newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), true);
return new Pair<>(x, newS);
}
@@ -256,7 +281,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
*/
@Override
public Matrix<States, States> getP() {
return m_S.transpose().times(m_S);
return m_S.times(m_S.transpose());
}
/**
@@ -280,7 +305,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
*/
@Override
public void setP(Matrix<States, States> newP) {
m_S = newP.lltDecompose(false);
m_S = newP.lltDecompose(true);
}
/**
@@ -347,14 +372,28 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
var discQ = Discretization.discretizeAQ(contA, m_contQ, dt).getSecond();
var squareRootDiscQ = discQ.lltDecompose(true);
// Generate sigma points around the state mean
//
// equation (17)
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
// Project each sigma point forward in time according to the
// dynamics f(x, u)
//
// sigmas = 𝒳ₖ₋₁
// sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
//
// equation (18)
for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
Matrix<States, N1> x = sigmas.extractColumnVector(i);
m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dt));
}
// Pass the predicted sigmas (𝒳) through the Unscented Transform
// to compute the prior state mean and covariance
//
// equations (18) (19) and (20)
var ret =
squareRootUnscentedTransform(
m_states,
@@ -459,7 +498,15 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
final var discR = Discretization.discretizeR(R, m_dt);
final var squareRootDiscR = discR.lltDecompose(true);
// Transform sigma points into measurement space
// Generate new sigma points from the prior mean and covariance
// and transform them into measurement space using h(x, u)
//
// sigmas = 𝒳
// sigmasH = 𝒴
//
// This differs from equation (22) which uses
// the prior sigma points, regenerating them allows
// multiple measurement updates per time update
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
@@ -467,7 +514,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
sigmasH.setColumn(i, hRet);
}
// Mean and covariance of prediction passed through unscented transform
// Pass the predicted measurement sigmas through the Unscented Transform
// to compute the mean predicted measurement and square-root innovation
// covariance.
//
// equations (23) (24) and (25)
var transRet =
squareRootUnscentedTransform(
m_states,
@@ -481,30 +532,54 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
var yHat = transRet.getFirst();
var Sy = transRet.getSecond();
// Compute cross covariance of the state and the measurements
// Compute cross covariance of the predicted state and measurement sigma
// points given as:
//
// 2n
// P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
// i=0
//
// equation (26)
Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
}
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
// Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
// is equivalent to MATLAB's \ operator, so we need to rearrange to use
// that.
//
// K = (P_{xy} / S_{y}ᵀ) / S_{y}
// K = (S_{y} \ P_{xy})ᵀ / S_{y}
// K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
//
// equation (27)
Matrix<States, R> K =
Sy.transpose()
.solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
// Compute the posterior state mean
//
// x̂ = x̂⁻ + K(y − ŷ⁻)
//
// second part of equation (27)
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
// Compute the intermediate matrix U for downdating
// the square-root covariance
//
// equation (28)
Matrix<States, R> U = K.times(Sy);
// Downdate the posterior square-root state covariance
//
// equation (29)
for (int i = 0; i < rows.getNum(); i++) {
m_S.rankUpdate(U.extractColumnVector(i), -1, false);
m_S.rankUpdate(U.extractColumnVector(i), -1, true);
}
}
}

50
wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
View File

@@ -21,8 +21,8 @@ public class Debouncer {
kBoth
}
private final double m_debounceTime;
private final DebounceType m_debounceType;
private double m_debounceTime;
private DebounceType m_debounceType;
private boolean m_baseline;
private double m_prevTime;
@@ -40,11 +40,7 @@ public class Debouncer {
resetTimer();
m_baseline =
switch (m_debounceType) {
case kBoth, kRising -> false;
case kFalling -> true;
};
m_baseline = m_debounceType == DebounceType.kFalling;
}
/**
@@ -86,4 +82,44 @@ public class Debouncer {
return m_baseline;
}
}
/**
* Sets the time to debounce.
*
* @param time The number of seconds the value must change from baseline for the filtered value to
* change.
*/
public void setDebounceTime(double time) {
m_debounceTime = time;
}
/**
* Gets the time to debounce.
*
* @return The number of seconds the value must change from baseline for the filtered value to
* change.
*/
public double getDebounceTime() {
return m_debounceTime;
}
/**
* Sets the debounce type.
*
* @param type Which type of state change the debouncing will be performed on.
*/
public void setDebounceType(DebounceType type) {
m_debounceType = type;
m_baseline = m_debounceType == DebounceType.kFalling;
}
/**
* Gets the debounce type.
*
* @return Which type of state change the debouncing will be performed on.
*/
public DebounceType getDebounceType() {
return m_debounceType;
}
}

7
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
View File

@@ -42,7 +42,7 @@ public class TrapezoidProfile {
private int m_direction;
private final Constraints m_constraints;
private State m_current;
private State m_current = new State();
private double m_endAccel;
private double m_endFullSpeed;
@@ -186,7 +186,8 @@ public class TrapezoidProfile {
* Returns the time left until a target distance in the profile is reached.
*
* @param target The target distance.
* @return The time left until a target distance in the profile is reached.
* @return The time left until a target distance in the profile is reached, or zero if no goal was
* set.
*/
public double timeLeftUntil(double target) {
double position = m_current.position * m_direction;
@@ -252,7 +253,7 @@ public class TrapezoidProfile {
/**
* Returns the total time the profile takes to reach the goal.
*
* @return The total time the profile takes to reach the goal.
* @return The total time the profile takes to reach the goal, or zero if no goal was set.
*/
public double totalTime() {
return m_endDecel;