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

[wpimath] Fix dt type in C++ tests (#8179)

Browse Source 
The UKF test was calling `.value()` on an implicit
`units::millisecond_t` type assuming it was `units::second_t`.

I normalized the rest of the dt declarations while I was at it.
This commit is contained in:
Tyler Veness
2025-08-16 22:51:13 -07:00
committed by GitHub
parent 46a3318324
commit 0d9e850e22
11 changed files with 32 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
View File

@@ -29,12 +29,12 @@ class LinearQuadraticRegulatorTest {
var qElms = VecBuilder.fill(0.02, 0.4);
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var dt = 0.005;
var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
assertEquals(522.153, K.get(0, 0), 0.1);
assertEquals(38.2, K.get(0, 1), 0.1);
assertEquals(522.87006795347486, K.get(0, 0), 1e-6);
assertEquals(38.239878385020411, K.get(0, 1), 1e-6);
}
@Test
@@ -65,12 +65,12 @@ class LinearQuadraticRegulatorTest {
var qElms = VecBuilder.fill(0.01745, 0.08726);
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var dt = 0.005;
var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
assertEquals(19.16, K.get(0, 0), 0.1);
assertEquals(3.32, K.get(0, 1), 0.1);
assertEquals(19.339349883583761, K.get(0, 0), 1e-6);
assertEquals(3.3542559517421582, K.get(0, 1), 1e-6);
}
/**

6
wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
View File

@@ -22,7 +22,7 @@ import java.util.Random;
import org.junit.jupiter.api.Test;
class LinearSystemLoopTest {
private static final double kDt = 0.00505;
private static final double kDt = 0.005;
private static final double kPositionStddev = 0.0001;
private static final Random random = new Random();
@@ -45,12 +45,12 @@ class LinearSystemLoopTest {
(LinearSystem<N2, N1, N1>) m_plant.slice(0),
VecBuilder.fill(0.02, 0.4),
VecBuilder.fill(12.0),
0.00505);
0.005);
@SuppressWarnings("unchecked")
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(
(LinearSystem<N2, N1, N1>) m_plant.slice(0), m_controller, m_observer, 12, 0.00505);
(LinearSystem<N2, N1, N1>) m_plant.slice(0), m_controller, m_observer, 12, 0.005);
private static void updateTwoState(
LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {

4
wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -68,7 +68,7 @@ class ExtendedKalmanFilterTest {
@Test
void testInit() {
double dtSeconds = 0.00505;
double dtSeconds = 0.005;
assertDoesNotThrow(
() -> {
@@ -98,7 +98,7 @@ class ExtendedKalmanFilterTest {
@Test
void testConvergence() {
double dtSeconds = 0.00505;
double dtSeconds = 0.005;
double rbMeters = 0.8382 / 2.0; // Robot radius
ExtendedKalmanFilter<N5, N2, N3> observer =

2
wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
View File

@@ -29,7 +29,7 @@ import org.junit.jupiter.api.Test;
class KalmanFilterTest {
private static LinearSystem<N2, N1, N2> elevatorPlant;
private static final double kDt = 0.00505;
private static final double kDt = 0.005;
static {
createElevator();

6
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
View File

@@ -12,7 +12,7 @@ namespace frc {
TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
constexpr auto maxA = 2_mps_sq;
constexpr auto maxAlpha = 2_rad_per_s_sq;
@@ -105,7 +105,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -173,7 +173,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
constexpr auto minA = -1_mps_sq;
constexpr auto maxA = 2_mps_sq;
constexpr auto maxAlpha = 2_rad_per_s_sq;

4
wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
View File

@@ -20,7 +20,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
constexpr auto kVAngular = 1_V / 1_rad_per_s;
constexpr auto kAAngular = 1_V / 1_rad_per_s_sq;
constexpr auto trackwidth = 1_m;
constexpr auto dt = 20_ms;
constexpr units::second_t dt = 20_ms;
frc::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
@@ -54,7 +54,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
constexpr auto kALinear = 1_V / 1_mps_sq;
constexpr auto kVAngular = 1_V / 1_mps;
constexpr auto kAAngular = 1_V / 1_mps_sq;
constexpr auto dt = 20_ms;
constexpr units::second_t dt = 20_ms;
frc::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular};

4
wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
View File

@@ -10,7 +10,7 @@
namespace frc {
TEST(ImplicitModelFollowerTest, SameModel) {
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -54,7 +54,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
TEST(ImplicitModelFollowerTest, SlowerRefModel) {
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);

12
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
View File

@@ -31,10 +31,10 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K();
EXPECT_NEAR(522.15314269, K(0, 0), 1e-6);
EXPECT_NEAR(38.20138596, K(0, 1), 1e-6);
EXPECT_NEAR(522.87006795347486, K(0, 0), 1e-6);
EXPECT_NEAR(38.239878385020411, K(0, 1), 1e-6);
}
TEST(LinearQuadraticRegulatorTest, ArmGains) {
@@ -56,11 +56,11 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms}
LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5_ms}
.K();
EXPECT_NEAR(19.16, K(0, 0), 1e-1);
EXPECT_NEAR(3.32, K(0, 1), 1e-1);
EXPECT_NEAR(19.339349883583761, K(0, 0), 1e-6);
EXPECT_NEAR(3.3542559517421582, K(0, 1), 1e-6);
}
TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {

4
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
View File

@@ -61,7 +61,7 @@ frc::Vectord<5> GlobalMeasurementModel(
} // namespace
TEST(ExtendedKalmanFilterTest, Init) {
constexpr auto dt = 0.00505_s;
constexpr units::second_t dt = 5_ms;
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
@@ -80,7 +80,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
}
TEST(ExtendedKalmanFilterTest, Convergence) {
constexpr auto dt = 0.00505_s;
constexpr units::second_t dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,

6
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -68,7 +68,7 @@ frc::Vectord<5> DriveGlobalMeasurementModel(
}
TEST(UnscentedKalmanFilterTest, DriveInit) {
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
@@ -94,7 +94,7 @@ TEST(UnscentedKalmanFilterTest, DriveInit) {
}
TEST(UnscentedKalmanFilterTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
@@ -206,7 +206,7 @@ TEST(UnscentedKalmanFilterTest, LinearUKF) {
}
TEST(UnscentedKalmanFilterTest, RoundTripP) {
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
frc::UnscentedKalmanFilter<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },

4
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -56,7 +56,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
constexpr units::second_t dt = 1_s;
// T
// Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
@@ -88,7 +88,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}};
frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
constexpr units::second_t dt = 5_ms;
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