Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use C++23 std::expected (#8823)

Browse Source
This commit is contained in:
Gold856
2026-07-02 02:10:52 -04:00
committed by GitHub
parent 1299abc173
commit 4e7dd4cfbb
37 changed files with 133 additions and 2709 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp
View File

@@ -4,6 +4,7 @@
#pragma once
#include <expected>
#include <string_view>
#include <Eigen/Cholesky>
@@ -11,7 +12,6 @@
#include <Eigen/LU>
#include "wpi/math/system/LinearSystemUtil.hpp"
#include "wpi/util/expected"
namespace wpi::math {
@@ -166,7 +166,7 @@ Eigen::Matrix<double, States, States> DARE(
* @return Solution to the DARE on success, or DAREError on failure.
*/
template <int States, int Inputs>
wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
std::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
@@ -175,20 +175,20 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
if (checkPreconditions) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
return wpi::util::unexpected{DAREError::RNotSymmetric};
return std::unexpected{DAREError::RNotSymmetric};
}
}
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
return wpi::util::unexpected{DAREError::RNotPositiveDefinite};
return std::unexpected{DAREError::RNotPositiveDefinite};
}
if (checkPreconditions) {
// Require Q be symmetric
if ((Q - Q.transpose()).norm() > 1e-10) {
return wpi::util::unexpected{DAREError::QNotSymmetric};
return std::unexpected{DAREError::QNotSymmetric};
}
// Require Q be positive semidefinite
@@ -203,12 +203,12 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
auto Q_ldlt = Q.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
return wpi::util::unexpected{DAREError::QNotPositiveSemidefinite};
return std::unexpected{DAREError::QNotPositiveSemidefinite};
}
// Require (A, B) pair be stabilizable
if (!IsStabilizable<States, Inputs>(A, B)) {
return wpi::util::unexpected{DAREError::ABNotStabilizable};
return std::unexpected{DAREError::ABNotStabilizable};
}
// Require (A, C) pair be detectable where Q = CᵀC
@@ -221,7 +221,7 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
Q_ldlt.transpositionsP();
if (!IsDetectable<States, States>(A, C)) {
return wpi::util::unexpected{DAREError::ACNotDetectable};
return std::unexpected{DAREError::ACNotDetectable};
}
}
@@ -281,7 +281,7 @@ J = Σ [uₖ] [0 R][uₖ] ΔT
@return Solution to the DARE on success, or DAREError on failure.
*/
template <int States, int Inputs>
wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
std::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
@@ -291,14 +291,14 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
if (checkPreconditions) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
return wpi::util::unexpected{DAREError::RNotSymmetric};
return std::unexpected{DAREError::RNotSymmetric};
}
}
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
return wpi::util::unexpected{DAREError::RNotPositiveDefinite};
return std::unexpected{DAREError::RNotPositiveDefinite};
}
// This is a change of variables to make the DARE that includes Q, R, and N
@@ -320,7 +320,7 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
if (checkPreconditions) {
// Require Q be symmetric
if ((Q_2 - Q_2.transpose()).norm() > 1e-10) {
return wpi::util::unexpected{DAREError::QNotSymmetric};
return std::unexpected{DAREError::QNotSymmetric};
}
// Require Q be positive semidefinite
@@ -335,12 +335,12 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
auto Q_ldlt = Q_2.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
return wpi::util::unexpected{DAREError::QNotPositiveSemidefinite};
return std::unexpected{DAREError::QNotPositiveSemidefinite};
}
// Require (A, B) pair be stabilizable
if (!IsStabilizable<States, Inputs>(A_2, B)) {
return wpi::util::unexpected{DAREError::ABNotStabilizable};
return std::unexpected{DAREError::ABNotStabilizable};
}
// Require (A, C) pair be detectable where Q = CᵀC
@@ -353,7 +353,7 @@ wpi::util::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
Q_ldlt.transpositionsP();
if (!IsDetectable<States, States>(A_2, C)) {
return wpi::util::unexpected{DAREError::ACNotDetectable};
return std::unexpected{DAREError::ACNotDetectable};
}
}

27
wpimath/src/test/native/cpp/DARETest.cpp
View File

@@ -4,25 +4,24 @@
#include "wpi/math/linalg/DARE.hpp"
#include <expected>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#include "wpi/math/fmt/Eigen.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/util/expected"
#include "wpi/util/print.hpp"
// 2x1
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
bool checkPreconditions);
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
@@ -31,15 +30,13 @@ wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
bool checkPreconditions);
// 4x1
extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R,
bool checkPreconditions);
extern template wpi::util::expected<Eigen::Matrix<double, 4, 4>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
@@ -48,15 +45,13 @@ wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
bool checkPreconditions);
// 2x2
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
bool checkPreconditions);
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
@@ -65,15 +60,13 @@ wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
bool checkPreconditions);
// 2x3
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R,
bool checkPreconditions);
extern template wpi::util::expected<Eigen::Matrix<double, 2, 2>,
wpi::math::DAREError>
extern template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,

4
wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
View File

@@ -4,13 +4,13 @@
#include "wpi/math/linalg/DARE.hpp"
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
bool checkPreconditions);
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,

4
wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
View File

@@ -4,13 +4,13 @@
#include "wpi/math/linalg/DARE.hpp"
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
bool checkPreconditions);
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,

4
wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
View File

@@ -4,13 +4,13 @@
#include "wpi/math/linalg/DARE.hpp"
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R,
bool checkPreconditions);
template wpi::util::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 2, 2>, wpi::math::DAREError>
wpi::math::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,

9
wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
View File

@@ -2,16 +2,17 @@
// 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/linalg/DARE.hpp"
#include "wpi/util/expected"
#include <expected>
template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
#include "wpi/math/linalg/DARE.hpp"
template std::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R,
bool checkPreconditions);
template wpi::util::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
template std::expected<Eigen::Matrix<double, 4, 4>, wpi::math::DAREError>
wpi::math::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,