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

SCRIPT namespace replacements

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
View File

@@ -7,13 +7,13 @@
#include "wpi/math/util/ComputerVisionUtil.hpp"
TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};
frc::Transform3d cameraToObject{frc::Translation3d{1_m, 1_m, 1_m},
frc::Rotation3d{0_deg, -20_deg, 45_deg}};
frc::Transform3d robotToCamera{frc::Translation3d{1_m, 0_m, 2_m},
frc::Rotation3d{0_deg, 0_deg, 25_deg}};
frc::Pose3d object = robot + robotToCamera + cameraToObject;
wpi::math::Pose3d robot{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 30_deg}};
wpi::math::Transform3d cameraToObject{wpi::math::Translation3d{1_m, 1_m, 1_m},
wpi::math::Rotation3d{0_deg, -20_deg, 45_deg}};
wpi::math::Transform3d robotToCamera{wpi::math::Translation3d{1_m, 0_m, 2_m},
wpi::math::Rotation3d{0_deg, 0_deg, 25_deg}};
wpi::math::Pose3d object = robot + robotToCamera + cameraToObject;
EXPECT_EQ(robot,
frc::ObjectToRobotPose(object, cameraToObject, robotToCamera));
wpi::math::ObjectToRobotPose(object, cameraToObject, robotToCamera));
}

184
wpimath/src/test/native/cpp/DARETest.cpp
View File

