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

6
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -6,11 +6,9 @@
#include "gtest/gtest.h"
#include "units/angle.h"
#define EXPECT_UNITS_EQ(a, b) \
EXPECT_DOUBLE_EQ((a).to<double>(), (b).to<double>())
#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
#define EXPECT_UNITS_NEAR(a, b, c) \
EXPECT_NEAR((a).to<double>(), (b).to<double>(), c)
#define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c)
TEST(MathUtilTest, ApplyDeadband) {
// < 0

186
wpimath/src/test/native/cpp/UnitsTest.cpp
View File

@@ -1324,7 +1324,7 @@ TEST_F(UnitContainer, scalarTypeImplicitConversion) {
}
TEST_F(UnitContainer, valueMethod) {
double test = meter_t(3.0).to<double>();
double test = meter_t(3.0).value();
EXPECT_DOUBLE_EQ(3.0, test);
auto test2 = meter_t(4.0).value();
@@ -1333,7 +1333,7 @@ TEST_F(UnitContainer, valueMethod) {
}
TEST_F(UnitContainer, convertMethod) {
double test = meter_t(3.0).convert<feet>().to<double>();
double test = meter_t(3.0).convert<feet>().value();
EXPECT_NEAR(9.84252, test, 5.0e-6);
}
@@ -1562,13 +1562,13 @@ TEST_F(UnitContainer, nameAndAbbreviation) {
TEST_F(UnitContainer, negative) {
meter_t a(5.3);
meter_t b(-5.3);
EXPECT_NEAR(a.to<double>(), -b.to<double>(), 5.0e-320);
EXPECT_NEAR(b.to<double>(), -a.to<double>(), 5.0e-320);
EXPECT_NEAR(a.value(), -b.value(), 5.0e-320);
EXPECT_NEAR(b.value(), -a.value(), 5.0e-320);
dB_t c(2.87);
dB_t d(-2.87);
EXPECT_NEAR(c.to<double>(), -d.to<double>(), 5.0e-320);
EXPECT_NEAR(d.to<double>(), -c.to<double>(), 5.0e-320);
EXPECT_NEAR(c.value(), -d.value(), 5.0e-320);
EXPECT_NEAR(d.value(), -c.value(), 5.0e-320);
ppm_t e = -1 * ppm_t(10);
EXPECT_EQ(e, -ppm_t(10));
@@ -1579,7 +1579,7 @@ TEST_F(UnitContainer, concentration) {
ppb_t a(ppm_t(1));
EXPECT_EQ(ppb_t(1000), a);
EXPECT_EQ(0.000001, a);
EXPECT_EQ(0.000001, a.to<double>());
EXPECT_EQ(0.000001, a.value());
scalar_t b(ppm_t(1));
EXPECT_EQ(0.000001, b);
@@ -1789,7 +1789,7 @@ TEST_F(UnitConversion, time) {
year_t twoYears(2.0);
week_t twoYearsInWeeks = twoYears;
EXPECT_NEAR(week_t(104.286).to<double>(), twoYearsInWeeks.to<double>(),
EXPECT_NEAR(week_t(104.286).value(), twoYearsInWeeks.value(),
5.0e-4);
double test;
@@ -1826,8 +1826,8 @@ TEST_F(UnitConversion, time) {
TEST_F(UnitConversion, angle) {
angle::degree_t quarterCircleDeg(90.0);
angle::radian_t quarterCircleRad = quarterCircleDeg;
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).to<double>(),
quarterCircleRad.to<double>(), 5.0e-12);
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).value(),
quarterCircleRad.value(), 5.0e-12);
double test;
@@ -2631,7 +2631,7 @@ TEST_F(UnitConversion, pi) {
EXPECT_TRUE(constants::pi < 4.0);
// explicit conversion
EXPECT_NEAR(3.14159, constants::pi.to<double>(), 5.0e-6);
EXPECT_NEAR(3.14159, constants::pi.value(), 5.0e-6);
// auto multiplication
EXPECT_TRUE(
@@ -2640,16 +2640,16 @@ TEST_F(UnitConversion, pi) {
(std::is_same<meter_t, decltype(meter_t(1) * constants::pi)>::value));
EXPECT_NEAR(constants::detail::PI_VAL,
(constants::pi * meter_t(1)).to<double>(), 5.0e-10);
(constants::pi * meter_t(1)).value(), 5.0e-10);
EXPECT_NEAR(constants::detail::PI_VAL,
(meter_t(1) * constants::pi).to<double>(), 5.0e-10);
(meter_t(1) * constants::pi).value(), 5.0e-10);
// explicit multiplication
meter_t a = constants::pi * meter_t(1);
meter_t b = meter_t(1) * constants::pi;
EXPECT_NEAR(constants::detail::PI_VAL, a.to<double>(), 5.0e-10);
EXPECT_NEAR(constants::detail::PI_VAL, b.to<double>(), 5.0e-10);
EXPECT_NEAR(constants::detail::PI_VAL, a.value(), 5.0e-10);
EXPECT_NEAR(constants::detail::PI_VAL, b.value(), 5.0e-10);
// auto division
EXPECT_TRUE(
@@ -2658,16 +2658,16 @@ TEST_F(UnitConversion, pi) {
(std::is_same<second_t, decltype(second_t(1) / constants::pi)>::value));
EXPECT_NEAR(constants::detail::PI_VAL,
(constants::pi / second_t(1)).to<double>(), 5.0e-10);
(constants::pi / second_t(1)).value(), 5.0e-10);
EXPECT_NEAR(1.0 / constants::detail::PI_VAL,
(second_t(1) / constants::pi).to<double>(), 5.0e-10);
(second_t(1) / constants::pi).value(), 5.0e-10);
// explicit
hertz_t c = constants::pi / second_t(1);
second_t d = second_t(1) / constants::pi;
EXPECT_NEAR(constants::detail::PI_VAL, c.to<double>(), 5.0e-10);
EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.to<double>(), 5.0e-10);
EXPECT_NEAR(constants::detail::PI_VAL, c.value(), 5.0e-10);
EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.value(), 5.0e-10);
}
TEST_F(UnitConversion, constants) {
@@ -2774,12 +2774,12 @@ TEST_F(UnitMath, acos) {
(std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(acos(scalar_t(0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(2).to<double>(),
acos(scalar_t(-0.41614683654)).to<double>(), 5.0e-11);
EXPECT_NEAR(angle::radian_t(2).value(),
acos(scalar_t(-0.41614683654)).value(), 5.0e-11);
EXPECT_NEAR(
angle::degree_t(135).to<double>(),
angle::degree_t(135).value(),
angle::degree_t(acos(scalar_t(-0.70710678118654752440084436210485)))
.to<double>(),
.value(),
5.0e-12);
}
@@ -2788,12 +2788,12 @@ TEST_F(UnitMath, asin) {
(std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(asin(scalar_t(0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(1.14159265).to<double>(),
asin(scalar_t(0.90929742682)).to<double>(), 5.0e-9);
EXPECT_NEAR(angle::radian_t(1.14159265).value(),
asin(scalar_t(0.90929742682)).value(), 5.0e-9);
EXPECT_NEAR(
angle::degree_t(45).to<double>(),
angle::degree_t(45).value(),
angle::degree_t(asin(scalar_t(0.70710678118654752440084436210485)))
.to<double>(),
.value(),
5.0e-12);
}
@@ -2802,32 +2802,32 @@ TEST_F(UnitMath, atan) {
(std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(atan(scalar_t(0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(-1.14159265).to<double>(),
atan(scalar_t(-2.18503986326)).to<double>(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(-45).to<double>(),
angle::degree_t(atan(scalar_t(-1.0))).to<double>(), 5.0e-12);
EXPECT_NEAR(angle::radian_t(-1.14159265).value(),
atan(scalar_t(-2.18503986326)).value(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(-45).value(),
angle::degree_t(atan(scalar_t(-1.0))).value(), 5.0e-12);
}
TEST_F(UnitMath, atan2) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(atan2(
scalar_t(1), scalar_t(1)))>::type>::value));
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).to<double>(),
atan2(scalar_t(2), scalar_t(2)).to<double>(), 5.0e-12);
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).value(),
atan2(scalar_t(2), scalar_t(2)).value(), 5.0e-12);
EXPECT_NEAR(
angle::degree_t(45).to<double>(),
angle::degree_t(atan2(scalar_t(2), scalar_t(2))).to<double>(),
angle::degree_t(45).value(),
angle::degree_t(atan2(scalar_t(2), scalar_t(2))).value(),
5.0e-12);
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(atan2(
scalar_t(1), scalar_t(1)))>::type>::value));
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).to<double>(),
atan2(scalar_t(1), scalar_t(std::sqrt(3))).to<double>(),
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).value(),
atan2(scalar_t(1), scalar_t(std::sqrt(3))).value(),
5.0e-12);
EXPECT_NEAR(angle::degree_t(30).to<double>(),
EXPECT_NEAR(angle::degree_t(30).value(),
angle::degree_t(atan2(scalar_t(1), scalar_t(std::sqrt(3))))
.to<double>(),
.value(),
5.0e-12);
}
@@ -2859,30 +2859,30 @@ TEST_F(UnitMath, acosh) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
acosh(scalar_t(0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(1.316957896924817).to<double>(),
acosh(scalar_t(2.0)).to<double>(), 5.0e-11);
EXPECT_NEAR(angle::degree_t(75.456129290216893).to<double>(),
angle::degree_t(acosh(scalar_t(2.0))).to<double>(), 5.0e-12);
EXPECT_NEAR(angle::radian_t(1.316957896924817).value(),
acosh(scalar_t(2.0)).value(), 5.0e-11);
EXPECT_NEAR(angle::degree_t(75.456129290216893).value(),
angle::degree_t(acosh(scalar_t(2.0))).value(), 5.0e-12);
}
TEST_F(UnitMath, asinh) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
asinh(scalar_t(0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(1.443635475178810).to<double>(),
asinh(scalar_t(2)).to<double>(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(82.714219883108939).to<double>(),
angle::degree_t(asinh(scalar_t(2))).to<double>(), 5.0e-12);
EXPECT_NEAR(angle::radian_t(1.443635475178810).value(),
asinh(scalar_t(2)).value(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(82.714219883108939).value(),
angle::degree_t(asinh(scalar_t(2))).value(), 5.0e-12);
}
TEST_F(UnitMath, atanh) {
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
atanh(scalar_t(0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(0.549306144334055).to<double>(),
atanh(scalar_t(0.5)).to<double>(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(31.472923730945389).to<double>(),
angle::degree_t(atanh(scalar_t(0.5))).to<double>(), 5.0e-12);
EXPECT_NEAR(angle::radian_t(0.549306144334055).value(),
atanh(scalar_t(0.5)).value(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(31.472923730945389).value(),
angle::degree_t(atanh(scalar_t(0.5))).value(), 5.0e-12);
}
TEST_F(UnitMath, exp) {
@@ -2954,14 +2954,14 @@ TEST_F(UnitMath, sqrt) {
EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
typename std::decay<decltype(sqrt(
square_meter_t(4.0)))>::type>::value));
EXPECT_NEAR(meter_t(2.0).to<double>(),
sqrt(square_meter_t(4.0)).to<double>(), 5.0e-9);
EXPECT_NEAR(meter_t(2.0).value(),
sqrt(square_meter_t(4.0)).value(), 5.0e-9);
EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(
sqrt(steradian_t(16.0)))>::type>::value));
EXPECT_NEAR(angle::radian_t(4.0).to<double>(),
sqrt(steradian_t(16.0)).to<double>(), 5.0e-9);
EXPECT_NEAR(angle::radian_t(4.0).value(),
sqrt(steradian_t(16.0)).value(), 5.0e-9);
EXPECT_TRUE((std::is_convertible<typename std::decay<foot_t>::type,
typename std::decay<decltype(sqrt(
@@ -2970,9 +2970,9 @@ TEST_F(UnitMath, sqrt) {
// for rational conversion (i.e. no integral root) let's check a bunch of
// different ways this could go wrong
foot_t resultFt = sqrt(square_foot_t(10.0));
EXPECT_NEAR(foot_t(3.16227766017).to<double>(),
sqrt(square_foot_t(10.0)).to<double>(), 5.0e-9);
EXPECT_NEAR(foot_t(3.16227766017).to<double>(), resultFt.to<double>(),
EXPECT_NEAR(foot_t(3.16227766017).value(),
sqrt(square_foot_t(10.0)).value(), 5.0e-9);
EXPECT_NEAR(foot_t(3.16227766017).value(), resultFt.value(),
5.0e-9);
EXPECT_EQ(resultFt, sqrt(square_foot_t(10.0)));
}
@@ -2981,19 +2981,19 @@ TEST_F(UnitMath, hypot) {
EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
typename std::decay<decltype(hypot(
meter_t(3.0), meter_t(4.0)))>::type>::value));
EXPECT_NEAR(meter_t(5.0).to<double>(),
(hypot(meter_t(3.0), meter_t(4.0))).to<double>(), 5.0e-9);
EXPECT_NEAR(meter_t(5.0).value(),
(hypot(meter_t(3.0), meter_t(4.0))).value(), 5.0e-9);
EXPECT_TRUE((std::is_same<typename std::decay<foot_t>::type,
typename std::decay<decltype(hypot(
foot_t(3.0), meter_t(1.2192)))>::type>::value));
EXPECT_NEAR(foot_t(5.0).to<double>(),
(hypot(foot_t(3.0), meter_t(1.2192))).to<double>(), 5.0e-9);
EXPECT_NEAR(foot_t(5.0).value(),
(hypot(foot_t(3.0), meter_t(1.2192))).value(), 5.0e-9);
}
TEST_F(UnitMath, ceil) {
double val = 101.1;
EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).to<double>());
EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).value());
EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
typename std::decay<decltype(
ceil(meter_t(val)))>::type>::value));
@@ -3006,7 +3006,7 @@ TEST_F(UnitMath, floor) {
TEST_F(UnitMath, fmod) {
EXPECT_EQ(std::fmod(100.0, 101.2),
fmod(meter_t(100.0), meter_t(101.2)).to<double>());
fmod(meter_t(100.0), meter_t(101.2)).value());
}
TEST_F(UnitMath, trunc) {
@@ -3029,8 +3029,8 @@ TEST_F(UnitMath, copysign) {
TEST_F(UnitMath, fdim) {
EXPECT_EQ(meter_t(0.0), fdim(meter_t(8.0), meter_t(10.0)));
EXPECT_EQ(meter_t(2.0), fdim(meter_t(10.0), meter_t(8.0)));
EXPECT_NEAR(meter_t(9.3904).to<double>(),
fdim(meter_t(10.0), foot_t(2.0)).to<double>(),
EXPECT_NEAR(meter_t(9.3904).value(),
fdim(meter_t(10.0), foot_t(2.0)).value(),
5.0e-320); // not sure why they aren't comparing exactly equal,
// but clearly they are.
}
@@ -3200,7 +3200,7 @@ TEST_F(CompileTimeArithmetic, unit_value_add) {
EXPECT_TRUE((
std::is_same<typename std::decay<foot_t>::type,
typename std::decay<decltype(sumf::value())>::type>::value));
EXPECT_NEAR(5.92125984, sumf::value().to<double>(), 5.0e-8);
EXPECT_NEAR(5.92125984, sumf::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, sumf>::value));
@@ -3211,7 +3211,7 @@ TEST_F(CompileTimeArithmetic, unit_value_add) {
EXPECT_TRUE((
std::is_same<typename std::decay<celsius_t>::type,
typename std::decay<decltype(sumc::value())>::type>::value));
EXPECT_NEAR(2.11111111111, sumc::value().to<double>(), 5.0e-8);
EXPECT_NEAR(2.11111111111, sumc::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
sumc>::value));
@@ -3222,7 +3222,7 @@ TEST_F(CompileTimeArithmetic, unit_value_add) {
EXPECT_TRUE((
std::is_same<typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(sumr::value())>::type>::value));
EXPECT_NEAR(1.05235988, sumr::value().to<double>(), 5.0e-8);
EXPECT_NEAR(1.05235988, sumr::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::angle_unit, sumr>::value));
}
@@ -3241,7 +3241,7 @@ TEST_F(CompileTimeArithmetic, unit_value_subtract) {
EXPECT_TRUE((std::is_same<
typename std::decay<foot_t>::type,
typename std::decay<decltype(difff::value())>::type>::value));
EXPECT_NEAR(-3.92125984, difff::value().to<double>(), 5.0e-8);
EXPECT_NEAR(-3.92125984, difff::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, difff>::value));
@@ -3252,7 +3252,7 @@ TEST_F(CompileTimeArithmetic, unit_value_subtract) {
EXPECT_TRUE((std::is_same<
typename std::decay<celsius_t>::type,
typename std::decay<decltype(diffc::value())>::type>::value));
EXPECT_NEAR(-0.11111111111, diffc::value().to<double>(), 5.0e-8);
EXPECT_NEAR(-0.11111111111, diffc::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
diffc>::value));
@@ -3263,7 +3263,7 @@ TEST_F(CompileTimeArithmetic, unit_value_subtract) {
EXPECT_TRUE((std::is_same<
typename std::decay<angle::radian_t>::type,
typename std::decay<decltype(diffr::value())>::type>::value));
EXPECT_NEAR(0.947640122, diffr::value().to<double>(), 5.0e-8);
EXPECT_NEAR(0.947640122, diffr::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::angle_unit, diffr>::value));
}
@@ -3282,7 +3282,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) {
EXPECT_TRUE((std::is_same<
typename std::decay<square_meter_t>::type,
typename std::decay<decltype(productM::value())>::type>::value));
EXPECT_NEAR(4.0, productM::value().to<double>(), 5.0e-7);
EXPECT_NEAR(4.0, productM::value().value(), 5.0e-7);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::area_unit, productM>::value));
@@ -3290,7 +3290,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) {
EXPECT_TRUE((std::is_same<
typename std::decay<square_foot_t>::type,
typename std::decay<decltype(productF::value())>::type>::value));
EXPECT_NEAR(43.0556444224, productF::value().to<double>(), 5.0e-6);
EXPECT_NEAR(43.0556444224, productF::value().value(), 5.0e-6);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::area_unit, productF>::value));
@@ -3299,7 +3299,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) {
(std::is_same<
typename std::decay<square_foot_t>::type,
typename std::decay<decltype(productF2::value())>::type>::value));
EXPECT_NEAR(43.0556444224, productF2::value().to<double>(), 5.0e-8);
EXPECT_NEAR(43.0556444224, productF2::value().value(), 5.0e-8);
EXPECT_TRUE((
traits::is_unit_value_t_category<category::area_unit, productF2>::value));
@@ -3313,8 +3313,8 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) {
EXPECT_TRUE((std::is_convertible<
typename std::decay<torque::newton_meter_t>::type,
typename std::decay<decltype(productN::value())>::type>::value));
EXPECT_NEAR(32.8084, productN::value().to<double>(), 5.0e-8);
EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().to<double>()),
EXPECT_NEAR(32.8084, productN::value().value(), 5.0e-8);
EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().value()),
5.0e-7);
EXPECT_TRUE((traits::is_unit_value_t_category<category::torque_unit,
productN>::value));
@@ -3326,9 +3326,9 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) {
EXPECT_TRUE((std::is_same<
typename std::decay<steradian_t>::type,
typename std::decay<decltype(productR::value())>::type>::value));
EXPECT_NEAR(2.42, productR::value().to<double>(), 5.0e-8);
EXPECT_NEAR(2.42, productR::value().value(), 5.0e-8);
EXPECT_NEAR(7944.39137,
(productR::value().convert<degrees_squared>().to<double>()),
(productR::value().convert<degrees_squared>().value()),
5.0e-6);
EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
productR>::value));
@@ -3348,7 +3348,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) {
EXPECT_TRUE((std::is_same<
typename std::decay<scalar_t>::type,
typename std::decay<decltype(productM::value())>::type>::value));
EXPECT_NEAR(1, productM::value().to<double>(), 5.0e-7);
EXPECT_NEAR(1, productM::value().value(), 5.0e-7);
EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
productM>::value));
@@ -3356,7 +3356,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) {
EXPECT_TRUE((std::is_same<
typename std::decay<scalar_t>::type,
typename std::decay<decltype(productF::value())>::type>::value));
EXPECT_NEAR(1.0, productF::value().to<double>(), 5.0e-6);
EXPECT_NEAR(1.0, productF::value().value(), 5.0e-6);
EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
productF>::value));
@@ -3365,7 +3365,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) {
(std::is_same<
typename std::decay<scalar_t>::type,
typename std::decay<decltype(productF2::value())>::type>::value));
EXPECT_NEAR(1.0, productF2::value().to<double>(), 5.0e-8);
EXPECT_NEAR(1.0, productF2::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
productF2>::value));
@@ -3376,7 +3376,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) {
(std::is_same<
typename std::decay<meters_per_second_t>::type,
typename std::decay<decltype(productMS::value())>::type>::value));
EXPECT_NEAR(0.2, productMS::value().to<double>(), 5.0e-8);
EXPECT_NEAR(0.2, productMS::value().value(), 5.0e-8);
EXPECT_TRUE((traits::is_unit_value_t_category<category::velocity_unit,
productMS>::value));
@@ -3387,9 +3387,9 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) {
(std::is_same<
typename std::decay<radians_per_second_t>::type,
typename std::decay<decltype(productRS::value())>::type>::value));
EXPECT_NEAR(2, productRS::value().to<double>(), 5.0e-8);
EXPECT_NEAR(2, productRS::value().value(), 5.0e-8);
EXPECT_NEAR(114.592,
(productRS::value().convert<degrees_per_second>().to<double>()),
(productRS::value().convert<degrees_per_second>().value()),
5.0e-4);
EXPECT_TRUE((traits::is_unit_value_t_category<category::angular_velocity_unit,
productRS>::value));
@@ -3402,7 +3402,7 @@ TEST_F(CompileTimeArithmetic, unit_value_power) {
EXPECT_TRUE((std::is_convertible<
typename std::decay<square_meter_t>::type,
typename std::decay<decltype(sq::value())>::type>::value));
EXPECT_NEAR(4, sq::value().to<double>(), 5.0e-8);
EXPECT_NEAR(4, sq::value().value(), 5.0e-8);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::area_unit, sq>::value));
@@ -3412,9 +3412,9 @@ TEST_F(CompileTimeArithmetic, unit_value_power) {
EXPECT_TRUE((std::is_convertible<
typename std::decay<steradian_t>::type,
typename std::decay<decltype(sqr::value())>::type>::value));
EXPECT_NEAR(3.24, sqr::value().to<double>(), 5.0e-8);
EXPECT_NEAR(3.24, sqr::value().value(), 5.0e-8);
EXPECT_NEAR(10636.292574038049895092690529904,
(sqr::value().convert<degrees_squared>().to<double>()), 5.0e-10);
(sqr::value().convert<degrees_squared>().value()), 5.0e-10);
EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
sqr>::value));
}
@@ -3426,7 +3426,7 @@ TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
EXPECT_TRUE((std::is_convertible<
typename std::decay<meter_t>::type,
typename std::decay<decltype(root::value())>::type>::value));
EXPECT_NEAR(3.16227766017, root::value().to<double>(), 5.0e-9);
EXPECT_NEAR(3.16227766017, root::value().value(), 5.0e-9);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, root>::value));
@@ -3436,8 +3436,8 @@ TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
EXPECT_TRUE((std::is_convertible<
typename std::decay<mile_t>::type,
typename std::decay<decltype(rooth::value())>::type>::value));
EXPECT_NEAR(2.69920623253, rooth::value().to<double>(), 5.0e-8);
EXPECT_NEAR(269.920623, rooth::value().convert<meters>().to<double>(),
EXPECT_NEAR(2.69920623253, rooth::value().value(), 5.0e-8);
EXPECT_NEAR(269.920623, rooth::value().convert<meters>().value(),
5.0e-6);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::length_unit, rooth>::value));
@@ -3446,9 +3446,9 @@ TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
using rootr = unit_value_sqrt<rRatio>;
EXPECT_TRUE((traits::is_angle_unit<decltype(rootr::value())>::value));
EXPECT_NEAR(1.3416407865, rootr::value().to<double>(), 5.0e-8);
EXPECT_NEAR(1.3416407865, rootr::value().value(), 5.0e-8);
EXPECT_NEAR(76.870352574,
rootr::value().convert<angle::degrees>().to<double>(), 5.0e-6);
rootr::value().convert<angle::degrees>().value(), 5.0e-6);
EXPECT_TRUE(
(traits::is_unit_value_t_category<category::angle_unit, rootr>::value));
}

