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

21
wpimath/src/test/native/cpp/EigenTest.cpp
View File

@@ -2,34 +2,34 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Eigen/Core"
#include "Eigen/LU"
#include "frc/EigenCore.h"
#include "gtest/gtest.h"
TEST(EigenTest, Multiplication) {
Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
frc::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}};
const auto result = m1 * m2;
Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
frc::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
EXPECT_TRUE(expectedResult.isApprox(result));
Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
Eigen::Matrix<double, 3, 4> m4{
frc::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
frc::Matrixd<3, 4> m4{
{3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
const auto result2 = m3 * m4;
Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
{22.13, 9.82, 13.28, 23.53}};
frc::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
{22.13, 9.82, 13.28, 23.53}};
EXPECT_TRUE(expectedResult2.isApprox(result2));
}
TEST(EigenTest, Transpose) {
Eigen::Vector<double, 3> vec{1, 2, 3};
frc::Vectord<3> vec{1, 2, 3};
const auto transpose = vec.transpose();
@@ -39,8 +39,7 @@ TEST(EigenTest, Transpose) {
}
TEST(EigenTest, Inverse) {
Eigen::Matrix<double, 3, 3> mat{
{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
frc::Matrixd<3, 3> mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
const auto inverse = mat.inverse();
const auto identity = Eigen::MatrixXd::Identity(3, 3);

7
wpimath/src/test/native/cpp/StateSpaceTest.cpp
View File

@@ -7,7 +7,7 @@
#include <cmath>
#include <random>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -45,8 +45,7 @@ class StateSpaceTest : public testing::Test {
void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
double noise) {
Eigen::Vector<double, 1> y =
plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
Vectord<1> y = plant.CalculateY(loop.Xhat(), loop.U()) + Vectord<1>{noise};
loop.Correct(y);
loop.Predict(kDt);
}
@@ -55,7 +54,7 @@ TEST_F(StateSpaceTest, CorrectPredictLoop) {
std::default_random_engine generator;
std::normal_distribution<double> dist{0.0, kPositionStddev};
Eigen::Vector<double, 2> references{2.0, 0.0};
Vectord<2> references{2.0, 0.0};
loop.SetNextR(references);
for (int i = 0; i < 1000; i++) {

60
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -6,13 +6,13 @@
#include <array>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/NumericalIntegration.h"
TEST(StateSpaceUtilTest, MakeMatrix) {
// Column vector
Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
frc::Vectord<2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
EXPECT_NEAR(mat1(0), 1.0, 1e-3);
EXPECT_NEAR(mat1(1), 2.0, 1e-3);
@@ -22,15 +22,14 @@ TEST(StateSpaceUtilTest, MakeMatrix) {
EXPECT_NEAR(mat2(1), 2.0, 1e-3);
// Square matrix
Eigen::Matrix<double, 2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0);
frc::Matrixd<2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0);
EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3);
EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3);
EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3);
// Nonsquare matrix with more rows than columns
Eigen::Matrix<double, 3, 2> mat4 =
frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
frc::Matrixd<3, 2> mat4 = frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3);
EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3);
@@ -39,8 +38,7 @@ TEST(StateSpaceUtilTest, MakeMatrix) {
EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3);
// Nonsquare matrix with more columns than rows
Eigen::Matrix<double, 2, 3> mat5 =
frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
frc::Matrixd<2, 3> mat5 = frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3);
EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3);
@@ -50,7 +48,7 @@ TEST(StateSpaceUtilTest, MakeMatrix) {
}
TEST(StateSpaceUtilTest, CostParameterPack) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
frc::Matrixd<3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -63,7 +61,7 @@ TEST(StateSpaceUtilTest, CostParameterPack) {
}
TEST(StateSpaceUtilTest, CostArray) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
frc::Matrixd<3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -76,7 +74,7 @@ TEST(StateSpaceUtilTest, CostArray) {
}
TEST(StateSpaceUtilTest, CovParameterPack) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -89,7 +87,7 @@ TEST(StateSpaceUtilTest, CovParameterPack) {
}
TEST(StateSpaceUtilTest, CovArray) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
frc::Matrixd<3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -102,59 +100,59 @@ TEST(StateSpaceUtilTest, CovArray) {
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, IsStabilizable) {
Eigen::Matrix<double, 2, 1> B{0, 1};
frc::Matrixd<2, 1> B{0, 1};
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
EXPECT_FALSE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
EXPECT_FALSE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
EXPECT_FALSE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
EXPECT_FALSE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
EXPECT_TRUE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
EXPECT_TRUE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
EXPECT_TRUE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
EXPECT_TRUE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
}
TEST(StateSpaceUtilTest, IsDetectable) {
Eigen::Matrix<double, 1, 2> C{0, 1};
frc::Matrixd<1, 2> C{0, 1};
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
EXPECT_FALSE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
EXPECT_FALSE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
EXPECT_TRUE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
EXPECT_TRUE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
EXPECT_TRUE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
EXPECT_TRUE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
}

