Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +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

7
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -82,7 +82,7 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.to<double>();
.value();
if (error > maxError) {
maxError = error;
@@ -92,8 +92,7 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_NEAR(
0.0, errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
0.2);
EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
0.2);
EXPECT_NEAR(0.0, maxError, 0.4);
}

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

@@ -42,12 +42,12 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
auto v = 0.5 * (vl + vr);
return Eigen::Vector<double, 5>{
v.to<double>() * std::cos(x(2)), v.to<double>() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).to<double>(),
k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>()),
k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>())};
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()) +
k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
Eigen::Vector<double, 3> LocalMeasurementModel(
@@ -106,28 +106,27 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
Eigen::Vector<double, 2>::Zero());
observer.SetXhat(Eigen::Vector<double, 5>{
trajectory.InitialPose().Translation().X().to<double>(),
trajectory.InitialPose().Translation().Y().to<double>(),
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0});
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).to<double>());
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).to<double>());
ref.velocity * (1 + (ref.curvature * rb).value());
Eigen::Vector<double, 5> nextR{ref.pose.Translation().X().to<double>(),
ref.pose.Translation().Y().to<double>(),
ref.pose.Rotation().Radians().to<double>(),
vl.to<double>(), vr.to<double>()};
Eigen::Vector<double, 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());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.to<double>();
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
@@ -144,12 +143,12 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
observer.Xhat(0), 1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
observer.Xhat(1), 1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
observer.Xhat(2), 1.0);
ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
}

5
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -74,7 +74,7 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
wheelSpeeds);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.to<double>();
.value();
if (error > maxError) {
maxError = error;
@@ -84,7 +84,6 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
0.2);
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
EXPECT_LT(maxError, 0.4);
}

5
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -74,7 +74,7 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.to<double>();
.value();
if (error > maxError) {
maxError = error;
@@ -84,7 +84,6 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
0.2);
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
EXPECT_LT(maxError, 0.4);
}

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

@@ -44,12 +44,12 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
auto v = 0.5 * (vl + vr);
return Eigen::Vector<double, 5>{
v.to<double>() * std::cos(x(2)), v.to<double>() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).to<double>(),
k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>()),
k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>())};
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()) +
k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
Eigen::Vector<double, 3> LocalMeasurementModel(
@@ -110,30 +110,29 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
Eigen::Vector<double, 2>::Zero());
observer.SetXhat(Eigen::Vector<double, 5>{
trajectory.InitialPose().Translation().X().to<double>(),
trajectory.InitialPose().Translation().Y().to<double>(),
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0});
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
auto trueXhat = observer.Xhat();
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).to<double>());
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).to<double>());
ref.velocity * (1 + (ref.curvature * rb).value());
Eigen::Vector<double, 5> nextR{ref.pose.Translation().X().to<double>(),
ref.pose.Translation().Y().to<double>(),
ref.pose.Rotation().Radians().to<double>(),
vl.to<double>(), vr.to<double>()};
Eigen::Vector<double, 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());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.to<double>();
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
@@ -155,12 +154,12 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
observer.Xhat(0), 1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
observer.Xhat(1), 1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
observer.Xhat(2), 1.0);
ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
}