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] Make LTV controller constructors use faster DARE solver (#5543)

Browse Source 
Made JNI modifications to expose the faster function, made the API use
the typesafe Matrix API, and synchronized the documentation with C++.

Sped up C++ LTV diff drive test from 20 ms to 15 ms.
Sped up C++ LTV unicycle test from 15 ms to 10 ms.
This commit is contained in:
Tyler Veness
2023-08-17 13:56:15 -07:00
committed by GitHub
parent 6953a303b3
commit 0cf6e37dc1
9 changed files with 596 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -62,6 +62,84 @@ frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: dareDetailABQR
* Signature: ([D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Eigen::MatrixXd RmatCopy{Rmat};
auto R_llt = RmatCopy.llt();
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: dareDetailABQRN
* Signature: ([D[D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
JDoubleArrayRef nativeN{env, N};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Nmat{nativeN.array().data(), states, inputs};
Eigen::MatrixXd Rcopy{Rmat};
auto R_llt = Rcopy.llt();
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt, Nmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: dareABQR
@@ -72,33 +150,28 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Amat{nativeA, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Bmat{nativeB, states, inputs};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Qmat{nativeQ, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Rmat{nativeR, inputs, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
try {
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
env->ReleaseDoubleArrayElements(R, nativeR, 0);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
jclass cls = env->FindClass("java/lang/IllegalArgumentException");
@@ -118,38 +191,32 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{
jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
jdouble* nativeN = env->GetDoubleArrayElements(N, nullptr);
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
JDoubleArrayRef nativeN{env, N};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Amat{nativeA, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Bmat{nativeB, states, inputs};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Qmat{nativeQ, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Rmat{nativeR, inputs, inputs};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Nmat{nativeN, states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Nmat{nativeN.array().data(), states, inputs};
try {
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
env->ReleaseDoubleArrayElements(R, nativeR, 0);
env->ReleaseDoubleArrayElements(N, nativeN, 0);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
jclass cls = env->FindClass("java/lang/IllegalArgumentException");