[wpimath] Move Java classes to edu.wpi.first.math (#3316)

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

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+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.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+class ControlAffinePlantInversionFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<N2, N1>(
+            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);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculateState() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<N2, N1>(
+            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);
+  }
+
+  @SuppressWarnings("ParameterName")
+  protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
+    var result = new Matrix<>(Nat.N2(), Nat.N1());
+
+    result =
+        Matrix.mat(Nat.N2(), Nat.N2())
+            .fill(1.000, 0, 0, 1.000)
+            .times(x)
+            .plus(VecBuilder.fill(0, 1).times(u));
+
+    return result;
+  }
+
+  @SuppressWarnings("ParameterName")
+  protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
+    var result = new Matrix<>(Nat.N2(), Nat.N1());
+
+    result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
+
+    return result;
+  }
+}

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

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+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.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+class LinearPlantInversionFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
+        new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
+
+    assertEquals(
+        47.502599,
+        feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0),
+        0.002);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java

@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+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;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+public class LinearQuadraticRegulatorTest {
+  public static LinearSystem<N2, N1, N1> elevatorPlant;
+  static LinearSystem<N2, N1, N1> armPlant;
+
+  static {
+    createArm();
+    createElevator();
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public static void createArm() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 4.0;
+    var r = 0.4;
+    var J = 1d / 3d * m * r * r;
+    var G = 100.0;
+
+    armPlant = LinearSystemId.createSingleJointedArmSystem(motors, J, G);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public static void createElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testLQROnElevator() {
+
+    var qElms = VecBuilder.fill(0.02, 0.4);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(elevatorPlant, qElms, rElms, dt);
+
+    var k = controller.getK();
+
+    assertEquals(522.153, k.get(0, 0), 0.1);
+    assertEquals(38.2, k.get(0, 1), 0.1);
+  }
+
+  @Test
+  public void testFourMotorElevator() {
+
+    var dt = 0.020;
+
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
+
+    var regulator =
+        new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
+
+    assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
+    assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testLQROnArm() {
+
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 4.0;
+    var r = 0.4;
+    var G = 100.0;
+
+    var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
+
+    var qElms = VecBuilder.fill(0.01745, 0.08726);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
+
+    var k = controller.getK();
+
+    assertEquals(19.16, k.get(0, 0), 0.1);
+    assertEquals(3.32, k.get(0, 1), 0.1);
+  }
+
+  @Test
+  public void testLatencyCompensate() {
+    var dt = 0.02;
+
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
+
+    var regulator =
+        new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
+
+    regulator.latencyCompensate(plant, dt, 0.01);
+
+    assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
+    assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java

@@ -0,0 +1,157 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+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.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+public class LinearSystemLoopTest {
+  public static final double kDt = 0.00505;
+  private static final double kPositionStddev = 0.0001;
+  private static final Random random = new Random();
+
+  LinearSystem<N2, N1, N1> m_plant =
+      LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
+
+  KalmanFilter<N2, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N2(), Nat.N1(), m_plant, VecBuilder.fill(0.05, 1.0), VecBuilder.fill(0.0001), kDt);
+
+  LinearQuadraticRegulator<N2, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), 0.00505);
+
+  private final LinearSystemLoop<N2, N1, N1> m_loop =
+      new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
+
+  @SuppressWarnings("LocalVariableName")
+  private static void updateTwoState(
+      LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
+    Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(VecBuilder.fill(noise));
+
+    loop.correct(y);
+    loop.predict(kDt);
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public void testStateSpaceEnabled() {
+
+    m_loop.reset(VecBuilder.fill(0, 0));
+    Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
+    var constraints = new TrapezoidProfile.Constraints(4, 3);
+    m_loop.setNextR(references);
+
+    TrapezoidProfile profile;
+    TrapezoidProfile.State state;
+    for (int i = 0; i < 1000; i++) {
+      profile =
+          new TrapezoidProfile(
+              constraints,
+              new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
+              new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
+      state = profile.calculate(kDt);
+      m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
+
+      updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
+      var u = m_loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+    }
+
+    assertEquals(2.0, m_loop.getXHat(0), 0.05);
+    assertEquals(0.0, m_loop.getXHat(1), 0.5);
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public void testFlywheelEnabled() {
+
+    LinearSystem<N1, N1, N1> plant =
+        LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
+    KalmanFilter<N1, N1, N1> observer =
+        new KalmanFilter<>(
+            Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt);
+
+    var qElms = VecBuilder.fill(9.0);
+    var rElms = VecBuilder.fill(12.0);
+
+    var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, kDt);
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
+
+    var loop = new LinearSystemLoop<>(controller, feedforward, observer, 12);
+
+    loop.reset(VecBuilder.fill(0.0));
+    var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
+
+    loop.setNextR(references);
+
+    List<Double> timeData = new ArrayList<>();
+    List<Double> xHat = new ArrayList<>();
+    List<Double> volt = new ArrayList<>();
+    List<Double> reference = new ArrayList<>();
+    List<Double> error = new ArrayList<>();
+
+    var time = 0.0;
+    while (time < 10) {
+
+      if (time > 10) {
+        break;
+      }
+
+      loop.setNextR(references);
+
+      Matrix<N1, N1> y =
+          plant
+              .calculateY(loop.getXHat(), loop.getU())
+              .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev));
+
+      loop.correct(y);
+      loop.predict(kDt);
+      var u = loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+
+      xHat.add(loop.getXHat(0) / 2d / Math.PI * 60);
+      volt.add(u);
+      timeData.add(time);
+      reference.add(references.get(0, 0) / 2d / Math.PI * 60);
+      error.add(loop.getError(0) / 2d / Math.PI * 60);
+      time += kDt;
+    }
+
+    //    XYChart bigChart = new XYChartBuilder().build();
+    //    bigChart.addSeries("Xhat, RPM", timeData, xHat);
+    //    bigChart.addSeries("Reference, RPM", timeData, reference);
+    //    bigChart.addSeries("Error, RPM", timeData, error);
+
+    //    XYChart smolChart = new XYChartBuilder().build();
+    //    smolChart.addSeries("Volts, V", timeData, volt);
+
+    //  try {
+    //      new SwingWrapper<>(List.of(bigChart, smolChart)).displayChartMatrix();
+    //      Thread.sleep(10000000);
+    //    } catch (InterruptedException e) { e.printStackTrace(); }
+
+    assertEquals(0.0, loop.getError(0), 0.1);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class RamseteControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    final var controller = new RamseteController(2.0, 0.7);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final double kDt = 0.02;
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+
+      var output = controller.calculate(robotPose, state);
+      robotPose =
+          robotPose.exp(
+              new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}