Skip to content

Commit

Permalink
[wpimath] Clean up StateSpaceUtil (wpilibsuite#5891)
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul @Starlight220
calcmogul authored and Starlight220 committed Dec 1, 2023
1 parent af3937f commit aaf3466
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 118 deletions.
21 changes: 6 additions & 15 deletions wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B) {
return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(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<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);

Eigen::Vector3d PoseToVector(const Pose2d& pose) {
return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
Expand Down
179 changes: 76 additions & 103 deletions wpimath/src/main/native/include/frc/StateSpaceUtil.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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 {
Expand Down Expand Up @@ -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;
}

Expand All @@ -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) {
Expand Down Expand Up @@ -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.
*
Expand All @@ -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].
*
Expand Down
17 changes: 17 additions & 0 deletions wpiutil/src/main/native/include/wpi/Algorithm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
#pragma once

#include <algorithm>
#include <cstddef>
#include <utility>
#include <vector>

namespace wpi {
Expand All @@ -15,4 +17,19 @@ typename std::vector<T>::iterator insert_sorted(std::vector<T>& 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 <typename F, typename... Ts>
constexpr void for_each(F&& f, Ts&&... elems) {
[&]<size_t... Is>(std::index_sequence<Is...>) {
(f(Is, elems), ...);
}(std::index_sequence_for<Ts...>{});
}

} // namespace wpi

0 comments on commit aaf3466

Please sign in to comment.