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

[wpimath] Refactor StateSpaceUtil into separate files (#8421)

Browse Source 
* Moved makeWhiteNoiseVector() to random.Normal.normal()
* Moved isControllable() and isDetectable() to system.LinearSystemUtil
* Renamed makeCostMatrix() to costMatrix() (Java)
* Renamed makeCovarianceMatrix() to covarianceMatrix() (Java)
* Renamed MakeCostMatrix() to CostMatrix() (C++)
* Renamed MakeCovMatrix() to CovarianceMatrix() (C++)
* Removed deprecated poseTo3dVector(), poseTo4dVector(), poseToVector()
* Removed clampInputMaxMagnitude()
* We don't use it, and Eigen has this functionality built in via `u =
u.array().min(u_max.array()).max(u_min.array());`
* Simplified implementation of desaturateInputVector()
This commit is contained in:
Tyler Veness
2025-11-29 10:28:38 -08:00
committed by GitHub
parent c8e6ce1ca4
commit a79f86ade3
51 changed files with 755 additions and 741 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/native/cpp/util/LinearSystemUtil.cpp Normal file
View File

@@ -0,0 +1,21 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/system/LinearSystemUtil.hpp"
#include <Eigen/Core>
namespace wpi::math {
template bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B);
template bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B);
template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
template bool IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& C);
} // namespace wpi::math

43
wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp
View File

@@ -5,27 +5,11 @@
#include "wpi/math/util/StateSpaceUtil.hpp"
#include <limits>
#include <span>
namespace wpi::math {
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);
template bool IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& C);
template Eigen::VectorXd ClampInputMaxMagnitude<Eigen::Dynamic>(
const Eigen::VectorXd& u, const Eigen::VectorXd& umin,
const Eigen::VectorXd& umax);
template Eigen::VectorXd DesaturateInputVector<Eigen::Dynamic>(
const Eigen::VectorXd& u, double maxMagnitude);
Eigen::MatrixXd MakeCostMatrix(const std::span<const double> costs) {
Eigen::MatrixXd CostMatrix(const std::span<const double> costs) {
Eigen::MatrixXd result{costs.size(), costs.size()};
result.setZero();
@@ -39,25 +23,7 @@ Eigen::MatrixXd MakeCostMatrix(const std::span<const double> costs) {
return result;
}
Eigen::VectorXd MakeWhiteNoiseVector(const std::span<const double> 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<const double> stdDevs) {
Eigen::MatrixXd CovarianceMatrix(const std::span<const double> stdDevs) {
Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()};
result.setZero();
@@ -68,4 +34,7 @@ Eigen::MatrixXd MakeCovMatrix(const std::span<const double> stdDevs) {
return result;
}
template Eigen::VectorXd DesaturateInputVector<Eigen::Dynamic>(
const Eigen::VectorXd& u, double maxMagnitude);
} // namespace wpi::math