Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

Replace .to<double>() and .template to<double>() with .value() (#3667)

Browse Source 
It's a less verbose way to do the same thing.
This commit is contained in:
Tyler Veness
2021-10-25 08:58:12 -07:00
committed by GitHub
parent 6bc1db44bc
commit 181723e573
134 changed files with 782 additions and 826 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -21,7 +21,7 @@ template <int States>
void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA) {
*discA = (contA * dt.to<double>()).exp();
*discA = (contA * dt.value()).exp();
}
/**
@@ -42,8 +42,8 @@ void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
// Matrices are blocked here to minimize matrix exponentiation calculations
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.to<double>();
Mcont.template block<States, Inputs>(0, States) = contB * dt.to<double>();
Mcont.template block<States, States>(0, 0) = contA * dt.value();
Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
// Discretize A and B with the given timestep
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
@@ -76,8 +76,7 @@ void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
Eigen::Matrix<double, 2 * States, 2 * States> phi =
(M * dt.to<double>()).exp();
Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
@@ -122,7 +121,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Eigen::Matrix<double, States, States> lastTerm = Q;
double lastCoeff = dt.to<double>();
double lastCoeff = dt.value();
// Aᵀⁿ
Eigen::Matrix<double, States, States> Atn = contA.transpose();
@@ -132,7 +131,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
lastTerm = -contA * lastTerm + Q * Atn;
lastCoeff *= dt.to<double>() / static_cast<double>(i);
lastCoeff *= dt.value() / static_cast<double>(i);
phi12 += lastTerm * lastCoeff;
@@ -156,7 +155,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
template <int Outputs>
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
return R / dt.to<double>();
return R / dt.value();
}
} // namespace frc

5
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -50,8 +50,7 @@ class LinearSystemLoop {
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
return frc::NormalizeInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -97,7 +96,7 @@ class LinearSystemLoop {
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
u, maxVoltage.value());
}) {}
/**

16
wpimath/src/main/native/include/frc/system/NumericalIntegration.h
View File

@@ -23,7 +23,7 @@ namespace frc {
*/
template <typename F, typename T>
T RK4(F&& f, T x, units::second_t dt) {
const auto h = dt.to<double>();
const auto h = dt.value();
T k1 = f(x);
T k2 = f(x + h * 0.5 * k1);
@@ -43,7 +43,7 @@ T RK4(F&& f, T x, units::second_t dt) {
*/
template <typename F, typename T, typename U>
T RK4(F&& f, T x, U u, units::second_t dt) {
const auto h = dt.to<double>();
const auto h = dt.value();
T k1 = f(x, u);
T k2 = f(x + h * 0.5 * k1, u);
@@ -91,13 +91,13 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
double truncationError;
double dtElapsed = 0.0;
double h = dt.to<double>();
double h = dt.value();
// Loop until we've gotten to our desired dt
while (dtElapsed < dt.to<double>()) {
while (dtElapsed < dt.value()) {
do {
// Only allow us to advance up to the dt remaining
h = std::min(h, dt.to<double>() - dtElapsed);
h = std::min(h, dt.value() - dtElapsed);
// Notice how the derivative in the Wikipedia notation is dy/dx.
// That means their y is our x and their x is our t
@@ -167,13 +167,13 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
double truncationError;
double dtElapsed = 0.0;
double h = dt.to<double>();
double h = dt.value();
// Loop until we've gotten to our desired dt
while (dtElapsed < dt.to<double>()) {
while (dtElapsed < dt.value()) {
do {
// Only allow us to advance up to the dt remaining
h = std::min(h, dt.to<double>() - dtElapsed);
h = std::min(h, dt.value() - dtElapsed);
// clang-format off
T k1 = f(x, u);

55
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -47,9 +47,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.to<double>()}};
Eigen::Matrix<double, 2, 1> B{
0.0, (G * motor.Kt / (motor.R * r * m)).to<double>()};
.value()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * r * m)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -71,10 +71,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
DCMotor motor, units::kilogram_square_meter_t J, double G) {
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0,
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * J)).to<double>()};
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -107,9 +105,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
Eigen::Matrix<double, 1, 1> A{-kV.template to<double>() /
kA.template to<double>()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.template to<double>()};
Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -142,10 +139,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
static LinearSystem<2, 1, 1> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0, -kV.template to<double>() / kA.template to<double>()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.template to<double>()};
Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -170,12 +165,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
double A1 = -(kVlinear.to<double>() / kAlinear.to<double>() +
kVangular.to<double>() / kAangular.to<double>());
double A2 = -(kVlinear.to<double>() / kAlinear.to<double>() -
kVangular.to<double>() / kAangular.to<double>());
double B1 = 1.0 / kAlinear.to<double>() + 1.0 / kAangular.to<double>();
double B2 = 1.0 / kAlinear.to<double>() - 1.0 / kAangular.to<double>();
double A1 = -(kVlinear.value() / kAlinear.value() +
kVangular.value() / kAangular.value());
double A2 = -(kVlinear.value() / kAlinear.value() -
kVangular.value() / kAangular.value());
double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
Eigen::Matrix<double, 2, 2> A =
0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
@@ -239,8 +234,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
units::kilogram_square_meter_t J,
double G) {
Eigen::Matrix<double, 1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).to<double>()};
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
@@ -269,15 +264,15 @@ class WPILIB_DLLEXPORT LinearSystemId {
auto C2 = G * motor.Kt / (motor.R * r);
Eigen::Matrix<double, 2, 2> A{
{((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>()}};
{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Eigen::Matrix<double, 2, 2> B{
{((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>()}};
{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};