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] Catch incorrect parameters to state-space models earlier (#3680)

Browse Source 
This allows giving more descriptive exceptions than if they are thrown
later in KalmanFilter, for example.

Fixes #3678.
This commit is contained in:
Tyler Veness
2021-10-27 20:18:57 -07:00
committed by GitHub
parent 8d04606c4d
commit 187f50a344
4 changed files with 246 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
View File

@@ -33,6 +33,7 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
* @param b The input matrix B.
* @param c The output matrix C.
* @param d The feedthrough matrix D.
* @throws IllegalArgumentException if any matrix element isn't finite.
*/
@SuppressWarnings("ParameterName")
public LinearSystem(
@@ -40,6 +41,39 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
Matrix<States, Inputs> b,
Matrix<Outputs, States> c,
Matrix<Outputs, Inputs> d) {
for (int row = 0; row < a.getNumRows(); ++row) {
for (int col = 0; col < a.getNumCols(); ++col) {
if (!Double.isFinite(a.get(row, col))) {
throw new IllegalArgumentException(
"Elements of A aren't finite. This is usually due to model implementation errors.");
}
}
}
for (int row = 0; row < b.getNumRows(); ++row) {
for (int col = 0; col < b.getNumCols(); ++col) {
if (!Double.isFinite(b.get(row, col))) {
throw new IllegalArgumentException(
"Elements of B aren't finite. This is usually due to model implementation errors.");
}
}
}
for (int row = 0; row < c.getNumRows(); ++row) {
for (int col = 0; col < c.getNumCols(); ++col) {
if (!Double.isFinite(c.get(row, col))) {
throw new IllegalArgumentException(
"Elements of C aren't finite. This is usually due to model implementation errors.");
}
}
}
for (int row = 0; row < d.getNumRows(); ++row) {
for (int col = 0; col < d.getNumCols(); ++col) {
if (!Double.isFinite(d.get(row, col))) {
throw new IllegalArgumentException(
"Elements of D aren't finite. This is usually due to model implementation errors.");
}
}
}
this.m_A = a;
this.m_B = b;
this.m_C = c;

92
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -25,10 +25,21 @@ public final class LinearSystemId {
* @param radiusMeters The radius of thd driving drum of the elevator, in meters.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or G &lt;= 0.
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N1> createElevatorSystem(
DCMotor motor, double massKg, double radiusMeters, double G) {
if (massKg <= 0.0) {
throw new IllegalArgumentException("massKg must be greater than zero.");
}
if (radiusMeters <= 0.0) {
throw new IllegalArgumentException("radiusMeters must be greater than zero.");
}
if (G <= 0) {
throw new IllegalArgumentException("G must be greater than zero.");
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
@@ -55,10 +66,18 @@ public final class LinearSystemId {
* @param jKgMetersSquared The moment of inertia J of the flywheel.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
DCMotor motor, double jKgMetersSquared, double G) {
if (jKgMetersSquared <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (G <= 0.0) {
throw new IllegalArgumentException("G must be greater than zero.");
}
return new LinearSystem<>(
VecBuilder.fill(
-G
@@ -81,6 +100,7 @@ public final class LinearSystemId {
* @param JKgMetersSquared the moment of inertia of the robot.
* @param G the gearing reduction as output over input.
* @return A LinearSystem representing a differential drivetrain.
* @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or G &lt;= 0.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
@@ -90,6 +110,22 @@ public final class LinearSystemId {
double rbMeters,
double JKgMetersSquared,
double G) {
if (massKg <= 0.0) {
throw new IllegalArgumentException("massKg must be greater than zero.");
}
if (rMeters <= 0.0) {
throw new IllegalArgumentException("rMeters must be greater than zero.");
}
if (rbMeters <= 0.0) {
throw new IllegalArgumentException("rbMeters must be greater than zero.");
}
if (JKgMetersSquared <= 0.0) {
throw new IllegalArgumentException("JKgMetersSquared must be greater than zero.");
}
if (G <= 0.0) {
throw new IllegalArgumentException("G must be greater than zero.");
}
var C1 =
-(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
@@ -117,6 +153,13 @@ public final class LinearSystemId {
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
DCMotor motor, double jKgSquaredMeters, double G) {
if (jKgSquaredMeters <= 0.0) {
throw new IllegalArgumentException("jKgSquaredMeters must be greater than zero.");
}
if (G <= 0.0) {
throw new IllegalArgumentException("G must be greater than zero.");
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
@@ -142,10 +185,18 @@ public final class LinearSystemId {
* @param kV The velocity gain, in volts per (units per second)
* @param kA The acceleration gain, in volts per (units per second squared)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
if (kV <= 0.0) {
throw new IllegalArgumentException("Kv must be greater than zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
return new LinearSystem<>(
VecBuilder.fill(-kV / kA),
VecBuilder.fill(1.0 / kA),
@@ -164,10 +215,18 @@ public final class LinearSystemId {
* @param kV The velocity gain, in volts per (units per second)
* @param kA The acceleration gain, in volts per (units per second squared)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
if (kV <= 0.0) {
throw new IllegalArgumentException("Kv must be greater than zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
VecBuilder.fill(0.0, 1.0 / kA),
@@ -187,11 +246,26 @@ public final class LinearSystemId {
* @param kVAngular The angular velocity gain, volts per (meter per second).
* @param kAAngular The angular acceleration gain, volts per (meter per second squared).
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
* kAAngular &lt;= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
if (kVLinear <= 0.0) {
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
}
if (kALinear <= 0.0) {
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
}
if (kVAngular <= 0.0) {
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
}
if (kAAngular <= 0.0) {
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
}
final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
@@ -217,11 +291,29 @@ public final class LinearSystemId {
* @param kAAngular The angular acceleration gain, volts per (radians per second squared).
* @param trackwidth The width of the drivetrain in meters.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
* kAAngular &lt;= 0, or trackwidth &lt;= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
if (kVLinear <= 0.0) {
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
}
if (kALinear <= 0.0) {
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
}
if (kVAngular <= 0.0) {
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
}
if (kAAngular <= 0.0) {
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
}
if (trackwidth <= 0.0) {
throw new IllegalArgumentException("trackwidth must be greater than zero.");
}
// We want to find a factor to include in Kv,angular that will convert
// `u = Kv,angular omega` to `u = Kv,angular v`.
//