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

[wpimath] DARE: Use wpi::expected instead of exceptions (#7312)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-31 20:37:57 -07:00
committed by GitHub
parent 21980c7447
commit 9f6f267f5c
19 changed files with 717 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
View File

@@ -97,7 +97,7 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
Matrixd<5, 2> discB;
DiscretizeAB(A, B, dt, &discA, &discB);
Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
auto S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
// K = (BᵀSB + R)⁻¹BᵀSA
m_table.insert(velocity, (discB.transpose() * S * discB + R)

2
wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
View File

@@ -103,7 +103,7 @@ LTVUnicycleController::LTVUnicycleController(
Matrixd<3, 2> discB;
DiscretizeAB(A, B, dt, &discA, &discB);
Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
auto S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
// K = (BᵀSB + R)⁻¹BᵀSA
m_table.insert(velocity, (discB.transpose() * S * discB + R)

62
wpimath/src/main/native/cpp/jni/DAREJNI.cpp
View File

@@ -5,12 +5,14 @@
#include <jni.h>
#include <stdexcept>
#include <string>
#include <wpi/jni_util.h>
#include "Exceptions.h"
#include "edu_wpi_first_math_jni_DAREJNI.h"
#include "frc/DARE.h"
#include "frc/fmt/Eigen.h"
using namespace wpi::java;
@@ -47,8 +49,8 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQR
Eigen::MatrixXd RmatCopy{Rmat};
auto R_llt = RmatCopy.llt();
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt);
auto result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat,
Qmat, R_llt);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
@@ -88,7 +90,7 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQRN
Eigen::MatrixXd Rcopy{Rmat};
auto R_llt = Rcopy.llt();
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
auto result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt, Nmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
@@ -122,13 +124,24 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareABQR
Eigen::RowMajor>>
Rmat{nativeR.data(), inputs, inputs};
try {
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
illegalArgEx.Throw(env, e.what());
if (auto result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat)) {
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
// K = (BᵀSB + R)⁻¹BᵀSA
} else if (result.error() == frc::DAREError::QNotSymmetric ||
result.error() == frc::DAREError::QNotPositiveSemidefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nQ =\n{}\n", to_string(result.error()), Qmat));
} else if (result.error() == frc::DAREError::RNotSymmetric ||
result.error() == frc::DAREError::RNotPositiveDefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nR =\n{}\n", to_string(result.error()), Rmat));
} else if (result.error() == frc::DAREError::ABNotStabilizable) {
illegalArgEx.Throw(env, fmt::format("{}\n\nA =\n{}\nB =\n{}\n",
to_string(result.error()), Amat, Bmat));
} else if (result.error() == frc::DAREError::ACNotDetectable) {
illegalArgEx.Throw(env, fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(result.error()), Amat, Qmat));
}
}
@@ -164,13 +177,28 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareABQRN
Eigen::RowMajor>>
Nmat{nativeN.data(), states, inputs};
try {
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
illegalArgEx.Throw(env, e.what());
if (auto result = frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
Rmat, Nmat)) {
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
} else if (result.error() == frc::DAREError::QNotSymmetric ||
result.error() == frc::DAREError::QNotPositiveSemidefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nQ =\n{}\n", to_string(result.error()), Qmat));
} else if (result.error() == frc::DAREError::RNotSymmetric ||
result.error() == frc::DAREError::RNotPositiveDefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nR =\n{}\n", to_string(result.error()), Rmat));
} else if (result.error() == frc::DAREError::ABNotStabilizable) {
illegalArgEx.Throw(
env,
fmt::format("{}\n\nA =\n{}\nB =\n{}\n", to_string(result.error()),
Amat - Bmat * Rmat.llt().solve(Nmat.transpose()), Bmat));
} else if (result.error() == frc::DAREError::ACNotDetectable) {
auto R_llt = Rmat.llt();
illegalArgEx.Throw(
env, fmt::format("{}\n\nA =\n{}\nQ =\n{}\n", to_string(result.error()),
Amat - Bmat * R_llt.solve(Nmat.transpose()),
Qmat - Nmat * R_llt.solve(Nmat.transpose())));
}
}