4
wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

@@ -37,7 +37,7 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
constexpr auto kDt = 0.02_s;
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
@@ -62,5 +62,5 @@ TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
frc::ChassisSpeeds speeds = controller.Calculate(
frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.to<double>());
EXPECT_EQ(0, speeds.omega.value());
}

2
wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
View File

@@ -38,7 +38,7 @@ TEST_F(PIDInputOutputTest, IntegralGainOutput) {
out = controller->Calculate(0.025, 0);
}
EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out);
}
TEST_F(PIDInputOutputTest, DerivativeGainOutput) {

2
wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
View File

@@ -104,7 +104,7 @@ TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutput) {
out = controller->Calculate(0.025_deg, 0_deg);
}
EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out);
}
TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutput) {

2
wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -26,7 +26,7 @@ TEST(RamseteControllerTest, ReachesReference) {
constexpr auto kDt = 0.02_s;
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
auto [vx, vy, omega] = controller.Calculate(robotPose, state);
static_cast<void>(vy);

6
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -33,14 +33,14 @@ TEST(SimpleMotorFeedforwardTest, Calculate) {
Eigen::Vector<double, 1> nextR{3.0};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).to<double>(), 0.002);
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).to<double>(), 0.002);
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 1_mps / dt).to<double>(), 2.0);
simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0);
}
} // namespace frc

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

