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

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.

wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java

@@ -61,24 +61,29 @@ public final class StateSpaceUtil {
   /**
    * Creates a cost matrix from the given vector for use with LQR.
    *
-   * <p>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.
+   * <p>The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is
+   * placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to
+   * zero.
    *
-   * @param <States> Nat representing the states of the system.
-   * @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 <Elements> Nat representing the number of system states or inputs.
+   * @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.
    */
   @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num> Matrix<States, States> makeCostMatrix(
-      Matrix<States, N1> costs) {
-    Matrix<States, States> result =
-        new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
+  public static <Elements extends Num> Matrix<Elements, Elements> makeCostMatrix(
+      Matrix<Elements, N1> tolerances) {
+    Matrix<Elements, Elements> result =
+        new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows()));
     result.fill(0.0);
 
-    for (int i = 0; i < costs.getNumRows(); i++) {
-      result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
+    for (int i = 0; i < tolerances.getNumRows(); i++) {
+      if (tolerances.get(i, 0) == Double.POSITIVE_INFINITY) {
+        result.set(i, i, 0.0);
+      } else {
+        result.set(i, i, 1.0 / (Math.pow(tolerances.get(i, 0), 2)));
+      }
     }
 
     return result;

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -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;
 }