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] 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

29
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

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