31
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
View File

@@ -6,47 +6,46 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u) {
return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
Vectord<2> Dynamics(const Vectord<2>& x, const Vectord<1>& u) {
return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
Matrixd<2, 1>{0.0, 1.0} * u;
}
Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
Vectord<2> StateDynamics(const Vectord<2>& x) {
return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
const Eigen::Vector<double, 1>&)>
std::function<Vectord<2>(const Vectord<2>&, const Vectord<1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, units::second_t{0.02}};
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
modelDynamics = [](auto& x) { return StateDynamics(x); };
std::function<Vectord<2>(const Vectord<2>&)> modelDynamics = [](auto& x) {
return StateDynamics(x);
};
Eigen::Matrix<double, 2, 1> B{0, 1};
Matrixd<2, 1> B{0, 1};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, B, units::second_t(0.02)};
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}

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

@@ -24,40 +24,37 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
maxAlpha};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xAccelLimiter{0.0, 0.0};
// Ensure voltage exceeds acceleration before limiting
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{12.0, 12.0};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(units::math::abs(a), maxA);
}
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{-12.0, 12.0};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_GT(units::math::abs(alpha), maxAlpha);
}
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
@@ -66,19 +63,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
}
// Backward
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
@@ -87,19 +83,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
}
// Rotate CCW
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
@@ -123,19 +118,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
DifferentialDriveAccelerationLimiter accelLimiter{
plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xAccelLimiter{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
@@ -144,15 +139,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
// Backward
x.setZero();
xAccelLimiter.setZero();
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
@@ -161,15 +156,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
// Rotate CCW
x.setZero();
xAccelLimiter.setZero();
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));

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

@@ -19,11 +19,11 @@ TEST(ImplicitModelFollowerTest, SameModel) {
ImplicitModelFollower<2, 2> imf{plant, plant};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xImf{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xImf{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -33,7 +33,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
// Backward
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -43,7 +43,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
// Rotate CCW
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -68,11 +68,11 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
ImplicitModelFollower<2, 2> imf{plant, plantRef};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xImf{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xImf{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -84,7 +84,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
// Backward
x.setZero();
xImf.setZero();
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -96,7 +96,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
// Rotate CCW
x.setZero();
xImf.setZero();
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);

10
wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
View File

@@ -6,21 +6,21 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
Eigen::Matrix<double, 2, 1> B{0, 1};
Matrixd<2, 2> A{{1, 0}, {0, 1}};
Matrixd<2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
units::second_t(0.02)};
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}

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

@@ -6,7 +6,7 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
@@ -30,7 +30,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
Eigen::Matrix<double, 1, 2> K =
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
EXPECT_NEAR(522.15314269, K(0, 0), 1e-6);
@@ -54,7 +54,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
motors, 1.0 / 3.0 * m * r * r, G);
}();
Eigen::Matrix<double, 1, 2> K =
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms}
.K();
@@ -77,7 +77,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
Eigen::Matrix<double, 1, 2> K =
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
EXPECT_NEAR(10.38, K(0, 0), 1e-1);
@@ -99,50 +99,44 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
* @param dt Discretization timestep.
*/
template <int States, int Inputs>
Eigen::Matrix<double, Inputs, States> GetImplicitModelFollowingK(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, States>& Aref, units::second_t dt) {
Matrixd<Inputs, States> GetImplicitModelFollowingK(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, States>& Aref, units::second_t dt) {
// Discretize real dynamics
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
// Discretize desired dynamics
Eigen::Matrix<double, States, States> discAref;
Matrixd<States, States> discAref;
DiscretizeA<States>(Aref, dt, &discAref);
Eigen::Matrix<double, States, States> Qimf =
Matrixd<States, States> Qimf =
(discA - discAref).transpose() * Q * (discA - discAref);
Eigen::Matrix<double, Inputs, Inputs> Rimf =
discB.transpose() * Q * discB + R;
Eigen::Matrix<double, States, Inputs> Nimf =
(discA - discAref).transpose() * Q * discB;
Matrixd<Inputs, Inputs> Rimf = discB.transpose() * Q * discB + R;
Matrixd<States, Inputs> Nimf = (discA - discAref).transpose() * Q * discB;
return LinearQuadraticRegulator<States, Inputs>{A, B, Qimf, Rimf, Nimf, dt}
.K();
}
TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) {
Eigen::Matrix<double, 2, 2> A{Eigen::Matrix<double, 2, 2>::Zero()};
Eigen::Matrix<double, 2, 2> B{Eigen::Matrix<double, 2, 2>::Identity()};
Eigen::Matrix<double, 2, 2> Q{Eigen::Matrix<double, 2, 2>::Identity()};
Eigen::Matrix<double, 2, 2> R{Eigen::Matrix<double, 2, 2>::Identity()};
Matrixd<2, 2> A{Matrixd<2, 2>::Zero()};
Matrixd<2, 2> B{Matrixd<2, 2>::Identity()};
Matrixd<2, 2> Q{Matrixd<2, 2>::Identity()};
Matrixd<2, 2> R{Matrixd<2, 2>::Identity()};
// QR overload
Eigen::Matrix<double, 2, 2> K =
LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
Matrixd<2, 2> K = LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10);
EXPECT_NEAR(0.0, K(0, 1), 1e-10);
EXPECT_NEAR(0.0, K(1, 0), 1e-10);
EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10);
// QRN overload
Eigen::Matrix<double, 2, 2> N{Eigen::Matrix<double, 2, 2>::Identity()};
Eigen::Matrix<double, 2, 2> Kimf =
LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
Matrixd<2, 2> N{Matrixd<2, 2>::Identity()};
Matrixd<2, 2> Kimf = LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10);
EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10);
@@ -153,21 +147,19 @@ TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) {
double Kv = 3.02;
double Ka = 0.642;
Eigen::Matrix<double, 2, 2> A{{0, 1}, {0, -Kv / Ka}};
Eigen::Matrix<double, 2, 1> B{{0}, {1.0 / Ka}};
Eigen::Matrix<double, 2, 2> Q{{1, 0}, {0, 0.2}};
Eigen::Matrix<double, 1, 1> R{0.25};
Matrixd<2, 2> A{{0, 1}, {0, -Kv / Ka}};
Matrixd<2, 1> B{{0}, {1.0 / Ka}};
Matrixd<2, 2> Q{{1, 0}, {0, 0.2}};
Matrixd<1, 1> R{0.25};
// QR overload
Eigen::Matrix<double, 1, 2> K =
LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10);
EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
// QRN overload
Eigen::Matrix<double, 2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
Eigen::Matrix<double, 1, 2> Kimf =
GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
}

