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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,8 +6,8 @@
#include <functional>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/RungeKuttaTimeVarying.h"
@@ -15,17 +15,16 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeA) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
Eigen::Vector<double, 2> x0{1, 1};
Eigen::Matrix<double, 2, 2> discA;
frc::Vectord<2> x0{1, 1};
frc::Matrixd<2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
Eigen::Vector<double, 2> x1Discrete = discA * x0;
frc::Vectord<2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
0.0 * x0(0) + 1.0 * x0(1)};
frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -33,20 +32,20 @@ TEST(DiscretizationTest, DiscretizeA) {
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeAB) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 1> contB{0, 1};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 1> contB{0, 1};
Eigen::Vector<double, 2> x0{1, 1};
Eigen::Vector<double, 1> u{1};
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 1> discB;
frc::Vectord<2> x0{1, 1};
frc::Vectord<1> u{1};
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 1> discB;
frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
frc::Vectord<2> x1Discrete = discA * x0 + discB * u;
// We now have pos = vel = accel = 1, which should give us:
Eigen::Vector<double, 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)};
frc::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);
}
@@ -55,24 +54,23 @@ TEST(DiscretizationTest, DiscretizeAB) {
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
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 *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
@@ -85,24 +83,23 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeFastModelAQ) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
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;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
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 *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
@@ -113,14 +110,14 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
frc::Matrixd<2, 2> discQTaylor;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
@@ -128,16 +125,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
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 *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -157,14 +153,14 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
frc::Matrixd<2, 2> discQTaylor;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
@@ -172,16 +168,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
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 *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -201,10 +196,10 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
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}};
Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
frc::Matrixd<2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
EXPECT_LT((discRTruth - discR).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscR:\n"

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

@@ -16,49 +16,43 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
ASSERT_TRUE(model.A().isApprox(
Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
0.001));
frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
ASSERT_TRUE(model.B().isApprox(
Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
0.001));
ASSERT_TRUE(model.C().isApprox(
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(
Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
frc::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));
ASSERT_TRUE(
model.D().isApprox(frc::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,
0.05_m, 12);
ASSERT_TRUE(model.A().isApprox(
Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
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));
}
TEST(LinearSystemIDTest, FlywheelSystem) {
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
ASSERT_TRUE(
model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
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));
}
TEST(LinearSystemIDTest, DCMotorSystem) {
auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
ASSERT_TRUE(model.A().isApprox(
Eigen::Matrix<double, 2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 2, 1>{0, 1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 0.0}, 0.001));
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));
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));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -70,9 +64,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
ASSERT_TRUE(model.A().isApprox(
Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
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));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -84,6 +77,6 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
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));
}

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

@@ -10,48 +10,46 @@
// Tests that integrating dx/dt = e^x works.
TEST(NumericalIntegrationTest, Exponential) {
Eigen::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
Eigen::Vector<double, 1> y1 = frc::RK4(
[](const Eigen::Vector<double, 1>& x) {
return Eigen::Vector<double, 1>{std::exp(x(0))};
},
frc::Vectord<1> y1 = frc::RK4(
[](const frc::Vectord<1>& x) { return frc::Vectord<1>{std::exp(x(0))}; },
y0, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works when we provide a U.
TEST(NumericalIntegrationTest, ExponentialWithU) {
Eigen::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
Eigen::Vector<double, 1> y1 = frc::RK4(
[](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
return Eigen::Vector<double, 1>{std::exp(u(0) * x(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))};
},
y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
y0, frc::Vectord<1>{1.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKF45.
TEST(NumericalIntegrationTest, ExponentialRKF45) {
Eigen::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
Eigen::Vector<double, 1> y1 = frc::RKF45(
[](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
return Eigen::Vector<double, 1>{std::exp(x(0))};
frc::Vectord<1> y1 = frc::RKF45(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>{std::exp(x(0))};
},
y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
y0, frc::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
Eigen::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
Eigen::Vector<double, 1> y1 = frc::RKDP(
[](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
return Eigen::Vector<double, 1>{std::exp(x(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))};
},
y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
y0, frc::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}

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

@@ -6,53 +6,46 @@
#include "frc/system/NumericalJacobian.h"
Eigen::Matrix<double, 4, 4> A{
{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
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}};
// Function from which to recover A and B
Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<4> AxBuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
return A * x + B * u;
}
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax) {
Eigen::Matrix<double, 4, 4> newA =
frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu) {
Eigen::Matrix<double, 4, 2> newB =
frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
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}};
// Function from which to recover C and D
Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<3> CxDuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
return C * x + D * u;
}
// Test that we can recover C from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Cx) {
Eigen::Matrix<double, 3, 4> newC =
frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newC.isApprox(C));
}
// Test that we can recover D from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Du) {
Eigen::Matrix<double, 3, 2> newD =
frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}

16
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
View File

@@ -9,9 +9,9 @@
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
(std::pow(std::exp(t) + 1.0, 2.0))};
frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) {
return frc::Vectord<1>{12.0 * std::exp(t) /
(std::pow(std::exp(t) + 1.0, 2.0))};
}
} // namespace
@@ -23,12 +23,12 @@ Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, const Eigen::Vector<double, 1>& x) {
return Eigen::Vector<double, 1>{
x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
frc::Vectord<1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, const frc::Vectord<1>& x) {
return frc::Vectord<1>{x(0) *
(2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);