diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java index a4bdd1fe12..954cb8635d 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java @@ -72,7 +72,7 @@ public class AprilTagDetection { * @return Homography matrix */ public Matrix getHomographyMatrix() { - return new MatBuilder<>(Nat.N3(), Nat.N3()).fill(m_homography); + return MatBuilder.fill(Nat.N3(), Nat.N3(), m_homography); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java index 2ee010f194..80c9d0dc9d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java @@ -14,8 +14,47 @@ import org.ejml.simple.SimpleMatrix; * @param The number of columns of the desired matrix. */ public class MatBuilder { - final Nat m_rows; - final Nat m_cols; + /** + * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by + * row, left to right with the given data). + * + * @param rows The number of rows of the matrix. + * @param cols The number of columns of the matrix. + * @param data The data to fill the matrix with. + * @param The number of rows of the matrix. + * @param The number of columns of the matrix. + * @return The constructed matrix. + */ + public static final Matrix fill( + Nat rows, Nat cols, double... data) { + if (Objects.requireNonNull(data).length != rows.getNum() * cols.getNum()) { + throw new IllegalArgumentException( + "Invalid matrix data provided. Wanted " + + rows.getNum() + + " x " + + cols.getNum() + + " matrix, but got " + + data.length + + " elements"); + } + return new Matrix<>(new SimpleMatrix(rows.getNum(), cols.getNum(), true, data)); + } + + protected final Nat m_rows; + protected final Nat m_cols; + + /** + * Creates a new {@link MatBuilder} with the given dimensions. + * + * @param rows The number of rows of the matrix. + * @param cols The number of columns of the matrix. + * @deprecated Use {@link MatBuilder#fill} instead. + */ + @Deprecated(since = "2024", forRemoval = true) + public MatBuilder(Nat rows, Nat cols) { + this.m_rows = Objects.requireNonNull(rows); + this.m_cols = Objects.requireNonNull(cols); + } /** * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by @@ -25,28 +64,6 @@ public class MatBuilder { * @return The constructed matrix. */ public final Matrix fill(double... data) { - if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) { - throw new IllegalArgumentException( - "Invalid matrix data provided. Wanted " - + this.m_rows.getNum() - + " x " - + this.m_cols.getNum() - + " matrix, but got " - + data.length - + " elements"); - } else { - return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data)); - } - } - - /** - * Creates a new {@link MatBuilder} with the given dimensions. - * - * @param rows The number of rows of the matrix. - * @param cols The number of columns of the matrix. - */ - public MatBuilder(Nat rows, Nat cols) { - this.m_rows = Objects.requireNonNull(rows); - this.m_cols = Objects.requireNonNull(cols); + return fill(m_rows, m_cols, data); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java index b7a86fa7d0..8ac8d137e0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java @@ -654,7 +654,10 @@ public class Matrix { * @param The number of rows of the desired matrix as a generic. * @param The number of columns of the desired matrix as a generic. * @return A builder to construct the matrix. + * @deprecated Use {@link MatBuilder#fill} instead. */ + @Deprecated(since = "2024", forRemoval = true) + @SuppressWarnings("removal") public static MatBuilder mat(Nat rows, Nat cols) { return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols)); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java index 9ba91b0ea5..0e47ae3533 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java @@ -17,26 +17,22 @@ import edu.wpi.first.math.numbers.N9; import java.util.Objects; import org.ejml.simple.SimpleMatrix; -/** - * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices). - * - * @param The dimension of the vector to be constructed. - */ -public class VecBuilder extends MatBuilder { - public VecBuilder(Nat rows) { - super(rows, Nat.N1()); +/** A class for constructing vectors (Nx1 matrices). */ +public final class VecBuilder { + private VecBuilder() { + throw new UnsupportedOperationException("this is a utility class!"); } - private Vector fillVec(double... data) { - if (Objects.requireNonNull(data).length != this.m_rows.getNum()) { + private static Vector fillVec(Nat rows, double... data) { + if (Objects.requireNonNull(data).length != rows.getNum()) { throw new IllegalArgumentException( "Invalid vector data provided. Wanted " - + this.m_rows.getNum() + + rows.getNum() + " vector, but got " + data.length + " elements"); } - return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data)); + return new Vector<>(new SimpleMatrix(data)); } /** @@ -46,7 +42,7 @@ public class VecBuilder extends MatBuilder { * @return 1x1 vector */ public static Vector fill(double n1) { - return new VecBuilder<>(Nat.N1()).fillVec(n1); + return fillVec(Nat.N1(), n1); } /** @@ -57,7 +53,7 @@ public class VecBuilder extends MatBuilder { * @return 2x1 vector */ public static Vector fill(double n1, double n2) { - return new VecBuilder<>(Nat.N2()).fillVec(n1, n2); + return fillVec(Nat.N2(), n1, n2); } /** @@ -69,7 +65,7 @@ public class VecBuilder extends MatBuilder { * @return 3x1 vector */ public static Vector fill(double n1, double n2, double n3) { - return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3); + return fillVec(Nat.N3(), n1, n2, n3); } /** @@ -82,7 +78,7 @@ public class VecBuilder extends MatBuilder { * @return 4x1 vector */ public static Vector fill(double n1, double n2, double n3, double n4) { - return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4); + return fillVec(Nat.N4(), n1, n2, n3, n4); } /** @@ -96,7 +92,7 @@ public class VecBuilder extends MatBuilder { * @return 5x1 vector */ public static Vector fill(double n1, double n2, double n3, double n4, double n5) { - return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5); + return fillVec(Nat.N5(), n1, n2, n3, n4, n5); } /** @@ -111,7 +107,7 @@ public class VecBuilder extends MatBuilder { * @return 6x1 vector */ public static Vector fill(double n1, double n2, double n3, double n4, double n5, double n6) { - return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6); + return fillVec(Nat.N6(), n1, n2, n3, n4, n5, n6); } /** @@ -128,7 +124,7 @@ public class VecBuilder extends MatBuilder { */ public static Vector fill( double n1, double n2, double n3, double n4, double n5, double n6, double n7) { - return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7); + return fillVec(Nat.N7(), n1, n2, n3, n4, n5, n6, n7); } /** @@ -146,7 +142,7 @@ public class VecBuilder extends MatBuilder { */ public static Vector fill( double n1, double n2, double n3, double n4, double n5, double n6, double n7, double n8) { - return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8); + return fillVec(Nat.N8(), n1, n2, n3, n4, n5, n6, n7, n8); } /** @@ -173,7 +169,7 @@ public class VecBuilder extends MatBuilder { double n7, double n8, double n9) { - return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9); + return fillVec(Nat.N9(), n1, n2, n3, n4, n5, n6, n7, n8, n9); } /** @@ -202,6 +198,6 @@ public class VecBuilder extends MatBuilder { double n8, double n9, double n10) { - return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10); + return fillVec(Nat.N10(), n1, n2, n3, n4, n5, n6, n7, n8, n9, n10); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java index 8c47765fd9..d2ef273bbe 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java @@ -5,7 +5,10 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; @@ -80,10 +83,10 @@ public class DifferentialDriveAccelerationLimiter { */ public DifferentialDriveWheelVoltages calculate( double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) { - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage); + Matrix u = VecBuilder.fill(leftVoltage, rightVoltage); // Find unconstrained wheel accelerations - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity); + var x = VecBuilder.fill(leftVelocity, rightVelocity); var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u)); // Convert from wheel accelerations to linear and angular accelerations @@ -98,9 +101,7 @@ public class DifferentialDriveAccelerationLimiter { // [α] [-1/trackwidth 1/trackwidth][dxdt(1)] // // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth] - var M = - new MatBuilder<>(Nat.N2(), Nat.N2()) - .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth); + var M = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth); var accels = M.times(dxdt); // Constrain the linear and angular accelerations diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index cfe7c2b892..476a1c55ad 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -4,7 +4,7 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -93,8 +93,8 @@ public class ElevatorFeedforward { var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); - var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); - var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity); + var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity); return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 2be2a478ec..fa287831bc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.numbers.N1; @@ -88,46 +89,48 @@ public class LTVDifferentialDriveController { // Control law derivation is in section 8.7 of // https://file.tavsys.net/control/controls-engineering-in-frc.pdf var A = - new MatBuilder<>(Nat.N5(), Nat.N5()) - .fill( - 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)); + 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 = - new MatBuilder<>(Nat.N5(), Nat.N2()) - .fill( - 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)); + 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); @@ -136,11 +139,7 @@ public class LTVDifferentialDriveController { // Ax = -Bu // x = -A⁻¹Bu double maxV = - plant - .getA() - .solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))) - .times(-1.0) - .get(0, 0); + 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( @@ -201,13 +200,12 @@ public class LTVDifferentialDriveController { public void setTolerance( Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) { m_tolerance = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - poseTolerance.getX(), - poseTolerance.getY(), - poseTolerance.getRotation().getRadians(), - leftVelocityTolerance, - rightVelocityTolerance); + VecBuilder.fill( + poseTolerance.getX(), + poseTolerance.getY(), + poseTolerance.getRotation().getRadians(), + leftVelocityTolerance, + rightVelocityTolerance); } /** @@ -234,13 +232,12 @@ public class LTVDifferentialDriveController { // This implements the linear time-varying differential drive controller in // theorem 9.6.3 of https://tavsys.net/controls-in-frc. var x = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - currentPose.getX(), - currentPose.getY(), - currentPose.getRotation().getRadians(), - leftVelocity, - rightVelocity); + VecBuilder.fill( + currentPose.getX(), + currentPose.getY(), + currentPose.getRotation().getRadians(), + leftVelocity, + rightVelocity); var inRobotFrame = Matrix.eye(Nat.N5()); inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0))); @@ -249,13 +246,12 @@ public class LTVDifferentialDriveController { inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0))); var r = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - poseRef.getX(), - poseRef.getY(), - poseRef.getRotation().getRadians(), - leftVelocityRef, - rightVelocityRef); + VecBuilder.fill( + poseRef.getX(), + poseRef.getY(), + 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))); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index 7e3391f733..c4e7358c9f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -149,7 +149,7 @@ public class LTVUnicycleController { // A = [0 0 v] B = [0 0] // [0 0 0] [0 1] var A = new Matrix<>(Nat.N3(), Nat.N3()); - var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0); + 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); @@ -226,8 +226,12 @@ public class LTVUnicycleController { var K = m_table.get(linearVelocityRef); var e = - new MatBuilder<>(Nat.N3(), Nat.N1()) - .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians()); + MatBuilder.fill( + Nat.N3(), + Nat.N1(), + m_poseError.getX(), + m_poseError.getY(), + m_poseError.getRotation().getRadians()); var u = K.times(e); return new ChassisSpeeds( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 714d70527a..63ceb8fa8f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -4,7 +4,7 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -62,8 +62,8 @@ public class SimpleMotorFeedforward { var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); - var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); - var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity); + var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity); return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index b434523687..6b7ab6efac 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -8,7 +8,6 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; @@ -201,9 +200,7 @@ public class Rotation3d implements Interpolatable { // so a 180 degree rotation is required. Any orthogonal vector can be used // for it. Q in the QR decomposition is an orthonormal basis, so it // contains orthogonal unit vectors. - var X = - new MatBuilder<>(Nat.N3(), Nat.N1()) - .fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); + var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); final var qr = DecompositionFactory_DDRM.qr(3, 1); qr.decompose(X.getStorage().getMatrix()); final var Q = qr.getQ(null, false); diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index 4665aa976e..3ab305872d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.system.plant; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -40,20 +41,17 @@ public final class LinearSystemId { } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()) - .fill( - 0, - 1, - 0, - -Math.pow(gearing, 2) - * motor.KtNMPerAmp - / (motor.rOhms - * radiusMeters - * radiusMeters - * massKg - * motor.KvRadPerSecPerVolt)), + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -Math.pow(gearing, 2) + * motor.KtNMPerAmp + / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)), VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), - Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), + MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -108,15 +106,16 @@ public final class LinearSystemId { } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()) - .fill( - 0, - 1, - 0, - -gearing - * gearing - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -gearing + * gearing + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); @@ -149,7 +148,7 @@ public final class LinearSystemId { } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA), + MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA), VecBuilder.fill(0, 1 / kA), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); @@ -201,10 +200,10 @@ public final class LinearSystemId { final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; - var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1); - var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2); - var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0); - var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0); + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); + var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); return new LinearSystem<>(A, B, C, D); } @@ -229,16 +228,17 @@ public final class LinearSystemId { } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()) - .fill( - 0, - 1, - 0, - -Math.pow(gearing, 2) - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -Math.pow(gearing, 2) + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), - Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), + MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -302,9 +302,9 @@ public final class LinearSystemId { } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA), + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), VecBuilder.fill(0.0, 1.0 / kA), - Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0), + MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0), VecBuilder.fill(0.0)); } @@ -347,10 +347,10 @@ public final class LinearSystemId { final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular); return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1), - Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1), - Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1), - Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0)); + MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1), + MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), + MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0)); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java b/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java index 8ee11e5e22..d4bc0356b9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java @@ -18,29 +18,28 @@ import org.junit.jupiter.api.Test; class MatrixTest { @Test void testMatrixMultiplication() { - var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 0.0, 1.0); - var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 0.0, 0.0, 2.5); + var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 0.0, 1.0); + var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 0.0, 0.0, 2.5); Matrix result = mat1.times(mat2); - assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5)); + assertEquals(result, MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 2.5, 0.0, 2.5)); - var mat3 = Matrix.mat(Nat.N2(), Nat.N3()).fill(1.0, 3.0, 0.5, 2.0, 4.3, 1.2); + var mat3 = MatBuilder.fill(Nat.N2(), Nat.N3(), 1.0, 3.0, 0.5, 2.0, 4.3, 1.2); var mat4 = - Matrix.mat(Nat.N3(), Nat.N4()) - .fill(3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0); + MatBuilder.fill( + Nat.N3(), Nat.N4(), 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0); Matrix result2 = mat3.times(mat4); assertTrue( - Matrix.mat(Nat.N2(), Nat.N4()) - .fill(12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53) + MatBuilder.fill(Nat.N2(), Nat.N4(), 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53) .isEqual(result2, 1E-9)); } @Test void testMatrixVectorMultiplication() { - var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 1.0, 0.0, 1.0); + var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 1.0, 0.0, 1.0); var vec = VecBuilder.fill(3.0, 2.0); @@ -54,19 +53,19 @@ class MatrixTest { Matrix transpose = vec.transpose(); - assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose); + assertEquals(MatBuilder.fill(Nat.N1(), Nat.N3(), 1.0, 2.0, 3.0), transpose); } @Test void testSolve() { - var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0); var vec1 = VecBuilder.fill(1.0, 2.0); var solve1 = mat1.solve(vec1); assertEquals(VecBuilder.fill(0.0, 0.5), solve1); - var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + var mat2 = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); var vec2 = VecBuilder.fill(1.0, 2.0, 3.0); var solve2 = mat2.solve(vec2); @@ -76,7 +75,7 @@ class MatrixTest { @Test void testInverse() { - var mat = Matrix.mat(Nat.N3(), Nat.N3()).fill(1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5); + var mat = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5); var inv = mat.inv(); @@ -87,33 +86,33 @@ class MatrixTest { @Test void testUninvertableMatrix() { - var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 2.0, 1.0); + var singularMatrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 2.0, 1.0); assertThrows(SingularMatrixException.class, singularMatrix::inv); } @Test void testMatrixScalarArithmetic() { - var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 4.0, 5.0, 6.0), mat.plus(2.0)); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 2.0, 3.0), mat.minus(1.0)); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 4.0, 6.0, 8.0), mat.times(2.0)); - assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3)); + assertTrue(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3)); } @Test void testMatrixMatrixArithmetic() { - var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0); - var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(5.0, 6.0, 7.0, 8.0); + var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 5.0, 6.0, 7.0, 8.0); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0), mat1.minus(mat2)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), -4.0, -4.0, -4.0, -4.0), mat1.minus(mat2)); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 8.0, 10.0, 12.0), mat1.plus(mat2)); } @Test @@ -121,14 +120,14 @@ class MatrixTest { var matrix = Matrix.eye(Nat.N2()); var result = matrix.exp(); - assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9)); + assertTrue(result.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9)); - matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4); + matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4); result = matrix.times(0.01).exp(); assertTrue( result.isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993), 1E-8)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java index ee33f01526..74ff401269 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java @@ -61,48 +61,48 @@ class StateSpaceUtilTest extends UtilityClassTest { // First eigenvalue is uncontrollable and unstable. // Second eigenvalue is controllable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5); assertFalse(StateSpaceUtil.isStabilizable(A, B)); // First eigenvalue is uncontrollable and marginally stable. // Second eigenvalue is controllable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5); assertFalse(StateSpaceUtil.isStabilizable(A, B)); // First eigenvalue is uncontrollable and stable. // Second eigenvalue is controllable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5); assertTrue(StateSpaceUtil.isStabilizable(A, B)); // First eigenvalue is uncontrollable and stable. // Second eigenvalue is controllable and unstable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2); assertTrue(StateSpaceUtil.isStabilizable(A, B)); } @Test void testIsDetectable() { Matrix A; - Matrix C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1); + Matrix C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1); // First eigenvalue is unobservable and unstable. // Second eigenvalue is observable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5); assertFalse(StateSpaceUtil.isDetectable(A, C)); // First eigenvalue is unobservable and marginally stable. // Second eigenvalue is observable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5); assertFalse(StateSpaceUtil.isDetectable(A, C)); // First eigenvalue is unobservable and stable. // Second eigenvalue is observable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5); assertTrue(StateSpaceUtil.isDetectable(A, C)); // First eigenvalue is unobservable and stable. // Second eigenvalue is observable and unstable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2); assertTrue(StateSpaceUtil.isDetectable(A, C)); } @@ -143,14 +143,14 @@ class StateSpaceUtilTest extends UtilityClassTest { var wrappedResult = wrappedMatrix.exp(); assertTrue( - wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9)); + wrappedResult.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9)); - var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4); + var matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4); wrappedResult = matrix.times(0.01).exp(); assertTrue( wrappedResult.isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993), 1E-8)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java index fcd9de1416..60300fb838 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java @@ -6,6 +6,7 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -35,13 +36,12 @@ class ControlAffinePlantInversionFeedforwardTest { } protected Matrix getDynamics(Matrix x, Matrix u) { - return Matrix.mat(Nat.N2(), Nat.N2()) - .fill(1.000, 0, 0, 1.000) + return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000) .times(x) .plus(VecBuilder.fill(0, 1).times(u)); } protected Matrix getStateDynamics(Matrix x) { - return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x); + return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000).times(x); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java index faf55634ad..4ec558d4e7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java @@ -9,8 +9,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; @@ -26,52 +28,38 @@ class DifferentialDriveAccelerationLimiterTest { var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xAccelLimiter = VecBuilder.fill(0.0, 0.0); // Ensure voltage exceeds acceleration before limiting { final var accels = - plant - .getA() - .times(xAccelLimiter) - .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); + plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(Math.abs(a) > maxA); } { final var accels = - plant - .getA() - .times(xAccelLimiter) - .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0))); + plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(-12.0, 12.0))); final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(alpha) > maxAlpha); } // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(a) <= maxA); @@ -79,28 +67,20 @@ class DifferentialDriveAccelerationLimiterTest { } // Backward - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(a) <= maxA); @@ -108,28 +88,20 @@ class DifferentialDriveAccelerationLimiterTest { } // Rotate CCW - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(a) <= maxA); @@ -148,21 +120,18 @@ class DifferentialDriveAccelerationLimiterTest { // unconstrained systems should match var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xAccelLimiter = VecBuilder.fill(0.0, 0.0); // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); @@ -171,17 +140,14 @@ class DifferentialDriveAccelerationLimiterTest { // Backward x.fill(0.0); xAccelLimiter.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); @@ -190,17 +156,14 @@ class DifferentialDriveAccelerationLimiterTest { // Rotate CCW x.fill(0.0); xAccelLimiter.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); @@ -220,16 +183,13 @@ class DifferentialDriveAccelerationLimiterTest { var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xAccelLimiter = VecBuilder.fill(0.0, 0.0); // Ensure voltage exceeds acceleration before limiting { final var accels = - plant - .getA() - .times(xAccelLimiter) - .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); + plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(Math.abs(a) > maxA); assertTrue(Math.abs(a) > -minA); @@ -237,55 +197,39 @@ class DifferentialDriveAccelerationLimiterTest { // a should always be within [minA, maxA] // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(minA <= a && a <= maxA); } // Backward - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(minA <= a && a <= maxA); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index 6c978ac986..1499302950 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -6,7 +6,7 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -27,8 +27,8 @@ class ElevatorFeedforwardTest { assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002); - var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka); - var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka); + var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka); + var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka); final double dt = 0.02; var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java index 674cba3dc3..cfcc71d1ee 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java @@ -7,8 +7,9 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; @@ -22,11 +23,11 @@ class ImplicitModelFollowerTest { var imf = new ImplicitModelFollower(plant, plant); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xImf = VecBuilder.fill(0.0, 0.0); // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -36,7 +37,7 @@ class ImplicitModelFollowerTest { } // Backward - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -46,7 +47,7 @@ class ImplicitModelFollowerTest { } // Rotate CCW - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -67,11 +68,11 @@ class ImplicitModelFollowerTest { var imf = new ImplicitModelFollower(plant, plantRef); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xImf = VecBuilder.fill(0.0, 0.0); // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -83,7 +84,7 @@ class ImplicitModelFollowerTest { // Backward x.fill(0.0); xImf.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -95,7 +96,7 @@ class ImplicitModelFollowerTest { // Rotate CCW x.fill(0.0); xImf.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java index efb9c5ead8..e977de8792 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java @@ -87,9 +87,14 @@ class LTVDifferentialDriveControllerTest { final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); var x = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0); + MatBuilder.fill( + Nat.N5(), + Nat.N1(), + robotPose.getX(), + robotPose.getY(), + robotPose.getRotation().getRadians(), + 0.0, + 0.0); final var totalTime = trajectory.getTotalTimeSeconds(); for (int i = 0; i < (totalTime / kDt); ++i) { @@ -105,7 +110,7 @@ class LTVDifferentialDriveControllerTest { NumericalIntegration.rkdp( LTVDifferentialDriveControllerTest::dynamics, x, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right), + VecBuilder.fill(output.left, output.right), kDt); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java index 1aba6d7927..eb8d36a503 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java @@ -6,6 +6,7 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -16,7 +17,7 @@ import org.junit.jupiter.api.Test; class LinearPlantInversionFeedforwardTest { @Test void testCalculate() { - Matrix A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + Matrix A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); Matrix B = VecBuilder.fill(0, 1); LinearPlantInversionFeedforward feedforward = diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index 634ed1cda7..e915388cd3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -6,6 +6,7 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -111,10 +112,10 @@ class LinearQuadraticRegulatorTest { @Test void testMatrixOverloadsWithSingleIntegrator() { - var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0); - var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); - var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); - var R = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0); + var B = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); + var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); + var R = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); // QR overload var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK(); @@ -124,7 +125,7 @@ class LinearQuadraticRegulatorTest { assertEquals(0.99750312499512261, K.get(1, 1), 1e-10); // QRN overload - var N = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var N = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); var Kimf = new LinearQuadraticRegulator<>(A, B, Q, R, N, 0.005).getK(); assertEquals(1.0, Kimf.get(0, 0), 1e-10); assertEquals(0.0, Kimf.get(0, 1), 1e-10); @@ -137,10 +138,10 @@ class LinearQuadraticRegulatorTest { double Kv = 3.02; double Ka = 0.642; - var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / Ka); - var B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1.0 / Ka); - var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.2); - var R = Matrix.mat(Nat.N1(), Nat.N1()).fill(0.25); + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / Ka); + var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1.0 / Ka); + var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.2); + var R = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.25); // QR overload var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK(); @@ -148,7 +149,7 @@ class LinearQuadraticRegulatorTest { assertEquals(0.51182128351092726, K.get(0, 1), 1e-10); // QRN overload - var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0)); + var Aref = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / (Ka * 5.0)); var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005); assertEquals(0.0, Kimf.get(0, 0), 1e-10); assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index fd4e428877..e690dafb2c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -6,7 +6,7 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -20,8 +20,8 @@ class SimpleMotorFeedforwardTest { double Ka = 0.6; double dt = 0.02; - var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka); - var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka); + var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -Kv / Ka); + var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka); var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java index c36e72f663..eb30e494cc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java @@ -6,6 +6,7 @@ package edu.wpi.first.math.estimator; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -16,7 +17,7 @@ class AngleStatisticsTest { void testMean() { // 3 states, 2 sigmas var sigmas = - Matrix.mat(Nat.N3(), Nat.N2()).fill(1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2); + MatBuilder.fill(Nat.N3(), Nat.N2(), 1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2); // Weights need to produce the mean of the sigmas var weights = new Matrix<>(Nat.N2(), Nat.N1()); weights.fill(1.0 / sigmas.getNumCols()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index 848fe9392a..f5f0925568 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -7,6 +7,7 @@ package edu.wpi.first.math.estimator; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -47,16 +48,16 @@ class KalmanFilterTest { // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ LinearSystem m_swerveObserverSystem = new LinearSystem<>( - Matrix.mat(Nat.N6(), Nat.N6()) - .fill( // A - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0), - Matrix.mat(Nat.N6(), Nat.N3()) - .fill( // B - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1), - Matrix.mat(Nat.N3(), Nat.N6()) - .fill( // C - 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0), + MatBuilder.fill( + Nat.N6(), Nat.N6(), // A + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0), + MatBuilder.fill( + Nat.N6(), Nat.N3(), // B + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1), + MatBuilder.fill( + Nat.N3(), Nat.N6(), // C + 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0), new Matrix<>(Nat.N3(), Nat.N3())); // D @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java index 2131c35d5d..468eb51360 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java @@ -6,7 +6,7 @@ package edu.wpi.first.math.estimator; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import org.junit.jupiter.api.Test; @@ -17,13 +17,23 @@ class MerweScaledSigmaPointsTest { var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2()); var points = merweScaledSigmaPoints.squareRootSigmaPoints( - VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1)); + VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1)); assertTrue( points.isEqual( - Matrix.mat(Nat.N2(), Nat.N5()) - .fill( - 0.0, 0.00173205, 0.0, -0.00173205, 0.0, 0.0, 0.0, 0.00173205, 0.0, -0.00173205), + MatBuilder.fill( + Nat.N2(), + Nat.N5(), + 0.0, + 0.00173205, + 0.0, + -0.00173205, + 0.0, + 0.0, + 0.0, + 0.00173205, + 0.0, + -0.00173205), 1E-6)); } @@ -32,12 +42,23 @@ class MerweScaledSigmaPointsTest { var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2()); var points = merweScaledSigmaPoints.squareRootSigmaPoints( - VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10))); + VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10))); assertTrue( points.isEqual( - Matrix.mat(Nat.N2(), Nat.N5()) - .fill(1.0, 1.00173205, 1.0, 0.99826795, 1.0, 2.0, 2.0, 2.00547723, 2.0, 1.99452277), + MatBuilder.fill( + Nat.N2(), + Nat.N5(), + 1.0, + 1.00173205, + 1.0, + 0.99826795, + 1.0, + 2.0, + 2.0, + 2.00547723, + 2.0, + 1.99452277), 1E-6)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index a8e4d3b25e..7faa82a3ea 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -264,7 +265,7 @@ class UnscentedKalmanFilterTest { VecBuilder.fill(0.0, 0.0), dtSeconds); - var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0); + var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0); observer.setP(P); assertTrue(observer.getP().isEqual(P, 1e-9)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java index d80344d93f..008a65fabf 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -93,8 +93,7 @@ class Rotation3dTest { assertEquals(expected2, rot2); // Matrix that isn't orthogonal - final var R3 = - new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0); + final var R3 = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0); assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3)); // Matrix that's orthogonal but not special orthogonal diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java index 7f62933102..830135d2b3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java @@ -19,7 +19,7 @@ class DiscretizationTest { // analytically, @Test void testDiscretizeA() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0); final var x0 = VecBuilder.fill(1, 1); final var discA = Discretization.discretizeA(contA, 1.0); @@ -37,8 +37,8 @@ class DiscretizationTest { // analytically, @Test void testDiscretizeAB() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); - final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0); + final var contB = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1); final var x0 = VecBuilder.fill(1, 1); final var u = VecBuilder.fill(1); @@ -63,8 +63,8 @@ class DiscretizationTest { // 0 @Test void testDiscretizeSlowModelAQ() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); - final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0); + final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); final double dt = 1.0; @@ -95,8 +95,8 @@ class DiscretizationTest { // 0 @Test void testDiscretizeFastModelAQ() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29); - final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -1406.29); + final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0025, 0, 0, 1); final var dt = 0.005; @@ -125,8 +125,8 @@ class DiscretizationTest { // Test that DiscretizeR() works @Test void testDiscretizeR() { - var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0); - var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0); + var contR = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 0.0, 0.0, 1.0); + var discRTruth = MatBuilder.fill(Nat.N2(), Nat.N2(), 4.0, 0.0, 0.0, 2.0); var discR = Discretization.discretizeR(contR, 0.5); diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index 3f316b3ba5..0c2ca235f5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -7,6 +7,7 @@ package edu.wpi.first.math.system; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -23,31 +24,31 @@ class LinearSystemIDTest { model .getA() .isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), + MatBuilder.fill(Nat.N2(), Nat.N2(), -10.14132, 3.06598, 3.06598, -10.14132), 0.001)); assertTrue( model .getB() .isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001)); + MatBuilder.fill(Nat.N2(), Nat.N2(), 4.2590, -1.28762, -1.2876, 4.2590), 0.001)); assertTrue( - model.getC().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001)); + model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), 0.001)); assertTrue( - model.getD().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001)); + model.getD().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0), 0.001)); } @Test void testElevatorSystem() { var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12); assertTrue( - model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -99.05473), 0.001)); + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001)); assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001)); - assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), 0.001)); + assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), 0.001)); assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); } @@ -68,7 +69,7 @@ class LinearSystemIDTest { void testDCMotorSystem() { var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0); assertTrue( - model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001)); + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001)); assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001)); @@ -85,7 +86,7 @@ class LinearSystemIDTest { var ka = 0.5; var model = LinearSystemId.identifyPositionSystem(kv, ka); - assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka)); + assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka)); assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java index 3db878e8e3..e0757eafe8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java @@ -14,8 +14,8 @@ import org.junit.jupiter.api.Test; class RungeKuttaTimeVaryingTest { private static Matrix rungeKuttaTimeVaryingSolution(double t) { - return new MatBuilder<>(Nat.N1(), Nat.N1()) - .fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0)); + return MatBuilder.fill( + Nat.N1(), Nat.N1(), 12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0)); } // Tests RK4 with a time varying solution. From @@ -32,8 +32,8 @@ class RungeKuttaTimeVaryingTest { final var y1 = RungeKuttaTimeVarying.rungeKuttaTimeVarying( (Double t, Matrix x) -> { - return new MatBuilder<>(Nat.N1(), Nat.N1()) - .fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)); + return MatBuilder.fill( + Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)); }, 5.0, y0,