2
wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
View File

@@ -52,7 +52,7 @@ TEST_P(LinearFilterNoiseTest, NoiseReduce) {
std::normal_distribution<double> distr{0.0, 10.0};
for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
double theory = GetData(t.to<double>());
double theory = GetData(t.value());
double noise = distr(gen);
filterError += std::abs(m_filter.Calculate(theory + noise) - theory);
noiseGenError += std::abs(noise - theory);

13
wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
View File

@@ -106,7 +106,7 @@ class LinearFilterOutputTest
TEST_P(LinearFilterOutputTest, Output) {
double filterOutput = 0.0;
for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
filterOutput = m_filter.Calculate(m_data(t.to<double>()));
filterOutput = m_filter.Calculate(m_data(t.value()));
}
RecordProperty("LinearFilterOutput", filterOutput);
@@ -126,18 +126,17 @@ void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
h);
for (int i = min / h.to<double>(); i < max / h.to<double>(); ++i) {
for (int i = min / h.value(); i < max / h.value(); ++i) {
// Let filter initialize
if (i < static_cast<int>(min / h.to<double>()) + Samples) {
filter.Calculate(f(i * h.to<double>()));
if (i < static_cast<int>(min / h.value()) + Samples) {
filter.Calculate(f(i * h.value()));
continue;
}
// The order of accuracy is O(h^(N - d)) where N is number of stencil
// points and d is order of derivative
EXPECT_NEAR(dfdx(i * h.to<double>()),
filter.Calculate(f(i * h.to<double>())),
10.0 * std::pow(h.to<double>(), Samples - Derivative));
EXPECT_NEAR(dfdx(i * h.value()), filter.Calculate(f(i * h.value())),
10.0 * std::pow(h.value(), Samples - Derivative));
}
}

18
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -17,9 +17,9 @@ TEST(Pose2dTest, TransformBy) {
const auto transformed = initial + transform;
EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
}
TEST(Pose2dTest, RelativeTo) {
@@ -28,10 +28,10 @@ TEST(Pose2dTest, RelativeTo) {
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
kEpsilon);
}
@@ -53,7 +53,7 @@ TEST(Pose2dTest, Minus) {
const auto transform = final - initial;
EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
}

16
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -17,38 +17,38 @@ TEST(Rotation2dTest, RadiansToDegrees) {
const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
EXPECT_NEAR(rot1.Degrees().to<double>(), 60.0, kEpsilon);
EXPECT_NEAR(rot2.Degrees().to<double>(), 45.0, kEpsilon);
EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
}
TEST(Rotation2dTest, DegreesToRadians) {
const auto rot1 = Rotation2d(45.0_deg);
const auto rot2 = Rotation2d(30.0_deg);
EXPECT_NEAR(rot1.Radians().to<double>(), wpi::numbers::pi / 4.0, kEpsilon);
EXPECT_NEAR(rot2.Radians().to<double>(), wpi::numbers::pi / 6.0, kEpsilon);
EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
auto sum = zero + Rotation2d(90.0_deg);
EXPECT_NEAR(sum.Radians().to<double>(), wpi::numbers::pi / 2.0, kEpsilon);
EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByNonZero) {
auto rot = Rotation2d(90.0_deg);
rot = rot + Rotation2d(30.0_deg);
EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
}
TEST(Rotation2dTest, Minus) {
const auto rot1 = Rotation2d(70.0_deg);
const auto rot2 = Rotation2d(30.0_deg);
EXPECT_NEAR((rot1 - rot2).Degrees().to<double>(), 40.0, kEpsilon);
EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
}
TEST(Rotation2dTest, Equality) {

22
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -21,12 +21,10 @@ TEST(Transform2dTest, Inverse) {
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
kEpsilon);
EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
kEpsilon);
EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
untransformed.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
EXPECT_NEAR(initial.Rotation().Degrees().value(),
untransformed.Rotation().Degrees().value(), kEpsilon);
}
TEST(Transform2dTest, Composition) {
@@ -37,10 +35,10 @@ TEST(Transform2dTest, Composition) {
auto transformedSeparate = initial + transform1 + transform2;
auto transformedCombined = initial + (transform1 + transform2);
EXPECT_NEAR(transformedSeparate.X().to<double>(),
transformedCombined.X().to<double>(), kEpsilon);
EXPECT_NEAR(transformedSeparate.Y().to<double>(),
transformedCombined.Y().to<double>(), kEpsilon);
EXPECT_NEAR(transformedSeparate.Rotation().Degrees().to<double>(),
transformedCombined.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
kEpsilon);
EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
kEpsilon);
EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
transformedCombined.Rotation().Degrees().value(), kEpsilon);
}

