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 core State-space classes (#2614)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
2020-08-14 23:40:33 -07:00
committed by GitHub
parent e5b84e2f87
commit 3b283ab9aa
84 changed files with 11747 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,12 +10,39 @@
#include <wpi/jni_util.h>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "unsupported/Eigen/MatrixFunctions"
using namespace wpi::java;
bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
// This function checks 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[lambda * I - A, B] < n.
int n = B.rows(), m = B.cols();
Eigen::EigenSolver<Eigen::MatrixXd> es(A);
for (int i = 0; i < n; i++) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1)
continue;
Eigen::MatrixXcd E(n, n + m);
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
if (qr.rank() != n) {
return false;
}
}
return true;
}
extern "C" {
/*
@@ -57,4 +84,54 @@ Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
/*
* Class: edu_wpi_first_wpiutil_math_WPIMathJNI
* Method: exp
* Signature: ([DI[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_wpiutil_math_WPIMathJNI_exp
(JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
{
jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Amat{arrayBody, rows, rows};
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
Amat.exp();
env->ReleaseDoubleArrayElements(src, arrayBody, 0);
env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
}
/*
* Class: edu_wpi_first_wpiutil_math_WPIMathJNI
* Method: isStabilizable
* Signature: (II[D[D)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_wpiutil_math_WPIMathJNI_isStabilizable
(JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
jdoubleArray bSrc)
{
jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr);
jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr);
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
A{nativeA, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
B{nativeB, states, inputs};
bool isStabilizable = check_stabilizable(A, B);
env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
return isStabilizable;
}
} // extern "C"