@@ -15,52 +15,52 @@
#include "wpi/util/print.hpp"
// 2x1
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N, bool checkPreconditions);
// 4x1
extern template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 4, 1>& N, bool checkPreconditions);
// 2x2
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N, bool checkPreconditions);
// 2x3
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R,
@@ -76,9 +76,9 @@ void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
}
if (::testing::Test::HasFailure()) {
wpi::print("lhs =\n{}\n", lhs);
wpi::print("rhs =\n{}\n", rhs);
wpi::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
wpi::util::print("lhs =\n{}\n", lhs);
wpi::util::print("rhs =\n{}\n", rhs);
wpi::util::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
}
}
@@ -131,13 +131,13 @@ TEST(DARETest, NonInvertibleA_ABQR) {
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
// Riccati Equation"
frc::Matrixd<4, 4> A{
wpi::math::Matrixd<4, 4> A{
{0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
frc::Matrixd<1, 1> R{0.25};
wpi::math::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
wpi::math::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
wpi::math::Matrixd<1, 1> R{0.25};
auto ret = frc::DARE<4, 1>(A, B, Q, R);
auto ret = wpi::math::DARE<4, 1>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -150,19 +150,19 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
// Riccati Equation"
frc::Matrixd<4, 4> A{
wpi::math::Matrixd<4, 4> A{
{0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
frc::Matrixd<1, 1> R{0.25};
wpi::math::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
wpi::math::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
wpi::math::Matrixd<1, 1> R{0.25};
frc::Matrixd<4, 4> Aref{
wpi::math::Matrixd<4, 4> Aref{
{0.25, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
Q = (A - Aref).transpose() * Q * (A - Aref);
R = B.transpose() * Q * B + R;
frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
wpi::math::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
auto ret = frc::DARE<4, 1>(A, B, Q, R, N);
auto ret = wpi::math::DARE<4, 1>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -172,12 +172,12 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
}
TEST(DARETest, InvertibleA_ABQR) {
frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
frc::Matrixd<2, 1> B{{0}, {1}};
frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
frc::Matrixd<1, 1> R{{0.3}};
wpi::math::Matrixd<2, 2> A{{1, 1}, {0, 1}};
wpi::math::Matrixd<2, 1> B{{0}, {1}};
wpi::math::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
wpi::math::Matrixd<1, 1> R{{0.3}};
auto ret = frc::DARE<2, 1>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 1>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -187,17 +187,17 @@ TEST(DARETest, InvertibleA_ABQR) {
}
TEST(DARETest, InvertibleA_ABQRN) {
frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
frc::Matrixd<2, 1> B{{0}, {1}};
frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
frc::Matrixd<1, 1> R{0.3};
wpi::math::Matrixd<2, 2> A{{1, 1}, {0, 1}};
wpi::math::Matrixd<2, 1> B{{0}, {1}};
wpi::math::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
wpi::math::Matrixd<1, 1> R{0.3};
frc::Matrixd<2, 2> Aref{{0.5, 1}, {0, 1}};
wpi::math::Matrixd<2, 2> Aref{{0.5, 1}, {0, 1}};
Q = (A - Aref).transpose() * Q * (A - Aref);
R = B.transpose() * Q * B + R;
frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
wpi::math::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
auto ret = frc::DARE<2, 1>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 1>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -209,12 +209,12 @@ TEST(DARETest, InvertibleA_ABQRN) {
TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
// The first generalized eigenvalue of (S, T) is stable
frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
frc::Matrixd<2, 1> B{{0}, {1}};
frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
frc::Matrixd<1, 1> R{1};
wpi::math::Matrixd<2, 2> A{{0, 1}, {0, 0}};
wpi::math::Matrixd<2, 1> B{{0}, {1}};
wpi::math::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
wpi::math::Matrixd<1, 1> R{1};
auto ret = frc::DARE<2, 1>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 1>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -226,17 +226,17 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
// The first generalized eigenvalue of (S, T) is stable
frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
frc::Matrixd<2, 1> B{{0}, {1}};
frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
frc::Matrixd<1, 1> R{1};
wpi::math::Matrixd<2, 2> A{{0, 1}, {0, 0}};
wpi::math::Matrixd<2, 1> B{{0}, {1}};
wpi::math::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
wpi::math::Matrixd<1, 1> R{1};
frc::Matrixd<2, 2> Aref{{0, 0.5}, {0, 0}};
wpi::math::Matrixd<2, 2> Aref{{0, 0.5}, {0, 0}};
Q = (A - Aref).transpose() * Q * (A - Aref);
R = B.transpose() * Q * B + R;
frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
wpi::math::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
auto ret = frc::DARE<2, 1>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 1>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -251,7 +251,7 @@ TEST(DARETest, IdentitySystem_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -267,7 +267,7 @@ TEST(DARETest, IdentitySystem_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -278,11 +278,11 @@ TEST(DARETest, IdentitySystem_ABQRN) {
TEST(DARETest, MoreInputsThanStates_ABQR) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
const wpi::math::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
auto ret = frc::DARE<2, 3>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 3>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -293,12 +293,12 @@ TEST(DARETest, MoreInputsThanStates_ABQR) {
TEST(DARETest, MoreInputsThanStates_ABQRN) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
const wpi::math::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
const frc::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
const wpi::math::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
auto ret = frc::DARE<2, 3>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 3>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
@@ -313,9 +313,9 @@ TEST(DARETest, QNotSymmetric_ABQR) {
const Eigen::Matrix2d Q{{1.0, 1.0}, {0.0, 1.0}};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotSymmetric);
EXPECT_EQ(ret.error(), wpi::math::DAREError::QNotSymmetric);
}
TEST(DARETest, QNotPositiveSemidefinite_ABQR) {
@@ -324,9 +324,9 @@ TEST(DARETest, QNotPositiveSemidefinite_ABQR) {
const Eigen::Matrix2d Q{-Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotPositiveSemidefinite);
EXPECT_EQ(ret.error(), wpi::math::DAREError::QNotPositiveSemidefinite);
}
TEST(DARETest, QNotSymmetric_ABQRN) {
@@ -336,9 +336,9 @@ TEST(DARETest, QNotSymmetric_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotSymmetric);
EXPECT_EQ(ret.error(), wpi::math::DAREError::QNotSymmetric);
}
TEST(DARETest, QNotPositiveSemidefinite_ABQRN) {
@@ -348,9 +348,9 @@ TEST(DARETest, QNotPositiveSemidefinite_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotPositiveSemidefinite);
EXPECT_EQ(ret.error(), wpi::math::DAREError::QNotPositiveSemidefinite);
}
TEST(DARETest, RNotSymmetric_ABQR) {
@@ -359,9 +359,9 @@ TEST(DARETest, RNotSymmetric_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{{1.0, 1.0}, {0.0, 1.0}};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::RNotSymmetric);
EXPECT_EQ(ret.error(), wpi::math::DAREError::RNotSymmetric);
}
TEST(DARETest, RNotPositiveDefinite_ABQR) {
@@ -370,14 +370,14 @@ TEST(DARETest, RNotPositiveDefinite_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
auto ret1 = frc::DARE<2, 2>(A, B, Q, R1);
auto ret1 = wpi::math::DARE<2, 2>(A, B, Q, R1);
EXPECT_FALSE(ret1);
EXPECT_EQ(ret1.error(), frc::DAREError::RNotPositiveDefinite);
EXPECT_EQ(ret1.error(), wpi::math::DAREError::RNotPositiveDefinite);
const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
auto ret2 = frc::DARE<2, 2>(A, B, Q, R2);
auto ret2 = wpi::math::DARE<2, 2>(A, B, Q, R2);
EXPECT_FALSE(ret2);
EXPECT_EQ(ret2.error(), frc::DAREError::RNotPositiveDefinite);
EXPECT_EQ(ret2.error(), wpi::math::DAREError::RNotPositiveDefinite);
}
TEST(DARETest, RNotSymmetric_ABQRN) {
@@ -387,9 +387,9 @@ TEST(DARETest, RNotSymmetric_ABQRN) {
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{{1.0, 1.0}, {0.0, 1.0}};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::RNotSymmetric);
EXPECT_EQ(ret.error(), wpi::math::DAREError::RNotSymmetric);
}
TEST(DARETest, RNotPositiveDefinite_ABQRN) {
@@ -399,14 +399,14 @@ TEST(DARETest, RNotPositiveDefinite_ABQRN) {
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
auto ret1 = frc::DARE<2, 2>(A, B, Q, R1, N);
auto ret1 = wpi::math::DARE<2, 2>(A, B, Q, R1, N);
EXPECT_FALSE(ret1);
EXPECT_EQ(ret1.error(), frc::DAREError::RNotPositiveDefinite);
EXPECT_EQ(ret1.error(), wpi::math::DAREError::RNotPositiveDefinite);
const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
auto ret2 = frc::DARE<2, 2>(A, B, Q, R2, N);
auto ret2 = wpi::math::DARE<2, 2>(A, B, Q, R2, N);
EXPECT_FALSE(ret2);
EXPECT_EQ(ret2.error(), frc::DAREError::RNotPositiveDefinite);
EXPECT_EQ(ret2.error(), wpi::math::DAREError::RNotPositiveDefinite);
}
TEST(DARETest, ABNotStabilizable_ABQR) {
@@ -415,9 +415,9 @@ TEST(DARETest, ABNotStabilizable_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ABNotStabilizable);
EXPECT_EQ(ret.error(), wpi::math::DAREError::ABNotStabilizable);
}
TEST(DARETest, ABNotStabilizable_ABQRN) {
@@ -427,9 +427,9 @@ TEST(DARETest, ABNotStabilizable_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ABNotStabilizable);
EXPECT_EQ(ret.error(), wpi::math::DAREError::ABNotStabilizable);
}
TEST(DARETest, ACNotDetectable_ABQR) {
@@ -438,9 +438,9 @@ TEST(DARETest, ACNotDetectable_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ACNotDetectable);
EXPECT_EQ(ret.error(), wpi::math::DAREError::ACNotDetectable);
}
TEST(DARETest, ACNotDetectable_ABQRN) {
@@ -450,9 +450,9 @@ TEST(DARETest, ACNotDetectable_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Zero()};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = wpi::math::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ACNotDetectable);
EXPECT_EQ(ret.error(), wpi::math::DAREError::ACNotDetectable);
}
TEST(DARETest, QDecomposition) {
@@ -465,12 +465,12 @@ TEST(DARETest, QDecomposition) {
// (A, C₁) should be detectable pair
const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
auto ret1 = frc::DARE<2, 2>(A, B, Q_1, R);
auto ret1 = wpi::math::DARE<2, 2>(A, B, Q_1, R);
EXPECT_TRUE(ret1);
// (A, C₂) shouldn't be detectable pair
const Eigen::Matrix2d C_2 = C_1.transpose();
const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
auto ret2 = frc::DARE<2, 2>(A, B, Q_2, R);
auto ret2 = wpi::math::DARE<2, 2>(A, B, Q_2, R);
EXPECT_FALSE(ret2);
}

8
wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
View File

@@ -4,13 +4,13 @@
#include "wpi/math/linalg/DARE.hpp"
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,

8
wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
View File

@@ -4,13 +4,13 @@
#include "wpi/math/linalg/DARE.hpp"
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,

8
wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
View File

@@ -4,13 +4,13 @@
#include "wpi/math/linalg/DARE.hpp"
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R,

8
wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
View File

@@ -5,13 +5,13 @@
#include "wpi/math/linalg/DARE.hpp"
#include "wpi/util/expected"
template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R,

12
wpimath/src/test/native/cpp/EigenTest.cpp
View File

@@ -8,22 +8,22 @@
#include "wpi/math/linalg/EigenCore.hpp"
TEST(EigenTest, Multiplication) {
frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
frc::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}};
wpi::math::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
wpi::math::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}};
const auto result = m1 * m2;
frc::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
wpi::math::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
EXPECT_TRUE(expectedResult.isApprox(result));
frc::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
frc::Matrixd<3, 4> m4{
wpi::math::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
wpi::math::Matrixd<3, 4> m4{
{3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
const auto result2 = m3 * m4;
frc::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
wpi::math::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
{22.13, 9.82, 13.28, 23.53}};
EXPECT_TRUE(expectedResult2.isApprox(result2));

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

@@ -21,256 +21,256 @@
TEST(MathUtilTest, ApplyDeadbandUnityScale) {
// < 0
EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
EXPECT_DOUBLE_EQ(-1.0, wpi::math::ApplyDeadband(-1.0, 0.02));
EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
frc::ApplyDeadband(-0.03, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02));
wpi::math::ApplyDeadband(-0.03, 0.02));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(-0.02, 0.02));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(-0.01, 0.02));
// == 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(0.0, 0.02));
// > 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(0.01, 0.02));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(0.02, 0.02));
EXPECT_DOUBLE_EQ((0.03 - 0.02) / (1.0 - 0.02),
frc::ApplyDeadband(0.03, 0.02));
EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
wpi::math::ApplyDeadband(0.03, 0.02));
EXPECT_DOUBLE_EQ(1.0, wpi::math::ApplyDeadband(1.0, 0.02));
}
TEST(MathUtilTest, ApplyDeadbandArbitraryScale) {
// < 0
EXPECT_DOUBLE_EQ(-2.5, frc::ApplyDeadband(-2.5, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02, 2.5));
EXPECT_DOUBLE_EQ(-2.5, wpi::math::ApplyDeadband(-2.5, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(-0.02, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(-0.01, 0.02, 2.5));
// == 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(0.0, 0.02, 2.5));
// > 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02, 2.5));
EXPECT_DOUBLE_EQ(2.5, frc::ApplyDeadband(2.5, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(0.01, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, wpi::math::ApplyDeadband(0.02, 0.02, 2.5));
EXPECT_DOUBLE_EQ(2.5, wpi::math::ApplyDeadband(2.5, 0.02, 2.5));
}
TEST(MathUtilTest, ApplyDeadbandUnits) {
// < 0
EXPECT_UNITS_EQ(-20_rad,
frc::ApplyDeadband<units::radian_t>(-20_rad, 1_rad, 20_rad));
wpi::math::ApplyDeadband<wpi::units::radian_t>(-20_rad, 1_rad, 20_rad));
}
TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
EXPECT_DOUBLE_EQ(
80.0,
frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
wpi::math::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
}
TEST(MathUtilTest, ApplyDeadband2dUnityScale) {
EXPECT_EQ((Eigen::Vector2d{{0.0}, {1.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {1.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {1.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-1.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {-1.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {-1.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{-1.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{-1.0}, {0.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{-1.0}, {0.0}}, 0.02));
// == 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02));
// > 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{(0.03 - 0.02) / (1.0 - 0.02)}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.03}, {0.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.03}, {0.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{1.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{1.0}, {0.0}}, 0.02));
wpi::math::ApplyDeadband(Eigen::Vector2d{{1.0}, {0.0}}, 0.02));
}
TEST(MathUtilTest, ApplyDeadband2dArbitraryScale) {
EXPECT_EQ((Eigen::Vector2d{{0.0}, {2.5}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {2.5}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {2.5}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-2.5}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {-2.5}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {-2.5}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{-2.5}, {0.0}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{-2.5}, {0.0}}, 0.02, 2.5));
// == 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02, 2.5));
// > 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{2.5}, {0.0}}, 0.02, 2.5));
wpi::math::ApplyDeadband(Eigen::Vector2d{{2.5}, {0.0}}, 0.02, 2.5));
}
TEST(MathUtilTest, ApplyDeadband2dLargeMaxMagnitude) {
EXPECT_EQ((Eigen::Vector2d{{80.0}, {0.0}}),
(frc::ApplyDeadband(Eigen::Vector2d{{100.0}, {0.0}}, 20.0,
(wpi::math::ApplyDeadband(Eigen::Vector2d{{100.0}, {0.0}}, 20.0,
std::numeric_limits<double>::infinity())));
}
TEST(MathUtilTest, ApplyDeadband2dUnits) {
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 2.5_mps}),
frc::ApplyDeadband(
Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 2.5_mps},
EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 2.5_mps}),
wpi::math::ApplyDeadband(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 2.5_mps},
0.02_mps, 2.5_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps}),
frc::ApplyDeadband(
Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps},
EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}),
wpi::math::ApplyDeadband(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps},
0.02_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps}),
frc::ApplyDeadband(
Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps},
EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps}),
wpi::math::ApplyDeadband(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps},
0.02_mps, 2.5_mps));
}
TEST(MathUtilTest, CopyDirectionPow) {
EXPECT_DOUBLE_EQ(0.5, frc::CopyDirectionPow(0.5, 1.0));
EXPECT_DOUBLE_EQ(-0.5, frc::CopyDirectionPow(-0.5, 1.0));
EXPECT_DOUBLE_EQ(0.5, wpi::math::CopyDirectionPow(0.5, 1.0));
EXPECT_DOUBLE_EQ(-0.5, wpi::math::CopyDirectionPow(-0.5, 1.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopyDirectionPow(0.5, 2.0));
EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopyDirectionPow(-0.5, 2.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5, wpi::math::CopyDirectionPow(0.5, 2.0));
EXPECT_DOUBLE_EQ(-(0.5 * 0.5), wpi::math::CopyDirectionPow(-0.5, 2.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopyDirectionPow(0.5, 0.5));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopyDirectionPow(-0.5, 0.5));
EXPECT_DOUBLE_EQ(std::sqrt(0.5), wpi::math::CopyDirectionPow(0.5, 0.5));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5), wpi::math::CopyDirectionPow(-0.5, 0.5));
EXPECT_DOUBLE_EQ(0.0, frc::CopyDirectionPow(0.0, 2.0));
EXPECT_DOUBLE_EQ(1.0, frc::CopyDirectionPow(1.0, 2.0));
EXPECT_DOUBLE_EQ(-1.0, frc::CopyDirectionPow(-1.0, 2.0));
EXPECT_DOUBLE_EQ(0.0, wpi::math::CopyDirectionPow(0.0, 2.0));
EXPECT_DOUBLE_EQ(1.0, wpi::math::CopyDirectionPow(1.0, 2.0));
EXPECT_DOUBLE_EQ(-1.0, wpi::math::CopyDirectionPow(-1.0, 2.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopyDirectionPow(0.8, 0.3));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopyDirectionPow(-0.8, 0.3));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), wpi::math::CopyDirectionPow(0.8, 0.3));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), wpi::math::CopyDirectionPow(-0.8, 0.3));
}
TEST(MathUtilTest, CopyDirectionPowWithMaxMagnitude) {
EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(5.0, wpi::math::CopyDirectionPow(5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(-5.0, wpi::math::CopyDirectionPow(-5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopyDirectionPow(5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopyDirectionPow(-5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, wpi::math::CopyDirectionPow(5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, wpi::math::CopyDirectionPow(-5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopyDirectionPow(5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, wpi::math::CopyDirectionPow(5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10,
frc::CopyDirectionPow(-5.0, 0.5, 10.0));
wpi::math::CopyDirectionPow(-5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(0.0, frc::CopyDirectionPow(0.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(0.0, wpi::math::CopyDirectionPow(0.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(5.0, wpi::math::CopyDirectionPow(5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(-5.0, wpi::math::CopyDirectionPow(-5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3) * 100,
frc::CopyDirectionPow(80.0, 0.3, 100.0));
wpi::math::CopyDirectionPow(80.0, 0.3, 100.0));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3) * 100,
frc::CopyDirectionPow(-80.0, 0.3, 100.0));
wpi::math::CopyDirectionPow(-80.0, 0.3, 100.0));
}
TEST(MathUtilTest, CopyDirectionPowWithUnits) {
EXPECT_UNITS_EQ(
0_mps, frc::CopyDirectionPow<units::meters_per_second_t>(0_mps, 2.0));
0_mps, wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(0_mps, 2.0));
EXPECT_UNITS_EQ(
1_mps, frc::CopyDirectionPow<units::meters_per_second_t>(1_mps, 2.0));
1_mps, wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(1_mps, 2.0));
EXPECT_UNITS_EQ(
-1_mps, frc::CopyDirectionPow<units::meters_per_second_t>(-1_mps, 2.0));
-1_mps, wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(-1_mps, 2.0));
EXPECT_UNITS_EQ(
units::meters_per_second_t{0.5 * 0.5 * 10},
frc::CopyDirectionPow<units::meters_per_second_t>(5_mps, 2.0, 10_mps));
wpi::units::meters_per_second_t{0.5 * 0.5 * 10},
wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(5_mps, 2.0, 10_mps));
EXPECT_UNITS_EQ(
units::meters_per_second_t{-0.5 * 0.5 * 10},
frc::CopyDirectionPow<units::meters_per_second_t>(-5_mps, 2.0, 10_mps));
wpi::units::meters_per_second_t{-0.5 * 0.5 * 10},
wpi::math::CopyDirectionPow<wpi::units::meters_per_second_t>(-5_mps, 2.0, 10_mps));
}
TEST(MathUtilTest, CopyDirectionPow2d) {
EXPECT_EQ((Eigen::Vector2d{{0.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 1.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 1.0));
EXPECT_EQ((Eigen::Vector2d{{-0.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 1.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 1.0));
EXPECT_EQ((Eigen::Vector2d{{0.25}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{-0.25}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5)}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 0.5));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 0.5));
EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5)}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 0.5));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 0.5));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{1.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{1.0}, {0.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{1.0}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{-1.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-1.0}, {0.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-1.0}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {1.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {1.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {1.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-1.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-1.0}}, 2.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-1.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3)}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.8}}, 0.3));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.8}}, 0.3));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3)}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-0.8}}, 0.3));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-0.8}}, 0.3));
}
TEST(MathUtilTest, CopyDirectionPow2dMaxDistance) {
EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 1.0, 10.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 1.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 1.0, 10.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 1.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 10.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 10.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5) * 10.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 0.5, 10.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 0.5, 10.0));
EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5) * 10.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 0.5, 10.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 0.5, 10.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0, 5.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0, 5.0));
EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 5.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 5.0));
EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 5.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 5.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3) * 100.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {80.0}}, 0.3, 100.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {80.0}}, 0.3, 100.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3) * 100.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-80.0}}, 0.3, 100.0));
wpi::math::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-80.0}}, 0.3, 100.0));
}
TEST(MathUtilTest, CopyDirectionPow2dUnits) {
EXPECT_EQ(
(Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps}, 2.0));
(Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}),
wpi::math::CopyDirectionPow(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{1_mps, 0_mps}, 2.0));
EXPECT_EQ(
(Eigen::Vector<units::meters_per_second_t, 2>{-1_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{-1_mps, 0_mps}, 2.0));
(Eigen::Vector<wpi::units::meters_per_second_t, 2>{-1_mps, 0_mps}),
wpi::math::CopyDirectionPow(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{-1_mps, 0_mps}, 2.0));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps}, 2.0,
EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps}),
wpi::math::CopyDirectionPow(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{0_mps, 0_mps}, 2.0,
5_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{5_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{5_mps, 0_mps}, 2.0,
EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{5_mps, 0_mps}),
wpi::math::CopyDirectionPow(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{5_mps, 0_mps}, 2.0,
5_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{-5_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{-5_mps, 0_mps},
EXPECT_EQ((Eigen::Vector<wpi::units::meters_per_second_t, 2>{-5_mps, 0_mps}),
wpi::math::CopyDirectionPow(
Eigen::Vector<wpi::units::meters_per_second_t, 2>{-5_mps, 0_mps},
2.0, 5_mps));
}
@@ -279,156 +279,156 @@ TEST(MathUtilTest, InputModulus) {
// result of an angle reference minus the measurement.
// Test symmetric range
EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0, wpi::math::InputModulus(170.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
wpi::math::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
wpi::math::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0, wpi::math::InputModulus(-170.0 - 170.0, -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0,
frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
wpi::math::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0,
frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
wpi::math::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
// Test range starting at zero
EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, wpi::math::InputModulus(170.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, wpi::math::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0,
frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
wpi::math::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
// Test asymmetric range that doesn't start at zero
EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
EXPECT_DOUBLE_EQ(-20.0, wpi::math::InputModulus(170.0 - (-170.0), -170.0, 190.0));
// Test range with both positive endpoints
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, wpi::math::InputModulus(0.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, wpi::math::InputModulus(1.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, wpi::math::InputModulus(2.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, wpi::math::InputModulus(3.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, wpi::math::InputModulus(4.0, 1.0, 3.0));
// Test all supported types
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_EQ(-20, frc::InputModulus<int>(170 - (-170), -170, 190));
EXPECT_EQ(-20_deg, frc::InputModulus<units::degree_t>(170_deg - (-170_deg),
wpi::math::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_EQ(-20, wpi::math::InputModulus<int>(170 - (-170), -170, 190));
EXPECT_EQ(-20_deg, wpi::math::InputModulus<wpi::units::degree_t>(170_deg - (-170_deg),
-170_deg, 190_deg));
}
TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_NEAR(
frc::AngleModulus(units::radian_t{-2000 * std::numbers::pi / 180}),
units::radian_t{160 * std::numbers::pi / 180}, 1e-10);
wpi::math::AngleModulus(wpi::units::radian_t{-2000 * std::numbers::pi / 180}),
wpi::units::radian_t{160 * std::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(
frc::AngleModulus(units::radian_t{358 * std::numbers::pi / 180}),
units::radian_t{-2 * std::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * std::numbers::pi}),
wpi::math::AngleModulus(wpi::units::radian_t{358 * std::numbers::pi / 180}),
wpi::units::radian_t{-2 * std::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(wpi::math::AngleModulus(wpi::units::radian_t{2.0 * std::numbers::pi}),
0_rad, 1e-10);
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{5 * std::numbers::pi}),
units::radian_t{std::numbers::pi});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-5 * std::numbers::pi}),
units::radian_t{std::numbers::pi});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{std::numbers::pi / 2}),
units::radian_t{std::numbers::pi / 2});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
units::radian_t{-std::numbers::pi / 2});
EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{5 * std::numbers::pi}),
wpi::units::radian_t{std::numbers::pi});
EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{-5 * std::numbers::pi}),
wpi::units::radian_t{std::numbers::pi});
EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{std::numbers::pi / 2}),
wpi::units::radian_t{std::numbers::pi / 2});
EXPECT_UNITS_EQ(wpi::math::AngleModulus(wpi::units::radian_t{-std::numbers::pi / 2}),
wpi::units::radian_t{-std::numbers::pi / 2});
}
TEST(MathUtilTest, IsNear) {
// The answer is always 42
// Positive integer checks
EXPECT_TRUE(frc::IsNear(42, 42, 1));
EXPECT_TRUE(frc::IsNear(42, 41, 2));
EXPECT_TRUE(frc::IsNear(42, 43, 2));
EXPECT_FALSE(frc::IsNear(42, 44, 1));
EXPECT_TRUE(wpi::math::IsNear(42, 42, 1));
EXPECT_TRUE(wpi::math::IsNear(42, 41, 2));
EXPECT_TRUE(wpi::math::IsNear(42, 43, 2));
EXPECT_FALSE(wpi::math::IsNear(42, 44, 1));
// Negative integer checks
EXPECT_TRUE(frc::IsNear(-42, -42, 1));
EXPECT_TRUE(frc::IsNear(-42, -41, 2));
EXPECT_TRUE(frc::IsNear(-42, -43, 2));
EXPECT_FALSE(frc::IsNear(-42, -44, 1));
EXPECT_TRUE(wpi::math::IsNear(-42, -42, 1));
EXPECT_TRUE(wpi::math::IsNear(-42, -41, 2));
EXPECT_TRUE(wpi::math::IsNear(-42, -43, 2));
EXPECT_FALSE(wpi::math::IsNear(-42, -44, 1));
// Mixed sign integer checks
EXPECT_FALSE(frc::IsNear(-42, 42, 1));
EXPECT_FALSE(frc::IsNear(-42, 41, 2));
EXPECT_FALSE(frc::IsNear(-42, 43, 2));
EXPECT_FALSE(frc::IsNear(42, -42, 1));
EXPECT_FALSE(frc::IsNear(42, -41, 2));
EXPECT_FALSE(frc::IsNear(42, -43, 2));
EXPECT_FALSE(wpi::math::IsNear(-42, 42, 1));
EXPECT_FALSE(wpi::math::IsNear(-42, 41, 2));
EXPECT_FALSE(wpi::math::IsNear(-42, 43, 2));
EXPECT_FALSE(wpi::math::IsNear(42, -42, 1));
EXPECT_FALSE(wpi::math::IsNear(42, -41, 2));
EXPECT_FALSE(wpi::math::IsNear(42, -43, 2));
// Floating point checks
EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
EXPECT_TRUE(wpi::math::IsNear<double>(42, 41.5, 1));
EXPECT_TRUE(wpi::math::IsNear<double>(42, 42.5, 1));
EXPECT_TRUE(wpi::math::IsNear<double>(42, 41.5, 0.75));
EXPECT_TRUE(wpi::math::IsNear<double>(42, 42.5, 0.75));
// Wraparound checks
EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
EXPECT_TRUE(wpi::math::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
EXPECT_TRUE(wpi::math::IsNear(0, -356, 5, 0, 360));
EXPECT_TRUE(wpi::math::IsNear(0, 4, 5, 0, 360));
EXPECT_TRUE(wpi::math::IsNear(0, -4, 5, 0, 360));
EXPECT_TRUE(wpi::math::IsNear(400, 41, 5, 0, 360));
EXPECT_TRUE(wpi::math::IsNear(400, -319, 5, 0, 360));
EXPECT_TRUE(wpi::math::IsNear(400, 401, 5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear<double>(0, 356, 2.5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear<double>(0, -356, 2.5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear<double>(0, 4, 2.5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear<double>(0, -4, 2.5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear(400, 35, 5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear(400, -315, 5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(wpi::math::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
}
TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
const frc::Translation2d translation1{0_m, 0_m};
const frc::Translation2d translation2{2_m, 2_m};
const wpi::math::Translation2d translation1{0_m, 0_m};
const wpi::math::Translation2d translation2{2_m, 2_m};
const frc::Translation2d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
const wpi::math::Translation2d result1 =
wpi::math::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
const frc::Translation2d expected1{2_m, 2_m};
const wpi::math::Translation2d expected1{2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
const frc::Translation2d translation3{1_m, 1_m};
const frc::Translation2d translation4{3_m, 3_m};
const wpi::math::Translation2d translation3{1_m, 1_m};
const wpi::math::Translation2d translation4{3_m, 3_m};
const frc::Translation2d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
const wpi::math::Translation2d result2 =
wpi::math::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
const frc::Translation2d expected2{
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
const wpi::math::Translation2d expected2{
wpi::units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
wpi::units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
EXPECT_EQ(result2, expected2);
}
TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
const frc::Translation3d translation1{0_m, 0_m, 0_m};
const frc::Translation3d translation2{2_m, 2_m, 2_m};
const wpi::math::Translation3d translation1{0_m, 0_m, 0_m};
const wpi::math::Translation3d translation2{2_m, 2_m, 2_m};
const frc::Translation3d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
const wpi::math::Translation3d result1 =
wpi::math::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
const frc::Translation3d expected1{2_m, 2_m, 2_m};
const wpi::math::Translation3d expected1{2_m, 2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
const frc::Translation3d translation3{1_m, 1_m, 1_m};
const frc::Translation3d translation4{3_m, 3_m, 3_m};
const wpi::math::Translation3d translation3{1_m, 1_m, 1_m};
const wpi::math::Translation3d translation4{3_m, 3_m, 3_m};
const frc::Translation3d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
const wpi::math::Translation3d result2 =
wpi::math::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
const frc::Translation3d expected2{
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
const wpi::math::Translation3d expected2{
wpi::units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
wpi::units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
wpi::units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
EXPECT_EQ(result2, expected2);
}

4
wpimath/src/test/native/cpp/ProtoTestBase.hpp
View File

@@ -15,8 +15,8 @@ class ProtoTest : public testing::Test {};
TYPED_TEST_SUITE_P(ProtoTest);
TYPED_TEST_P(ProtoTest, RoundTrip) {
wpi::ProtobufMessage<decltype(TypeParam::kTestData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(TypeParam::kTestData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, TypeParam::kTestData));
auto unpacked_data = message.Unpack(buf);

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

@@ -17,7 +17,7 @@
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
constexpr double kPositionStddev = 0.0001;
constexpr auto kDt = 0.00505_s;
@@ -36,7 +36,7 @@ class StateSpaceTest : public testing::Test {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};
@@ -67,4 +67,4 @@ TEST_F(StateSpaceTest, CorrectPredictLoop) {
EXPECT_NEAR(loop.Xhat(1), 0.0, 0.5);
}
} // namespace frc
} // namespace wpi::math

38
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/util/StateSpaceUtil.hpp"
TEST(StateSpaceUtilTest, CostParameterPack) {
constexpr frc::Matrixd<3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCostMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -21,7 +21,7 @@ TEST(StateSpaceUtilTest, CostParameterPack) {
}
TEST(StateSpaceUtilTest, CostArray) {
constexpr frc::Matrixd<3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCostMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -34,7 +34,7 @@ TEST(StateSpaceUtilTest, CostArray) {
}
TEST(StateSpaceUtilTest, CostDynamic) {
Eigen::MatrixXd mat = frc::MakeCostMatrix(std::vector{1.0, 2.0, 3.0});
Eigen::MatrixXd mat = wpi::math::MakeCostMatrix(std::vector{1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -47,7 +47,7 @@ TEST(StateSpaceUtilTest, CostDynamic) {
}
TEST(StateSpaceUtilTest, CovParameterPack) {
constexpr frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCovMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -60,7 +60,7 @@ TEST(StateSpaceUtilTest, CovParameterPack) {
}
TEST(StateSpaceUtilTest, CovArray) {
constexpr frc::Matrixd<3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::MakeCovMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -73,7 +73,7 @@ TEST(StateSpaceUtilTest, CovArray) {
}
TEST(StateSpaceUtilTest, CovDynamic) {
Eigen::MatrixXd mat = frc::MakeCovMatrix(std::vector{1.0, 2.0, 3.0});
Eigen::MatrixXd mat = wpi::math::MakeCovMatrix(std::vector{1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -87,63 +87,63 @@ TEST(StateSpaceUtilTest, CovDynamic) {
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
[[maybe_unused]]
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
wpi::math::Vectord<2> vec = wpi::math::MakeWhiteNoiseVector(2.0, 3.0);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
[[maybe_unused]]
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
wpi::math::Vectord<2> vec = wpi::math::MakeWhiteNoiseVector<2>({2.0, 3.0});
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorDynamic) {
[[maybe_unused]]
Eigen::VectorXd vec = frc::MakeWhiteNoiseVector(std::vector{2.0, 3.0});
Eigen::VectorXd vec = wpi::math::MakeWhiteNoiseVector(std::vector{2.0, 3.0});
}
TEST(StateSpaceUtilTest, IsStabilizable) {
frc::Matrixd<2, 1> B{0, 1};
wpi::math::Matrixd<2, 1> B{0, 1};
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
EXPECT_FALSE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
(wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
EXPECT_FALSE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
(wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
EXPECT_TRUE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
(wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
EXPECT_TRUE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
(wpi::math::IsStabilizable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
}
TEST(StateSpaceUtilTest, IsDetectable) {
frc::Matrixd<1, 2> C{0, 1};
wpi::math::Matrixd<1, 2> C{0, 1};
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
(wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
(wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
EXPECT_TRUE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
(wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
EXPECT_TRUE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
(wpi::math::IsDetectable<2, 1>(wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
}

24
wpimath/src/test/native/cpp/StructTestBase.hpp
View File

@@ -16,45 +16,45 @@ TYPED_TEST_SUITE_P(StructTest);
// For these tests:
// TypeParam defines Type, kTestData, and CheckEq
// Type is the data type
// StructType is the instantiation of wpi::Struct<>
// StructType is the instantiation of wpi::util::Struct<>
TYPED_TEST_P(StructTest, RoundTrip) {
using Type = typename TypeParam::Type;
using StructType = wpi::Struct<Type>;
using StructType = wpi::util::Struct<Type>;
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
wpi::PackStruct(buffer, TypeParam::kTestData);
wpi::util::PackStruct(buffer, TypeParam::kTestData);
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
Type unpacked_data = wpi::util::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
TYPED_TEST_P(StructTest, DoublePack) {
using Type = typename TypeParam::Type;
using StructType = wpi::Struct<Type>;
using StructType = wpi::util::Struct<Type>;
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
wpi::PackStruct(buffer, TypeParam::kTestData);
wpi::PackStruct(buffer, TypeParam::kTestData);
wpi::util::PackStruct(buffer, TypeParam::kTestData);
wpi::util::PackStruct(buffer, TypeParam::kTestData);
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
Type unpacked_data = wpi::util::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
TYPED_TEST_P(StructTest, DoubleUnpack) {
using Type = typename TypeParam::Type;
using StructType = wpi::Struct<Type>;
using StructType = wpi::util::Struct<Type>;
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
wpi::PackStruct(buffer, TypeParam::kTestData);
wpi::util::PackStruct(buffer, TypeParam::kTestData);
{
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
Type unpacked_data = wpi::util::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
{
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
Type unpacked_data = wpi::util::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
}

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

@@ -51,46 +51,46 @@
#include "wpi/units/voltage.hpp"
#include "wpi/units/volume.hpp"
using namespace units::acceleration;
using namespace units::angle;
using namespace units::angular_acceleration;
using namespace units::angular_jerk;
using namespace units::angular_velocity;
using namespace units::area;
using namespace units::capacitance;
using namespace units::charge;
using namespace units::concentration;
using namespace units::conductance;
using namespace units::data;
using namespace units::data_transfer_rate;
using namespace units::density;
using namespace units::dimensionless;
using namespace units::energy;
using namespace units::frequency;
using namespace units::illuminance;
using namespace units::impedance;
using namespace units::inductance;
using namespace units::length;
using namespace units::luminous_flux;
using namespace units::luminous_intensity;
using namespace units::magnetic_field_strength;
using namespace units::magnetic_flux;
using namespace units::mass;
using namespace units::math;
using namespace units::power;
using namespace units::pressure;
using namespace units::radiation;
using namespace units::solid_angle;
using namespace units::temperature;
using namespace units::time;
using namespace units::torque;
using namespace units::velocity;
using namespace units::voltage;
using namespace units::volume;
using namespace wpi::units::acceleration;
using namespace wpi::units::angle;
using namespace wpi::units::angular_acceleration;
using namespace wpi::units::angular_jerk;
using namespace wpi::units::angular_velocity;
using namespace wpi::units::area;
using namespace wpi::units::capacitance;
using namespace wpi::units::charge;
using namespace wpi::units::concentration;
using namespace wpi::units::conductance;
using namespace wpi::units::data;
using namespace wpi::units::data_transfer_rate;
using namespace wpi::units::density;
using namespace wpi::units::dimensionless;
using namespace wpi::units::energy;
using namespace wpi::units::frequency;
using namespace wpi::units::illuminance;
using namespace wpi::units::impedance;
using namespace wpi::units::inductance;
using namespace wpi::units::length;
using namespace wpi::units::luminous_flux;
using namespace wpi::units::luminous_intensity;
using namespace wpi::units::magnetic_field_strength;
using namespace wpi::units::magnetic_flux;
using namespace wpi::units::mass;
using namespace wpi::units::math;
using namespace wpi::units::power;
using namespace wpi::units::pressure;
using namespace wpi::units::radiation;
using namespace wpi::units::solid_angle;
using namespace wpi::units::temperature;
using namespace wpi::units::time;
using namespace wpi::units::torque;
using namespace wpi::units::velocity;
using namespace wpi::units::voltage;
using namespace wpi::units::volume;
using namespace units;
#if !defined(_MSC_VER) || _MSC_VER > 1800
using namespace units::literals;
using namespace wpi::units::literals;
#endif
namespace {
@@ -452,7 +452,7 @@ TEST_F(TypeTraits, is_substance_unit) {
TEST_F(TypeTraits, is_luminous_intensity_unit) {
EXPECT_TRUE((traits::is_luminous_intensity_unit_v<candela>));
EXPECT_FALSE(
(traits::is_luminous_intensity_unit_v<units::radiation::rad>));
(traits::is_luminous_intensity_unit_v<wpi::units::radiation::rad>));
EXPECT_FALSE((traits::is_luminous_intensity_unit_v<double>));
EXPECT_TRUE((traits::is_luminous_intensity_unit_v<candela_t>));
@@ -534,19 +534,19 @@ TEST_F(TypeTraits, is_acceleration_unit) {
}
TEST_F(TypeTraits, is_force_unit) {
EXPECT_TRUE((traits::is_force_unit_v<units::force::newton>));
EXPECT_TRUE((traits::is_force_unit_v<units::force::dynes>));
EXPECT_TRUE((traits::is_force_unit_v<wpi::units::force::newton>));
EXPECT_TRUE((traits::is_force_unit_v<wpi::units::force::dynes>));
EXPECT_FALSE((traits::is_force_unit_v<meter>));
EXPECT_FALSE((traits::is_force_unit_v<double>));
EXPECT_TRUE((traits::is_force_unit_v<units::force::newton_t>));
EXPECT_TRUE((traits::is_force_unit_v<const units::force::newton_t>));
EXPECT_TRUE((traits::is_force_unit_v<const units::force::newton_t&>));
EXPECT_TRUE((traits::is_force_unit_v<units::force::dyne_t>));
EXPECT_TRUE((traits::is_force_unit_v<wpi::units::force::newton_t>));
EXPECT_TRUE((traits::is_force_unit_v<const wpi::units::force::newton_t>));
EXPECT_TRUE((traits::is_force_unit_v<const wpi::units::force::newton_t&>));
EXPECT_TRUE((traits::is_force_unit_v<wpi::units::force::dyne_t>));
EXPECT_FALSE((traits::is_force_unit_v<watt_t>));
EXPECT_TRUE((traits::is_force_unit_v<units::force::dyne_t,
units::force::newton_t>));
EXPECT_FALSE((traits::is_force_unit_v<watt_t, units::force::newton_t>));
EXPECT_TRUE((traits::is_force_unit_v<wpi::units::force::dyne_t,
wpi::units::force::newton_t>));
EXPECT_FALSE((traits::is_force_unit_v<watt_t, wpi::units::force::newton_t>));
}
TEST_F(TypeTraits, is_pressure_unit) {
@@ -677,7 +677,7 @@ TEST_F(TypeTraits, is_magnetic_flux_unit) {
TEST_F(TypeTraits, is_magnetic_field_strength_unit) {
EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<
units::magnetic_field_strength::tesla>));
wpi::units::magnetic_field_strength::tesla>));
EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<gauss>));
EXPECT_FALSE((traits::is_magnetic_field_strength_unit_v<volt>));
EXPECT_FALSE((traits::is_magnetic_field_strength_unit_v<double>));
@@ -919,13 +919,13 @@ TEST_F(UnitManipulators, dimensionalAnalysis) {
// REALLY handy if the unit types aren't know (i.e. they themselves are
// template parameters), as you can get the resulting unit of the operation.
using velocity = units::detail::unit_divide<meters, second>;
using velocity = wpi::units::detail::unit_divide<meters, second>;
bool shouldBeTrue = std::is_same_v<meters_per_second, velocity>;
EXPECT_TRUE(shouldBeTrue);
using acceleration1 = unit<std::ratio<1>, category::acceleration_unit>;
using acceleration2 = units::detail::unit_divide<
meters, units::detail::unit_multiply<seconds, seconds>>;
using acceleration2 = wpi::units::detail::unit_divide<
meters, wpi::units::detail::unit_multiply<seconds, seconds>>;
shouldBeTrue = std::is_same_v<acceleration1, acceleration2>;
EXPECT_TRUE(shouldBeTrue);
}
@@ -964,7 +964,7 @@ TEST_F(UnitContainer, has_value_member) {
}
TEST_F(UnitContainer, make_unit) {
auto dist = units::make_unit<meter_t>(5);
auto dist = wpi::units::make_unit<meter_t>(5);
EXPECT_EQ(meter_t(5), dist);
}
@@ -1380,17 +1380,17 @@ TEST_F(UnitContainer, cout) {
// undefined unit
testing::internal::CaptureStdout();
std::cout << units::math::cpow<4>(meter_t(2));
std::cout << wpi::units::math::cpow<4>(meter_t(2));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("16 m^4", output.c_str());
testing::internal::CaptureStdout();
std::cout << units::math::cpow<3>(foot_t(2));
std::cout << wpi::units::math::cpow<3>(foot_t(2));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("8 cu_ft", output.c_str());
testing::internal::CaptureStdout();
std::cout << std::setprecision(9) << units::math::cpow<4>(foot_t(2));
std::cout << std::setprecision(9) << wpi::units::math::cpow<4>(foot_t(2));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("0.138095597 m^4", output.c_str());
@@ -1427,59 +1427,59 @@ TEST_F(UnitContainer, cout) {
#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
TEST_F(UnitContainer, fmtlib) {
testing::internal::CaptureStdout();
wpi::print("{}", degree_t(349.87));
wpi::util::print("{}", degree_t(349.87));
std::string output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("349.87 deg", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", meter_t(1.0));
wpi::util::print("{}", meter_t(1.0));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("1 m", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", dB_t(31.0));
wpi::util::print("{}", dB_t(31.0));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("31 dB", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", volt_t(21.79));
wpi::util::print("{}", volt_t(21.79));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("21.79 V", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", dBW_t(12.0));
wpi::util::print("{}", dBW_t(12.0));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("12 dBW", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", dBm_t(120.0));
wpi::util::print("{}", dBm_t(120.0));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("120 dBm", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", miles_per_hour_t(72.1));
wpi::util::print("{}", miles_per_hour_t(72.1));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("72.1 mph", output.c_str());
// undefined unit
testing::internal::CaptureStdout();
wpi::print("{}", units::math::cpow<4>(meter_t(2)));
wpi::util::print("{}", wpi::units::math::cpow<4>(meter_t(2)));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("16 m^4", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{}", units::math::cpow<3>(foot_t(2)));
wpi::util::print("{}", wpi::units::math::cpow<3>(foot_t(2)));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("8 cu_ft", output.c_str());
testing::internal::CaptureStdout();
wpi::print("{:.9}", units::math::cpow<4>(foot_t(2)));
wpi::util::print("{:.9}", wpi::units::math::cpow<4>(foot_t(2)));
output = testing::internal::GetCapturedStdout();
EXPECT_STREQ("0.138095597 m^4", output.c_str());
// constants
testing::internal::CaptureStdout();
wpi::print("{:.8}", constants::k_B);
wpi::util::print("{:.8}", constants::k_B);
output = testing::internal::GetCapturedStdout();
#if defined(_MSC_VER) && (_MSC_VER <= 1800)
EXPECT_STREQ("1.3806485e-023 m^2 kg s^-2 K^-1", output.c_str());
@@ -1488,7 +1488,7 @@ TEST_F(UnitContainer, fmtlib) {
#endif
testing::internal::CaptureStdout();
wpi::print("{:.9}", constants::mu_B);
wpi::util::print("{:.9}", constants::mu_B);
output = testing::internal::GetCapturedStdout();
#if defined(_MSC_VER) && (_MSC_VER <= 1800)
EXPECT_STREQ("9.27400999e-024 m^2 A", output.c_str());
@@ -1497,7 +1497,7 @@ TEST_F(UnitContainer, fmtlib) {
#endif
testing::internal::CaptureStdout();
wpi::print("{:.7}", constants::sigma);
wpi::util::print("{:.7}", constants::sigma);
output = testing::internal::GetCapturedStdout();
#if defined(_MSC_VER) && (_MSC_VER <= 1800)
EXPECT_STREQ("5.670367e-008 kg s^-3 K^-4", output.c_str());
@@ -1509,10 +1509,10 @@ TEST_F(UnitContainer, fmtlib) {
TEST_F(UnitContainer, to_string) {
foot_t a(3.5);
EXPECT_STREQ("3.5 ft", units::length::to_string(a).c_str());
EXPECT_STREQ("3.5 ft", wpi::units::length::to_string(a).c_str());
meter_t b(8);
EXPECT_STREQ("8 m", units::length::to_string(b).c_str());
EXPECT_STREQ("8 m", wpi::units::length::to_string(b).c_str());
}
TEST_F(UnitContainer, DISABLED_to_string_locale) {
@@ -1530,10 +1530,10 @@ TEST_F(UnitContainer, DISABLED_to_string_locale) {
EXPECT_EQ(point_de, ',');
kilometer_t de = 2_km;
EXPECT_STREQ("2 km", units::length::to_string(de).c_str());
EXPECT_STREQ("2 km", wpi::units::length::to_string(de).c_str());
de = 2.5_km;
EXPECT_STREQ("2,5 km", units::length::to_string(de).c_str());
EXPECT_STREQ("2,5 km", wpi::units::length::to_string(de).c_str());
// US locale
#if defined(_MSC_VER)
@@ -1547,20 +1547,20 @@ TEST_F(UnitContainer, DISABLED_to_string_locale) {
EXPECT_EQ(point_us, '.');
mile_t us = 2_mi;
EXPECT_STREQ("2 mi", units::length::to_string(us).c_str());
EXPECT_STREQ("2 mi", wpi::units::length::to_string(us).c_str());
us = 2.5_mi;
EXPECT_STREQ("2.5 mi", units::length::to_string(us).c_str());
EXPECT_STREQ("2.5 mi", wpi::units::length::to_string(us).c_str());
}
TEST_F(UnitContainer, nameAndAbbreviation) {
foot_t a(3.5);
EXPECT_STREQ("ft", units::abbreviation(a));
EXPECT_STREQ("ft", wpi::units::abbreviation(a));
EXPECT_STREQ("ft", a.abbreviation());
EXPECT_STREQ("foot", a.name());
meter_t b(8);
EXPECT_STREQ("m", units::abbreviation(b));
EXPECT_STREQ("m", wpi::units::abbreviation(b));
EXPECT_STREQ("m", b.abbreviation());
EXPECT_STREQ("meter", b.name());
}
@@ -2033,17 +2033,17 @@ TEST_F(UnitConversion, acceleration) {
TEST_F(UnitConversion, force) {
double test;
test = convert<units::force::newton, units::force::newton>(1.0);
test = convert<wpi::units::force::newton, wpi::units::force::newton>(1.0);
EXPECT_NEAR(1.0, test, 5.0e-5);
test = convert<units::force::newton, units::force::pounds>(6.3);
test = convert<wpi::units::force::newton, wpi::units::force::pounds>(6.3);
EXPECT_NEAR(1.4163, test, 5.0e-5);
test = convert<units::force::newton, units::force::dynes>(5.0);
test = convert<wpi::units::force::newton, wpi::units::force::dynes>(5.0);
EXPECT_NEAR(500000.0, test, 5.0e-5);
test = convert<units::force::newtons, units::force::poundals>(2.1);
test = convert<wpi::units::force::newtons, wpi::units::force::poundals>(2.1);
EXPECT_NEAR(15.1893, test, 5.0e-5);
test = convert<units::force::newtons, units::force::kiloponds>(173.0);
test = convert<wpi::units::force::newtons, wpi::units::force::kiloponds>(173.0);
EXPECT_NEAR(17.6411, test, 5.0e-5);
test = convert<units::force::poundals, units::force::kiloponds>(21.879);
test = convert<wpi::units::force::poundals, wpi::units::force::kiloponds>(21.879);
EXPECT_NEAR(0.308451933, test, 5.0e-10);
}
@@ -2635,16 +2635,16 @@ TEST_F(UnitConversion, data_transfer_rate) {
TEST_F(UnitConversion, pi) {
EXPECT_TRUE(
units::traits::is_dimensionless_unit_v<decltype(constants::pi)>);
EXPECT_TRUE(units::traits::is_dimensionless_unit_v<constants::PI>);
wpi::units::traits::is_dimensionless_unit_v<decltype(constants::pi)>);
EXPECT_TRUE(wpi::units::traits::is_dimensionless_unit_v<constants::PI>);
// implicit conversion/arithmetic
EXPECT_NEAR(3.14159, constants::pi, 5.0e-6);
EXPECT_NEAR(6.28318531, (2 * constants::pi), 5.0e-9);
EXPECT_NEAR(6.28318531, (constants::pi + constants::pi), 5.0e-9);
EXPECT_NEAR(0.0, (constants::pi - constants::pi), 5.0e-9);
EXPECT_NEAR(31.00627668, units::math::cpow<3>(constants::pi), 5.0e-10);
EXPECT_NEAR(0.0322515344, (1.0 / units::math::cpow<3>(constants::pi)),
EXPECT_NEAR(31.00627668, wpi::units::math::cpow<3>(constants::pi), 5.0e-10);
EXPECT_NEAR(0.0322515344, (1.0 / wpi::units::math::cpow<3>(constants::pi)),
5.0e-11);
EXPECT_TRUE(constants::detail::PI_VAL == constants::pi);
EXPECT_TRUE(1.0 != constants::pi);
@@ -2748,10 +2748,10 @@ TEST_F(UnitConversion, std_chrono) {
}
TEST_F(UnitConversion, squaredTemperature) {
using squared_celsius = units::compound_unit<squared<celsius>>;
using squared_celsius_t = units::unit_t<squared_celsius>;
using squared_celsius = wpi::units::compound_unit<squared<celsius>>;
using squared_celsius_t = wpi::units::unit_t<squared_celsius>;
const squared_celsius_t right(100);
const celsius_t rootRight = units::math::sqrt(right);
const celsius_t rootRight = wpi::units::math::sqrt(right);
EXPECT_EQ(celsius_t(10), rootRight);
}
@@ -3127,8 +3127,8 @@ TEST_F(Constexpr, arithmetic) {
constexpr auto result5(meter_t(1) - meter_t(1));
constexpr auto result6(meter_t(1) * meter_t(1));
constexpr auto result7(meter_t(1) / meter_t(1));
constexpr auto result8(units::math::cpow<2>(meter_t(2)));
constexpr auto result9 = units::math::cpow<3>(2_m);
constexpr auto result8(wpi::units::math::cpow<2>(meter_t(2)));
constexpr auto result9 = wpi::units::math::cpow<3>(2_m);
constexpr auto result10 = 2_m * 2_m;
EXPECT_TRUE(noexcept(result0));
@@ -3329,7 +3329,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) {
EXPECT_TRUE((
traits::is_unit_value_t_category_v<category::area_unit, productF2>));
using nRatio = unit_value_t<units::force::newton, 5>;
using nRatio = unit_value_t<wpi::units::force::newton, 5>;
using productN = unit_value_multiply<nRatio, ftRatio>;
EXPECT_FALSE(
@@ -3517,11 +3517,11 @@ TEST_F(CaseStudies, pythagoreanTheorum) {
TEST(Units, overloadResolution) {
// Slight hack to get nested functions
struct Scope {
static bool f(units::meter_t) {
static bool f(wpi::units::meter_t) {
return true;
};
static bool f(units::second_t) {
static bool f(wpi::units::second_t) {
return false;
};
};

48
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
View File

@@ -35,22 +35,22 @@ using Kg_unit = decltype(1_V);
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::volt_t input, units::second_t dt) {
frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
wpi::math::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
wpi::units::radian_t currentAngle,
wpi::units::radians_per_second_t currentVelocity,
wpi::units::volt_t input, wpi::units::second_t dt) {
wpi::math::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
wpi::math::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
return frc::RK4(
[&](const frc::Matrixd<2, 1>& x,
const frc::Matrixd<1, 1>& u) -> frc::Matrixd<2, 1> {
frc::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::sgn(x(1)) -
return wpi::math::RK4(
[&](const wpi::math::Matrixd<2, 1>& x,
const wpi::math::Matrixd<1, 1>& u) -> wpi::math::Matrixd<2, 1> {
wpi::math::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::util::sgn(x(1)) -
Kg.value() / Ka.value() * std::cos(x(0))};
return A * x + B * u + c;
},
frc::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},
frc::Matrixd<1, 1>{input.value()}, dt);
wpi::math::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},
wpi::math::Matrixd<1, 1>{input.value()}, dt);
}
/**
@@ -66,12 +66,12 @@ frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
* @param input The input voltage.
* @param dt The simulation time.
*/
void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
void CalculateAndSimulate(const wpi::math::ArmFeedforward& armFF, Ks_unit Ks,
Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
wpi::units::radian_t currentAngle,
wpi::units::radians_per_second_t currentVelocity,
wpi::units::radians_per_second_t nextVelocity,
wpi::units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
EXPECT_NEAR(
nextVelocity.value(),
@@ -86,7 +86,7 @@ TEST(ArmFeedforwardTest, Calculate) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(angle, angular velocity)
EXPECT_NEAR(
@@ -112,7 +112,7 @@ TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
constexpr auto currentAngle = 1_rad;
constexpr auto currentVelocity = 0.02_rad_per_s;
@@ -123,7 +123,7 @@ TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
EXPECT_DOUBLE_EQ(
armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
(Ks + Kv * currentVelocity + Ka * averageAccel +
Kg * units::math::cos(currentAngle))
Kg * wpi::units::math::cos(currentAngle))
.value());
}
@@ -132,7 +132,7 @@ TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
0_rad_per_s, 20_ms);
@@ -143,7 +143,7 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
@@ -162,7 +162,7 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
@@ -191,7 +191,7 @@ TEST(ArmFeedforwardTest, NegativeGains) {
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
wpi::math::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(armFF.GetKv().value(), 0);
EXPECT_EQ(armFF.GetKa().value(), 0);

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

@@ -7,13 +7,13 @@
#include "wpi/math/controller/BangBangController.hpp"
TEST(BangBangInputOutputTest, ShouldOutput) {
frc::BangBangController controller;
wpi::math::BangBangController controller;
EXPECT_DOUBLE_EQ(controller.Calculate(0, 1), 1);
}
TEST(BangBangInputOutputTest, ShouldNotOutput) {
frc::BangBangController controller;
wpi::math::BangBangController controller;
EXPECT_DOUBLE_EQ(controller.Calculate(1, 0), 0);
}

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

@@ -7,7 +7,7 @@
#include "wpi/math/controller/BangBangController.hpp"
TEST(BangBangToleranceTest, InTolerance) {
frc::BangBangController controller{0.1};
wpi::math::BangBangController controller{0.1};
controller.SetSetpoint(1);
controller.Calculate(1);
@@ -15,7 +15,7 @@ TEST(BangBangToleranceTest, InTolerance) {
}
TEST(BangBangToleranceTest, OutOfTolerance) {
frc::BangBangController controller{0.1};
wpi::math::BangBangController controller{0.1};
controller.SetSetpoint(1);
controller.Calculate(0);

8
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
Vectord<2> Dynamics(const Vectord<2>& x, const Vectord<1>& u) {
return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
@@ -20,7 +20,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) {
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
wpi::math::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
20_ms};
Vectord<2> r{2, 2};
@@ -30,7 +30,7 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
wpi::math::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
&StateDynamics, Matrixd<2, 1>{{0.0}, {1.0}}, 20_ms};
Vectord<2> r{2, 2};
@@ -39,4 +39,4 @@ TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
} // namespace frc
} // namespace wpi::math

100
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
View File

@@ -8,11 +8,11 @@
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/units/math.hpp"
namespace frc {
namespace wpi::math {
TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto maxA = 2_mps_sq;
constexpr auto maxAlpha = 2_rad_per_s_sq;
@@ -31,15 +31,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
{
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(units::math::abs(a), maxA);
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(wpi::units::math::abs(a), maxA);
}
{
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_GT(units::math::abs(alpha), maxAlpha);
EXPECT_GT(wpi::units::math::abs(alpha), maxAlpha);
}
// Forward
@@ -47,19 +47,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_LE(units::math::abs(a), maxA);
EXPECT_LE(units::math::abs(alpha), maxAlpha);
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
// Backward
@@ -67,19 +67,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_LE(units::math::abs(a), maxA);
EXPECT_LE(units::math::abs(alpha), maxAlpha);
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
// Rotate CCW
@@ -87,25 +87,25 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_LE(units::math::abs(a), maxA);
EXPECT_LE(units::math::abs(alpha), maxAlpha);
EXPECT_LE(wpi::units::math::abs(a), maxA);
EXPECT_LE(wpi::units::math::abs(alpha), maxAlpha);
}
}
TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -126,9 +126,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
@@ -143,9 +143,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
@@ -160,9 +160,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
@@ -173,7 +173,7 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
constexpr auto trackwidth = 0.9_m;
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto minA = -1_mps_sq;
constexpr auto maxA = 2_mps_sq;
constexpr auto maxAlpha = 2_rad_per_s_sq;
@@ -193,9 +193,9 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
{
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(units::math::abs(a), maxA);
EXPECT_GT(units::math::abs(a), -minA);
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(wpi::units::math::abs(a), maxA);
EXPECT_GT(wpi::units::math::abs(a), -minA);
}
// a should always be within [minA, maxA]
@@ -204,15 +204,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GE(a, minA);
EXPECT_LE(a, maxA);
}
@@ -222,15 +222,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
accelLimiter.Calculate(wpi::units::meters_per_second_t{xAccelLimiter(0)},
wpi::units::meters_per_second_t{xAccelLimiter(1)},
wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)});
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
wpi::units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GE(a, minA);
EXPECT_LE(a, maxA);
}
@@ -254,4 +254,4 @@ TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) {
std::invalid_argument);
}
} // namespace frc
} // namespace wpi::math

16
wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
View File

@@ -20,12 +20,12 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
constexpr auto kVAngular = 1_V / 1_rad_per_s;
constexpr auto kAAngular = 1_V / 1_rad_per_s_sq;
constexpr auto trackwidth = 1_m;
constexpr units::second_t dt = 20_ms;
constexpr wpi::units::second_t dt = 20_ms;
frc::DifferentialDriveFeedforward differentialDriveFeedforward{
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
frc::LinearSystem<2, 2, 2> plant =
frc::LinearSystemId::IdentifyDrivetrainSystem(
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {
@@ -54,12 +54,12 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
constexpr auto kALinear = 1_V / 1_mps_sq;
constexpr auto kVAngular = 1_V / 1_mps;
constexpr auto kAAngular = 1_V / 1_mps_sq;
constexpr units::second_t dt = 20_ms;
constexpr wpi::units::second_t dt = 20_ms;
frc::DifferentialDriveFeedforward differentialDriveFeedforward{
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular};
frc::LinearSystem<2, 2, 2> plant =
frc::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
kVAngular, kAAngular);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {

20
wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
View File

@@ -19,24 +19,24 @@ static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m;
static constexpr auto Kg = 1_V;
TEST(ElevatorFeedforwardTest, Calculate) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
frc::Matrixd<1, 1> B{1.0 / Ka.value()};
constexpr units::second_t dt = 20_ms;
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
wpi::math::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
wpi::math::Matrixd<1, 1> B{1.0 / Ka.value()};
constexpr wpi::units::second_t dt = 20_ms;
wpi::math::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::Vectord<1> r{2.0};
frc::Vectord<1> nextR{3.0};
wpi::math::Vectord<1> r{2.0};
wpi::math::Vectord<1> nextR{3.0};
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
elevatorFF.Calculate(2_mps, 3_mps).value(), 0.002);
}
TEST(ElevatorFeedforwardTest, AchievableVelocity) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.MaxAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
5, 0.002);
EXPECT_NEAR(elevatorFF.MinAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
@@ -44,7 +44,7 @@ TEST(ElevatorFeedforwardTest, AchievableVelocity) {
}
TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, 2_m / 1_s).value(),
3.75, 0.002);
EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, -2_m / 1_s).value(),
@@ -56,7 +56,7 @@ TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
}
TEST(ElevatorFeedforwardTest, NegativeGains) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, -Kv, -Ka};
wpi::math::ElevatorFeedforward elevatorFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(elevatorFF.GetKv().value(), 0);
EXPECT_EQ(elevatorFF.GetKa().value(), 0);
}

8
wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
View File

@@ -7,10 +7,10 @@
#include "wpi/math/controller/ImplicitModelFollower.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
namespace frc {
namespace wpi::math {
TEST(ImplicitModelFollowerTest, SameModel) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -54,7 +54,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
TEST(ImplicitModelFollowerTest, SlowerRefModel) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
@@ -106,4 +106,4 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
}
}
} // namespace frc
} // namespace wpi::math

40
wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
View File

@@ -14,10 +14,10 @@
#include "wpi/units/math.hpp"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
EXPECT_LE(wpi::units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
/**
@@ -45,14 +45,14 @@ static constexpr auto kLinearV = 3.02_V / 1_mps;
static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
static constexpr auto kAngularV = 1.382_V / 1_mps;
static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem(
static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
kLinearV, kLinearA, kAngularV, kAngularA);
static constexpr auto kTrackwidth = 0.9_m;
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
frc::Vectord<5> xdot;
wpi::math::Vectord<5> xdot;
xdot(0) = v * std::cos(x(State::kHeading));
xdot(1) = v * std::sin(x(State::kHeading));
xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / kTrackwidth)
@@ -62,18 +62,18 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
}
TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
constexpr units::second_t kDt = 20_ms;
constexpr wpi::units::second_t kDt = 20_ms;
frc::LTVDifferentialDriveController controller{
wpi::math::LTVDifferentialDriveController controller{
plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
wpi::math::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
auto waypoints = std::vector{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> x = frc::Vectord<5>::Zero();
wpi::math::Vectord<5> x = wpi::math::Vectord<5>::Zero();
x(State::kX) = robotPose.X().value();
x(State::kY) = robotPose.Y().value();
x(State::kHeading) = robotPose.Rotation().Radians().value();
@@ -82,21 +82,21 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
robotPose =
frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)},
units::radian_t{x(State::kHeading)}};
wpi::math::Pose2d{wpi::units::meter_t{x(State::kX)}, wpi::units::meter_t{x(State::kY)},
wpi::units::radian_t{x(State::kHeading)}};
auto [leftVoltage, rightVoltage] = controller.Calculate(
robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
units::meters_per_second_t{x(State::kRightVelocity)}, state);
robotPose, wpi::units::meters_per_second_t{x(State::kLeftVelocity)},
wpi::units::meters_per_second_t{x(State::kRightVelocity)}, state);
x = frc::RKDP(&Dynamics, x,
frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
x = wpi::math::RKDP(&Dynamics, x,
wpi::math::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
kDt);
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
EXPECT_NEAR_UNITS(wpi::math::AngleModulus(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}

22
wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
View File

@@ -10,21 +10,21 @@
#include "wpi/units/math.hpp"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
EXPECT_LE(wpi::units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
static constexpr wpi::units::meter_t kTolerance{1 / 12.0};
static constexpr wpi::units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
TEST(LTVUnicycleControllerTest, ReachesReference) {
constexpr units::second_t kDt = 20_ms;
constexpr wpi::units::second_t kDt = 20_ms;
frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
wpi::math::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
wpi::math::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
auto waypoints = std::vector{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
auto totalTime = trajectory.TotalTime();
@@ -33,13 +33,13 @@ TEST(LTVUnicycleControllerTest, ReachesReference) {
auto [vx, vy, omega] = controller.Calculate(robotPose, state);
static_cast<void>(vy);
robotPose = robotPose + frc::Twist2d{vx * kDt, 0_m, omega * kDt}.Exp();
robotPose = robotPose + wpi::math::Twist2d{vx * kDt, 0_m, omega * kDt}.Exp();
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
EXPECT_NEAR_UNITS(wpi::math::AngleModulus(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);
}

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

@@ -10,13 +10,13 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
Matrixd<2, 2> A{{1, 0}, {0, 1}};
Matrixd<2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
wpi::math::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
@@ -24,4 +24,4 @@ TEST(LinearPlantInversionFeedforwardTest, Calculate) {
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}
} // namespace frc
} // namespace wpi::math

14
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
View File

@@ -13,7 +13,7 @@
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/units/time.hpp"
namespace frc {
namespace wpi::math {
TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
LinearSystem<2, 1, 1> plant = [] {
@@ -28,7 +28,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K();
@@ -50,7 +50,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
// Gear ratio
constexpr double G = 100.0;
return frc::LinearSystemId::SingleJointedArmSystem(motors,
return wpi::math::LinearSystemId::SingleJointedArmSystem(motors,
1.0 / 3.0 * m * r * r, G)
.Slice(0);
}();
@@ -76,7 +76,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
@@ -103,7 +103,7 @@ template <int States, int Inputs>
Matrixd<Inputs, States> GetImplicitModelFollowingK(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, States>& Aref, units::second_t dt) {
const Matrixd<States, States>& Aref, wpi::units::second_t dt) {
// Discretize real dynamics
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
@@ -178,7 +178,7 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms};
@@ -188,4 +188,4 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
}
} // namespace frc
} // namespace wpi::math

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

@@ -7,7 +7,7 @@
#include "wpi/math/controller/PIDController.hpp"
TEST(PIDInputOutputTest, ContinuousInput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetP(1);
controller.EnableContinuousInput(-180, 180);
@@ -18,7 +18,7 @@ TEST(PIDInputOutputTest, ContinuousInput) {
}
TEST(PIDInputOutputTest, ProportionalGainOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetP(4);
@@ -26,7 +26,7 @@ TEST(PIDInputOutputTest, ProportionalGainOutput) {
}
TEST(PIDInputOutputTest, IntegralGainOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetI(4);
@@ -40,7 +40,7 @@ TEST(PIDInputOutputTest, IntegralGainOutput) {
}
TEST(PIDInputOutputTest, DerivativeGainOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetD(4);
@@ -51,7 +51,7 @@ TEST(PIDInputOutputTest, DerivativeGainOutput) {
}
TEST(PIDInputOutputTest, IZoneNoOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetI(1);
controller.SetIZone(1);
@@ -62,7 +62,7 @@ TEST(PIDInputOutputTest, IZoneNoOutput) {
}
TEST(PIDInputOutputTest, IZoneOutput) {
frc::PIDController controller{0.0, 0.0, 0.0};
wpi::math::PIDController controller{0.0, 0.0, 0.0};
controller.SetI(1);
controller.SetIZone(1);

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

@@ -11,14 +11,14 @@ static constexpr double kRange = 200;
static constexpr double kTolerance = 10.0;
TEST(PIDToleranceTest, InitialTolerance) {
frc::PIDController controller{0.5, 0.0, 0.0};
wpi::math::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
EXPECT_FALSE(controller.AtSetpoint());
}
TEST(PIDToleranceTest, AbsoluteTolerance) {
frc::PIDController controller{0.5, 0.0, 0.0};
wpi::math::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);
EXPECT_FALSE(controller.AtSetpoint());

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

@@ -12,86 +12,86 @@
#include "wpi/units/angular_velocity.hpp"
TEST(ProfiledPIDInputOutputTest, ContinuousInput1) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(-180_deg, 180_deg);
static constexpr units::degree_t kSetpoint{-179.0};
static constexpr units::degree_t kMeasurement{-179.0};
static constexpr units::degree_t kGoal{179.0};
static constexpr wpi::units::degree_t kSetpoint{-179.0};
static constexpr wpi::units::degree_t kMeasurement{-179.0};
static constexpr wpi::units::degree_t kGoal{179.0};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
180_deg);
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput2) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(-units::radian_t{std::numbers::pi},
units::radian_t{std::numbers::pi});
controller.EnableContinuousInput(-wpi::units::radian_t{std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
static constexpr units::radian_t kSetpoint{-3.4826633343199735};
static constexpr units::radian_t kMeasurement{-3.1352207333939606};
static constexpr units::radian_t kGoal{-3.534162788601621};
static constexpr wpi::units::radian_t kSetpoint{-3.4826633343199735};
static constexpr wpi::units::radian_t kMeasurement{-3.1352207333939606};
static constexpr wpi::units::radian_t kGoal{-3.534162788601621};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
units::radian_t{std::numbers::pi});
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::units::radian_t{std::numbers::pi});
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput3) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(-units::radian_t{std::numbers::pi},
units::radian_t{std::numbers::pi});
controller.EnableContinuousInput(-wpi::units::radian_t{std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
static constexpr units::radian_t kSetpoint{-3.5176604690006377};
static constexpr units::radian_t kMeasurement{3.1191729343822456};
static constexpr units::radian_t kGoal{2.709680418117445};
static constexpr wpi::units::radian_t kSetpoint{-3.5176604690006377};
static constexpr wpi::units::radian_t kMeasurement{3.1191729343822456};
static constexpr wpi::units::radian_t kGoal{2.709680418117445};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
units::radian_t{std::numbers::pi});
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::units::radian_t{std::numbers::pi});
}
TEST(ProfiledPIDInputOutputTest, ContinuousInput4) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(1);
controller.EnableContinuousInput(0_rad,
units::radian_t{2.0 * std::numbers::pi});
wpi::units::radian_t{2.0 * std::numbers::pi});
static constexpr units::radian_t kSetpoint{2.78};
static constexpr units::radian_t kMeasurement{3.12};
static constexpr units::radian_t kGoal{2.71};
static constexpr wpi::units::radian_t kSetpoint{2.78};
static constexpr wpi::units::radian_t kMeasurement{3.12};
static constexpr wpi::units::radian_t kGoal{2.71};
controller.Reset(kSetpoint);
EXPECT_LT(controller.Calculate(kMeasurement, kGoal), 0.0);
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller.GetSetpoint().position - kMeasurement),
units::radian_t{std::numbers::pi});
EXPECT_LT(wpi::units::math::abs(controller.GetSetpoint().position - kMeasurement),
wpi::units::radian_t{std::numbers::pi});
}
TEST(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetP(4);
@@ -100,7 +100,7 @@ TEST(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
}
TEST(ProfiledPIDInputOutputTest, IntegralGainOutput) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetI(4);
@@ -115,7 +115,7 @@ TEST(ProfiledPIDInputOutputTest, IntegralGainOutput) {
}
TEST(ProfiledPIDInputOutputTest, DerivativeGainOutput) {
frc::ProfiledPIDController<units::degree> controller{
wpi::math::ProfiledPIDController<wpi::units::degree> controller{
0.0, 0.0, 0.0, {360_deg_per_s, 180_deg_per_s_sq}};
controller.SetD(4);

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

@@ -14,19 +14,19 @@
#include "wpi/units/time.hpp"
#include "wpi/units/velocity.hpp"
namespace frc {
namespace wpi::math {
TEST(SimpleMotorFeedforwardTest, Calculate) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 3_V / 1_mps;
constexpr auto Ka = 0.6_V / 1_mps_sq;
constexpr units::second_t dt = 20_ms;
constexpr wpi::units::second_t dt = 20_ms;
constexpr Matrixd<1, 1> A{{-Kv.value() / Ka.value()}};
constexpr Matrixd<1, 1> B{{1.0 / Ka.value()}};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka};
wpi::math::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> simpleMotor{Ks, Kv, Ka};
constexpr Vectord<1> r{{2.0}};
constexpr Vectord<1> nextR{{3.0}};
@@ -46,11 +46,11 @@ TEST(SimpleMotorFeedforwardTest, NegativeGains) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = -3_V / 1_mps;
constexpr auto Ka = -0.6_V / 1_mps_sq;
constexpr units::second_t dt = 0_ms;
frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka, dt};
constexpr wpi::units::second_t dt = 0_ms;
wpi::math::SimpleMotorFeedforward<wpi::units::meter> simpleMotor{Ks, Kv, Ka, dt};
EXPECT_EQ(simpleMotor.GetKv().value(), 0);
EXPECT_EQ(simpleMotor.GetKa().value(), 0);
EXPECT_EQ(simpleMotor.GetDt().value(), 0.02);
}
} // namespace frc
} // namespace wpi::math

6
wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/controller/ArmFeedforward.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -19,8 +19,8 @@ const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
} // namespace
TEST(ArmFeedforwardProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

2
wpimath/src/test/native/cpp/controller/proto/DifferentialDriveFeedforwardProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "../../ProtoTestBase.hpp"
#include "wpi/math/controller/DifferentialDriveFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
struct DifferentialDriveFeedforwardProtoTestData {
using Type = DifferentialDriveFeedforward;

8
wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp
View File

@@ -7,19 +7,19 @@
#include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using ProtoType = wpi::Protobuf<frc::DifferentialDriveWheelVoltages>;
using ProtoType = wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>;
const DifferentialDriveWheelVoltages kExpectedData =
DifferentialDriveWheelVoltages{0.174_V, 0.191_V};
} // namespace
TEST(DifferentialDriveWheelVoltagesProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/controller/ElevatorFeedforward.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -20,8 +20,8 @@ constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
} // namespace
TEST(ElevatorFeedforwardProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

12
wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp
View File

@@ -10,15 +10,15 @@
#include "wpi/units/acceleration.hpp"
#include "wpi/units/velocity.hpp"
using namespace frc;
using namespace wpi::math;
template <typename T>
struct SimpleMotorFeedforwardProtoTestData {
using Type = SimpleMotorFeedforward<T>;
inline static const Type kTestData = {
units::volt_t{0.4}, units::volt_t{4.0} / (units::unit_t<T>{1} / 1_s),
units::volt_t{0.7} / (units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
wpi::units::volt_t{0.4}, wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
wpi::units::volt_t{0.7} / (wpi::units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
static void CheckEq(const Type& testData, const Type& data) {
EXPECT_EQ(testData.GetKs().value(), data.GetKs().value());
@@ -30,10 +30,10 @@ struct SimpleMotorFeedforwardProtoTestData {
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardMeters, ProtoTest,
SimpleMotorFeedforwardProtoTestData<units::meters>);
SimpleMotorFeedforwardProtoTestData<wpi::units::meters>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardFeet, ProtoTest,
SimpleMotorFeedforwardProtoTestData<units::feet>);
SimpleMotorFeedforwardProtoTestData<wpi::units::feet>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardRadians, ProtoTest,
SimpleMotorFeedforwardProtoTestData<units::radians>);
SimpleMotorFeedforwardProtoTestData<wpi::units::radians>);

4
wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/controller/ArmFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::ArmFeedforward>;
using StructType = wpi::util::Struct<wpi::math::ArmFeedforward>;
static constexpr auto Ks = 1.91_V;
static constexpr auto Kg = 2.29_V;

2
wpimath/src/test/native/cpp/controller/struct/DifferentialDriveFeedforwardStructTest.cpp
View File

@@ -7,7 +7,7 @@
#include "../../StructTestBase.hpp"
#include "wpi/math/controller/DifferentialDriveFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
struct DifferentialDriveFeedforwardStructTestData {
using Type = DifferentialDriveFeedforward;

4
wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages>;
const DifferentialDriveWheelVoltages kExpectedData{
DifferentialDriveWheelVoltages{0.174_V, 0.191_V}};
} // namespace

4
wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/controller/ElevatorFeedforward.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::ElevatorFeedforward>;
using StructType = wpi::util::Struct<wpi::math::ElevatorFeedforward>;
static constexpr auto Ks = 1.91_V;
static constexpr auto Kg = 2.29_V;

12
wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp
View File

@@ -10,15 +10,15 @@
#include "wpi/units/acceleration.hpp"
#include "wpi/units/velocity.hpp"
using namespace frc;
using namespace wpi::math;
template <typename T>
struct SimpleMotorFeedforwardStructTestData {
using Type = SimpleMotorFeedforward<T>;
inline static const Type kTestData = {
units::volt_t{0.4}, units::volt_t{4.0} / (units::unit_t<T>{1} / 1_s),
units::volt_t{0.7} / (units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
wpi::units::volt_t{0.4}, wpi::units::volt_t{4.0} / (wpi::units::unit_t<T>{1} / 1_s),
wpi::units::volt_t{0.7} / (wpi::units::unit_t<T>{1} / 1_s / 1_s), 25_ms};
static void CheckEq(const Type& testData, const Type& data) {
EXPECT_EQ(testData.GetKs().value(), data.GetKs().value());
@@ -30,10 +30,10 @@ struct SimpleMotorFeedforwardStructTestData {
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardMeters, StructTest,
SimpleMotorFeedforwardStructTestData<units::meters>);
SimpleMotorFeedforwardStructTestData<wpi::units::meters>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardFeet, StructTest,
SimpleMotorFeedforwardStructTestData<units::feet>);
SimpleMotorFeedforwardStructTestData<wpi::units::feet>);
INSTANTIATE_TYPED_TEST_SUITE_P(
SimpleMotorFeedforwardRadians, StructTest,
SimpleMotorFeedforwardStructTestData<units::radians>);
SimpleMotorFeedforwardStructTestData<wpi::units::radians>);

14
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/linalg/EigenCore.hpp"
TEST(AngleStatisticsTest, Mean) {
frc::Matrixd<3, 3> sigmas{
wpi::math::Matrixd<3, 3> sigmas{
{1, 1.2, 0},
{359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
{1, 2, 0}};
@@ -19,7 +19,7 @@ TEST(AngleStatisticsTest, Mean) {
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(frc::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
.isApprox(wpi::math::AngleMean<3, 3>(sigmas, weights, 1), 1e-3));
}
TEST(AngleStatisticsTest, Mean_DynamicSize) {
@@ -32,7 +32,7 @@ TEST(AngleStatisticsTest, Mean_DynamicSize) {
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(frc::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
.isApprox(wpi::math::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
sigmas, weights, 1),
1e-3));
}
@@ -41,7 +41,7 @@ TEST(AngleStatisticsTest, Residual) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
EXPECT_TRUE(wpi::math::AngleResidual<3>(a, b, 1).isApprox(
Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
}
@@ -49,7 +49,7 @@ TEST(AngleStatisticsTest, Residual_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
EXPECT_TRUE(wpi::math::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{0, 2 * std::numbers::pi / 180, 1}}));
}
@@ -57,13 +57,13 @@ TEST(AngleStatisticsTest, Add) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
EXPECT_TRUE(wpi::math::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}
TEST(AngleStatisticsTest, Add_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
EXPECT_TRUE(wpi::math::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{2, 0, 3}}));
}

224
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -22,43 +22,43 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
units::meter_t leftDistance = 0_m;
units::meter_t rightDistance = 0_m;
wpi::units::meter_t leftDistance = 0_m;
wpi::units::meter_t rightDistance = 0_m;
estimator.ResetPosition(frc::Rotation3d{}, leftDistance, rightDistance,
frc::Pose3d{startingPose});
estimator.ResetPosition(wpi::math::Rotation3d{}, leftDistance, rightDistance,
wpi::math::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print(
wpi::util::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
"right\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
@@ -66,9 +66,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -77,7 +77,7 @@ void testFollowTrajectory(
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
estimator.AddVisionMeasurement(wpi::math::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
@@ -92,13 +92,13 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
leftDistance, rightDistance);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -119,14 +119,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -153,73 +153,73 @@ void testFollowTrajectory(
}
TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{kinematics,
frc::Rotation3d{},
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{},
wpi::math::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{kinematics,
frc::Rotation3d{},
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{},
wpi::math::Pose3d{},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
for (wpi::units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -231,33 +231,33 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator3d estimator{
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation3d{}, 0_m, 0_m);
estimator.UpdateWithTime(0_s, wpi::math::Rotation3d{}, 0_m, 0_m);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}},
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
@@ -265,9 +265,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
@@ -275,9 +275,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
@@ -286,22 +286,22 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{},
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
{0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation3d{}, 0_m, 0_m);
estimator.UpdateWithTime(time, wpi::math::Rotation3d{}, 0_m, 0_m);
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -319,9 +319,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{},
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
@@ -331,60 +331,60 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{},
units::meter_t{time}, units::meter_t{time});
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wpi::units::meter_t{time}, wpi::units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(
frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s);
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
0_m,
0_m,
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -397,8 +397,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
estimator.ResetPosition(frc::Rotation3d{}, 1_m, 1_m,
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
estimator.ResetPosition(wpi::math::Rotation3d{}, 1_m, 1_m,
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -408,7 +408,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation3d{}, 2_m, 2_m);
estimator.Update(wpi::math::Rotation3d{}, 2_m, 2_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -418,7 +418,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -429,7 +429,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
estimator.Update(frc::Rotation3d{}, 3_m, 3_m);
estimator.Update(wpi::math::Rotation3d{}, 3_m, 3_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -440,7 +440,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -451,7 +451,7 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
estimator.ResetPose(wpi::math::Pose3d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -23,43 +23,43 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
units::meter_t leftDistance = 0_m;
units::meter_t rightDistance = 0_m;
wpi::units::meter_t leftDistance = 0_m;
wpi::units::meter_t rightDistance = 0_m;
estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance,
estimator.ResetPosition(wpi::math::Rotation2d{}, leftDistance, rightDistance,
startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print(
wpi::util::print(
"time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
"right\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
@@ -67,9 +67,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -93,12 +93,12 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation(),
leftDistance, rightDistance);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -119,14 +119,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -149,65 +149,65 @@ void testFollowTrajectory(
}
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
for (wpi::units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -219,50 +219,50 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::DifferentialDriveKinematics kinematics{1.0_m};
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
wpi::math::Rotation2d{},
0_m,
0_m,
frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.02, 0.02, 0.01},
{0.1, 0.1, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, 0_m, 0_m);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
wpi::math::Pose2d{0_m, 0_m, wpi::math::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
wpi::math::Pose2d{3_m, 1_m, wpi::math::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
wpi::math::Pose2d{2_m, 4_m, wpi::math::Rotation2d{180_deg}}, 0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
@@ -270,22 +270,22 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
estimator.UpdateWithTime(time, wpi::math::Rotation2d{}, 0_m, 0_m);
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -298,9 +298,9 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
}
TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, 0_m, 0_m, wpi::math::Pose2d{},
{1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
// Returns empty when null
@@ -310,60 +310,60 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{},
units::meter_t{time}, units::meter_t{time});
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wpi::units::meter_t{time}, wpi::units::meter_t{time});
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
wpi::math::DifferentialDriveKinematics kinematics{1_m};
wpi::math::DifferentialDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
wpi::math::Rotation2d{},
0_m,
0_m,
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
wpi::math::Pose2d{-1_m, -1_m, wpi::math::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -374,8 +374,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m,
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
estimator.ResetPosition(wpi::math::Rotation2d{}, 1_m, 1_m,
wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -383,7 +383,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, 2_m, 2_m);
estimator.Update(wpi::math::Rotation2d{}, 2_m, 2_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -391,7 +391,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -400,7 +400,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, 3_m, 3_m);
estimator.Update(wpi::math::Rotation2d{}, 3_m, 3_m);
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -409,7 +409,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -418,7 +418,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
estimator.ResetPose(wpi::math::Pose2d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -18,8 +18,8 @@
namespace {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
wpi::math::Vectord<5> Dynamics(const wpi::math::Vectord<5>& x, const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
@@ -29,18 +29,18 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
(motors.Kv * motors.R * wpi::units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
auto k1 = (1 / m + wpi::units::math::pow<2>(rb) / J);
auto k2 = (1 / m - wpi::units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
wpi::units::meters_per_second_t vl{x(3)};
wpi::units::meters_per_second_t vr{x(4)};
wpi::units::volt_t Vl{u(0)};
wpi::units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
return wpi::math::Vectord<5>{
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()) +
@@ -49,59 +49,59 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> LocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
wpi::math::Vectord<3> LocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> GlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
wpi::math::Vectord<5> GlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
TEST(ExtendedKalmanFilterTest, Init) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
frc::Vectord<2> u{12.0, 12.0};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
}
TEST(ExtendedKalmanFilterTest, Convergence) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
wpi::math::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(Dynamics, wpi::math::Vectord<5>::Zero(),
wpi::math::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -109,20 +109,20 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
wpi::units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
wpi::units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
wpi::math::Vectord<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, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY = LocalMeasurementModel(nextR, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
@@ -133,7 +133,7 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());

6
wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
View File

@@ -15,7 +15,7 @@
#include "wpi/units/time.hpp"
TEST(KalmanFilterTest, Flywheel) {
auto motor = frc::DCMotor::NEO();
auto flywheel = frc::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
frc::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
auto motor = wpi::math::DCMotor::NEO();
auto flywheel = wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
}

264
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View File

@@ -16,40 +16,40 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
frc::MecanumDriveWheelPositions wheelPositions{};
wpi::math::MecanumDriveWheelPositions wheelPositions{};
estimator.ResetPosition(frc::Rotation3d{}, wheelPositions,
frc::Pose3d{startingPose});
estimator.ResetPosition(wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
@@ -57,9 +57,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -68,7 +68,7 @@ void testFollowTrajectory(
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
estimator.AddVisionMeasurement(wpi::math::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
@@ -85,13 +85,13 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
wheelPositions);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -111,14 +111,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -145,73 +145,73 @@ void testFollowTrajectory(
}
TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, wheelPositions,
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, wheelPositions,
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.1}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wheelPositions,
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
for (wpi::units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -223,36 +223,36 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator3d estimator{
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
wheelPositions,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation3d{}, wheelPositions);
estimator.UpdateWithTime(0_s, wpi::math::Rotation3d{}, wheelPositions);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}},
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
@@ -260,9 +260,9 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
@@ -270,9 +270,9 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
@@ -281,25 +281,25 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation3d{},
frc::MecanumDriveWheelPositions{});
estimator.UpdateWithTime(time, wpi::math::Rotation3d{},
wpi::math::MecanumDriveWheelPositions{});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -317,12 +317,12 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
}
TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{},
frc::Pose3d{}, {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics, wpi::math::Rotation3d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{}, {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -331,64 +331,64 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
frc::MecanumDriveWheelPositions wheelPositions{
units::meter_t{time}, units::meter_t{time}, units::meter_t{time},
units::meter_t{time}};
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{},
wpi::math::MecanumDriveWheelPositions wheelPositions{
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(
frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s);
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator3d estimator{
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
frc::MecanumDriveWheelPositions{},
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Rotation3d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -401,8 +401,8 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset position
estimator.ResetPosition(frc::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
estimator.ResetPosition(wpi::math::Rotation3d{}, {1_m, 1_m, 1_m, 1_m},
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -412,7 +412,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation3d{}, {2_m, 2_m, 2_m, 2_m});
estimator.Update(wpi::math::Rotation3d{}, {2_m, 2_m, 2_m, 2_m});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -422,7 +422,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -433,7 +433,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test orientation
estimator.Update(frc::Rotation3d{}, {3_m, 3_m, 3_m, 3_m});
estimator.Update(wpi::math::Rotation3d{}, {3_m, 3_m, 3_m, 3_m});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -444,7 +444,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -455,7 +455,7 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
estimator.ResetPose(wpi::math::Pose3d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -17,39 +17,39 @@
#include "wpi/util/print.hpp"
void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
frc::MecanumDriveWheelPositions wheelPositions{};
wpi::math::MecanumDriveWheelPositions wheelPositions{};
estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose);
estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions, startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
@@ -57,9 +57,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -85,12 +85,12 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation(),
wheelPositions);
if (debug) {
wpi::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(),
groundTruthState.pose.Y().value(),
@@ -110,14 +110,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -140,73 +140,73 @@ void testFollowTrajectory(
}
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}
TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
for (wpi::units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -218,50 +218,50 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{},
wheelPositions, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions);
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, wheelPositions);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
wpi::math::Pose2d{0_m, 0_m, wpi::math::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
wpi::math::Pose2d{3_m, 1_m, wpi::math::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
wpi::math::Pose2d{2_m, 4_m, wpi::math::Rotation2d{180_deg}}, 0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
@@ -269,25 +269,25 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation2d{},
frc::MecanumDriveWheelPositions{});
estimator.UpdateWithTime(time, wpi::math::Rotation2d{},
wpi::math::MecanumDriveWheelPositions{});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -300,12 +300,12 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
}
TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
// Returns empty when null
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
@@ -314,64 +314,64 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
frc::MecanumDriveWheelPositions wheelPositions{
units::meter_t{time}, units::meter_t{time}, units::meter_t{time},
units::meter_t{time}};
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{},
wpi::math::MecanumDriveWheelPositions wheelPositions{
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
wpi::units::meter_t{time}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(MecanumDrivePoseEstimatorTest, TestReset) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
frc::MecanumDriveWheelPositions{},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
wpi::math::Rotation2d{},
wpi::math::MecanumDriveWheelPositions{},
wpi::math::Pose2d{-1_m, -1_m, wpi::math::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -382,8 +382,8 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
estimator.ResetPosition(wpi::math::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},
wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}});
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -391,7 +391,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
estimator.Update(wpi::math::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -399,7 +399,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -408,7 +408,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
estimator.Update(wpi::math::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
@@ -417,7 +417,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -426,7 +426,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
estimator.ResetPose(wpi::math::Pose2d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

14
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
View File

@@ -7,24 +7,24 @@
#include "wpi/math/estimator/MerweScaledSigmaPoints.hpp"
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
wpi::math::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
(points - wpi::math::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
wpi::math::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
wpi::math::Vectord<2>{1.0, 2.0},
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
(points - wpi::math::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}

186
wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -25,9 +25,9 @@
namespace {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
@@ -37,18 +37,18 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
(motors.Kv * motors.R * wpi::units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
auto k1 = (1 / m + wpi::units::math::pow<2>(rb) / J);
auto k2 = (1 / m - wpi::units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
wpi::units::meters_per_second_t vl{x(3)};
wpi::units::meters_per_second_t vr{x(4)};
wpi::units::volt_t Vl{u(0)};
wpi::units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
return wpi::math::Vectord<5>{
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()) +
@@ -57,70 +57,70 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
TEST(MerweUKFTest, DriveInit) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 2 * 5 + 1>(2),
frc::AngleMean<3, 2 * 5 + 1>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 2 * 5 + 1>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(MerweUKFTest, DriveConvergence) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
wpi::math::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 2 * 5 + 1>(2),
frc::AngleMean<3, 2 * 5 + 1>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleMean<3, 2 * 5 + 1>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -130,36 +130,36 @@ TEST(MerweUKFTest, DriveConvergence) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
wpi::units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
wpi::units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
wpi::math::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
trueXhat = wpi::math::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 2 * 5 + 1>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -175,26 +175,26 @@ TEST(MerweUKFTest, DriveConvergence) {
}
TEST(MerweUKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::MerweUKF<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
wpi::math::MerweUKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
wpi::math::Matrixd<1, 1> discA;
wpi::math::Matrixd<1, 1> discB;
wpi::math::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
wpi::math::Vectord<1> ref{100.0};
wpi::math::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
@@ -206,24 +206,24 @@ TEST(MerweUKFTest, LinearUKF) {
}
TEST(MerweUKFTest, RoundTripP) {
constexpr units::second_t dt = 5_ms;
constexpr wpi::units::second_t dt = 5_ms;
frc::MerweUKF<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
wpi::math::MerweUKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
{0.0, 0.0},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
wpi::math::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -231,11 +231,11 @@ frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
return wpi::math::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -244,11 +244,11 @@ frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
return wpi::math::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
wpi::math::Vectord<1> MotorControlInput(double t) {
return wpi::math::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
@@ -256,7 +256,7 @@ frc::Vectord<1> MotorControlInput(double t) {
}
TEST(MerweUKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr wpi::units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
@@ -265,36 +265,36 @@ TEST(MerweUKFTest, MotorConvergence) {
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
std::vector<wpi::math::Vectord<4>> states(steps + 1);
std::vector<wpi::math::Vectord<1>> inputs(steps);
std::vector<wpi::math::Vectord<3>> measurements(steps);
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
wpi::math::Matrixd<4, 4> discA;
wpi::math::Matrixd<4, 1> discB;
wpi::math::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
wpi::math::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {

18
wpimath/src/test/native/cpp/estimator/S3SigmaPointsTest.cpp
View File

@@ -11,9 +11,9 @@ TEST(S3SigmaPointsTest, Simplex) {
constexpr double beta = 2;
constexpr size_t N = 2;
frc::S3SigmaPoints<N> sigmaPoints{alpha, beta};
wpi::math::S3SigmaPoints<N> sigmaPoints{alpha, beta};
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<N>::Zero(), frc::Matrixd<N, N>::Identity());
wpi::math::Vectord<N>::Zero(), wpi::math::Matrixd<N, N>::Identity());
auto v1 = points.template block<2, 1>(0, 1);
auto v2 = points.template block<2, 1>(0, 2);
@@ -27,24 +27,24 @@ TEST(S3SigmaPointsTest, Simplex) {
}
TEST(S3SigmaPointsTest, ZeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
wpi::math::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
wpi::math::Vectord<2>{0.0, 0.0}, wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
(points - wpi::math::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
{0.0, -0.00070711, -0.00070711, 0.00141421}})
.norm() < 1e-7);
}
TEST(S3SigmaPointsTest, NonzeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
wpi::math::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
wpi::math::Vectord<2>{1.0, 2.0},
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
(points - wpi::math::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
{2.0, 1.99776393, 1.99776393, 2.00447214}})
.norm() < 1e-7);
}

180
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp
View File

@@ -25,9 +25,9 @@
namespace {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
@@ -37,18 +37,18 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
(motors.Kv * motors.R * wpi::units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
auto k1 = (1 / m + wpi::units::math::pow<2>(rb) / J);
auto k2 = (1 / m - wpi::units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
wpi::units::meters_per_second_t vl{x(3)};
wpi::units::meters_per_second_t vr{x(4)};
wpi::units::volt_t Vl{u(0)};
wpi::units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
return wpi::math::Vectord<5>{
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()) +
@@ -57,70 +57,70 @@ frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
TEST(S3UKFTest, DriveInit) {
constexpr auto dt = 5_ms;
frc::S3UKF<5, 2, 3> observer{DriveDynamics,
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5 + 2>(2),
frc::AngleMean<3, 5 + 2>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5 + 2>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(S3UKFTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::S3UKF<5, 2, 3> observer{DriveDynamics,
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5 + 2>(2),
frc::AngleMean<3, 5 + 2>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
wpi::math::Vectord<5> r = wpi::math::Vectord<5>::Zero();
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -130,36 +130,36 @@ TEST(S3UKFTest, DriveConvergence) {
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
wpi::units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
wpi::units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
wpi::math::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
trueXhat = wpi::math::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5 + 2>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -175,26 +175,26 @@ TEST(S3UKFTest, DriveConvergence) {
}
TEST(S3UKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::S3UKF<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
wpi::math::S3UKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
wpi::math::Matrixd<1, 1> discA;
wpi::math::Matrixd<1, 1> discB;
wpi::math::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
wpi::math::Vectord<1> ref{100.0};
wpi::math::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
@@ -208,22 +208,22 @@ TEST(S3UKFTest, LinearUKF) {
TEST(S3UKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::S3UKF<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
wpi::math::S3UKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
{0.0, 0.0},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
wpi::math::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -231,11 +231,11 @@ frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
return wpi::math::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -244,11 +244,11 @@ frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
return wpi::math::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
wpi::math::Vectord<1> MotorControlInput(double t) {
return wpi::math::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
@@ -256,7 +256,7 @@ frc::Vectord<1> MotorControlInput(double t) {
}
TEST(S3UKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr wpi::units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
@@ -265,36 +265,36 @@ TEST(S3UKFTest, MotorConvergence) {
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
std::vector<wpi::math::Vectord<4>> states(steps + 1);
std::vector<wpi::math::Vectord<1>> inputs(steps);
std::vector<wpi::math::Vectord<3>> measurements(steps);
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
constexpr wpi::math::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
constexpr wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
wpi::math::Matrixd<4, 4> discA;
wpi::math::Matrixd<4, 1> discB;
wpi::math::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::S3UKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
wpi::math::S3UKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {

304
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -18,40 +18,40 @@
#include "wpi/util/timestamp.h"
void testFollowTrajectory(
const frc::SwerveDriveKinematics<4>& kinematics,
frc::SwerveDrivePoseEstimator3d<4>& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator3d<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
estimator.ResetPosition(frc::Rotation3d{}, positions,
frc::Pose3d{startingPose});
estimator.ResetPosition(wpi::math::Rotation3d{}, positions,
wpi::math::Pose3d{startingPose});
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
@@ -59,9 +59,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -70,7 +70,7 @@ void testFollowTrajectory(
if (!visionPoses.empty() &&
visionPoses.front().first + kVisionUpdateDelay < t) {
auto visionEntry = visionPoses.front();
estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second},
estimator.AddVisionMeasurement(wpi::math::Pose3d{visionEntry.second},
visionEntry.first);
visionPoses.erase(visionPoses.begin());
visionLog.push_back({t, visionEntry.first, visionEntry.second});
@@ -87,13 +87,13 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation()},
positions);
if (debug) {
wpi::print(
wpi::util::print(
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(),
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
@@ -113,14 +113,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -147,79 +147,79 @@ void testFollowTrajectory(
}
TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, frc::Rotation3d{}, {fl, fr, bl, br},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, frc::Rotation3d{}, {fl, fr, bl, br},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}};
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
for (wpi::units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -231,39 +231,39 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator3d<4> estimator{
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
{fl, fr, bl, br},
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
{0.1, 0.1, 0.1, 0.1},
{0.45, 0.45, 0.45, 0.45}};
estimator.UpdateWithTime(0_s, frc::Rotation3d{}, {fl, fr, bl, br});
estimator.UpdateWithTime(0_s, wpi::math::Rotation3d{}, {fl, fr, bl, br});
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}},
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
0_deg);
@@ -271,9 +271,9 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
90_deg);
@@ -281,9 +281,9 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() -
180_deg);
@@ -292,29 +292,29 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, frc::Rotation3d{}, {fl, fr, bl, br},
frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
wpi::math::SwerveDrivePoseEstimator3d<4> estimator{
kinematics, wpi::math::Rotation3d{}, {fl, fr, bl, br},
wpi::math::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation3d{}, {fl, fr, bl, br});
estimator.UpdateWithTime(time, wpi::math::Rotation3d{}, {fl, fr, bl, br});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
{0.1, 0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -332,15 +332,15 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
}
TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveDrivePoseEstimator3d estimator{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose3d{},
wpi::math::Rotation3d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose3d{},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -351,67 +351,67 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::array<frc::SwerveModulePosition, 4> wheelPositions{
{frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}}};
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{},
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(
frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(
frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s);
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveDrivePoseEstimator3d estimator{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation3d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}},
wpi::math::Rotation3d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
{1.0, 1.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0}};
@@ -425,11 +425,11 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
estimator.ResetPosition(
frc::Rotation3d{},
wpi::math::Rotation3d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}});
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
}
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
@@ -441,8 +441,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test orientation and wheel positions
{
frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -454,7 +454,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -466,8 +466,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
// Test orientation
{
frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation3d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -480,7 +480,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -491,7 +491,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose
estimator.ResetPose(frc::Pose3d{});
estimator.ResetPose(wpi::math::Pose3d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

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

@@ -19,39 +19,39 @@
#include "wpi/util/timestamp.h"
void testFollowTrajectory(
const frc::SwerveDriveKinematics<4>& kinematics,
frc::SwerveDrivePoseEstimator<4>& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<frc::Pose2d(frc::Trajectory::State&)>
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
const units::second_t dt, const units::second_t kVisionUpdateRate,
const units::second_t kVisionUpdateDelay, const bool checkError,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
const bool debug) {
wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose);
estimator.ResetPosition(wpi::math::Rotation2d{}, positions, startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t t = 0_s;
wpi::units::second_t t = 0_s;
std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
if (debug) {
wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate`
// seconds since the last vision measurement
@@ -59,9 +59,9 @@ void testFollowTrajectory(
visionPoses.back().first + kVisionUpdateRate < t) {
auto visionPose =
visionMeasurementGenerator(groundTruthState) +
frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.05_rad}};
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
visionPoses.push_back({t, visionPose});
}
@@ -87,12 +87,12 @@ void testFollowTrajectory(
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad} -
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
trajectory.InitialPose().Rotation(),
positions);
if (debug) {
wpi::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
xhat.Y().value(), xhat.Rotation().Radians().value(),
groundTruthState.pose.X().value(),
groundTruthState.pose.Y().value(),
@@ -112,14 +112,14 @@ void testFollowTrajectory(
}
if (debug) {
wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
units::second_t apply_time;
units::second_t measure_time;
frc::Pose2d vision_pose;
wpi::units::second_t apply_time;
wpi::units::second_t measure_time;
wpi::math::Pose2d vision_pose;
for (auto record : visionLog) {
std::tie(apply_time, measure_time, vision_pose) = record;
wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(),
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
measure_time.value(), vision_pose.X().value(),
vision_pose.Y().value(),
vision_pose.Rotation().Radians().value());
@@ -142,79 +142,79 @@ void testFollowTrajectory(
}
TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
20_ms, 100_ms, 250_ms, true, false);
}
TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
for (units::degree_t offset_direction_degs = 0_deg;
for (wpi::units::degree_t offset_direction_degs = 0_deg;
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
for (units::degree_t offset_heading_degs = 0_deg;
for (wpi::units::degree_t offset_heading_degs = 0_deg;
offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
auto pose_offset = frc::Rotation2d{offset_direction_degs};
auto heading_offset = frc::Rotation2d{offset_heading_degs};
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
auto initial_pose =
trajectory.InitialPose() +
frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
pose_offset.Sin() * 1_m},
heading_offset};
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](frc::Trajectory::State& state) {
return frc::ChassisSpeeds{state.velocity, 0_mps,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
@@ -226,53 +226,53 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{},
{fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{},
{fl, fr, bl, br}, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, {fl, fr, bl, br});
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
wpi::math::Pose2d{0_m, 0_m, wpi::math::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
wpi::math::Pose2d{3_m, 1_m, wpi::math::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
wpi::math::Pose2d{2_m, 4_m, wpi::math::Rotation2d{180_deg}}, 0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
auto dx = wpi::units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = wpi::units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = wpi::units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
@@ -280,29 +280,29 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
wpi::math::SwerveModulePosition fl;
wpi::math::SwerveModulePosition fr;
wpi::math::SwerveModulePosition bl;
wpi::math::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
wpi::math::SwerveDrivePoseEstimator<4> estimator{
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
estimator.UpdateWithTime(time, wpi::math::Rotation2d{}, {fl, fr, bl, br});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
@@ -315,15 +315,15 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
}
TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveDrivePoseEstimator estimator{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{},
wpi::math::Rotation2d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose2d{},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -334,67 +334,67 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
// Add a tiny tolerance for the upper bound because of floating point rounding
// error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
wpi::array<frc::SwerveModulePosition, 4> wheelPositions{
{frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}}};
estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{},
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
wheelPositions);
}
// Sample at an added time
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
// Sampling before the oldest value returns the oldest value
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Sampling after the newest value returns the newest value
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
// Add a vision measurement after the odometry measurements (while keeping all
// of the old odometry measurements)
estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
2.2_s);
// Make sure nothing changed (except the newest value)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
// Add a vision measurement before the odometry measurements that's still in
// the buffer
estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
0.9_s);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimatorTest, TestReset) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveDrivePoseEstimator estimator{
wpi::math::SwerveDriveKinematics<4> kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::SwerveDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
wpi::math::Rotation2d{},
{wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{},
wpi::math::SwerveModulePosition{}, wpi::math::SwerveModulePosition{}},
wpi::math::Pose2d{-1_m, -1_m, wpi::math::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
@@ -406,11 +406,11 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
estimator.ResetPosition(
frc::Rotation2d{},
wpi::math::Rotation2d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}});
}
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
@@ -420,8 +420,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test orientation and wheel positions
{
frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -431,7 +431,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -441,8 +441,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
// Test orientation
{
frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
@@ -453,7 +453,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
@@ -462,7 +462,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
estimator.ResetPose(wpi::math::Pose2d{});
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());

18
wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
View File

@@ -8,19 +8,19 @@
#include "wpi/units/time.hpp"
#include "wpi/util/timestamp.h"
static units::second_t now = 0_s;
static wpi::units::second_t now = 0_s;
class DebouncerTest : public ::testing::Test {
protected:
void SetUp() override {
WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
WPI_SetNowImpl([] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
}
void TearDown() override { WPI_SetNowImpl(nullptr); }
};
TEST_F(DebouncerTest, DebounceRising) {
frc::Debouncer debouncer{20_ms};
wpi::math::Debouncer debouncer{20_ms};
debouncer.Calculate(false);
EXPECT_FALSE(debouncer.Calculate(true));
@@ -31,7 +31,7 @@ TEST_F(DebouncerTest, DebounceRising) {
}
TEST_F(DebouncerTest, DebounceFalling) {
frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kFalling};
wpi::math::Debouncer debouncer{20_ms, wpi::math::Debouncer::DebounceType::kFalling};
debouncer.Calculate(true);
EXPECT_TRUE(debouncer.Calculate(false));
@@ -42,7 +42,7 @@ TEST_F(DebouncerTest, DebounceFalling) {
}
TEST_F(DebouncerTest, DebounceBoth) {
frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
wpi::math::Debouncer debouncer{20_ms, wpi::math::Debouncer::DebounceType::kBoth};
debouncer.Calculate(false);
EXPECT_FALSE(debouncer.Calculate(true));
@@ -58,20 +58,20 @@ TEST_F(DebouncerTest, DebounceBoth) {
}
TEST_F(DebouncerTest, DebounceParams) {
frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
wpi::math::Debouncer debouncer{20_ms, wpi::math::Debouncer::DebounceType::kBoth};
EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms);
EXPECT_TRUE(debouncer.GetDebounceType() ==
frc::Debouncer::DebounceType::kBoth);
wpi::math::Debouncer::DebounceType::kBoth);
debouncer.SetDebounceTime(100_ms);
EXPECT_TRUE(debouncer.GetDebounceTime() == 100_ms);
debouncer.SetDebounceType(frc::Debouncer::DebounceType::kFalling);
debouncer.SetDebounceType(wpi::math::Debouncer::DebounceType::kFalling);
EXPECT_TRUE(debouncer.GetDebounceType() ==
frc::Debouncer::DebounceType::kFalling);
wpi::math::Debouncer::DebounceType::kFalling);
EXPECT_TRUE(debouncer.Calculate(false));
}

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

@@ -27,14 +27,14 @@ static double GetData(double t) {
class LinearFilterNoiseTest
: public testing::TestWithParam<LinearFilterNoiseTestType> {
protected:
frc::LinearFilter<double> m_filter = [=] {
wpi::math::LinearFilter<double> m_filter = [=] {
switch (GetParam()) {
case kTestSinglePoleIIR:
return frc::LinearFilter<double>::SinglePoleIIR(
return wpi::math::LinearFilter<double>::SinglePoleIIR(
kSinglePoleIIRTimeConstant, kFilterStep);
break;
default:
return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
return wpi::math::LinearFilter<double>::MovingAverage(kMovAvgTaps);
break;
}
}();

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

@@ -51,21 +51,21 @@ static double GetPulseData(double t) {
class LinearFilterOutputTest
: public testing::TestWithParam<LinearFilterOutputTestType> {
protected:
frc::LinearFilter<double> m_filter = [=] {
wpi::math::LinearFilter<double> m_filter = [=] {
switch (GetParam()) {
case kTestSinglePoleIIR:
return frc::LinearFilter<double>::SinglePoleIIR(
return wpi::math::LinearFilter<double>::SinglePoleIIR(
kSinglePoleIIRTimeConstant, kFilterStep);
break;
case kTestHighPass:
return frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
return wpi::math::LinearFilter<double>::HighPass(kHighPassTimeConstant,
kFilterStep);
break;
case kTestMovAvg:
return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
return wpi::math::LinearFilter<double>::MovingAverage(kMovAvgTaps);
break;
default:
return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
return wpi::math::LinearFilter<double>::MovingAverage(kMovAvgTaps);
break;
}
}();
@@ -121,18 +121,18 @@ INSTANTIATE_TEST_SUITE_P(Tests, LinearFilterOutputTest,
kTestMovAvg, kTestPulse));
template <int Derivative, int Samples, typename F, typename DfDx>
void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
void AssertCentralResults(F&& f, DfDx&& dfdx, wpi::units::second_t h, double min,
double max) {
static_assert(Samples % 2 != 0, "Number of samples must be odd.");
// Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
wpi::array<int, Samples> stencil{wpi::empty_array};
wpi::util::array<int, Samples> stencil{wpi::util::empty_array};
for (int i = 0; i < Samples; ++i) {
stencil[i] = -(Samples - 1) / 2 + i;
}
auto filter =
frc::LinearFilter<double>::FiniteDifference<Derivative, Samples>(stencil,
wpi::math::LinearFilter<double>::FiniteDifference<Derivative, Samples>(stencil,
h);
for (int i = min / h.value(); i < max / h.value(); ++i) {
@@ -153,10 +153,10 @@ void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
}
template <int Derivative, int Samples, typename F, typename DfDx>
void AssertBackwardResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
void AssertBackwardResults(F&& f, DfDx&& dfdx, wpi::units::second_t h, double min,
double max) {
auto filter =
frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
wpi::math::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
h);
for (int i = min / h.value(); i < max / h.value(); ++i) {

8
wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/filter/MedianFilter.hpp"
TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
frc::MedianFilter<double> filter{10};
wpi::math::MedianFilter<double> filter{10};
filter.Calculate(3);
filter.Calculate(0);
@@ -17,7 +17,7 @@ TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
}
TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
frc::MedianFilter<double> filter{10};
wpi::math::MedianFilter<double> filter{10};
filter.Calculate(3);
filter.Calculate(0);
@@ -28,7 +28,7 @@ TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
}
TEST(MedianFilterTest, MedianFilterFullTestEven) {
frc::MedianFilter<double> filter{6};
wpi::math::MedianFilter<double> filter{6};
filter.Calculate(3);
filter.Calculate(0);
@@ -41,7 +41,7 @@ TEST(MedianFilterTest, MedianFilterFullTestEven) {
}
TEST(MedianFilterTest, MedianFilterFullTestOdd) {
frc::MedianFilter<double> filter{5};
wpi::math::MedianFilter<double> filter{5};
filter.Calculate(3);
filter.Calculate(0);

12
wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
View File

@@ -10,21 +10,21 @@
#include "wpi/units/velocity.hpp"
#include "wpi/util/timestamp.h"
static units::second_t now = 0_s;
static wpi::units::second_t now = 0_s;
class SlewRateLimiterTest : public ::testing::Test {
protected:
void SetUp() override {
WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
WPI_SetNowImpl([] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
}
void TearDown() override { WPI_SetNowImpl(nullptr); }
};
TEST_F(SlewRateLimiterTest, SlewRateLimit) {
WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
WPI_SetNowImpl([] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
frc::SlewRateLimiter<units::meters> limiter(1_mps);
wpi::math::SlewRateLimiter<wpi::units::meters> limiter(1_mps);
now += 1_s;
@@ -32,7 +32,7 @@ TEST_F(SlewRateLimiterTest, SlewRateLimit) {
}
TEST_F(SlewRateLimiterTest, SlewRateNoLimit) {
frc::SlewRateLimiter<units::meters> limiter(1_mps);
wpi::math::SlewRateLimiter<wpi::units::meters> limiter(1_mps);
now += 1_s;
@@ -40,7 +40,7 @@ TEST_F(SlewRateLimiterTest, SlewRateNoLimit) {
}
TEST_F(SlewRateLimiterTest, SlewRatePositiveNegativeLimit) {
frc::SlewRateLimiter<units::meters> limiter(1_mps, -0.5_mps);
wpi::math::SlewRateLimiter<wpi::units::meters> limiter(1_mps, -0.5_mps);
now += 1_s;

2
wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/geometry/Transform3d.hpp"
using namespace frc;
using namespace wpi::math;
void CheckPose3dConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
const CoordinateSystem& coordFrom,

60
wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp
View File

@@ -7,32 +7,32 @@
#include "wpi/math/geometry/Ellipse2d.hpp"
TEST(Ellipse2dTest, FocalPoints) {
constexpr frc::Pose2d center{1_m, 2_m, 0_deg};
constexpr frc::Ellipse2d ellipse{center, 5_m, 4_m};
constexpr wpi::math::Pose2d center{1_m, 2_m, 0_deg};
constexpr wpi::math::Ellipse2d ellipse{center, 5_m, 4_m};
const auto [a, b] = ellipse.FocalPoints();
EXPECT_EQ(frc::Translation2d(-2_m, 2_m), a);
EXPECT_EQ(frc::Translation2d(4_m, 2_m), b);
EXPECT_EQ(wpi::math::Translation2d(-2_m, 2_m), a);
EXPECT_EQ(wpi::math::Translation2d(4_m, 2_m), b);
}
TEST(Ellipse2dTest, Intersects) {
constexpr frc::Pose2d center{1_m, 2_m, 0_deg};
constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m};
constexpr wpi::math::Pose2d center{1_m, 2_m, 0_deg};
constexpr wpi::math::Ellipse2d ellipse{center, 2_m, 1_m};
constexpr frc::Translation2d pointA{1_m, 3_m};
constexpr frc::Translation2d pointB{0_m, 3_m};
constexpr wpi::math::Translation2d pointA{1_m, 3_m};
constexpr wpi::math::Translation2d pointB{0_m, 3_m};
EXPECT_TRUE(ellipse.Intersects(pointA));
EXPECT_FALSE(ellipse.Intersects(pointB));
}
TEST(Ellipse2dTest, Contains) {
constexpr frc::Pose2d center{-1_m, -2_m, 45_deg};
constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m};
constexpr wpi::math::Pose2d center{-1_m, -2_m, 45_deg};
constexpr wpi::math::Ellipse2d ellipse{center, 2_m, 1_m};
constexpr frc::Translation2d pointA{0_m, -1_m};
constexpr frc::Translation2d pointB{0.5_m, -2_m};
constexpr wpi::math::Translation2d pointA{0_m, -1_m};
constexpr wpi::math::Translation2d pointB{0.5_m, -2_m};
EXPECT_TRUE(ellipse.Contains(pointA));
EXPECT_FALSE(ellipse.Contains(pointB));
@@ -41,58 +41,58 @@ TEST(Ellipse2dTest, Contains) {
TEST(Ellipse2dTest, Distance) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m};
constexpr wpi::math::Pose2d center{1_m, 2_m, 270_deg};
constexpr wpi::math::Ellipse2d ellipse{center, 1_m, 2_m};
constexpr frc::Translation2d point1{2.5_m, 2_m};
constexpr wpi::math::Translation2d point1{2.5_m, 2_m};
EXPECT_NEAR(0, ellipse.Distance(point1).value(), kEpsilon);
constexpr frc::Translation2d point2{1_m, 2_m};
constexpr wpi::math::Translation2d point2{1_m, 2_m};
EXPECT_NEAR(0, ellipse.Distance(point2).value(), kEpsilon);
constexpr frc::Translation2d point3{1_m, 1_m};
constexpr wpi::math::Translation2d point3{1_m, 1_m};
EXPECT_NEAR(0, ellipse.Distance(point3).value(), kEpsilon);
constexpr frc::Translation2d point4{-1_m, 2.5_m};
constexpr wpi::math::Translation2d point4{-1_m, 2.5_m};
EXPECT_NEAR(0.19210128384806818, ellipse.Distance(point4).value(), kEpsilon);
}
TEST(Ellipse2dTest, Nearest) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m};
constexpr wpi::math::Pose2d center{1_m, 2_m, 270_deg};
constexpr wpi::math::Ellipse2d ellipse{center, 1_m, 2_m};
constexpr frc::Translation2d point1{2.5_m, 2_m};
constexpr wpi::math::Translation2d point1{2.5_m, 2_m};
auto nearestPoint1 = ellipse.Nearest(point1);
EXPECT_NEAR(2.5, nearestPoint1.X().value(), kEpsilon);
EXPECT_NEAR(2.0, nearestPoint1.Y().value(), kEpsilon);
constexpr frc::Translation2d point2{1_m, 2_m};
constexpr wpi::math::Translation2d point2{1_m, 2_m};
auto nearestPoint2 = ellipse.Nearest(point2);
EXPECT_NEAR(1.0, nearestPoint2.X().value(), kEpsilon);
EXPECT_NEAR(2.0, nearestPoint2.Y().value(), kEpsilon);
constexpr frc::Translation2d point3{1_m, 1_m};
constexpr wpi::math::Translation2d point3{1_m, 1_m};
auto nearestPoint3 = ellipse.Nearest(point3);
EXPECT_NEAR(1.0, nearestPoint3.X().value(), kEpsilon);
EXPECT_NEAR(1.0, nearestPoint3.Y().value(), kEpsilon);
constexpr frc::Translation2d point4{-1_m, 2.5_m};
constexpr wpi::math::Translation2d point4{-1_m, 2.5_m};
auto nearestPoint4 = ellipse.Nearest(point4);
EXPECT_NEAR(-0.8512799937611617, nearestPoint4.X().value(), kEpsilon);
EXPECT_NEAR(2.378405333174535, nearestPoint4.Y().value(), kEpsilon);
}
TEST(Ellipse2dTest, Equals) {
constexpr frc::Pose2d center1{1_m, 2_m, 90_deg};
constexpr frc::Ellipse2d ellipse1{center1, 2_m, 3_m};
constexpr wpi::math::Pose2d center1{1_m, 2_m, 90_deg};
constexpr wpi::math::Ellipse2d ellipse1{center1, 2_m, 3_m};
constexpr frc::Pose2d center2{1_m, 2_m, 90_deg};
constexpr frc::Ellipse2d ellipse2{center2, 2_m, 3_m};
constexpr wpi::math::Pose2d center2{1_m, 2_m, 90_deg};
constexpr wpi::math::Ellipse2d ellipse2{center2, 2_m, 3_m};
constexpr frc::Pose2d center3{1_m, 2_m, 90_deg};
constexpr frc::Ellipse2d ellipse3{center3, 3_m, 2_m};
constexpr wpi::math::Pose2d center3{1_m, 2_m, 90_deg};
constexpr wpi::math::Ellipse2d ellipse3{center3, 3_m, 2_m};
EXPECT_EQ(ellipse1, ellipse2);
EXPECT_NE(ellipse1, ellipse3);

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

@@ -9,7 +9,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Pose2dTest, RotateBy) {
constexpr auto x = 1_m;

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

@@ -9,14 +9,14 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/util/array.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Pose3dTest, RotateBy) {
constexpr auto x = 1_m;
constexpr auto y = 2_m;
const Pose3d initial{x, y, 0_m, Rotation3d{0_deg, 0_deg, 45_deg}};
constexpr units::radian_t yaw = 5_deg;
constexpr wpi::units::radian_t yaw = 5_deg;
const Rotation3d rotation{0_deg, 0_deg, yaw};
const auto rotated = initial.RotateBy(rotation);
@@ -67,7 +67,7 @@ TEST(Pose3dTest, TransformBy) {
EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
EXPECT_DOUBLE_EQ(transformed.Rotation().Z().value(),
units::radian_t{50_deg}.value());
wpi::units::radian_t{50_deg}.value());
}
TEST(Pose3dTest, RelativeTo) {
@@ -92,7 +92,7 @@ TEST(Pose3dTest, RotateAround) {
EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
EXPECT_NEAR(units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(),
EXPECT_NEAR(wpi::units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(),
1e-9);
}
@@ -140,7 +140,7 @@ TEST(Pose3dTest, ToPose2d) {
}
TEST(Pose3dTest, ComplexTwists) {
wpi::array<Pose3d, 5> initial_poses{
wpi::util::array<Pose3d, 5> initial_poses{
Pose3d{0.698303_m, -0.959096_m, 0.271076_m,
Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}},
Pose3d{0.634892_m, -0.765209_m, 0.117543_m,
@@ -153,7 +153,7 @@ TEST(Pose3dTest, ComplexTwists) {
Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}},
};
wpi::array<Pose3d, 5> final_poses{
wpi::util::array<Pose3d, 5> final_poses{
Pose3d{-0.230448_m, -0.511957_m, 0.198406_m,
Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}},
Pose3d{-0.088932_m, -0.343253_m, 0.095018_m,
@@ -190,7 +190,7 @@ TEST(Pose3dTest, ComplexTwists) {
}
TEST(Pose3dTest, TwistNaN) {
wpi::array<Pose3d, 2> initial_poses{
wpi::util::array<Pose3d, 2> initial_poses{
Pose3d{6.32_m, 4.12_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
1.9208309264993548E-8}}},
@@ -199,7 +199,7 @@ TEST(Pose3dTest, TwistNaN) {
2.0352360299846772E-7}}},
};
wpi::array<Pose3d, 2> final_poses{
wpi::util::array<Pose3d, 2> final_poses{
Pose3d{6.33_m, 4.15_m, 0.00_m,
Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
2.416890209039172E-8}}},

6
wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
View File

@@ -10,7 +10,7 @@
#include "wpi/units/angle.hpp"
#include "wpi/units/math.hpp"
using namespace frc;
using namespace wpi::math;
TEST(QuaternionTest, Init) {
// Identity
@@ -94,8 +94,8 @@ TEST(QuaternionTest, ScalarDivision) {
TEST(QuaternionTest, Multiply) {
// 90° CCW rotations around each axis
double c = units::math::cos(90_deg / 2.0);
double s = units::math::sin(90_deg / 2.0);
double c = wpi::units::math::cos(90_deg / 2.0);
double s = wpi::units::math::sin(90_deg / 2.0);
Quaternion xRot{c, s, 0.0, 0.0};
Quaternion yRot{c, 0.0, s, 0.0};
Quaternion zRot{c, 0.0, 0.0, s};

60
wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp
View File

@@ -7,10 +7,10 @@
#include "wpi/math/geometry/Rectangle2d.hpp"
TEST(Rectangle2dTest, NewWithCorners) {
constexpr frc::Translation2d cornerA{1_m, 2_m};
constexpr frc::Translation2d cornerB{4_m, 6_m};
constexpr wpi::math::Translation2d cornerA{1_m, 2_m};
constexpr wpi::math::Translation2d cornerB{4_m, 6_m};
frc::Rectangle2d rect{cornerA, cornerB};
wpi::math::Rectangle2d rect{cornerA, cornerB};
EXPECT_EQ(3.0, rect.XWidth().value());
EXPECT_EQ(4.0, rect.YWidth().value());
@@ -19,69 +19,69 @@ TEST(Rectangle2dTest, NewWithCorners) {
}
TEST(Rectangle2dTest, Intersects) {
constexpr frc::Pose2d center{4_m, 3_m, 90_deg};
constexpr frc::Rectangle2d rect{center, 2_m, 3_m};
constexpr wpi::math::Pose2d center{4_m, 3_m, 90_deg};
constexpr wpi::math::Rectangle2d rect{center, 2_m, 3_m};
EXPECT_TRUE(rect.Intersects(frc::Translation2d{5.5_m, 4_m}));
EXPECT_TRUE(rect.Intersects(frc::Translation2d{3_m, 2_m}));
EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 1.5_m}));
EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 3.5_m}));
EXPECT_TRUE(rect.Intersects(wpi::math::Translation2d{5.5_m, 4_m}));
EXPECT_TRUE(rect.Intersects(wpi::math::Translation2d{3_m, 2_m}));
EXPECT_FALSE(rect.Intersects(wpi::math::Translation2d{4_m, 1.5_m}));
EXPECT_FALSE(rect.Intersects(wpi::math::Translation2d{4_m, 3.5_m}));
}
TEST(Rectangle2dTest, Contains) {
constexpr frc::Pose2d center{2_m, 3_m, 45_deg};
constexpr frc::Rectangle2d rect{center, 3_m, 1_m};
constexpr wpi::math::Pose2d center{2_m, 3_m, 45_deg};
constexpr wpi::math::Rectangle2d rect{center, 3_m, 1_m};
EXPECT_TRUE(rect.Contains(frc::Translation2d{2_m, 3_m}));
EXPECT_TRUE(rect.Contains(frc::Translation2d{3_m, 4_m}));
EXPECT_FALSE(rect.Contains(frc::Translation2d{3_m, 3_m}));
EXPECT_TRUE(rect.Contains(wpi::math::Translation2d{2_m, 3_m}));
EXPECT_TRUE(rect.Contains(wpi::math::Translation2d{3_m, 4_m}));
EXPECT_FALSE(rect.Contains(wpi::math::Translation2d{3_m, 3_m}));
}
TEST(Rectangle2dTest, Distance) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
constexpr frc::Rectangle2d rect{center, 1_m, 2_m};
constexpr wpi::math::Pose2d center{1_m, 2_m, 270_deg};
constexpr wpi::math::Rectangle2d rect{center, 1_m, 2_m};
constexpr frc::Translation2d point1{2.5_m, 2_m};
constexpr wpi::math::Translation2d point1{2.5_m, 2_m};
EXPECT_NEAR(0.5, rect.Distance(point1).value(), kEpsilon);
constexpr frc::Translation2d point2{1_m, 2_m};
constexpr wpi::math::Translation2d point2{1_m, 2_m};
EXPECT_NEAR(0, rect.Distance(point2).value(), kEpsilon);
constexpr frc::Translation2d point3{1_m, 1_m};
constexpr wpi::math::Translation2d point3{1_m, 1_m};
EXPECT_NEAR(0.5, rect.Distance(point3).value(), kEpsilon);
constexpr frc::Translation2d point4{-1_m, 2.5_m};
constexpr wpi::math::Translation2d point4{-1_m, 2.5_m};
EXPECT_NEAR(1, rect.Distance(point4).value(), kEpsilon);
}
TEST(Rectangle2dTest, Nearest) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 1_m, 90_deg};
constexpr frc::Rectangle2d rect{center, 3_m, 4_m};
constexpr wpi::math::Pose2d center{1_m, 1_m, 90_deg};
constexpr wpi::math::Rectangle2d rect{center, 3_m, 4_m};
constexpr frc::Translation2d point1{1_m, 3_m};
constexpr wpi::math::Translation2d point1{1_m, 3_m};
auto nearestPoint1 = rect.Nearest(point1);
EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon);
EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon);
constexpr frc::Translation2d point2{0_m, 0_m};
constexpr wpi::math::Translation2d point2{0_m, 0_m};
auto nearestPoint2 = rect.Nearest(point2);
EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon);
EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon);
}
TEST(Rectangle2dTest, Equals) {
constexpr frc::Pose2d center1{2_m, 3_m, 0_deg};
constexpr frc::Rectangle2d rect1{center1, 5_m, 3_m};
constexpr wpi::math::Pose2d center1{2_m, 3_m, 0_deg};
constexpr wpi::math::Rectangle2d rect1{center1, 5_m, 3_m};
constexpr frc::Pose2d center2{2_m, 3_m, 0_deg};
constexpr frc::Rectangle2d rect2{center2, 5_m, 3_m};
constexpr wpi::math::Pose2d center2{2_m, 3_m, 0_deg};
constexpr wpi::math::Rectangle2d rect2{center2, 5_m, 3_m};
constexpr frc::Pose2d center3{2_m, 3_m, 0_deg};
constexpr frc::Rectangle2d rect3{center3, 3_m, 3_m};
constexpr wpi::math::Pose2d center3{2_m, 3_m, 0_deg};
constexpr wpi::math::Rectangle2d rect3{center3, 3_m, 3_m};
EXPECT_EQ(rect1, rect2);
EXPECT_NE(rect2, rect3);

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

@@ -9,11 +9,11 @@
#include "wpi/math/geometry/Rotation2d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Rotation2dTest, RadiansToDegrees) {
const Rotation2d rot1{units::radian_t{std::numbers::pi / 3.0}};
const Rotation2d rot2{units::radian_t{std::numbers::pi / 4.0}};
const Rotation2d rot1{wpi::units::radian_t{std::numbers::pi / 3.0}};
const Rotation2d rot2{wpi::units::radian_t{std::numbers::pi / 4.0}};
EXPECT_DOUBLE_EQ(60.0, rot1.Degrees().value());
EXPECT_DOUBLE_EQ(45.0, rot2.Degrees().value());

106
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -11,38 +11,38 @@
#include "wpi/math/geometry/Rotation3d.hpp"
#include "wpi/util/MathExtras.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Rotation3dTest, GimbalLockAccuracy) {
auto rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
auto rot2 = Rotation3d{units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
auto rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
auto rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
auto rot2 = Rotation3d{wpi::units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
auto rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result1 = rot1 + rot2 + rot3;
const auto expected1 =
Rotation3d{0_rad, -units::radian_t{std::numbers::pi / 2},
units::radian_t{std::numbers::pi / 2}};
Rotation3d{0_rad, -wpi::units::radian_t{std::numbers::pi / 2},
wpi::units::radian_t{std::numbers::pi / 2}};
EXPECT_EQ(expected1, result1);
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result1.X() + result1.Z()).value());
EXPECT_NEAR(-std::numbers::pi / 2, result1.Y().value(), 1e-7);
rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
rot3 = Rotation3d{units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{wpi::units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
rot3 = Rotation3d{wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result2 = rot1 + rot2 + rot3;
const auto expected2 =
Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
units::radian_t{std::numbers::pi / 2}};
Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2},
wpi::units::radian_t{std::numbers::pi / 2}};
EXPECT_EQ(expected2, result2);
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result2.Z() - result2.X()).value());
EXPECT_NEAR(std::numbers::pi / 2, result2.Y().value(), 1e-7);
rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
rot2 = Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 3}, 0_rad};
rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
const auto result3 = rot1 + rot2 + rot3;
const auto expected3 =
Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
units::radian_t{std::numbers::pi / 6}};
Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2},
wpi::units::radian_t{std::numbers::pi / 6}};
EXPECT_EQ(expected3, result3);
EXPECT_DOUBLE_EQ(std::numbers::pi / 6, (result3.Z() - result3.X()).value());
EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result3.Y().value());
@@ -50,22 +50,22 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot2{units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
const Rotation3d rot1{xAxis, wpi::units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot2{wpi::units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
const Rotation3d rvec1{Eigen::Vector3d{xAxis * std::numbers::pi / 3}};
EXPECT_EQ(rot1, rot2);
EXPECT_EQ(rot1, rvec1);
const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
const Rotation3d rot3{yAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot4{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
const Rotation3d rot3{yAxis, wpi::units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot4{0_rad, wpi::units::radian_t{std::numbers::pi / 3}, 0_rad};
const Rotation3d rvec2{Eigen::Vector3d{yAxis * std::numbers::pi / 3}};
EXPECT_EQ(rot3, rot4);
EXPECT_EQ(rot3, rvec2);
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d rot5{zAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot6{0_rad, 0_rad, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot5{zAxis, wpi::units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot6{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 3}};
const Rotation3d rvec3{Eigen::Vector3d{zAxis * std::numbers::pi / 3}};
EXPECT_EQ(rot5, rot6);
EXPECT_EQ(rot5, rvec3);
@@ -102,12 +102,12 @@ TEST(Rotation3dTest, InitTwoVector) {
// 90 degree CW rotation around y-axis
const Rotation3d rot1{xAxis, zAxis};
const Rotation3d expected1{yAxis, units::radian_t{-std::numbers::pi / 2.0}};
const Rotation3d expected1{yAxis, wpi::units::radian_t{-std::numbers::pi / 2.0}};
EXPECT_EQ(expected1, rot1);
// 45 degree CCW rotation around z-axis
const Rotation3d rot2{xAxis, Eigen::Vector3d{1.0, 1.0, 0.0}};
const Rotation3d expected2{zAxis, units::radian_t{std::numbers::pi / 4.0}};
const Rotation3d expected2{zAxis, wpi::units::radian_t{std::numbers::pi / 4.0}};
EXPECT_EQ(expected2, rot2);
// 0 degree rotation of x-axes
@@ -148,15 +148,15 @@ TEST(Rotation3dTest, InitTwoVector) {
TEST(Rotation3dTest, RadiansToDegrees) {
const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Rotation3d rot1{zAxis, units::radian_t{std::numbers::pi / 3}};
const Rotation3d rot1{zAxis, wpi::units::radian_t{std::numbers::pi / 3}};
EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
EXPECT_DOUBLE_EQ(units::radian_t{60_deg}.value(), rot1.Z().value());
EXPECT_DOUBLE_EQ(wpi::units::radian_t{60_deg}.value(), rot1.Z().value());
const Rotation3d rot2{zAxis, units::radian_t{std::numbers::pi / 4}};
const Rotation3d rot2{zAxis, wpi::units::radian_t{std::numbers::pi / 4}};
EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
EXPECT_DOUBLE_EQ(units::radian_t{45_deg}.value(), rot2.Z().value());
EXPECT_DOUBLE_EQ(wpi::units::radian_t{45_deg}.value(), rot2.Z().value());
}
TEST(Rotation3dTest, DegreesToRadians) {
@@ -259,7 +259,7 @@ TEST(Rotation3dTest, Minus) {
const auto rot1 = Rotation3d{zAxis, 70_deg};
const auto rot2 = Rotation3d{zAxis, 30_deg};
EXPECT_DOUBLE_EQ(40.0, units::degree_t{(rot1 - rot2).Z()}.value());
EXPECT_DOUBLE_EQ(40.0, wpi::units::degree_t{(rot1 - rot2).Z()}.value());
}
TEST(Rotation3dTest, AxisAngle) {
@@ -327,48 +327,48 @@ TEST(Rotation3dTest, Interpolate) {
// 50 + (70 - 50) * 0.5 = 60
auto rot1 = Rotation3d{xAxis, 50_deg};
auto rot2 = Rotation3d{xAxis, 70_deg};
auto interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
auto interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(60.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Z()}.value());
// -160 minus half distance between 170 and -160 (15) = -175
rot1 = Rotation3d{xAxis, 170_deg};
rot2 = Rotation3d{xAxis, -160_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(-175.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Z()}.value());
// 50 + (70 - 50) * 0.5 = 60
rot1 = Rotation3d{yAxis, 50_deg};
rot2 = Rotation3d{yAxis, 70_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(60.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Z()}.value());
// -160 plus half distance between 170 and -160 (165) = 5
rot1 = Rotation3d{yAxis, 170_deg};
rot2 = Rotation3d{yAxis, -160_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(-5.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.Z()}.value());
interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(-5.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.Z()}.value());
// 50 + (70 - 50) * 0.5 = 60
rot1 = Rotation3d{zAxis, 50_deg};
rot2 = Rotation3d{zAxis, 70_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Z()}.value());
interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(60.0, wpi::units::degree_t{interpolated.Z()}.value());
// -160 minus half distance between 170 and -160 (15) = -175
rot1 = Rotation3d{zAxis, 170_deg};
rot2 = Rotation3d{zAxis, -160_deg};
interpolated = wpi::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.X()}.value());
EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value());
EXPECT_DOUBLE_EQ(-175.0, wpi::units::degree_t{interpolated.Z()}.value());
}

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

@@ -11,7 +11,7 @@
#include "wpi/math/geometry/Transform2d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Transform2dTest, ToMatrix) {
Transform2d before{1_m, 2_m, 20_deg};

2
wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
View File

@@ -11,7 +11,7 @@
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/math/geometry/Translation3d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Transform3dTest, ToMatrix) {
Transform3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}};

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

@@ -8,7 +8,7 @@
#include "wpi/math/geometry/Translation2d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Translation2dTest, Sum) {
const Translation2d one{1_m, 3_m};

2
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -8,7 +8,7 @@
#include "wpi/math/geometry/Translation3d.hpp"
using namespace frc;
using namespace wpi::math;
static constexpr double kEpsilon = 1E-9;

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

@@ -9,7 +9,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5_m, 0_m, 0_rad};
@@ -22,7 +22,7 @@ TEST(Twist2dTest, Straight) {
TEST(Twist2dTest, QuarterCircle) {
const Twist2d quarterCircle{5_m / 2.0 * std::numbers::pi, 0_m,
units::radian_t{std::numbers::pi / 2.0}};
wpi::units::radian_t{std::numbers::pi / 2.0}};
const auto quarterCirclePose = quarterCircle.Exp();
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value());
@@ -57,8 +57,8 @@ TEST(Twist2dTest, Pose2dLog) {
const auto twist = (end - start).Log();
Twist2d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi}, 0_m,
units::radian_t{std::numbers::pi / 2.0}};
Twist2d expected{wpi::units::meter_t{5.0 / 2.0 * std::numbers::pi}, 0_m,
wpi::units::radian_t{std::numbers::pi / 2.0}};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose

10
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
View File

@@ -9,7 +9,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
using namespace frc;
using namespace wpi::math;
TEST(Twist3dTest, StraightX) {
const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
@@ -40,7 +40,7 @@ TEST(Twist3dTest, QuarterCircle) {
const Twist3d quarterCircle{
5_m / 2.0 * std::numbers::pi, 0_m, 0_m, 0_rad, 0_rad,
units::radian_t{std::numbers::pi / 2.0}};
wpi::units::radian_t{std::numbers::pi / 2.0}};
const auto quarterCirclePose = quarterCircle.Exp();
Transform3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
@@ -73,7 +73,7 @@ TEST(Twist3dTest, Pose3dLogX) {
const auto twist = (end - start).Log();
Twist3d expected{0_m, units::meter_t{5.0 / 2.0 * std::numbers::pi},
Twist3d expected{0_m, wpi::units::meter_t{5.0 / 2.0 * std::numbers::pi},
0_m, 90_deg,
0_deg, 0_deg};
EXPECT_EQ(expected, twist);
@@ -89,7 +89,7 @@ TEST(Twist3dTest, Pose3dLogY) {
const auto twist = (end - start).Log();
Twist3d expected{0_m, 0_m, units::meter_t{5.0 / 2.0 * std::numbers::pi},
Twist3d expected{0_m, 0_m, wpi::units::meter_t{5.0 / 2.0 * std::numbers::pi},
0_deg, 90_deg, 0_deg};
EXPECT_EQ(expected, twist);
@@ -104,7 +104,7 @@ TEST(Twist3dTest, Pose3dLogZ) {
const auto twist = (end - start).Log();
Twist3d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi},
Twist3d expected{wpi::units::meter_t{5.0 / 2.0 * std::numbers::pi},
0_m,
0_m,
0_deg,

6
wpimath/src/test/native/cpp/geometry/proto/Ellipse2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Ellipse2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const Ellipse2d kExpectedData{
} // namespace
TEST(Ellipse2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const Pose2d kExpectedData =
} // namespace
TEST(Pose2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -17,8 +17,8 @@ const Pose3d kExpectedData =
} // namespace
TEST(Pose3dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Quaternion.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -15,8 +15,8 @@ const Quaternion kExpectedData = Quaternion{1.1, 0.191, 35.04, 19.1};
} // namespace
TEST(QuaternionProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Rectangle2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Rectangle2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const Rectangle2d kExpectedData{
} // namespace
TEST(Rectangle2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -15,8 +15,8 @@ const Rotation2d kExpectedData = Rotation2d{1.91_rad};
} // namespace
TEST(Rotation2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Rotation3d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const Rotation3d kExpectedData =
} // namespace
TEST(Rotation3dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Transform2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const Transform2d kExpectedData =
} // namespace
TEST(Transform2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -17,8 +17,8 @@ const Transform3d kExpectedData =
} // namespace
TEST(Transform3dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -15,8 +15,8 @@ const Translation2d kExpectedData = Translation2d{3.504_m, 22.9_m};
} // namespace
TEST(Translation2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Translation3d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -15,8 +15,8 @@ const Translation3d kExpectedData = Translation3d{35.04_m, 22.9_m, 3.504_m};
} // namespace
TEST(Translation3dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -15,8 +15,8 @@ const Twist2d kExpectedData = Twist2d{2.29_m, 35.04_m, 35.04_rad};
} // namespace
TEST(Twist2dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Twist3d.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const Twist3d kExpectedData =
} // namespace
TEST(Twist3dProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

4
wpimath/src/test/native/cpp/geometry/struct/Ellipse2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Ellipse2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Ellipse2d>;
using StructType = wpi::util::Struct<wpi::math::Ellipse2d>;
const Ellipse2d kExpectedData{
Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Pose2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Pose2d>;
using StructType = wpi::util::Struct<wpi::math::Pose2d>;
const Pose2d kExpectedData{
Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Pose3d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Pose3d>;
using StructType = wpi::util::Struct<wpi::math::Pose3d>;
const Pose3d kExpectedData{
Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}};

4
wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Quaternion.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Quaternion>;
using StructType = wpi::util::Struct<wpi::math::Quaternion>;
const Quaternion kExpectedData{Quaternion{1.1, 0.191, 35.04, 19.1}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Rectangle2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Rectangle2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Rectangle2d>;
using StructType = wpi::util::Struct<wpi::math::Rectangle2d>;
const Rectangle2d kExpectedData{
Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Rotation2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Rotation2d>;
using StructType = wpi::util::Struct<wpi::math::Rotation2d>;
const Rotation2d kExpectedData{Rotation2d{1.91_rad}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Rotation3d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Rotation3d>;
using StructType = wpi::util::Struct<wpi::math::Rotation3d>;
const Rotation3d kExpectedData{
Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Transform2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Transform2d>;
using StructType = wpi::util::Struct<wpi::math::Transform2d>;
const Transform2d kExpectedData{
Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Transform3d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Transform3d>;
using StructType = wpi::util::Struct<wpi::math::Transform3d>;
const Transform3d kExpectedData{
Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}};

4
wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Translation2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Translation2d>;
using StructType = wpi::util::Struct<wpi::math::Translation2d>;
const Translation2d kExpectedData{Translation2d{3.504_m, 22.9_m}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Translation3d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Translation3d>;
using StructType = wpi::util::Struct<wpi::math::Translation3d>;
const Translation3d kExpectedData{Translation3d{35.04_m, 22.9_m, 3.504_m}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Twist2d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Twist2d>;
using StructType = wpi::util::Struct<wpi::math::Twist2d>;
const Twist2d kExpectedData{Twist2d{2.29_m, 35.04_m, 35.04_rad}};
} // namespace

4
wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/geometry/Twist3d.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::Twist3d>;
using StructType = wpi::util::Struct<wpi::math::Twist3d>;
const Twist3d kExpectedData{
Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}};
} // namespace

12
wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
View File

@@ -12,7 +12,7 @@
#include "wpi/units/time.hpp"
TEST(TimeInterpolatableBufferTest, AddSample) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d> buffer{10_s};
// No entries
buffer.AddSample(1_s, 0_rad);
@@ -32,7 +32,7 @@ TEST(TimeInterpolatableBufferTest, AddSample) {
}
TEST(TimeInterpolatableBufferTest, Interpolation) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d> buffer{10_s};
buffer.AddSample(0_s, 0_rad);
EXPECT_TRUE(buffer.Sample(0_s).value() == 0_rad);
@@ -47,12 +47,12 @@ TEST(TimeInterpolatableBufferTest, Interpolation) {
}
TEST(TimeInterpolatableBufferTest, Pose2d) {
frc::TimeInterpolatableBuffer<frc::Pose2d> buffer{10_s};
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose2d> buffer{10_s};
// We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
frc::Pose2d sample = buffer.Sample(0.5_s).value();
buffer.AddSample(0_s, wpi::math::Pose2d{0_m, 0_m, 90_deg});
buffer.AddSample(1_s, wpi::math::Pose2d{1_m, 1_m, 0_deg});
wpi::math::Pose2d sample = buffer.Sample(0.5_s).value();
EXPECT_TRUE(std::abs(sample.X().value() - (1.0 - 1.0 / std::sqrt(2.0))) <
0.01);

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

@@ -11,15 +11,15 @@
static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeedsTest, Discretize) {
constexpr frc::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr units::second_t duration = 1_s;
constexpr units::second_t dt = 10_ms;
constexpr wpi::math::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr wpi::units::second_t duration = 1_s;
constexpr wpi::units::second_t dt = 10_ms;
const auto speeds = target.Discretize(duration);
const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
const wpi::math::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
frc::Pose2d pose;
for (units::second_t time = 0_s; time < duration; time += dt) {
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
pose = pose + twist.Exp();
}
@@ -31,7 +31,7 @@ TEST(ChassisSpeedsTest, Discretize) {
TEST(ChassisSpeedsTest, ToRobotRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
@@ -41,7 +41,7 @@ TEST(ChassisSpeedsTest, ToRobotRelative) {
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg);
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
@@ -49,10 +49,10 @@ TEST(ChassisSpeedsTest, ToFieldRelative) {
}
TEST(ChassisSpeedsTest, Plus) {
const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const frc::ChassisSpeeds result = left + right;
const wpi::math::ChassisSpeeds result = left + right;
EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
@@ -60,10 +60,10 @@ TEST(ChassisSpeedsTest, Plus) {
}
TEST(ChassisSpeedsTest, Minus) {
const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const frc::ChassisSpeeds result = left - right;
const wpi::math::ChassisSpeeds result = left - right;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(0, result.vy.value(), kEpsilon);
@@ -71,9 +71,9 @@ TEST(ChassisSpeedsTest, Minus) {
}
TEST(ChassisSpeedsTest, UnaryMinus) {
const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds result = -speeds;
const wpi::math::ChassisSpeeds result = -speeds;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
@@ -81,9 +81,9 @@ TEST(ChassisSpeedsTest, UnaryMinus) {
}
TEST(ChassisSpeedsTest, Multiplication) {
const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds result = speeds * 2;
const wpi::math::ChassisSpeeds result = speeds * 2;
EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
@@ -91,9 +91,9 @@ TEST(ChassisSpeedsTest, Multiplication) {
}
TEST(ChassisSpeedsTest, Division) {
const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds result = speeds / 2;
const wpi::math::ChassisSpeeds result = speeds / 2;
EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);

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

@@ -12,7 +12,7 @@
#include "wpi/units/length.hpp"
#include "wpi/units/velocity.hpp"
using namespace frc;
using namespace wpi::math;
static constexpr double kEpsilon = 1E-9;
@@ -57,7 +57,7 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{
0.0_mps, 0.0_mps, units::radians_per_second_t{std::numbers::pi}};
0.0_mps, 0.0_mps, wpi::units::radians_per_second_t{std::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * std::numbers::pi, kEpsilon);
@@ -67,8 +67,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
units::meters_per_second_t{+0.381 * std::numbers::pi},
units::meters_per_second_t{-0.381 * std::numbers::pi}};
wpi::units::meters_per_second_t{+0.381 * std::numbers::pi},
wpi::units::meters_per_second_t{-0.381 * std::numbers::pi}};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);

Some files were not shown because too many files have changed in this diff Show More