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

[wpimath] 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

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