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

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