36
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -17,8 +17,8 @@ TEST(Translation2dTest, Sum) {
const auto sum = one + two;
EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
}
TEST(Translation2dTest, Difference) {
@@ -27,51 +27,51 @@ TEST(Translation2dTest, Difference) {
const auto difference = one - two;
EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
}
TEST(Translation2dTest, RotateBy) {
const Translation2d another{3.0_m, 0.0_m};
const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon);
}
TEST(Translation2dTest, Multiplication) {
const Translation2d original{3.0_m, 5.0_m};
const auto mult = original * 3;
EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
}
TEST(Translation2dTest, Division) {
const Translation2d original{3.0_m, 5.0_m};
const auto div = original / 2;
EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
}
TEST(Translation2dTest, Norm) {
const Translation2d one{3.0_m, 5.0_m};
EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon);
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon);
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
}
TEST(Translation2dTest, Equality) {
@@ -88,10 +88,10 @@ TEST(Translation2dTest, Inequality) {
TEST(Translation2dTest, PolarConstructor) {
Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
EXPECT_NEAR(one.X().to<double>(), 1.0, kEpsilon);
EXPECT_NEAR(one.Y().to<double>(), 1.0, kEpsilon);
EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
Translation2d two{2_m, Rotation2d(60_deg)};
EXPECT_NEAR(two.X().to<double>(), 1.0, kEpsilon);
EXPECT_NEAR(two.Y().to<double>(), std::sqrt(3.0), kEpsilon);
EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
}

25
wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -17,9 +17,9 @@ TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
}
TEST(Twist2dTest, QuarterCircle) {
@@ -27,19 +27,18 @@ TEST(Twist2dTest, QuarterCircle) {
units::radian_t(wpi::numbers::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
kEpsilon);
EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
EXPECT_NEAR(diagonalPose.X().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
}
TEST(Twist2dTest, Equality) {
@@ -60,7 +59,7 @@ TEST(Twist2dTest, Pose2dLog) {
const auto twist = start.Log(end);
EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(twist.dtheta.to<double>(), wpi::numbers::pi / 2.0, kEpsilon);
EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
}

6
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -11,7 +11,7 @@ TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}

32
wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
View File

@@ -20,8 +20,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) {
const ChassisSpeeds chassisSpeeds;
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
@@ -29,9 +29,9 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
const DifferentialDriveWheelSpeeds wheelSpeeds;
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
@@ -39,8 +39,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
@@ -48,9 +48,9 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
@@ -59,10 +59,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::numbers::pi,
kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::numbers::pi,
kEpsilon);
EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
@@ -72,7 +70,7 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon);
}

