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

8
wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp → wpimath/src/main/native/cpp/jni/LinearSystemUtilJNI.cpp
View File

@@ -6,8 +6,8 @@
#include <Eigen/Core>
#include "org_wpilib_math_jni_StateSpaceUtilJNI.h"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "org_wpilib_math_jni_LinearSystemUtilJNI.h"
#include "wpi/math/system/LinearSystemUtil.hpp"
#include "wpi/util/jni_util.hpp"
using namespace wpi::util::java;
@@ -15,12 +15,12 @@ using namespace wpi::util::java;
extern "C" {
/*
* Class: org_wpilib_math_jni_StateSpaceUtilJNI
* Class: org_wpilib_math_jni_LinearSystemUtilJNI
* Method: isStabilizable
* Signature: (II[D[D)Z
*/
JNIEXPORT jboolean JNICALL
Java_org_wpilib_math_jni_StateSpaceUtilJNI_isStabilizable
Java_org_wpilib_math_jni_LinearSystemUtilJNI_isStabilizable
(JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
jdoubleArray bSrc)
{

32
wpimath/src/main/native/cpp/random/Normal.cpp Normal file
View File

@@ -0,0 +1,32 @@
// 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/random/Normal.hpp"
#include <random>
#include <span>
#include <Eigen/Core>
namespace wpi::math {
Eigen::VectorXd Normal(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;
}
} // namespace wpi::math

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