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

[wpimath] Add static matrix support to DARE solver (#5536)

Browse Source 
Using static matrices where possible results in a 2x performance
improvement.
This commit is contained in:
Tyler Veness
2023-08-14 09:15:58 -07:00
committed by GitHub
parent 394cfeadbd
commit 03764dfe93
9 changed files with 354 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -11,7 +11,6 @@
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "frc/DARE.h"
@@ -21,44 +20,6 @@
using namespace wpi::java;
namespace {
/**
* Returns true if (A, B) is a stabilizable pair.
*
* (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank([λI - A, B]) < n where n is the number of states.
*
* @param A System matrix.
* @param B Input matrix.
*/
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> es{A, false};
for (int i = 0; i < A.rows(); ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1) {
continue;
}
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
std::vector<double> GetElementsFromTrajectory(
const frc::Trajectory& trajectory) {
std::vector<double> elements;
@@ -130,7 +91,8 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
Rmat{nativeR, inputs, inputs};
try {
Eigen::MatrixXd result = frc::DARE(Amat, Bmat, Qmat, Rmat);
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
@@ -179,7 +141,8 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
Nmat{nativeN, states, inputs};
try {
Eigen::MatrixXd result = frc::DARE(Amat, Bmat, Qmat, Rmat, Nmat);
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
@@ -314,7 +277,8 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
B{nativeB, states, inputs};
bool isStabilizable = IsStabilizable(A, B);
bool isStabilizable =
frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);