From f14af97dc76948c9d41e638e87bc869dd11d4c4d Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Wed, 30 Apr 2025 03:58:28 +1000 Subject: [PATCH] [wpimath] Support dynamic matrix sizes in StateSpaceUtil (#7942) --- .../src/main/native/cpp/StateSpaceUtil.cpp | 55 ++++++++++++++++++ .../main/native/include/frc/StateSpaceUtil.h | 58 ++++++++++++++++++- .../test/native/cpp/StateSpaceUtilTest.cpp | 31 ++++++++++ 3 files changed, 143 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index f0f6786e1a..01e987d5d4 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -4,6 +4,8 @@ #include "frc/StateSpaceUtil.h" +#include + namespace frc { template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, @@ -13,4 +15,57 @@ template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, template bool IsStabilizable( const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); +template bool IsDetectable( + const Eigen::MatrixXd& A, const Eigen::MatrixXd& C); + +template Eigen::VectorXd ClampInputMaxMagnitude( + const Eigen::VectorXd& u, const Eigen::VectorXd& umin, + const Eigen::VectorXd& umax); + +template Eigen::VectorXd DesaturateInputVector( + const Eigen::VectorXd& u, double maxMagnitude); + +Eigen::MatrixXd MakeCostMatrix(const std::span costs) { + Eigen::MatrixXd result{costs.size(), costs.size()}; + result.setZero(); + + for (size_t i = 0; i < costs.size(); ++i) { + if (costs[i] == std::numeric_limits::infinity()) { + result(i, i) = 0.0; + } else { + result(i, i) = 1.0 / (std::pow(costs[i], 2)); + } + } + return result; +} + +Eigen::VectorXd MakeWhiteNoiseVector(const std::span stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Eigen::VectorXd result{stdDevs.size()}; + for (size_t i = 0; i < stdDevs.size(); ++i) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDevs[i] == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDevs[i]}; + result(i) = distr(gen); + } + } + return result; +} + +Eigen::MatrixXd MakeCovMatrix(const std::span stdDevs) { + Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()}; + result.setZero(); + + for (size_t i = 0; i < stdDevs.size(); ++i) { + result(i, i) = std::pow(stdDevs[i], 2); + } + + return result; +} + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 1e8ae2a108..6c58485d45 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -123,6 +123,22 @@ constexpr Matrixd MakeCostMatrix(const std::array& costs) { return result; } +/** + * Creates a cost matrix from the given vector for use with LQR. + * + * The cost matrix is constructed using Bryson's rule. The inverse square of + * each element in the input is placed on the cost matrix diagonal. If a + * tolerance is infinity, its cost matrix entry is set to zero. + * + * @param costs A possibly variable length container. For a Q matrix, its + * elements are the maximum allowed excursions of the states from + * the reference. For an R matrix, its elements are the maximum + * allowed excursions of the control inputs from no actuation. + * @return State excursion or control effort cost matrix. + */ +WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix( + const std::span costs); + /** * Creates a covariance matrix from the given vector for use with Kalman * filters. @@ -152,6 +168,21 @@ constexpr Matrixd MakeCovMatrix(const std::array& stdDevs) { return result; } +/** + * Creates a covariance matrix from the given vector for use with Kalman + * filters. + * + * Each element is squared and placed on the covariance matrix diagonal. + * + * @param stdDevs A possibly variable length container. For a Q matrix, its + * elements are the standard deviations of each state from how + * the model behaves. For an R matrix, its elements are the + * standard deviations for each output measurement. + * @return Process noise or measurement noise covariance matrix. + */ +WPILIB_DLLEXPORT Eigen::MatrixXd MakeCovMatrix( + const std::span stdDevs); + template ... Ts> Vectord MakeWhiteNoiseVector(Ts... stdDevs) { std::random_device rd; @@ -200,6 +231,17 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { return result; } +/** + * Creates a vector of normally distributed white noise with the given noise + * intensities for each element. + * + * @param stdDevs A possibly variable length container whose elements are the + * standard deviations of each element of the noise vector. + * @return White noise vector. + */ +WPILIB_DLLEXPORT Eigen::VectorXd MakeWhiteNoiseVector( + const std::span stdDevs); + /** * Converts a Pose2d into a vector of [x, y, theta]. * @@ -311,6 +353,10 @@ bool IsDetectable(const Matrixd& A, return IsStabilizable(A.transpose(), C.transpose()); } +extern template WPILIB_DLLEXPORT bool +IsDetectable(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& C); + /** * Converts a Pose2d into a vector of [x, y, theta]. * @@ -341,12 +387,17 @@ constexpr Vectord ClampInputMaxMagnitude(const Vectord& u, const Vectord& umin, const Vectord& umax) { Vectord result; - for (int i = 0; i < Inputs; ++i) { + for (int i = 0; i < u.rows(); ++i) { result(i) = std::clamp(u(i), umin(i), umax(i)); } return result; } +extern template WPILIB_DLLEXPORT Eigen::VectorXd +ClampInputMaxMagnitude(const Eigen::VectorXd& u, + const Eigen::VectorXd& umin, + const Eigen::VectorXd& umax); + /** * Renormalize all inputs if any exceeds the maximum magnitude. Useful for * systems such as differential drivetrains. @@ -366,4 +417,9 @@ Vectord DesaturateInputVector(const Vectord& u, } return u; } + +extern template WPILIB_DLLEXPORT Eigen::VectorXd +DesaturateInputVector(const Eigen::VectorXd& u, + double maxMagnitude); + } // namespace frc diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp index fe1fb1d7ea..434524cd91 100644 --- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp @@ -33,6 +33,19 @@ TEST(StateSpaceUtilTest, CostArray) { EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); } +TEST(StateSpaceUtilTest, CostDynamic) { + Eigen::MatrixXd mat = frc::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); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); +} + TEST(StateSpaceUtilTest, CovParameterPack) { constexpr frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0); EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); @@ -59,6 +72,19 @@ TEST(StateSpaceUtilTest, CovArray) { EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); } +TEST(StateSpaceUtilTest, CovDynamic) { + Eigen::MatrixXd mat = frc::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); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); +} + TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) { [[maybe_unused]] frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0); @@ -69,6 +95,11 @@ TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) { frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0}); } +TEST(StateSpaceUtilTest, WhiteNoiseVectorDynamic) { + [[maybe_unused]] + Eigen::VectorXd vec = frc::MakeWhiteNoiseVector(std::vector{2.0, 3.0}); +} + TEST(StateSpaceUtilTest, IsStabilizable) { frc::Matrixd<2, 1> B{0, 1};