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

SCRIPT namespace replacements

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,16 +14,16 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeA) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
wpi::math::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Vectord<2> x0{1, 1};
frc::Matrixd<2, 2> discA;
wpi::math::Vectord<2> x0{1, 1};
wpi::math::Matrixd<2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
frc::Vectord<2> x1Discrete = discA * x0;
wpi::math::DiscretizeA<2>(contA, 1_s, &discA);
wpi::math::Vectord<2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
wpi::math::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -31,19 +31,19 @@ TEST(DiscretizationTest, DiscretizeA) {
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeAB) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 1> contB{0, 1};
wpi::math::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
wpi::math::Matrixd<2, 1> contB{0, 1};
frc::Vectord<2> x0{1, 1};
frc::Vectord<1> u{1};
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 1> discB;
wpi::math::Vectord<2> x0{1, 1};
wpi::math::Vectord<1> u{1};
wpi::math::Matrixd<2, 2> discA;
wpi::math::Matrixd<2, 1> discB;
frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
frc::Vectord<2> x1Discrete = discA * x0 + discB * u;
wpi::math::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
wpi::math::Vectord<2> x1Discrete = discA * x0 + discB * u;
// We now have pos = vel = accel = 1, which should give us:
frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
wpi::math::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
EXPECT_EQ(x1Truth, x1Discrete);
@@ -53,27 +53,27 @@ TEST(DiscretizationTest, DiscretizeAB) {
// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
wpi::math::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
wpi::math::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr units::second_t dt = 1_s;
constexpr wpi::units::second_t dt = 1_s;
// T
// Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
frc::Matrixd<2, 2> discQIntegrated =
frc::RKDP<std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
wpi::math::Matrixd<2, 2> discQIntegrated =
wpi::math::RKDP<std::function<wpi::math::Matrixd<2, 2>(wpi::units::second_t,
const wpi::math::Matrixd<2, 2>&)>,
wpi::math::Matrixd<2, 2>>(
[&](wpi::units::second_t t, const wpi::math::Matrixd<2, 2>&) {
return wpi::math::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
0_s, wpi::math::Matrixd<2, 2>::Zero(), dt);
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
wpi::math::Matrixd<2, 2> discA;
wpi::math::Matrixd<2, 2> discQ;
wpi::math::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscQ:\n"
@@ -85,27 +85,27 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeFastModelAQ) {
frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}};
frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
wpi::math::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}};
wpi::math::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
frc::Matrixd<2, 2> discQIntegrated =
frc::RKDP<std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
wpi::math::Matrixd<2, 2> discQIntegrated =
wpi::math::RKDP<std::function<wpi::math::Matrixd<2, 2>(wpi::units::second_t,
const wpi::math::Matrixd<2, 2>&)>,
wpi::math::Matrixd<2, 2>>(
[&](wpi::units::second_t t, const wpi::math::Matrixd<2, 2>&) {
return wpi::math::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
0_s, wpi::math::Matrixd<2, 2>::Zero(), dt);
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
wpi::math::Matrixd<2, 2> discA;
wpi::math::Matrixd<2, 2> discQ;
wpi::math::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
<< "Expected these to be nearly equal:\ndiscQ:\n"
@@ -115,10 +115,10 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
frc::Matrixd<2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
wpi::math::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
wpi::math::Matrixd<2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
frc::Matrixd<2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
wpi::math::Matrixd<2, 2> discR = wpi::math::DiscretizeR<2>(contR, 500_ms);
EXPECT_LT((discRTruth - discR).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscR:\n"

70
wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
View File

@@ -12,64 +12,64 @@
TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
auto model = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#else
constexpr auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
constexpr auto model = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#endif
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
wpi::math::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
ASSERT_TRUE(model.B().isApprox(
frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
wpi::math::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
ASSERT_TRUE(
model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
model.C().isApprox(wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(
model.D().isApprox(frc::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
model.D().isApprox(wpi::math::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
}
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
auto model = wpi::math::LinearSystemId::ElevatorSystem(wpi::math::DCMotor::NEO(2), 5_kg,
0.05_m, 12)
.Slice(0);
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 20.8}, 0.001));
ASSERT_TRUE(model.C().isApprox(wpi::math::Matrixd<1, 2>{1.0, 0.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, FlywheelSystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
auto model = wpi::math::LinearSystemId::FlywheelSystem(wpi::math::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = frc::LinearSystemId::FlywheelSystem(
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
constexpr auto model = wpi::math::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(wpi::math::Matrixd<1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, DCMotorSystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
auto model = wpi::math::LinearSystemId::DCMotorSystem(wpi::math::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = frc::LinearSystemId::DCMotorSystem(
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
constexpr auto model = wpi::math::LinearSystemId::DCMotorSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(
model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
model.A().isApprox(wpi::math::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<2, 1>{0, 1354.166667}, 0.001));
ASSERT_TRUE(
model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001));
model.C().isApprox(wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<2, 1>{0.0, 0.0}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -79,17 +79,17 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
auto model = wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -100,14 +100,14 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
auto model = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));
ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1.0 / ka}, 0.001));
}

50
wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -11,23 +11,23 @@
// Test that integrating dx/dt = eˣ works
TEST(NumericalIntegrationTest, Exponential) {
frc::Vectord<1> y0{0.0};
wpi::math::Vectord<1> y0{0.0};
frc::Vectord<1> y1 = frc::RK4(
[](const frc::Vectord<1>& x) { return frc::Vectord<1>{std::exp(x(0))}; },
wpi::math::Vectord<1> y1 = wpi::math::RK4(
[](const wpi::math::Vectord<1>& x) { return wpi::math::Vectord<1>{std::exp(x(0))}; },
y0, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Test that integrating dx/dt = eˣ works when we provide a u
TEST(NumericalIntegrationTest, ExponentialWithU) {
frc::Vectord<1> y0{0.0};
wpi::math::Vectord<1> y0{0.0};
frc::Vectord<1> y1 = frc::RK4(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>{std::exp(u(0) * x(0))};
wpi::math::Vectord<1> y1 = wpi::math::RK4(
[](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return wpi::math::Vectord<1>{std::exp(u(0) * x(0))};
},
y0, frc::Vectord<1>{1.0}, 0.1_s);
y0, wpi::math::Vectord<1>{1.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
@@ -40,11 +40,11 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
//
// x(t) = 12eᵗ/(eᵗ + 1)²
TEST(NumericalIntegrationTest, RK4TimeVarying) {
frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
wpi::math::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
frc::Vectord<1> y1 = frc::RK4(
[](units::second_t t, const frc::Vectord<1>& x) {
return frc::Vectord<1>{x(0) *
wpi::math::Vectord<1> y1 = wpi::math::RK4(
[](wpi::units::second_t t, const wpi::math::Vectord<1>& x) {
return wpi::math::Vectord<1>{x(0) *
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
@@ -54,23 +54,23 @@ TEST(NumericalIntegrationTest, RK4TimeVarying) {
// Tests that integrating dx/dt = 0 works with RKDP
TEST(NumericalIntegrationTest, ZeroRKDP) {
frc::Vectord<1> y1 = frc::RKDP(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>::Zero();
wpi::math::Vectord<1> y1 = wpi::math::RKDP(
[](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return wpi::math::Vectord<1>::Zero();
},
frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s);
wpi::math::Vectord<1>{0.0}, wpi::math::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), 0.0, 1e-3);
}
// Tests that integrating dx/dt = eˣ works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
frc::Vectord<1> y0{0.0};
wpi::math::Vectord<1> y0{0.0};
frc::Vectord<1> y1 = frc::RKDP(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>{std::exp(x(0))};
wpi::math::Vectord<1> y1 = wpi::math::RKDP(
[](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return wpi::math::Vectord<1>{std::exp(x(0))};
},
y0, frc::Vectord<1>{0.0}, 0.1_s);
y0, wpi::math::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
@@ -83,11 +83,11 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
//
// x(t) = 12eᵗ/(eᵗ + 1)²
TEST(NumericalIntegrationTest, RKDPTimeVarying) {
frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
wpi::math::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
frc::Vectord<1> y1 = frc::RKDP(
[](units::second_t t, const frc::Vectord<1>& x) {
return frc::Vectord<1>{x(0) *
wpi::math::Vectord<1> y1 = wpi::math::RKDP(
[](wpi::units::second_t t, const wpi::math::Vectord<1>& x) {
return wpi::math::Vectord<1>{x(0) *
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s, 1e-12);

40
wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
View File

@@ -6,25 +6,25 @@
#include "wpi/math/system/NumericalJacobian.hpp"
frc::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
frc::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
wpi::math::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
wpi::math::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
// Function from which to recover A and B
frc::Vectord<4> AxBuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
wpi::math::Vectord<4> AxBuFn(const wpi::math::Vectord<4>& x, const wpi::math::Vectord<2>& u) {
return A * x + B * u;
}
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax) {
frc::Matrixd<4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
wpi::math::Matrixd<4, 4> newA = wpi::math::NumericalJacobianX<4, 4, 2>(
AxBuFn, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu) {
frc::Matrixd<4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
wpi::math::Matrixd<4, 2> newB = wpi::math::NumericalJacobianU<4, 4, 2>(
AxBuFn, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
@@ -35,37 +35,37 @@ Eigen::VectorXd AxBuFn_DynamicSize(const Eigen::VectorXd& x,
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax_DynamicSize) {
Eigen::MatrixXd newA = frc::NumericalJacobianX(
AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
Eigen::MatrixXd newA = wpi::math::NumericalJacobianX(
AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu_DynamicSize) {
Eigen::MatrixXd newB = frc::NumericalJacobianU(
AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
Eigen::MatrixXd newB = wpi::math::NumericalJacobianU(
AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
wpi::math::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
wpi::math::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
// Function from which to recover C and D
frc::Vectord<3> CxDuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
wpi::math::Vectord<3> CxDuFn(const wpi::math::Vectord<4>& x, const wpi::math::Vectord<2>& u) {
return C * x + D * u;
}
// Test that we can recover C from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Cx) {
frc::Matrixd<3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
wpi::math::Matrixd<3, 4> newC = wpi::math::NumericalJacobianX<3, 4, 2>(
CxDuFn, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newC.isApprox(C));
}
// Test that we can recover D from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Du) {
frc::Matrixd<3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
wpi::math::Matrixd<3, 2> newD = wpi::math::NumericalJacobianU<3, 4, 2>(
CxDuFn, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}
@@ -75,13 +75,13 @@ Eigen::VectorXd CxDuFn_DynamicSize(const Eigen::VectorXd& x,
}
TEST(NumericalJacobianTest, Cx_DynamicSize) {
Eigen::MatrixXd newC = frc::NumericalJacobianX(
Eigen::MatrixXd newC = wpi::math::NumericalJacobianX(
CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2));
EXPECT_TRUE(newC.isApprox(C));
}
TEST(NumericalJacobianTest, Du_DynamicSize) {
Eigen::MatrixXd newD = frc::NumericalJacobianU(
Eigen::MatrixXd newD = wpi::math::NumericalJacobianU(
CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2));
EXPECT_TRUE(newD.isApprox(D));
}

8
wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp
View File

@@ -7,16 +7,16 @@
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
using ProtoType = wpi::Protobuf<frc::DCMotor>;
using ProtoType = wpi::util::Protobuf<wpi::math::DCMotor>;
inline constexpr DCMotor kExpectedData =
DCMotor{1.91_V, 19.1_Nm, 1.74_A, 2.29_A, 2.2_rad_per_s, 2};
TEST(DCMotorProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

4
wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp
View File

@@ -6,9 +6,9 @@
#include "wpi/math/system/plant/DCMotor.hpp"
using namespace frc;
using namespace wpi::math;
using StructType = wpi::Struct<frc::DCMotor>;
using StructType = wpi::util::Struct<wpi::math::DCMotor>;
inline constexpr DCMotor kExpectedData =
DCMotor{1.91_V, 19.1_Nm, 1.74_A, 2.29_A, 2.2_rad_per_s, 2};

2
wpimath/src/test/native/cpp/system/proto/LinearSystemProtoTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/proto/LinearSystemProto.hpp"
using namespace frc;
using namespace wpi::math;
struct LinearSystemProtoTestData {
using Type = LinearSystem<2, 3, 4>;

2
wpimath/src/test/native/cpp/system/struct/LinearSystemStructTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/struct/LinearSystemStruct.hpp"
using namespace frc;
using namespace wpi::math;
struct LinearSystemStructTestData {
using Type = LinearSystem<2, 3, 4>;