[wpilib] Allow disabling ElevatorSim gravity (#4145)

Closes #4144.

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

@@ -30,6 +30,9 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
   // The max allowable height for the elevator.
   private final double m_maxHeight;
 
+  // Whether the simulator should simulate gravity.
+  private final boolean m_simulateGravity;
+
   /**
    * Creates a simulated elevator mechanism.
    *
@@ -39,6 +42,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
    * @param minHeightMeters The min allowable height of the elevator.
    * @param maxHeightMeters The max allowable height of the elevator.
+   * @param simulateGravity Whether gravity should be simulated or not.
    */
   public ElevatorSim(
       LinearSystem<N2, N1, N1> plant,
@@ -46,8 +50,17 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double gearing,
       double drumRadiusMeters,
       double minHeightMeters,
-      double maxHeightMeters) {
-    this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
+      double maxHeightMeters,
+      boolean simulateGravity) {
+    this(
+        plant,
+        gearbox,
+        gearing,
+        drumRadiusMeters,
+        minHeightMeters,
+        maxHeightMeters,
+        simulateGravity,
+        null);
   }
 
   /**
@@ -59,6 +72,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
    * @param minHeightMeters The min allowable height of the elevator.
    * @param maxHeightMeters The max allowable height of the elevator.
+   * @param simulateGravity Whether gravity should be simulated or not.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
   public ElevatorSim(
@@ -68,6 +82,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double drumRadiusMeters,
       double minHeightMeters,
       double maxHeightMeters,
+      boolean simulateGravity,
       Matrix<N1, N1> measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
@@ -75,6 +90,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
     m_drumRadius = drumRadiusMeters;
     m_minHeight = minHeightMeters;
     m_maxHeight = maxHeightMeters;
+    m_simulateGravity = simulateGravity;
   }
 
   /**
@@ -86,6 +102,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
    * @param minHeightMeters The min allowable height of the elevator.
    * @param maxHeightMeters The max allowable height of the elevator.
+   * @param simulateGravity Whether gravity should be simulated or not.
    */
   public ElevatorSim(
       DCMotor gearbox,
@@ -93,9 +110,17 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double carriageMassKg,
       double drumRadiusMeters,
       double minHeightMeters,
-      double maxHeightMeters) {
+      double maxHeightMeters,
+      boolean simulateGravity) {
     this(
-        gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
+        gearbox,
+        gearing,
+        carriageMassKg,
+        drumRadiusMeters,
+        minHeightMeters,
+        maxHeightMeters,
+        simulateGravity,
+        null);
   }
 
   /**
@@ -107,6 +132,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
    * @param minHeightMeters The min allowable height of the elevator.
    * @param maxHeightMeters The max allowable height of the elevator.
+   * @param simulateGravity Whether gravity should be simulated or not.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
   public ElevatorSim(
@@ -116,6 +142,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double drumRadiusMeters,
       double minHeightMeters,
       double maxHeightMeters,
+      boolean simulateGravity,
       Matrix<N1, N1> measurementStdDevs) {
     super(
         LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
@@ -125,6 +152,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
     m_drumRadius = drumRadiusMeters;
     m_minHeight = minHeightMeters;
     m_maxHeight = maxHeightMeters;
+    m_simulateGravity = simulateGravity;
   }
 
   /**
@@ -222,10 +250,13 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
     // Calculate updated x-hat from Runge-Kutta.
     var updatedXhat =
         NumericalIntegration.rkdp(
-            (x, u_) ->
-                (m_plant.getA().times(x))
-                    .plus(m_plant.getB().times(u_))
-                    .plus(VecBuilder.fill(0, -9.8)),
+            (x, u_) -> {
+              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
+              if (m_simulateGravity) {
+                xdot = xdot.plus(VecBuilder.fill(0, -9.8));
+              }
+              return xdot;
+            },
             currentXhat,
             u,
             dtSeconds);

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -33,6 +33,7 @@ class ElevatorSimTest {
             0.75 * 25.4 / 1000.0,
             0.0,
             3.0,
+            true,
             VecBuilder.fill(0.01));
 
     try (var motor = new PWMVictorSPX(0);
@@ -62,18 +63,15 @@ class ElevatorSimTest {
 
   @Test
   void testMinMax() {
-    var plant =
-        LinearSystemId.createElevatorSystem(
-            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
-
     var sim =
         new ElevatorSim(
-            plant,
             DCMotor.getVex775Pro(4),
             14.67,
+            8.0,
             0.75 * 25.4 / 1000.0,
             0.0,
             1.0,
+            true,
             VecBuilder.fill(0.01));
 
     for (int i = 0; i < 100; i++) {
@@ -93,17 +91,21 @@ class ElevatorSimTest {
 
   @Test
   void testStability() {
-    var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10);
+    var sim =
+        new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, true);
 
     sim.setState(VecBuilder.fill(0, 0));
     sim.setInput(12);
-    for (int i = 0; i < 50; i++) {
+    for (int i = 0; i < 50; ++i) {
       sim.update(0.02);
     }
 
+    var system =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100);
     assertEquals(
-        sim.m_plant.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
+        system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
         sim.getPositionMeters(),
-        0.1);
+        0.01);
   }
 }