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

Clean up Java style (#5990)

Browse Source 
Also make equivalent changes in C++ where applicable.

Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
This commit is contained in:
Tyler Veness
2023-12-03 16:21:32 -08:00
committed by GitHub
parent 66172ab288
commit 2bb1409b82
113 changed files with 426 additions and 617 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -288,7 +288,7 @@ public class Matrix<R extends Num, C extends Num> {
* @return The resultant matrix.
*/
public Matrix<R, C> div(int value) {
return new Matrix<>(this.m_storage.divide((double) value));
return new Matrix<>(this.m_storage.divide(value));
}
/**
@@ -488,7 +488,7 @@ public class Matrix<R extends Num, C extends Num> {
* @return The element by element power of "this" and b.
*/
public final Matrix<R, C> elementPower(int b) {
return new Matrix<>(this.m_storage.elementPower((double) b));
return new Matrix<>(this.m_storage.elementPower(b));
}
/**
@@ -545,7 +545,7 @@ public class Matrix<R extends Num, C extends Num> {
*/
public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
int height, int width, int startingRow, int startingCol) {
return new Matrix<R2, C2>(
return new Matrix<>(
this.m_storage.extractMatrix(
startingRow, startingRow + height, startingCol, startingCol + width));
}
@@ -607,8 +607,7 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(new SimpleMatrix(temp.getNumRows(), temp.getNumCols()));
}
throw new RuntimeException(
"Cholesky decomposition failed! Input matrix:\n" + m_storage.toString());
throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + m_storage);
}
return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));

9
wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
View File

@@ -16,9 +16,8 @@ import java.util.function.Function;
* Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
*
* <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
* vector, the B matrix(continuous input matrix) is calculated through a {@link
* edu.wpi.first.math.system.NumericalJacobian}. In this case f has to be control-affine (of the
* form f(x) + Bu).
* vector, the B matrix(continuous input matrix) is calculated through a {@link NumericalJacobian}.
* In this case f has to be control-affine (of the form f(x) + Bu).
*
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
* <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
@@ -26,8 +25,8 @@ import java.util.function.Function;
* <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
* when the feedforward is created and remains constant.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
*/
public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
/** The current reference state. */

4
wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
View File

@@ -17,8 +17,8 @@ import org.ejml.simple.SimpleMatrix;
* system and makes it behave like some other system. This can be used to make a drivetrain more
* controllable during teleop driving by making it behave like a slower or more benign drivetrain.
*
* <p>For more on the underlying math, read appendix B.3 in
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
* <p>For more on the underlying math, read appendix B.3 in <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
*/
public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
// Computed controller output

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

