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

[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

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