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

[wpimath] Support zero cost entries in MakeCostMatrix() (#4100)

Browse Source 
The existing implementation will produce a cost of NaN if a tolerance of
infinity is entered, but the limit approaches zero. Being able to
specify that a state has no cost is useful, so this change adds support for
that.
This commit is contained in:
Tyler Veness
2022-03-19 20:40:26 -07:00
committed by GitHub
parent d0fef18378
commit d5cb6fed67
2 changed files with 31 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,6 +6,7 @@
#include <array>
#include <cmath>
#include <limits>
#include <random>
#include <type_traits>
@@ -32,7 +33,11 @@ void MatrixImpl(Matrix& result, T elem, Ts... elems) {
template <typename Matrix, typename T, typename... Ts>
void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
if (elem == std::numeric_limits<double>::infinity()) {
result(result.rows() - (sizeof...(Ts) + 1)) = 0.0;
} else {
result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
}
if constexpr (sizeof...(Ts) > 0) {
CostMatrixImpl(result, elems...);
}
@@ -116,20 +121,21 @@ Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
* 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 tolerance 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,
* its elements are the maximum allowed excursions of the control
* inputs from no actuation.
* @param tolerances An array. For a Q matrix, its elements are the maximum
* allowed excursions of the states from the reference. For an
* R matrix, its elements are the maximum allowed excursions
* of the control inputs from no actuation.
* @return State excursion or control effort cost matrix.
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
Ts... costs) {
Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CostMatrixImpl(result.diagonal(), costs...);
detail::CostMatrixImpl(result.diagonal(), tolerances...);
return result;
}