@@ -65,8 +65,8 @@ public class LTVDifferentialDriveController {
/**
* Constructs a linear time-varying differential drive controller.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
* <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 plant The differential drive velocity plant.

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

@@ -80,8 +80,8 @@ public class LTVUnicycleController {
/**
* Constructs a linear time-varying unicycle controller.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
* <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.
@@ -95,8 +95,8 @@ public class LTVUnicycleController {
/**
* Constructs a linear time-varying unicycle controller.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
* <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.

4
wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
View File

@@ -17,8 +17,8 @@ import org.ejml.simple.SimpleMatrix;
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
* where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
*/
public class LinearPlantInversionFeedforward<
States extends Num, Inputs extends Num, Outputs extends Num> {

31
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -19,8 +19,8 @@ import org.ejml.simple.SimpleMatrix;
* Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use
* the control law u = K(r - x).
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
*/
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
/** The current reference state. */
@@ -35,8 +35,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
/**
* Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
* <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 plant The plant being controlled.
@@ -61,8 +61,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
/**
* Constructs a controller with the given coefficients and plant.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
* <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 A Continuous system matrix of the plant being controlled.
@@ -107,14 +107,12 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var discB = discABPair.getSecond();
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
builder
.append(discA.getStorage().toString())
.append("\nB =\n")
.append(discB.getStorage().toString())
.append('\n');
var msg = builder.toString();
var msg =
"The system passed to the LQR is uncontrollable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nB =\n"
+ discB.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
@@ -258,8 +256,9 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
* can compute the control based on where the system will be after the time delay.
*
* <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
* derivation.
* <p>See <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
* appendix C.4 for a derivation.
*
* @param plant The plant being controlled.
* @param dtSeconds Discretization timestep in seconds.

14
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -30,9 +30,9 @@ import java.util.function.BiFunction;
* error covariance by linearizing the models around the state estimate, then applying the linear
* Kalman filter equations.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
* chapter 9 "Stochastic control theory".
*/
public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
@@ -59,8 +59,8 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
/**
* Constructs an extended Kalman filter.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states a Nat representing the number of states.
@@ -97,8 +97,8 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
/**
* Constructs an extended Kalman filter.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states a Nat representing the number of states.

25
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -25,9 +25,9 @@ import edu.wpi.first.math.system.LinearSystem;
* amount of the difference between the actual measurements and the measurements predicted by the
* model.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
* chapter 9 "Stochastic control theory".
*/
public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
@@ -45,8 +45,8 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
/**
* Constructs a Kalman filter with the given plant.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the states of the system.
@@ -82,15 +82,12 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
var C = plant.getC();
if (!StateSpaceUtil.isDetectable(discA, C)) {
var builder =
new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n");
builder
.append(discA.getStorage().toString())
.append("\nC =\n")
.append(C.getStorage().toString())
.append('\n');
var msg = builder.toString();
var msg =
"The system passed to the Kalman filter is unobservable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}

25
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -28,9 +28,9 @@ import edu.wpi.first.math.system.LinearSystem;
* <p>This class assumes predict() and correct() are called in pairs, so the Kalman gain converges
* to a steady-state value. If they aren't, use {@link KalmanFilter} instead.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
* chapter 9 "Stochastic control theory".
*/
public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
private final Nat<States> m_states;
@@ -46,8 +46,8 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
/**
* Constructs a steady-state Kalman filter with the given plant.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the states of the system.
@@ -81,15 +81,12 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
var C = plant.getC();
if (!StateSpaceUtil.isDetectable(discA, C)) {
var builder =
new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n");
builder
.append(discA.getStorage().toString())
.append("\nC =\n")
.append(C.getStorage().toString())
.append('\n');
var msg = builder.toString();
var msg =
"The system passed to the Kalman filter is unobservable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}

19
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -31,12 +31,13 @@ import org.ejml.simple.SimpleMatrix;
* <p>An unscented Kalman filter uses nonlinear state and measurement models. It propagates the
* error covariance using sigma points chosen to approximate the true probability distribution.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
* chapter 9 "Stochastic control theory".
*
* <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
* information about the SR-UKF, see https://www.researchgate.net/publication/3908304.
* information about the SR-UKF, see <a
* href="https://www.researchgate.net/publication/3908304">https://www.researchgate.net/publication/3908304</a>.
*/
public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
@@ -64,8 +65,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
/**
* Constructs an Unscented Kalman Filter.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the number of states.
@@ -104,8 +105,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* custom functions for arithmetic can be useful if you have angles in the state or measurements,
* because they allow you to correctly account for the modular nature of angle arithmetic.
*
* <p>See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the number of states.
@@ -206,7 +207,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
var qrStorage = Sbar.transpose().getStorage();
if (!qr.decompose(qrStorage.getDDRM())) {
throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage.toString());
throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
}
Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));

10
wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
View File

@@ -35,9 +35,13 @@ import org.ejml.simple.SimpleMatrix;
* PID controller out of this class!
*
* <p>For more on filters, we highly recommend the following articles:<br>
* https://en.wikipedia.org/wiki/Linear_filter<br>
* https://en.wikipedia.org/wiki/Iir_filter<br>
* https://en.wikipedia.org/wiki/Fir_filter<br>
* <a
* href="https://en.wikipedia.org/wiki/Linear_filter">https://en.wikipedia.org/wiki/Linear_filter</a>
* <br>
* <a href="https://en.wikipedia.org/wiki/Iir_filter">https://en.wikipedia.org/wiki/Iir_filter</a>
* <br>
* <a href="https://en.wikipedia.org/wiki/Fir_filter">https://en.wikipedia.org/wiki/Fir_filter</a>
* <br>
*
* <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
* Notifier for this or do it "inline" with code in a periodic function.

15
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -117,19 +117,15 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
// Require that the rotation matrix is special orthogonal. This is true if
// the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if (R.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) {
var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n");
builder.append(R.getStorage().toString()).append('\n');
var msg = builder.toString();
var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
if (Math.abs(R.det() - 1.0) > 1e-9) {
var builder =
new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n");
builder.append(R.getStorage().toString()).append('\n');
var msg = builder.toString();
var msg =
"Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
+ R.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
@@ -191,7 +187,6 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
// If the dot product is 1, the two vectors point in the same direction so
// there's no rotation. The default initialization of m_q will work.
m_q = new Quaternion();
return;
} else if (dotNorm < -1.0 + 1E-9) {
// If the dot product is -1, the two vectors point in opposite directions
// so a 180 degree rotation is required. Any orthogonal vector can be used

2
wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java
View File

@@ -87,7 +87,7 @@ public class TravelingSalesman {
* array.
*/
private <Poses extends Num> Vector<Poses> neighbor(Vector<Poses> state) {
var proposedState = new Vector<Poses>(state);
var proposedState = new Vector<>(state);
int rangeStart = (int) (Math.random() * (state.getNumRows() - 1));
int rangeEnd = (int) (Math.random() * (state.getNumRows() - 1));

4
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
View File

@@ -25,8 +25,8 @@ import org.ejml.simple.SimpleMatrix;
* of the controller (U is an output because that's what goes to the motors and Y is an input
* because that's what comes back from the sensors).
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
* <p>For more on the underlying math, read <a
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
*/
public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
View File

@@ -38,7 +38,7 @@ public interface TrajectoryConstraint {
/** Represents a minimum and maximum acceleration. */
class MinMax {
public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
public double maxAccelerationMetersPerSecondSq = Double.MAX_VALUE;
/**
* Constructs a MinMax.

6
wpimath/src/test/java/edu/wpi/first/math/DARETest.java
View File

@@ -42,8 +42,7 @@ class DARETest extends UtilityClassTest<DARE> {
.times((B.transpose().times(X).times(B).plus(R)).inv())
.times(B.transpose().times(X).times(A)))
.plus(Q);
assertMatrixEqual(
new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
assertMatrixEqual(new Matrix<>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
}
<States extends Num, Inputs extends Num> void assertDARESolution(
@@ -63,8 +62,7 @@ class DARETest extends UtilityClassTest<DARE> {
.times((B.transpose().times(X).times(B).plus(R)).inv())
.times(B.transpose().times(X).times(A).plus(N.transpose())))
.plus(Q);
assertMatrixEqual(
new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
assertMatrixEqual(new Matrix<>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
}
@Test

6
wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
View File

@@ -18,8 +18,7 @@ class ControlAffinePlantInversionFeedforwardTest {
@Test
void testCalculate() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
assertEquals(
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
@@ -28,8 +27,7 @@ class ControlAffinePlantInversionFeedforwardTest {
@Test
void testCalculateState() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
assertEquals(
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);

4
wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
View File

@@ -21,7 +21,7 @@ class ImplicitModelFollowerTest {
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant);
var imf = new ImplicitModelFollower<>(plant, plant);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
@@ -66,7 +66,7 @@ class ImplicitModelFollowerTest {
// Linear acceleration is slower, but angular acceleration is the same
var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef);
var imf = new ImplicitModelFollower<>(plant, plantRef);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);

2
wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
View File

@@ -21,7 +21,7 @@ class LinearPlantInversionFeedforwardTest {
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
new LinearPlantInversionFeedforward<>(A, B, 0.02);
assertEquals(
47.502599,

5
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
@@ -254,7 +255,7 @@ class DifferentialDrivePoseEstimatorTest {
"Estimator converged to one vision measurement: "
+ estimator.getEstimatedPosition().toString()
+ " -> "
+ measurement.toString();
+ measurement;
var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -263,7 +264,7 @@ class DifferentialDrivePoseEstimatorTest {
measurement.getRotation().getDegrees()
- estimator.getEstimatedPosition().getRotation().getDegrees());
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
}
}

3
wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -22,7 +22,6 @@ import edu.wpi.first.math.system.NumericalJacobian;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@@ -114,7 +113,7 @@ class ExtendedKalmanFilterTest {
dtSeconds);
List<Pose2d> waypoints =
Arrays.asList(
List.of(
new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory =

5
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
@@ -267,7 +268,7 @@ class MecanumDrivePoseEstimatorTest {
"Estimator converged to one vision measurement: "
+ estimator.getEstimatedPosition().toString()
+ " -> "
+ measurement.toString();
+ measurement;
var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -276,7 +277,7 @@ class MecanumDrivePoseEstimatorTest {
measurement.getRotation().getDegrees()
- estimator.getEstimatedPosition().getRotation().getDegrees());
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
}
}

5
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
@@ -288,7 +289,7 @@ class SwerveDrivePoseEstimatorTest {
"Estimator converged to one vision measurement: "
+ estimator.getEstimatedPosition().toString()
+ " -> "
+ measurement.toString();
+ measurement;
var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -297,7 +298,7 @@ class SwerveDrivePoseEstimatorTest {
measurement.getRotation().getDegrees()
- estimator.getEstimatedPosition().getRotation().getDegrees());
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
}
}

3
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
View File

@@ -26,7 +26,6 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@@ -137,7 +136,7 @@ class UnscentedKalmanFilterTest {
dtSeconds);
List<Pose2d> waypoints =
Arrays.asList(
List.of(
new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory =

20
wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
View File

@@ -129,9 +129,9 @@ class LinearFilterTest {
1,
3,
// f(x) = sin(x)
(double x) -> Math.sin(x),
Math::sin,
// df/dx = cos(x)
(double x) -> Math.cos(x),
Math::cos,
h,
-20.0,
20.0);
@@ -140,7 +140,7 @@ class LinearFilterTest {
1,
3,
// f(x) = ln(x)
(double x) -> Math.log(x),
Math::log,
// df/dx = 1/x
(double x) -> 1.0 / x,
h,
@@ -162,7 +162,7 @@ class LinearFilterTest {
2,
5,
// f(x) = sin(x)
(double x) -> Math.sin(x),
Math::sin,
// d²f/dx² = -sin(x)
(double x) -> -Math.sin(x),
h,
@@ -173,7 +173,7 @@ class LinearFilterTest {
2,
5,
// f(x) = ln(x)
(double x) -> Math.log(x),
Math::log,
// d²f/dx² = -1/x²
(double x) -> -1.0 / (x * x),
h,
@@ -201,9 +201,9 @@ class LinearFilterTest {
1,
2,
// f(x) = sin(x)
(double x) -> Math.sin(x),
Math::sin,
// df/dx = cos(x)
(double x) -> Math.cos(x),
Math::cos,
h,
-20.0,
20.0);
@@ -212,7 +212,7 @@ class LinearFilterTest {
1,
2,
// f(x) = ln(x)
(double x) -> Math.log(x),
Math::log,
// df/dx = 1/x
(double x) -> 1.0 / x,
h,
@@ -234,7 +234,7 @@ class LinearFilterTest {
2,
4,
// f(x) = sin(x)
(double x) -> Math.sin(x),
Math::sin,
// d²f/dx² = -sin(x)
(double x) -> -Math.sin(x),
h,
@@ -245,7 +245,7 @@ class LinearFilterTest {
2,
4,
// f(x) = ln(x)
(double x) -> Math.log(x),
Math::log,
// d²f/dx² = -1/x²
(double x) -> -1.0 / (x * x),
h,

10
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -11,7 +11,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
class Pose3dTest {
@@ -194,7 +194,7 @@ class Pose3dTest {
@Test
void testComplexTwists() {
var initial_poses =
Arrays.asList(
List.of(
new Pose3d(
new Translation3d(0.698303, -0.959096, 0.271076),
new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
@@ -212,7 +212,7 @@ class Pose3dTest {
new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
var final_poses =
Arrays.asList(
List.of(
new Pose3d(
new Translation3d(-0.230448, -0.511957, 0.198406),
new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
@@ -267,7 +267,7 @@ class Pose3dTest {
@Test
void testTwistNaN() {
var initial_poses =
Arrays.asList(
List.of(
new Pose3d(
new Translation3d(6.32, 4.12, 0.00),
new Rotation3d(
@@ -277,7 +277,7 @@ class Pose3dTest {
new Rotation3d(
new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
var final_poses =
Arrays.asList(
List.of(
new Pose3d(
new Translation3d(6.33, 4.15, 0.00),
new Rotation3d(

4
wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java
View File

@@ -21,7 +21,7 @@ class SimulatedAnnealingTest {
new SimulatedAnnealing<Double>(
2.0,
x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3),
x -> function.applyAsDouble(x));
function::applyAsDouble);
double solution = simulatedAnnealing.solve(-1.0, 5000);
@@ -38,7 +38,7 @@ class SimulatedAnnealingTest {
new SimulatedAnnealing<Double>(
2.0,
x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7),
x -> function.applyAsDouble(x));
function::applyAsDouble);
double solution = simulatedAnnealing.solve(-1.0, 5000);

3
wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
View File

@@ -14,7 +14,6 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@@ -151,7 +150,7 @@ class CubicHermiteSplineTest {
() ->
run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
Arrays.asList(new Translation2d(10, 10.5)),
List.of(new Translation2d(10, 10.5)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
}
}

7
wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
View File

@@ -34,12 +34,7 @@ class NumericalIntegrationTest {
void testZeroRKDP() {
var y1 =
NumericalIntegration.rkdp(
(x, u) -> {
return VecBuilder.fill(0);
},
VecBuilder.fill(0),
VecBuilder.fill(0),
0.1);
(x, u) -> VecBuilder.fill(0), VecBuilder.fill(0), VecBuilder.fill(0), 0.1);
assertEquals(0.0, y1.get(0, 0), 1e-3);
}

7
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
View File

@@ -31,10 +31,9 @@ class RungeKuttaTimeVaryingTest {
final var y1 =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N1, N1> x) -> {
return MatBuilder.fill(
Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
},
(Double t, Matrix<N1, N1> x) ->
MatBuilder.fill(
Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)),
5.0,
y0,
1.0);

5
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -11,7 +11,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
@@ -89,7 +88,7 @@ class DifferentialDriveVoltageConstraintTest {
() ->
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new ArrayList<Translation2d>(),
new ArrayList<>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
config));
@@ -97,7 +96,7 @@ class DifferentialDriveVoltageConstraintTest {
() ->
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new ArrayList<Translation2d>(),
new ArrayList<>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
config.setReversed(true)));
}

5
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
View File

@@ -16,7 +16,6 @@ import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@@ -73,7 +72,7 @@ class TrajectoryGeneratorTest {
void testMalformedTrajectory() {
var traj =
TrajectoryGenerator.generateTrajectory(
Arrays.asList(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))),
new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
@@ -86,7 +85,7 @@ class TrajectoryGeneratorTest {
void testQuinticCurvatureOptimization() {
Trajectory t =
TrajectoryGenerator.generateTrajectory(
Arrays.asList(
List.of(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(270)),