Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21: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

157
wpimath/src/main/native/include/frc/MathUtil.h
View File

@@ -10,9 +10,15 @@
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/angle.h"
#include "units/base.h"
#include "units/length.h"
#include "units/math.h"
#include "units/time.h"
#include "units/velocity.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -37,51 +43,55 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
magnitude = units::math::abs(value);
}
if (magnitude > 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 > T{0.0} ? value - deadband : value + deadband;
}
if (value > T{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);
}
} else {
if (magnitude < deadband) {
return T{0.0};
}
if (value > T{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.0 + deadband / (maxMagnitude - deadband)) * (value - deadband);
} else {
// 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.0 + deadband / (maxMagnitude - deadband)) * (value + deadband);
}
}
/**
@@ -207,4 +217,65 @@ constexpr std::signed_integral auto FloorMod(std::signed_integral auto x,
std::signed_integral auto y) {
return x - FloorDiv(x, y) * y;
}
/**
* 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
*/
constexpr Translation2d SlewRateLimit(const Translation2d& current,
const Translation2d& next,
units::second_t dt,
units::meters_per_second_t maxVelocity) {
if (maxVelocity < 0_mps) {
wpi::math::MathSharedStore::ReportError(
"maxVelocity must be a non-negative number, got {}!", maxVelocity);
return next;
}
Translation2d diff = next - current;
units::meter_t dist = diff.Norm();
if (dist < 1e-9_m) {
return next;
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
return current + diff * (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
*/
constexpr Translation3d SlewRateLimit(const Translation3d& current,
const Translation3d& next,
units::second_t dt,
units::meters_per_second_t maxVelocity) {
if (maxVelocity < 0_mps) {
wpi::math::MathSharedStore::ReportError(
"maxVelocity must be a non-negative number, got {}!", maxVelocity);
return next;
}
Translation3d diff = next - current;
units::meter_t dist = diff.Norm();
if (dist < 1e-9_m) {
return next;
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
return current + diff * (maxVelocity * dt / dist);
}
return next;
}
} // namespace frc

4
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -133,13 +133,13 @@ class ElevatorFeedforward {
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (kA == decltype(kA)(0)) {
if (kA < decltype(kA)(1e-9)) {
return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
} else {
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = 1.0 / A * (A_d - 1.0) * B;
double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
return kG + kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *

4
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -93,13 +93,13 @@ class SimpleMotorFeedforward {
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (kA == decltype(kA)(0)) {
if (kA < decltype(kA)(1e-9)) {
return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
} else {
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = 1.0 / A * (A_d - 1.0) * B;
double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
return kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *

4
wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
View File

@@ -51,7 +51,7 @@ class MerweScaledSigmaPoints {
/**
* Computes the sigma points for an unscented Kalman filter given the mean
* (x) and square-root covariance(S) of the filter.
* (x) and square-root covariance (S) of the filter.
*
* @param x An array of the means.
* @param S Square-root covariance of the filter.
@@ -68,6 +68,8 @@ class MerweScaledSigmaPoints {
Matrixd<States, States> U = eta * S;
Matrixd<States, 2 * States + 1> sigmas;
// equation (17)
sigmas.template block<States, 1>(0, 0) = x;
for (int k = 0; k < States; ++k) {
sigmas.template block<States, 1>(0, k + 1) =

78
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -43,7 +43,9 @@ namespace frc {
* "Stochastic control theory".
*
* <p> This class implements a square-root-form unscented Kalman filter
* (SR-UKF). For more information about the SR-UKF, see
* (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.
*
* @tparam States Number of states.
@@ -108,7 +110,7 @@ class UnscentedKalmanFilter {
}
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
* 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.
@@ -189,7 +191,7 @@ class UnscentedKalmanFilter {
/**
* Returns the reconstructed error covariance matrix P.
*/
StateMatrix P() const { return m_S.transpose() * m_S; }
StateMatrix P() const { return m_S * m_S.transpose(); }
/**
* Set the current square-root error covariance matrix S by taking the square
@@ -197,7 +199,7 @@ class UnscentedKalmanFilter {
*
* @param P The error covariance matrix P.
*/
void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
void SetP(const StateMatrix& P) { m_S = P.llt().matrixL(); }
/**
* Returns the state estimate x-hat.
@@ -252,14 +254,28 @@ class UnscentedKalmanFilter {
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
// Generate sigma points around the state mean
//
// equation (17)
Matrixd<States, 2 * States + 1> 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.NumSigmas(); ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = 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)
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
@@ -367,7 +383,15 @@ class UnscentedKalmanFilter {
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// 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
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
@@ -376,16 +400,26 @@ class UnscentedKalmanFilter {
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
// 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)
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// 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)
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -393,21 +427,39 @@ class UnscentedKalmanFilter {
.transpose();
}
// 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)
Matrixd<States, Rows> K =
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
// Compute the posterior state mean
//
// x̂ = x̂⁻ + K(y − ŷ⁻)
//
// second part of equation (27)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
// Compute the intermediate matrix U for downdating
// the square-root covariance
//
// equation (28)
Matrixd<States, Rows> U = K * Sy;
// Downdate the posterior square-root state covariance
//
// equation (29)
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}

38
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -47,12 +47,21 @@ SquareRootUnscentedTransform(
const Vectord<CovDim>&)>
residualFunc,
const Matrixd<CovDim, CovDim>& squareRootR) {
// 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
Vectord<CovDim> x = meanFunc(sigmas, Wm);
// Form an intermediate matrix S⁻ as:
//
// [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
//
// the part of equations (20) and (24) within the "qr{}"
Matrixd<CovDim, States * 2 + CovDim> Sbar;
for (int i = 0; i < States * 2; i++) {
Sbar.template block<CovDim, 1>(0, i) =
@@ -61,14 +70,29 @@ SquareRootUnscentedTransform(
}
Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
// Merwe defines the QR decomposition as Aᵀ = QR
// 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)
Matrixd<CovDim, CovDim> S = Sbar.transpose()
.householderQr()
.matrixQR()
.template block<CovDim, CovDim>(0, 0)
.template triangularView<Eigen::Upper>();
.template triangularView<Eigen::Upper>()
.transpose();
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
// Update or downdate the square-root covariance with (𝒳₀-x̂)
// depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
//
// equations (21) and (25)
Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
return std::make_tuple(x, S);

36
wpimath/src/main/native/include/frc/filter/Debouncer.h
View File

@@ -48,6 +48,42 @@ class WPILIB_DLLEXPORT Debouncer {
*/
bool Calculate(bool input);
/**
* Sets the time to debounce.
*
* @param time The number of seconds the value must change from baseline
* for the filtered value to change.
*/
constexpr void SetDebounceTime(units::second_t 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.
*/
constexpr units::second_t GetDebounceTime() const { return m_debounceTime; }
/**
* Set the debounce type.
*
* @param type Which type of state change the debouncing will be performed on.
*/
constexpr void SetDebounceType(DebounceType type) {
m_debounceType = type;
m_baseline = m_debounceType == DebounceType::kFalling;
}
/**
* Get the debounce type.
*
* @return Which type of state change the debouncing will be performed on.
*/
constexpr DebounceType GetDebounceType() const { return m_debounceType; }
private:
units::second_t m_debounceTime;
bool m_baseline;

14
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
View File

@@ -198,7 +198,8 @@ 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.
*/
constexpr units::second_t TimeLeftUntil(Distance_t target) const {
Distance_t position = m_current.position * m_direction;
@@ -269,7 +270,8 @@ 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.
*/
constexpr units::second_t TotalTime() const { return m_endDecel; }
@@ -309,14 +311,14 @@ class TrapezoidProfile {
}
// The direction of the profile, either 1 for forwards or -1 for inverted
int m_direction;
int m_direction = 1;
Constraints m_constraints;
State m_current;
units::second_t m_endAccel;
units::second_t m_endFullSpeed;
units::second_t m_endDecel;
units::second_t m_endAccel = 0_s;
units::second_t m_endFullSpeed = 0_s;
units::second_t m_endDecel = 0_s;
};
} // namespace frc