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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,9 +13,9 @@
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
namespace frc {
@@ -64,9 +64,9 @@ void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
}
template <int States, int Inputs>
bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> es{A};
for (int i = 0; i < States; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -107,12 +107,12 @@ template <int Rows, int Cols, typename... Ts,
std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
WPI_DEPRECATED(
"Use Eigen::Matrix or Eigen::Vector initializer list constructor")
Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
Matrixd<Rows, Cols> MakeMatrix(Ts... elems) {
static_assert(
sizeof...(elems) == Rows * Cols,
"Number of provided elements doesn't match matrix dimensionality");
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
detail::MatrixImpl<Rows, Cols>(result, elems...);
return result;
}
@@ -132,8 +132,7 @@ Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
Ts... tolerances) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CostMatrixImpl(result.diagonal(), tolerances...);
return result;
@@ -153,8 +152,7 @@ Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
Ts... stdDevs) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CovMatrixImpl(result.diagonal(), stdDevs...);
return result;
@@ -173,7 +171,7 @@ Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
* @return State excursion or control effort cost matrix.
*/
template <size_t N>
Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
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) {
@@ -195,8 +193,7 @@ Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
* @return Process noise or measurement noise covariance matrix.
*/
template <size_t N>
Eigen::Matrix<double, N, N> MakeCovMatrix(
const std::array<double, N>& stdDevs) {
Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -207,8 +204,8 @@ Eigen::Matrix<double, N, N> MakeCovMatrix(
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Eigen::Matrix<double, sizeof...(Ts), 1> result;
Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Matrixd<sizeof...(Ts), 1> result;
detail::WhiteNoiseVectorImpl(result, stdDevs...);
return result;
}
@@ -222,12 +219,11 @@ Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
* @return White noise vector.
*/
template <int N>
Eigen::Vector<double, N> MakeWhiteNoiseVector(
const std::array<double, N>& stdDevs) {
Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Eigen::Vector<double, N> result;
Vectord<N> result;
for (int i = 0; i < N; ++i) {
// Passing a standard deviation of 0.0 to std::normal_distribution is
// undefined behavior
@@ -249,7 +245,7 @@ Eigen::Vector<double, N> MakeWhiteNoiseVector(
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
Vectord<3> PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -259,7 +255,7 @@ Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
Vectord<4> PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
@@ -274,8 +270,8 @@ Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
* @param B Input matrix.
*/
template <int States, int Inputs>
bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
bool IsStabilizable(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
return detail::IsStabilizableImpl<States, Inputs>(A, B);
}
@@ -292,8 +288,8 @@ bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
* @param C Output matrix.
*/
template <int States, int Outputs>
bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, Outputs, States>& C) {
bool IsDetectable(const Matrixd<States, States>& A,
const Matrixd<Outputs, States>& C) {
return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
C.transpose());
}
@@ -301,14 +297,14 @@ bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
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 Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
@@ -318,7 +314,7 @@ WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
Vectord<3> PoseToVector(const Pose2d& pose);
/**
* Clamps input vector between system's minimum and maximum allowable input.
@@ -330,11 +326,10 @@ Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
* @return Clamped input vector.
*/
template <int Inputs>
Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Inputs>& umin,
const Eigen::Vector<double, Inputs>& umax) {
Eigen::Vector<double, Inputs> result;
Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
result(i) = std::clamp(u(i), umin(i), umax(i));
}
@@ -351,8 +346,8 @@ Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
* @return The normalizedInput
*/
template <int Inputs>
Eigen::Vector<double, Inputs> DesaturateInputVector(
const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();
if (maxValue > maxMagnitude) {