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

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -23,7 +23,8 @@ TEST(DiscretizationTest, DiscretizeA) {
wpi::math::Vectord<2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
wpi::math::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);
}
@@ -44,7 +45,7 @@ TEST(DiscretizationTest, DiscretizeAB) {
// We now have pos = vel = accel = 1, which should give us:
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)};
0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -61,15 +62,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
// T
// Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
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, wpi::math::Matrixd<2, 2>::Zero(), dt);
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, wpi::math::Matrixd<2, 2>::Zero(), dt);
wpi::math::Matrixd<2, 2> discA;
wpi::math::Matrixd<2, 2> discQ;
@@ -93,15 +94,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
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, wpi::math::Matrixd<2, 2>::Zero(), dt);
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, wpi::math::Matrixd<2, 2>::Zero(), dt);
wpi::math::Matrixd<2, 2> discA;
wpi::math::Matrixd<2, 2> discQ;

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

@@ -20,18 +20,19 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
#endif
ASSERT_TRUE(model.A().isApprox(
wpi::math::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(
wpi::math::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
ASSERT_TRUE(
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, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
ASSERT_TRUE(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, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
}
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = wpi::math::LinearSystemId::ElevatorSystem(wpi::math::DCMotor::NEO(2), 5_kg,
0.05_m, 12)
auto model = wpi::math::LinearSystemId::ElevatorSystem(
wpi::math::DCMotor::NEO(2), 5_kg, 0.05_m, 12)
.Slice(0);
ASSERT_TRUE(model.A().isApprox(
wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
@@ -42,8 +43,8 @@ TEST(LinearSystemIDTest, ElevatorSystem) {
TEST(LinearSystemIDTest, FlywheelSystem) {
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::FlywheelSystem(wpi::math::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
auto model = wpi::math::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = wpi::math::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
@@ -57,18 +58,19 @@ TEST(LinearSystemIDTest, FlywheelSystem) {
TEST(LinearSystemIDTest, DCMotorSystem) {
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::DCMotorSystem(wpi::math::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
auto model = wpi::math::LinearSystemId::DCMotorSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#else
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(
wpi::math::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(
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(wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
model.B().isApprox(wpi::math::Matrixd<2, 1>{0, 1354.166667}, 0.001));
ASSERT_TRUE(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));
}
@@ -79,8 +81,9 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
auto model =
wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
@@ -89,7 +92,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
ASSERT_TRUE(model.A().isApprox(
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));
ASSERT_TRUE(
model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -100,8 +104,9 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
auto model =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(

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

@@ -14,7 +14,9 @@ TEST(NumericalIntegrationTest, Exponential) {
wpi::math::Vectord<1> y0{0.0};
wpi::math::Vectord<1> y1 = wpi::math::RK4(
[](const wpi::math::Vectord<1>& x) { return wpi::math::Vectord<1>{std::exp(x(0))}; },
[](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);
}
@@ -40,12 +42,13 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
//
// x(t) = 12eᵗ/(eᵗ + 1)²
TEST(NumericalIntegrationTest, RK4TimeVarying) {
wpi::math::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)};
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)};
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),
@@ -83,12 +86,13 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
//
// x(t) = 12eᵗ/(eᵗ + 1)²
TEST(NumericalIntegrationTest, RKDPTimeVarying) {
wpi::math::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)};
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)};
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s, 1e-12);
EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),

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

@@ -6,11 +6,13 @@
#include "wpi/math/system/NumericalJacobian.hpp"
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, 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
wpi::math::Vectord<4> AxBuFn(const wpi::math::Vectord<4>& x, const wpi::math::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;
}
@@ -36,14 +38,16 @@ 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 = wpi::math::NumericalJacobianX(
AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
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 = wpi::math::NumericalJacobianU(
AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(), wpi::math::Vectord<2>::Zero());
AxBuFn_DynamicSize, wpi::math::Vectord<4>::Zero(),
wpi::math::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
@@ -51,7 +55,8 @@ 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
wpi::math::Vectord<3> CxDuFn(const wpi::math::Vectord<4>& x, const wpi::math::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;
}