From 7d90d0bcc30e1973b3a6d5bbfbaded62e71c2125 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 15 Nov 2023 21:13:12 -0800 Subject: [PATCH] [wpimath] Clean up StateSpaceUtil (#5891) --- .../src/main/native/cpp/StateSpaceUtil.cpp | 21 +- .../main/native/include/frc/StateSpaceUtil.h | 179 ++++++++---------- .../src/main/native/include/wpi/Algorithm.h | 17 ++ 3 files changed, 99 insertions(+), 118 deletions(-) diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index 758d2e772a..5dcf1a1798 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -18,21 +18,12 @@ Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { pose.Rotation().Sin()}; } -template <> -bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) { - return detail::IsStabilizableImpl<1, 1>(A, B); -} - -template <> -bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) { - return detail::IsStabilizableImpl<2, 1>(A, B); -} - -template <> -bool IsStabilizable(const Eigen::MatrixXd& A, - const Eigen::MatrixXd& B) { - return detail::IsStabilizableImpl(A, B); -} +template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, + const Matrixd<1, 1>& B); +template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, + const Matrixd<2, 1>& B); +template bool IsStabilizable( + const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); Eigen::Vector3d PoseToVector(const Pose2d& pose) { return Eigen::Vector3d{pose.X().value(), pose.Y().value(), diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 3aa2e75330..e2b638ca24 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -12,87 +12,13 @@ #include #include +#include #include -#include #include "frc/EigenCore.h" #include "frc/geometry/Pose2d.h" namespace frc { -namespace detail { - -template -void CostMatrixImpl(Matrix& result, T elem, Ts... elems) { - if (elem == std::numeric_limits::infinity()) { - result(result.rows() - (sizeof...(Ts) + 1)) = 0.0; - } else { - result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2); - } - if constexpr (sizeof...(Ts) > 0) { - CostMatrixImpl(result, elems...); - } -} - -template -void CovMatrixImpl(Matrix& result, T elem, Ts... elems) { - result(result.rows() - (sizeof...(Ts) + 1)) = std::pow(elem, 2); - if constexpr (sizeof...(Ts) > 0) { - CovMatrixImpl(result, elems...); - } -} - -template -void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) { - std::random_device rd; - std::mt19937 gen{rd()}; - std::normal_distribution<> distr{0.0, elem}; - - result(result.rows() - (sizeof...(Ts) + 1)) = distr(gen); - if constexpr (sizeof...(Ts) > 0) { - WhiteNoiseVectorImpl(result, elems...); - } -} - -template -bool IsStabilizableImpl(const Matrixd& A, - const Matrixd& B) { - Eigen::EigenSolver> es{A, false}; - - for (int i = 0; i < A.rows(); ++i) { - if (std::norm(es.eigenvalues()[i]) < 1) { - continue; - } - - if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) { - Eigen::Matrix, States, States + Inputs> E; - E << es.eigenvalues()[i] * Eigen::Matrix, States, - States>::Identity() - - A, - B; - - Eigen::ColPivHouseholderQR< - Eigen::Matrix, States, States + Inputs>> - qr{E}; - if (qr.rank() < States) { - return false; - } - } else { - Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; - E << es.eigenvalues()[i] * - Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - - A, - B; - - Eigen::ColPivHouseholderQR qr{E}; - if (qr.rank() < A.rows()) { - return false; - } - } - } - return true; -} - -} // namespace detail /** * Creates a cost matrix from the given vector for use with LQR. @@ -110,7 +36,16 @@ bool IsStabilizableImpl(const Matrixd& A, template ... Ts> Matrixd MakeCostMatrix(Ts... tolerances) { Eigen::DiagonalMatrix result; - detail::CostMatrixImpl(result.diagonal(), tolerances...); + auto& diag = result.diagonal(); + wpi::for_each( + [&](int i, double tolerance) { + if (tolerance == std::numeric_limits::infinity()) { + diag(i) = 0.0; + } else { + diag(i) = 1.0 / std::pow(tolerance, 2); + } + }, + tolerances...); return result; } @@ -129,7 +64,9 @@ Matrixd MakeCostMatrix(Ts... tolerances) { template ... Ts> Matrixd MakeCovMatrix(Ts... stdDevs) { Eigen::DiagonalMatrix result; - detail::CovMatrixImpl(result.diagonal(), stdDevs...); + auto& diag = result.diagonal(); + wpi::for_each([&](int i, double stdDev) { diag(i) = std::pow(stdDev, 2); }, + stdDevs...); return result; } @@ -150,7 +87,7 @@ template Matrixd MakeCostMatrix(const std::array& costs) { Eigen::DiagonalMatrix result; auto& diag = result.diagonal(); - for (size_t i = 0; i < N; ++i) { + for (size_t i = 0; i < costs.size(); ++i) { if (costs[i] == std::numeric_limits::infinity()) { diag(i) = 0.0; } else { @@ -183,9 +120,23 @@ Matrixd MakeCovMatrix(const std::array& stdDevs) { } template ... Ts> -Matrixd MakeWhiteNoiseVector(Ts... stdDevs) { - Matrixd result; - detail::WhiteNoiseVectorImpl(result, stdDevs...); +Vectord MakeWhiteNoiseVector(Ts... stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Vectord result; + wpi::for_each( + [&](int i, double stdDev) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDev == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDev}; + result(i) = distr(gen); + } + }, + stdDevs...); return result; } @@ -203,7 +154,7 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { std::mt19937 gen{rd()}; Vectord result; - for (int i = 0; i < N; ++i) { + 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) { @@ -251,9 +202,50 @@ Eigen::Vector4d PoseTo4dVector(const Pose2d& pose); template bool IsStabilizable(const Matrixd& A, const Matrixd& B) { - return detail::IsStabilizableImpl(A, B); + Eigen::EigenSolver> es{A, false}; + + for (int i = 0; i < A.rows(); ++i) { + if (std::norm(es.eigenvalues()[i]) < 1) { + continue; + } + + if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) { + Eigen::Matrix, States, States + Inputs> E; + E << es.eigenvalues()[i] * Eigen::Matrix, States, + States>::Identity() - + A, + B; + + Eigen::ColPivHouseholderQR< + Eigen::Matrix, States, States + Inputs>> + qr{E}; + if (qr.rank() < States) { + return false; + } + } else { + Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; + E << es.eigenvalues()[i] * + Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - + A, + B; + + Eigen::ColPivHouseholderQR qr{E}; + if (qr.rank() < A.rows()) { + return false; + } + } + } + return true; } +extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>( + const Matrixd<1, 1>& A, const Matrixd<1, 1>& B); +extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>( + const Matrixd<2, 2>& A, const Matrixd<2, 1>& B); +extern template WPILIB_DLLEXPORT bool +IsStabilizable(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& B); + /** * Returns true if (A, C) is a detectable pair. * @@ -269,28 +261,9 @@ bool IsStabilizable(const Matrixd& A, template bool IsDetectable(const Matrixd& A, const Matrixd& C) { - return detail::IsStabilizableImpl(A.transpose(), - C.transpose()); + return IsStabilizable(A.transpose(), C.transpose()); } -// Template specializations are used here to make common state-input pairs -// compile faster. -template <> -WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, - const Matrixd<1, 1>& B); - -// Template specializations are used here to make common state-input pairs -// compile faster. -template <> -WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, - const Matrixd<2, 1>& B); - -// Template specializations are used here to make common state-input pairs -// compile faster. -template <> -WPILIB_DLLEXPORT bool IsStabilizable( - const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); - /** * Converts a Pose2d into a vector of [x, y, theta]. * diff --git a/wpiutil/src/main/native/include/wpi/Algorithm.h b/wpiutil/src/main/native/include/wpi/Algorithm.h index 1fd2502c29..112bfc3485 100644 --- a/wpiutil/src/main/native/include/wpi/Algorithm.h +++ b/wpiutil/src/main/native/include/wpi/Algorithm.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include namespace wpi { @@ -15,4 +17,19 @@ typename std::vector::iterator insert_sorted(std::vector& vec, T const& item) { return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item); } + +/** + * Calls f(i, elem) for each element of elems where i is the index of the + * element in elems and elem is the element. + * + * @param f The callback. + * @param elems The elements. + */ +template +constexpr void for_each(F&& f, Ts&&... elems) { + [&](std::index_sequence) { + (f(Is, elems), ...); + }(std::index_sequence_for{}); +} + } // namespace wpi