[wpilib] Add physics simulation support with state-space (#2615)

This includes physics simulation support for arms/elevator models, as well as differential drivetrains.

Swerve might be added at a later date.

Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class ArmSimTest {
+  SingleJointedArmSim m_sim = new SingleJointedArmSim(
+      DCMotor.getVex775Pro(2),
+      100,
+      10 / 2.2,
+      Units.inchesToMeters(19.0 / 2.0),
+      -Math.PI,
+      0.0, 0.0);
+
+  @Test
+  public void testArmDisabled() {
+    m_sim.resetState(VecBuilder.fill(0.0, 0.0));
+
+    for (int i = 0; i < 12 / 0.02; i++) {
+      m_sim.setInput(0.0);
+      m_sim.update(0.020);
+    }
+
+    // the arm should swing down
+    assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
+  }
+}

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

@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.Vector;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N7;
+
+import org.junit.jupiter.api.Test;
+
+import java.util.List;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDrivetrainSimTest {
+  @Test
+  public void testConvergence() {
+    var motor = DCMotor.getNEO(2);
+    var plant = LinearSystemId.createDrivetrainVelocitySystem(
+        motor,
+        50,
+        Units.inchesToMeters(2),
+        Units.inchesToMeters(12),
+        0.5,
+        1.0);
+
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim = new DifferentialDrivetrainSim(plant,
+        motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020);
+    var ramsete = new RamseteController();
+    feedforward.reset(VecBuilder.fill(0, 0));
+
+    // ground truth
+    Matrix<N7, N1> groundTruthX = new Vector<>(Nat.N7());
+
+    var traj = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(),
+        List.of(),
+        new Pose2d(2, 2, new Rotation2d()),
+        new TrajectoryConfig(1, 1)
+            .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
+
+    for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
+      var state = traj.sample(0.020);
+      var ramseteOut = ramsete.calculate(sim.getEstimatedPosition(), state);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut);
+
+      var voltages = feedforward.calculate(
+          VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond));
+
+      // Sim periodic code
+      sim.setInputs(voltages.get(0, 0), voltages.get(1, 0));
+      sim.update(0.020);
+
+      // Update our ground truth
+      groundTruthX = RungeKutta.rungeKutta(sim::getDynamics, groundTruthX, voltages, 0.020);
+    }
+
+    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kX));
+    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kY));
+    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kHeading));
+  }
+
+  @Test
+  public void testCurrent() {
+    var motor = DCMotor.getNEO(2);
+    var plant = LinearSystemId.createDrivetrainVelocitySystem(
+        motor,
+        50,
+        Units.inchesToMeters(2),
+        Units.inchesToMeters(12),
+        0.5,
+        1.0);
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+
+    sim.setInputs(-12, -12);
+    for (int i = 0; i < 10; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDrawAmps() > 0);
+
+    sim.setInputs(12, 12);
+    for (int i = 0; i < 20; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDrawAmps() > 0);
+
+    sim.setInputs(-12, 12);
+    for (int i = 0; i < 30; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDrawAmps() > 0);
+  }
+
+  @Test
+  public void testWHeee() {
+    var motor = DCMotor.getNEO(2);
+    var plant = LinearSystemId.createDrivetrainVelocitySystem(
+        motor,
+        50,
+        Units.inchesToMeters(2),
+        Units.inchesToMeters(12),
+        2.0,
+        5.0);
+
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+
+    sim.setInputs(2, 4);
+
+    for (int i = 0; i < 10000; i++) {
+      sim.update(0.020);
+    }
+
+    System.out.println(sim.getEstimatedPosition());
+    assertTrue(Math.abs(sim.getEstimatedPosition().getX()) < 10000);
+  }
+}

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

@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class ElevatorSimTest {
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testStateSpaceSimWithElevator() {
+
+    var controller = new PIDController(10, 0, 0);
+
+    var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67,
+        0.0, 3.0, VecBuilder.fill(0.01));
+
+    var motor = new PWMVictorSPX(0);
+    var encoder = new Encoder(0, 1);
+    var encoderSim = new EncoderSim(encoder);
+
+    for (int i = 0; i < 100; i++) {
+      controller.setSetpoint(2.0);
+
+      double nextVoltage = controller.calculate(encoderSim.getDistance());
+
+      double currentBatteryVoltage = RobotController.getBatteryVoltage();
+      motor.set(nextVoltage / currentBatteryVoltage);
+
+      // ------ SimulationPeriodic() happens after user code -------
+
+      var u = VecBuilder.fill(motor.get() * currentBatteryVoltage);
+      sim.setInput(u);
+      sim.update(0.020);
+      var y = sim.getOutput();
+      encoderSim.setDistance(y.get(0, 0));
+    }
+
+    assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2);
+  }
+
+  @Test
+  public void testMinMax() {
+    var plant = LinearSystemId.createElevatorSystem(
+        DCMotor.getVex775Pro(4),
+        8.0,
+        0.75 * 25.4 / 1000.0,
+        14.67);
+
+    var sim = new ElevatorSim(plant, VecBuilder.fill(0.01),
+        DCMotor.getVex775Pro(4),
+        14.67, 0.75 * 25.4 / 1000.0, 0.0, 1.0);
+
+    for (int i = 0; i < 100; i++) {
+      sim.setInput(VecBuilder.fill(0));
+      sim.update(0.020);
+      var height = sim.getPositionMeters();
+      assertTrue(height >= -0.05);
+    }
+
+    for (int i = 0; i < 100; i++) {
+      sim.setInput(VecBuilder.fill(12.0));
+      sim.update(0.020);
+      var height = sim.getPositionMeters();
+      assertTrue(height <= 1.05);
+    }
+  }
+}