10
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -6,7 +6,7 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/SimpleMotorFeedforward.h"
#include "units/acceleration.h"
@@ -21,16 +21,16 @@ TEST(SimpleMotorFeedforwardTest, Calculate) {
double Ka = 0.6;
auto dt = 0.02_s;
Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
Matrixd<1, 1> A{-Kv / Ka};
Matrixd<1, 1> B{1.0 / Ka};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{
units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
units::volt_t{Ka} / 1_mps_sq};
Eigen::Vector<double, 1> r{2.0};
Eigen::Vector<double, 1> nextR{3.0};
Vectord<1> r{2.0};
Vectord<1> nextR{3.0};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);

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

@@ -6,11 +6,11 @@
#include <wpi/numbers>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/AngleStatistics.h"
TEST(AngleStatisticsTest, Mean) {
Eigen::Matrix<double, 3, 3> sigmas{
frc::Matrixd<3, 3> sigmas{
{1, 1.2, 0},
{359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
{1, 2, 0}};

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

@@ -7,8 +7,8 @@
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/NumericalJacobian.h"
@@ -18,8 +18,7 @@
namespace {
Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -41,7 +40,7 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return Eigen::Vector<double, 5>{
return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -50,16 +49,16 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
Eigen::Vector<double, 3> LocalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
return frc::Vectord<3>{x(2), x(3), x(4)};
}
Eigen::Vector<double, 5> GlobalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -71,7 +70,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
Eigen::Vector<double, 2> u{12.0, 12.0};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -98,14 +97,13 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
Eigen::Vector<double, 5>::Zero(),
Eigen::Vector<double, 2>::Zero());
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
observer.SetXhat(Eigen::Vector<double, 5>{
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -118,17 +116,15 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
Eigen::Vector<double, 5> nextR{
frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY =
LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
auto localY = LocalMeasurementModel(nextR, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);

12
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
View File

@@ -11,12 +11,10 @@ namespace {
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SigmaPoints(
Eigen::Vector<double, 2>{0.0, 0.0},
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points -
Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
(points - frc::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
@@ -24,12 +22,10 @@ TEST(MerweScaledSigmaPointsTest, ZeroMean) {
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SigmaPoints(
Eigen::Vector<double, 2>{1.0, 2.0},
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
frc::Vectord<2>{1.0, 2.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 10.0}});
EXPECT_TRUE(
(points -
Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
(points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}

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

@@ -7,8 +7,8 @@
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
@@ -20,8 +20,7 @@
namespace {
Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -43,7 +42,7 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return Eigen::Vector<double, 5>{
return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -52,16 +51,16 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
Eigen::Vector<double, 3> LocalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
return frc::Vectord<3>{x(2), x(3), x(4)};
}
Eigen::Vector<double, 5> GlobalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -73,7 +72,7 @@ TEST(UnscentedKalmanFilterTest, Init) {
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
Eigen::Vector<double, 2> u{12.0, 12.0};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -102,14 +101,13 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
Eigen::Vector<double, 5>::Zero(),
Eigen::Vector<double, 2>::Zero());
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
observer.SetXhat(Eigen::Vector<double, 5>{
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -124,17 +122,15 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
Eigen::Vector<double, 5> nextR{
frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY =
LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);

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);