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

[wpimath] Fix NaN in C++ MakeCostMatrix() that takes an array (#5194)

Browse Source 
Java's makeCostMatrix() and the C++ MakeCostMatrix() overload that takes
a parameter pack already do the right thing.
This commit is contained in:
Tyler Veness
2023-03-17 07:05:28 -07:00
committed by GitHub
parent 00abb8c1e0
commit df3242a40a
1 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -138,7 +138,8 @@ Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
* Creates a cost matrix from the given vector for use with LQR.
*
* The cost matrix is constructed using Bryson's rule. The inverse square of
* each element in the input is taken and placed on the cost matrix diagonal.
* each element in the input is placed on the cost matrix diagonal. If a
* tolerance is infinity, its cost matrix entry is set to zero.
*
* @param costs An array. For a Q matrix, its elements are the maximum allowed
* excursions of the states from the reference. For an R matrix,
@@ -151,7 +152,11 @@ Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
diag(i) = 1.0 / std::pow(costs[i], 2);
if (costs[i] == std::numeric_limits<double>::infinity()) {
diag(i) = 0.0;
} else {
diag(i) = 1.0 / std::pow(costs[i], 2);
}
}
return result;
}