mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Make LTV controller constructors use faster DARE solver (#5543)
Made JNI modifications to expose the faster function, made the API use the typesafe Matrix API, and synchronized the documentation with C++. Sped up C++ LTV diff drive test from 20 ms to 15 ms. Sped up C++ LTV unicycle test from 15 ms to 10 ms.
This commit is contained in:
Tyler Veness
@@ -12,33 +12,122 @@ public final class DARE {
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
* @param Q State cost matrix.
|
||||
* @param R Input cost matrix.
|
||||
* @return Solution of DARE.
|
||||
* @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
|
||||
* @throws IllegalArgumentException if R isn't symmetric positive definite.
|
||||
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
|
||||
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
|
||||
*/
|
||||
public static SimpleMatrix dare(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
|
||||
var S = new SimpleMatrix(A.getNumRows(), A.getNumCols());
|
||||
WPIMathJNI.dareABQR(
|
||||
A.getDDRM().getData(),
|
||||
B.getDDRM().getData(),
|
||||
Q.getDDRM().getData(),
|
||||
R.getDDRM().getData(),
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R) {
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareDetailABQR(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getDDRM().getData());
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
|
||||
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
* @param Q State cost matrix.
|
||||
* @param R Input cost matrix.
|
||||
* @param N State-input cross-term cost matrix.
|
||||
* @return Solution of DARE.
|
||||
*/
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N) {
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareDetailABQRN(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
N.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -57,40 +146,48 @@ public final class DARE {
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R) {
|
||||
return new Matrix<>(dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
*
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
* @param Q State cost matrix.
|
||||
* @param R Input cost matrix.
|
||||
* @param N State-input cross-term cost matrix.
|
||||
* @return Solution of DARE.
|
||||
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
|
||||
* @throws IllegalArgumentException if R isn't symmetric positive definite.
|
||||
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
|
||||
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
|
||||
*/
|
||||
public static SimpleMatrix dare(
|
||||
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
|
||||
var S = new SimpleMatrix(A.getNumRows(), A.getNumCols());
|
||||
WPIMathJNI.dareABQRN(
|
||||
A.getDDRM().getData(),
|
||||
B.getDDRM().getData(),
|
||||
Q.getDDRM().getData(),
|
||||
R.getDDRM().getData(),
|
||||
N.getDDRM().getData(),
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareABQR(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getDDRM().getData());
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete algebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
|
||||
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -111,7 +208,16 @@ public final class DARE {
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N) {
|
||||
return new Matrix<>(
|
||||
dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage(), N.getStorage()));
|
||||
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
|
||||
WPIMathJNI.dareABQRN(
|
||||
A.getStorage().getDDRM().getData(),
|
||||
B.getStorage().getDDRM().getData(),
|
||||
Q.getStorage().getDDRM().getData(),
|
||||
R.getStorage().getDDRM().getData(),
|
||||
N.getStorage().getDDRM().getData(),
|
||||
A.getNumCols(),
|
||||
B.getNumCols(),
|
||||
S.getStorage().getDDRM().getData());
|
||||
return S;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -38,6 +38,18 @@ public class Matrix<R extends Num, C extends Num> {
|
||||
Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(columns).getNum());
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
|
||||
* provided generic bounds match the shape of the provided {@link Matrix}.
|
||||
*
|
||||
* @param rows The number of rows of the matrix.
|
||||
* @param columns The number of columns of the matrix.
|
||||
* @param storage The double array to back this value.
|
||||
*/
|
||||
public Matrix(Nat<R> rows, Nat<C> columns, double[] storage) {
|
||||
this.m_storage = new SimpleMatrix(rows.getNum(), columns.getNum(), true, storage);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
|
||||
* provided generic bounds match the shape of the provided {@link Matrix}.
|
||||
|
||||
@@ -44,7 +44,100 @@ public final class WPIMathJNI {
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves the discrete alegebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met. Solves the discrete
|
||||
* alegebraic Riccati equation.
|
||||
*
|
||||
* @param A Array containing elements of A in row-major order.
|
||||
* @param B Array containing elements of B in row-major order.
|
||||
* @param Q Array containing elements of Q in row-major order.
|
||||
* @param R Array containing elements of R in row-major order.
|
||||
* @param states Number of states in A matrix.
|
||||
* @param inputs Number of inputs in B matrix.
|
||||
* @param S Array storage for DARE solution.
|
||||
*/
|
||||
public static native void dareDetailABQR(
|
||||
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
|
||||
|
||||
/**
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
|
||||
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param A Array containing elements of A in row-major order.
|
||||
* @param B Array containing elements of B in row-major order.
|
||||
* @param Q Array containing elements of Q in row-major order.
|
||||
* @param R Array containing elements of R in row-major order.
|
||||
* @param N Array containing elements of N in row-major order.
|
||||
* @param states Number of states in A matrix.
|
||||
* @param inputs Number of inputs in B matrix.
|
||||
* @param S Array storage for DARE solution.
|
||||
*/
|
||||
public static native void dareDetailABQRN(
|
||||
double[] A,
|
||||
double[] B,
|
||||
double[] Q,
|
||||
double[] R,
|
||||
double[] N,
|
||||
int states,
|
||||
int inputs,
|
||||
double[] S);
|
||||
|
||||
/**
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @param A Array containing elements of A in row-major order.
|
||||
* @param B Array containing elements of B in row-major order.
|
||||
@@ -62,7 +155,35 @@ public final class WPIMathJNI {
|
||||
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
|
||||
|
||||
/**
|
||||
* Solves the discrete alegebraic Riccati equation.
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
|
||||
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* @param A Array containing elements of A in row-major order.
|
||||
* @param B Array containing elements of B in row-major order.
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.DARE;
|
||||
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
@@ -15,6 +16,7 @@ import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.numbers.N5;
|
||||
import edu.wpi.first.math.system.Discretization;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
|
||||
@@ -146,11 +148,26 @@ public class LTVDifferentialDriveController {
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (Math.abs(velocity) < 1e-4) {
|
||||
m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
|
||||
A.set(State.kY.value, State.kHeading.value, 1e-4);
|
||||
} else {
|
||||
A.set(State.kY.value, State.kHeading.value, velocity);
|
||||
m_table.put(velocity, new LinearQuadraticRegulator<N5, N2, N5>(A, B, Q, R, dt).getK());
|
||||
}
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = DARE.dareDetail(discA, discB, Q, R);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
m_table.put(
|
||||
velocity,
|
||||
discB
|
||||
.transpose()
|
||||
.times(S)
|
||||
.times(discB)
|
||||
.plus(R)
|
||||
.solve(discB.transpose().times(S).times(discA)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.DARE;
|
||||
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
@@ -15,6 +16,7 @@ import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.system.Discretization;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
|
||||
/**
|
||||
@@ -152,11 +154,26 @@ public class LTVUnicycleController {
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (Math.abs(velocity) < 1e-4) {
|
||||
m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
|
||||
A.set(State.kY.value, State.kHeading.value, 1e-4);
|
||||
} else {
|
||||
A.set(State.kY.value, State.kHeading.value, velocity);
|
||||
m_table.put(velocity, new LinearQuadraticRegulator<N3, N2, N3>(A, B, Q, R, dt).getK());
|
||||
}
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = DARE.dareDetail(discA, discB, Q, R);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
m_table.put(
|
||||
velocity,
|
||||
discB
|
||||
.transpose()
|
||||
.times(S)
|
||||
.times(discB)
|
||||
.plus(R)
|
||||
.solve(discB.transpose().times(S).times(discA)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user