mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Remove LUTs from LTV controllers (#7521)
The Raspberry Pi 5 is fast enough that we no longer need it. ``` Running ./build/DAREBench Run on (4 X 2400 MHz CPU s) CPU Caches: L1 Data 64 KiB (x4) L1 Instruction 64 KiB (x4) L2 Unified 512 KiB (x4) L3 Unified 2048 KiB (x1) Load Average: 0.47, 0.72, 0.45 ***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead. ------------------------------------------------------------------------------- Benchmark Time CPU Iterations ------------------------------------------------------------------------------- DARE_WPIMath_Dynamic 34.4 us 34.4 us 20315 DARE_WPIMath_NoPrecondChecks_Dynamic 21.7 us 21.7 us 32266 DARE_WPIMath_Static 15.2 us 15.2 us 45878 DARE_WPIMath_NoPrecondChecks_Static 7.84 us 7.84 us 89316 DARE_SLICOT 79.4 us 79.4 us 8789 DARE_Drake 34.9 us 34.9 us 20074 ```
This commit is contained in:
Tyler Veness
@@ -5,7 +5,6 @@
|
||||
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;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
@@ -40,28 +39,19 @@ import edu.wpi.first.math.trajectory.Trajectory;
|
||||
public class LTVDifferentialDriveController {
|
||||
private final double m_trackwidth;
|
||||
|
||||
// LUT from drivetrain linear velocity to LQR gain
|
||||
private final InterpolatingMatrixTreeMap<Double, N2, N5> m_table =
|
||||
new InterpolatingMatrixTreeMap<>();
|
||||
// Continuous velocity dynamics
|
||||
private final Matrix<N2, N2> m_A;
|
||||
private final Matrix<N2, N2> m_B;
|
||||
|
||||
// LQR cost matrices
|
||||
private final Matrix<N5, N5> m_Q;
|
||||
private final Matrix<N2, N2> m_R;
|
||||
|
||||
private final double m_dt;
|
||||
|
||||
private Matrix<N5, N1> m_error = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
|
||||
/** States of the drivetrain system. */
|
||||
private enum State {
|
||||
kX(0),
|
||||
kY(1),
|
||||
kHeading(2),
|
||||
kLeftVelocity(3),
|
||||
kRightVelocity(4);
|
||||
|
||||
public final int value;
|
||||
|
||||
State(int i) {
|
||||
this.value = i;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying differential drive controller.
|
||||
*
|
||||
@@ -75,8 +65,6 @@ public class LTVDifferentialDriveController {
|
||||
* @param qelems The maximum desired error tolerance for each state.
|
||||
* @param relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep in seconds.
|
||||
* @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0 m/s or >=
|
||||
* 15 m/s.
|
||||
*/
|
||||
public LTVDifferentialDriveController(
|
||||
LinearSystem<N2, N2, N2> plant,
|
||||
@@ -85,96 +73,11 @@ public class LTVDifferentialDriveController {
|
||||
Vector<N2> relems,
|
||||
double dt) {
|
||||
m_trackwidth = trackwidth;
|
||||
|
||||
// Control law derivation is in section 8.7 of
|
||||
// https://file.tavsys.net/control/controls-engineering-in-frc.pdf
|
||||
var A =
|
||||
MatBuilder.fill(
|
||||
Nat.N5(),
|
||||
Nat.N5(),
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.5,
|
||||
0.5,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-1.0 / m_trackwidth,
|
||||
1.0 / m_trackwidth,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
plant.getA(0, 0),
|
||||
plant.getA(0, 1),
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
plant.getA(1, 0),
|
||||
plant.getA(1, 1));
|
||||
var B =
|
||||
MatBuilder.fill(
|
||||
Nat.N5(),
|
||||
Nat.N2(),
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
plant.getB(0, 0),
|
||||
plant.getB(0, 1),
|
||||
plant.getB(1, 0),
|
||||
plant.getB(1, 1));
|
||||
var Q = StateSpaceUtil.makeCostMatrix(qelems);
|
||||
var R = StateSpaceUtil.makeCostMatrix(relems);
|
||||
|
||||
// dx/dt = Ax + Bu
|
||||
// 0 = Ax + Bu
|
||||
// Ax = -Bu
|
||||
// x = -A⁻¹Bu
|
||||
double maxV =
|
||||
plant.getA().solve(plant.getB().times(VecBuilder.fill(12.0, 12.0))).times(-1.0).get(0, 0);
|
||||
|
||||
if (maxV <= 0.0) {
|
||||
throw new IllegalArgumentException(
|
||||
"Max velocity of plant with 12 V input must be greater than 0 m/s.");
|
||||
}
|
||||
if (maxV >= 15.0) {
|
||||
throw new IllegalArgumentException(
|
||||
"Max velocity of plant with 12 V input must be less than 15 m/s.");
|
||||
}
|
||||
|
||||
for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
|
||||
// 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) {
|
||||
A.set(State.kY.value, State.kHeading.value, 1e-4);
|
||||
} else {
|
||||
A.set(State.kY.value, State.kHeading.value, velocity);
|
||||
}
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = DARE.dareNoPrecond(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)));
|
||||
}
|
||||
m_A = plant.getA();
|
||||
m_B = plant.getB();
|
||||
m_Q = StateSpaceUtil.makeCostMatrix(qelems);
|
||||
m_R = StateSpaceUtil.makeCostMatrix(relems);
|
||||
m_dt = dt;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -230,20 +133,21 @@ public class LTVDifferentialDriveController {
|
||||
double leftVelocityRef,
|
||||
double rightVelocityRef) {
|
||||
// This implements the linear time-varying differential drive controller in
|
||||
// theorem 9.6.3 of https://tavsys.net/controls-in-frc.
|
||||
var x =
|
||||
VecBuilder.fill(
|
||||
currentPose.getX(),
|
||||
currentPose.getY(),
|
||||
currentPose.getRotation().getRadians(),
|
||||
leftVelocity,
|
||||
rightVelocity);
|
||||
// theorem 8.7.4 of https://controls-in-frc.link/
|
||||
//
|
||||
// [x ]
|
||||
// [y ] [Vₗ]
|
||||
// x = [θ ] u = [Vᵣ]
|
||||
// [vₗ]
|
||||
// [vᵣ]
|
||||
|
||||
var inRobotFrame = Matrix.eye(Nat.N5());
|
||||
inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0)));
|
||||
inRobotFrame.set(0, 1, Math.sin(x.get(State.kHeading.value, 0)));
|
||||
inRobotFrame.set(1, 0, -Math.sin(x.get(State.kHeading.value, 0)));
|
||||
inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0)));
|
||||
double velocity = (leftVelocity + rightVelocity) / 2.0;
|
||||
|
||||
// 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) {
|
||||
velocity = 1e-4;
|
||||
}
|
||||
|
||||
var r =
|
||||
VecBuilder.fill(
|
||||
@@ -252,12 +156,55 @@ public class LTVDifferentialDriveController {
|
||||
poseRef.getRotation().getRadians(),
|
||||
leftVelocityRef,
|
||||
rightVelocityRef);
|
||||
m_error = r.minus(x);
|
||||
m_error.set(
|
||||
State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0)));
|
||||
var x =
|
||||
VecBuilder.fill(
|
||||
currentPose.getX(),
|
||||
currentPose.getY(),
|
||||
currentPose.getRotation().getRadians(),
|
||||
leftVelocity,
|
||||
rightVelocity);
|
||||
|
||||
double velocity = (leftVelocity + rightVelocity) / 2.0;
|
||||
var K = m_table.get(velocity);
|
||||
m_error = r.minus(x);
|
||||
m_error.set(2, 0, MathUtil.angleModulus(m_error.get(2, 0)));
|
||||
|
||||
// spotless:off
|
||||
var A = MatBuilder.fill(Nat.N5(), Nat.N5(),
|
||||
0.0, 0.0, 0.0, 0.5, 0.5,
|
||||
0.0, 0.0, velocity, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, -1.0 / m_trackwidth, 1.0 / m_trackwidth,
|
||||
0.0, 0.0, 0.0, m_A.get(0, 0), m_A.get(0, 1),
|
||||
0.0, 0.0, 0.0, m_A.get(1, 0), m_A.get(1, 1));
|
||||
var B = MatBuilder.fill(Nat.N5(), Nat.N2(),
|
||||
0.0, 0.0,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0,
|
||||
m_B.get(0, 0), m_B.get(0, 1),
|
||||
m_B.get(1, 0), m_B.get(1, 1));
|
||||
// spotless:on
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, m_dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
var K =
|
||||
discB
|
||||
.transpose()
|
||||
.times(S)
|
||||
.times(discB)
|
||||
.plus(m_R)
|
||||
.solve(discB.transpose().times(S).times(discA));
|
||||
|
||||
// spotless:off
|
||||
var inRobotFrame = MatBuilder.fill(Nat.N5(), Nat.N5(),
|
||||
Math.cos(x.get(2, 0)), Math.sin(x.get(2, 0)), 0.0, 0.0, 0.0,
|
||||
-Math.sin(x.get(2, 0)), Math.cos(x.get(2, 0)), 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 1.0);
|
||||
// spotless:on
|
||||
|
||||
var u = K.times(inRobotFrame).times(m_error);
|
||||
|
||||
|
||||
@@ -5,7 +5,6 @@
|
||||
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;
|
||||
import edu.wpi.first.math.Nat;
|
||||
@@ -31,27 +30,16 @@ import edu.wpi.first.math.trajectory.Trajectory;
|
||||
* shown in theorem 8.9.1.
|
||||
*/
|
||||
public class LTVUnicycleController {
|
||||
// LUT from drivetrain linear velocity to LQR gain
|
||||
private final InterpolatingMatrixTreeMap<Double, N2, N3> m_table =
|
||||
new InterpolatingMatrixTreeMap<>();
|
||||
// LQR cost matrices
|
||||
private Matrix<N3, N3> m_Q;
|
||||
private Matrix<N2, N2> m_R;
|
||||
|
||||
private final double m_dt;
|
||||
|
||||
private Pose2d m_poseError;
|
||||
private Pose2d m_poseTolerance;
|
||||
private boolean m_enabled = true;
|
||||
|
||||
/** States of the drivetrain system. */
|
||||
private enum State {
|
||||
kX(0),
|
||||
kY(1),
|
||||
kHeading(2);
|
||||
|
||||
public final int value;
|
||||
|
||||
State(int i) {
|
||||
this.value = i;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller with default maximum desired error
|
||||
* tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
|
||||
@@ -60,21 +48,7 @@ public class LTVUnicycleController {
|
||||
* @param dt Discretization timestep in seconds.
|
||||
*/
|
||||
public LTVUnicycleController(double dt) {
|
||||
this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, 9.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller with default maximum desired error
|
||||
* tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
|
||||
* 2 rad/s).
|
||||
*
|
||||
* @param dt Discretization timestep in seconds.
|
||||
* @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
|
||||
* table. The default is 9 m/s.
|
||||
* @throws IllegalArgumentException if maxVelocity <= 0.
|
||||
*/
|
||||
public LTVUnicycleController(double dt, double maxVelocity) {
|
||||
this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
|
||||
this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -89,95 +63,9 @@ public class LTVUnicycleController {
|
||||
* @param dt Discretization timestep in seconds.
|
||||
*/
|
||||
public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
|
||||
this(qelems, relems, dt, 9.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller.
|
||||
*
|
||||
* <p>See <a
|
||||
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
|
||||
* for how to select the tolerances.
|
||||
*
|
||||
* @param qelems The maximum desired error tolerance for each state.
|
||||
* @param relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep in seconds.
|
||||
* @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
|
||||
* table. The default is 9 m/s.
|
||||
* @throws IllegalArgumentException if maxVelocity <= 0 m/s or >= 15 m/s.
|
||||
*/
|
||||
public LTVUnicycleController(
|
||||
Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
|
||||
if (maxVelocity <= 0.0) {
|
||||
throw new IllegalArgumentException("Max velocity must be greater than 0 m/s.");
|
||||
}
|
||||
if (maxVelocity >= 15.0) {
|
||||
throw new IllegalArgumentException("Max velocity must be less than 15 m/s.");
|
||||
}
|
||||
|
||||
// The change in global pose for a unicycle is defined by the following
|
||||
// three equations.
|
||||
//
|
||||
// ẋ = v cosθ
|
||||
// ẏ = v sinθ
|
||||
// θ̇ = ω
|
||||
//
|
||||
// Here's the model as a vector function where x = [x y θ]ᵀ and
|
||||
// u = [v ω]ᵀ.
|
||||
//
|
||||
// [v cosθ]
|
||||
// f(x, u) = [v sinθ]
|
||||
// [ ω ]
|
||||
//
|
||||
// To create an LQR, we need to linearize this.
|
||||
//
|
||||
// [0 0 −v sinθ] [cosθ 0]
|
||||
// ∂f(x, u)/∂x = [0 0 v cosθ] ∂f(x, u)/∂u = [sinθ 0]
|
||||
// [0 0 0 ] [ 0 1]
|
||||
//
|
||||
// We're going to make a cross-track error controller, so we'll apply a
|
||||
// clockwise rotation matrix to the global tracking error to transform it
|
||||
// into the robot's coordinate frame. Since the cross-track error is always
|
||||
// measured from the robot's coordinate frame, the model used to compute the
|
||||
// LQR should be linearized around θ = 0 at all times.
|
||||
//
|
||||
// [0 0 −v sin0] [cos0 0]
|
||||
// A = [0 0 v cos0] B = [sin0 0]
|
||||
// [0 0 0 ] [ 0 1]
|
||||
//
|
||||
// [0 0 0] [1 0]
|
||||
// A = [0 0 v] B = [0 0]
|
||||
// [0 0 0] [0 1]
|
||||
var A = new Matrix<>(Nat.N3(), Nat.N3());
|
||||
var B = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
|
||||
var Q = StateSpaceUtil.makeCostMatrix(qelems);
|
||||
var R = StateSpaceUtil.makeCostMatrix(relems);
|
||||
|
||||
for (double velocity = -maxVelocity; velocity < maxVelocity; velocity += 0.01) {
|
||||
// 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) {
|
||||
A.set(State.kY.value, State.kHeading.value, 1e-4);
|
||||
} else {
|
||||
A.set(State.kY.value, State.kHeading.value, velocity);
|
||||
}
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = DARE.dareNoPrecond(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)));
|
||||
}
|
||||
m_Q = StateSpaceUtil.makeCostMatrix(qelems);
|
||||
m_R = StateSpaceUtil.makeCostMatrix(relems);
|
||||
m_dt = dt;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -218,13 +106,78 @@ public class LTVUnicycleController {
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
|
||||
// The change in global pose for a unicycle is defined by the following
|
||||
// three equations.
|
||||
//
|
||||
// ẋ = v cosθ
|
||||
// ẏ = v sinθ
|
||||
// θ̇ = ω
|
||||
//
|
||||
// Here's the model as a vector function where x = [x y θ]ᵀ and
|
||||
// u = [v ω]ᵀ.
|
||||
//
|
||||
// [v cosθ]
|
||||
// f(x, u) = [v sinθ]
|
||||
// [ ω ]
|
||||
//
|
||||
// To create an LQR, we need to linearize this.
|
||||
//
|
||||
// [0 0 −v sinθ] [cosθ 0]
|
||||
// ∂f(x, u)/∂x = [0 0 v cosθ] ∂f(x, u)/∂u = [sinθ 0]
|
||||
// [0 0 0 ] [ 0 1]
|
||||
//
|
||||
// We're going to make a cross-track error controller, so we'll apply a
|
||||
// clockwise rotation matrix to the global tracking error to transform it
|
||||
// into the robot's coordinate frame. Since the cross-track error is always
|
||||
// measured from the robot's coordinate frame, the model used to compute the
|
||||
// LQR should be linearized around θ = 0 at all times.
|
||||
//
|
||||
// [0 0 −v sin0] [cos0 0]
|
||||
// A = [0 0 v cos0] B = [sin0 0]
|
||||
// [0 0 0 ] [ 0 1]
|
||||
//
|
||||
// [0 0 0] [1 0]
|
||||
// A = [0 0 v] B = [0 0]
|
||||
// [0 0 0] [0 1]
|
||||
|
||||
if (!m_enabled) {
|
||||
return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
|
||||
}
|
||||
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (Math.abs(linearVelocityRef) < 1e-4) {
|
||||
linearVelocityRef = 1e-4;
|
||||
}
|
||||
|
||||
m_poseError = poseRef.relativeTo(currentPose);
|
||||
|
||||
var K = m_table.get(linearVelocityRef);
|
||||
// spotless:off
|
||||
var A = MatBuilder.fill(Nat.N3(), Nat.N3(),
|
||||
0.0, 0.0, 0.0,
|
||||
0.0, 0.0, linearVelocityRef,
|
||||
0.0, 0.0, 0.0);
|
||||
var B = MatBuilder.fill(Nat.N3(), Nat.N2(),
|
||||
1.0, 0.0,
|
||||
0.0, 0.0,
|
||||
0.0, 1.0);
|
||||
// spotless:on
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, m_dt);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
var K =
|
||||
discB
|
||||
.transpose()
|
||||
.times(S)
|
||||
.times(discB)
|
||||
.plus(m_R)
|
||||
.solve(discB.transpose().times(S).times(discA));
|
||||
|
||||
var e =
|
||||
MatBuilder.fill(
|
||||
Nat.N3(),
|
||||
|
||||
Reference in New Issue
Block a user