6
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -18,7 +18,7 @@ TEST(DifferentialDriveOdometryTest, EncoderDistances) {
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
units::meter_t(5 * wpi::numbers::pi));
EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon);
}

92
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -25,38 +25,38 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(5.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
@@ -64,10 +64,10 @@ TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
units::radians_per_second_t(2 * wpi::numbers::pi)};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
@@ -75,19 +75,19 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
-150.79644737_mps, 150.79644737_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
@@ -96,19 +96,19 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
@@ -116,9 +116,9 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -126,10 +126,10 @@ TEST_F(MecanumDriveKinematicsTest,
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
EXPECT_NEAR(3.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -138,9 +138,9 @@ TEST_F(MecanumDriveKinematicsTest,
36.06_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(12.02, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, Normalize) {
@@ -149,8 +149,8 @@ TEST_F(MecanumDriveKinematicsTest, Normalize) {
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
}

24
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -26,9 +26,9 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
@@ -38,9 +38,9 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
@@ -50,9 +50,9 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
@@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}

118
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -28,15 +28,15 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(br.angle.Radians().value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
@@ -44,33 +44,33 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), 90.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, Rotation2d(90_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
@@ -78,15 +78,15 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
units::radians_per_second_t(2 * wpi::numbers::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
@@ -97,9 +97,9 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
@@ -107,15 +107,15 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
units::radians_per_second_t(2 * wpi::numbers::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.value(), 213.258, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
@@ -126,9 +126,9 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -137,15 +137,15 @@ TEST_F(SwerveDriveKinematicsTest,
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -109.44, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -70.56, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -157,9 +157,9 @@ TEST_F(SwerveDriveKinematicsTest,
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, Normalize) {
@@ -173,8 +173,8 @@ TEST_F(SwerveDriveKinematicsTest, Normalize) {
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}

18
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -31,9 +31,9 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) {
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
state, state);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
@@ -49,9 +49,9 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
@@ -65,7 +65,7 @@ TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
state, state);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}

16
wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
View File

@@ -13,15 +13,15 @@ TEST(SwerveModuleStateTest, Optimize) {
frc::SwerveModuleState refA{-2_mps, 180_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
frc::Rotation2d angleB{-50_deg};
frc::SwerveModuleState refB{4.7_mps, 41_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
EXPECT_NEAR(optimizedB.speed.to<double>(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -139.0, kEpsilon);
EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
}
TEST(SwerveModuleStateTest, NoOptimize) {
@@ -29,13 +29,13 @@ TEST(SwerveModuleStateTest, NoOptimize) {
frc::SwerveModuleState refA{2_mps, 89_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 89.0, kEpsilon);
EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
frc::Rotation2d angleB{0_deg};
frc::SwerveModuleState refB{-2_mps, -2_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
EXPECT_NEAR(optimizedB.speed.to<double>(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
}

31
wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -53,27 +53,26 @@ class CubicHermiteSplineTest : public ::testing::Test {
// Make sure the twist is under the tolerance defined by the Spline class.
auto twist = p0.first.Log(p1.first);
EXPECT_LT(std::abs(twist.dx.to<double>()),
SplineParameterizer::kMaxDx.to<double>());
EXPECT_LT(std::abs(twist.dy.to<double>()),
SplineParameterizer::kMaxDy.to<double>());
EXPECT_LT(std::abs(twist.dtheta.to<double>()),
SplineParameterizer::kMaxDtheta.to<double>());
EXPECT_LT(std::abs(twist.dx.value()),
SplineParameterizer::kMaxDx.value());
EXPECT_LT(std::abs(twist.dy.value()),
SplineParameterizer::kMaxDy.value());
EXPECT_LT(std::abs(twist.dtheta.value()),
SplineParameterizer::kMaxDtheta.value());
}
// Check first point.
EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().value(),
a.Rotation().Radians().value(), 1E-9);
// Check interior waypoints
bool interiorsGood = true;
for (auto& waypoint : waypoints) {
bool found = false;
for (auto& state : poses) {
if (std::abs(
waypoint.Distance(state.first.Translation()).to<double>()) <
if (std::abs(waypoint.Distance(state.first.Translation()).value()) <
1E-9) {
found = true;
}
@@ -84,10 +83,10 @@ class CubicHermiteSplineTest : public ::testing::Test {
EXPECT_TRUE(interiorsGood);
// Check last point.
EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
b.Rotation().Radians().value(), 1E-9);
static_cast<void>(duration);
}

28
wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
View File

@@ -40,25 +40,25 @@ class QuinticHermiteSplineTest : public ::testing::Test {
// Make sure the twist is under the tolerance defined by the Spline class.
auto twist = p0.first.Log(p1.first);
EXPECT_LT(std::abs(twist.dx.to<double>()),
SplineParameterizer::kMaxDx.to<double>());
EXPECT_LT(std::abs(twist.dy.to<double>()),
SplineParameterizer::kMaxDy.to<double>());
EXPECT_LT(std::abs(twist.dtheta.to<double>()),
SplineParameterizer::kMaxDtheta.to<double>());
EXPECT_LT(std::abs(twist.dx.value()),
SplineParameterizer::kMaxDx.value());
EXPECT_LT(std::abs(twist.dy.value()),
SplineParameterizer::kMaxDy.value());
EXPECT_LT(std::abs(twist.dtheta.value()),
SplineParameterizer::kMaxDtheta.value());
}
// Check first point.
EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().value(),
a.Rotation().Radians().value(), 1E-9);
// Check last point.
EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
b.Rotation().Radians().value(), 1E-9);
static_cast<void>(duration);
}

16
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -66,8 +66,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
@@ -96,8 +96,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
@@ -134,8 +134,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
@@ -178,8 +178,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
(contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);

2
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
View File

@@ -28,7 +28,7 @@ TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, const Eigen::Vector<double, 1>& x) {
return Eigen::Vector<double, 1>{
x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0)};
x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);

4
wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
View File

@@ -19,8 +19,8 @@ TEST(TrajectoryConcatenateTest, States) {
const auto& state = t.States()[i];
// Make sure that the timestamps are strictly increasing.
EXPECT_GT(state.t.to<double>(), time);
time = state.t.to<double>();
EXPECT_GT(state.t.value(), time);
time = state.t.value();
// Ensure that the states in t are the same as those in t1 and t2.
if (i < t1.States().size()) {

20
wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
View File

@@ -21,10 +21,10 @@ void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
auto a = a2.RelativeTo(a1);
auto b = b2.RelativeTo(b1);
EXPECT_NEAR(a.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(a.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(a.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
EXPECT_NEAR(a.X().value(), b.X().value(), 1E-9);
EXPECT_NEAR(a.Y().value(), b.Y().value(), 1E-9);
EXPECT_NEAR(a.Rotation().Radians().value(), b.Rotation().Radians().value(),
1E-9);
}
}
@@ -39,9 +39,9 @@ TEST(TrajectoryTransformsTest, TransformBy) {
auto firstPose = transformedTrajectory.Sample(0_s).pose;
EXPECT_NEAR(firstPose.X().to<double>(), 1.0, 1E-9);
EXPECT_NEAR(firstPose.Y().to<double>(), 2.0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
EXPECT_NEAR(firstPose.X().value(), 1.0, 1E-9);
EXPECT_NEAR(firstPose.Y().value(), 2.0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 30.0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
}
@@ -57,9 +57,9 @@ TEST(TrajectoryTransformsTest, RelativeTo) {
auto firstPose = transformedTrajectory.Sample(0_s).pose;
EXPECT_NEAR(firstPose.X().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Y().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.X().value(), 0, 1E-9);
EXPECT_NEAR(firstPose.Y().value(), 0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
}