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

[wpimath] Make LTV controller constructors use faster DARE solver (#5543)

Browse Source 
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
2023-08-17 13:56:15 -07:00
committed by GitHub
parent 6953a303b3
commit 0cf6e37dc1
9 changed files with 596 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View File

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

21
wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View File

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