Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Clean up StateSpaceUtil (#5891)

Browse Source
This commit is contained in:
Tyler Veness
2023-11-15 21:13:12 -08:00
committed by GitHub
parent 7755e45aac
commit 7d90d0bcc3
3 changed files with 99 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

179
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -12,87 +12,13 @@
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <wpi/Algorithm.h>
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
namespace frc {
namespace detail {
template <typename Matrix, typename T, typename... Ts>
void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
if (elem == std::numeric_limits<double>::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 <typename Matrix, typename T, typename... Ts>
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 <typename Matrix, typename T, typename... Ts>
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 <int States, int Inputs>
bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> 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<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, 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<Eigen::MatrixXcd> 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<States, States>& A,
template <std::same_as<double>... Ts>
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CostMatrixImpl(result.diagonal(), tolerances...);
auto& diag = result.diagonal();
wpi::for_each(
[&](int i, double tolerance) {
if (tolerance == std::numeric_limits<double>::infinity()) {
diag(i) = 0.0;
} else {
diag(i) = 1.0 / std::pow(tolerance, 2);
}
},
tolerances...);
return result;
}
@@ -129,7 +64,9 @@ Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
template <std::same_as<double>... Ts>
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> 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 <size_t N>
Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Eigen::DiagonalMatrix<double, N> 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<double>::infinity()) {
diag(i) = 0.0;
} else {
@@ -183,9 +120,23 @@ Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
}
template <std::same_as<double>... Ts>
Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Matrixd<sizeof...(Ts), 1> result;
detail::WhiteNoiseVectorImpl(result, stdDevs...);
Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Vectord<sizeof...(Ts)> 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<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
std::mt19937 gen{rd()};
Vectord<N> 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 <int States, int Inputs>
bool IsStabilizable(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
return detail::IsStabilizableImpl<States, Inputs>(A, B);
Eigen::EigenSolver<Matrixd<States, States>> 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<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, 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<Eigen::MatrixXcd> 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<Eigen::Dynamic, Eigen::Dynamic>(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<States, States>& A,
template <int States, int Outputs>
bool IsDetectable(const Matrixd<States, States>& A,
const Matrixd<Outputs, States>& C) {
return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
C.transpose());
return IsStabilizable<States, Outputs>(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<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
*