[wpilib] Clean up simulation physics API (#2739)

Some vestigial functions were never removed, and C++ single-jointed arm
sim was missing a flag for disabling gravity simulation. This is useful
for mechanisms like turrets.

Fixes #2738.

wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp

@@ -22,34 +22,35 @@ SingleJointedArmSim::SingleJointedArmSim(
     const LinearSystem<2, 1, 1>& system, const DCMotor motor, double G,
     units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle,
     units::radian_t maxAngle, bool addNoise,
-    const std::array<double, 1>& measurementStdDevs)
+    const std::array<double, 1>& measurementStdDevs, bool simulateGravity)
     : LinearSystemSim<2, 1, 1>(system, addNoise, measurementStdDevs),
       m_r(armLength),
       m_minAngle(minAngle),
       m_maxAngle(maxAngle),
       m_mass(mass),
       m_motor(motor),
-      m_gearing(G) {}
+      m_gearing(G),
+      m_simulateGravity(simulateGravity) {}
 
 SingleJointedArmSim::SingleJointedArmSim(
     const DCMotor& motor, units::kilogram_square_meter_t J, double G,
     units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle,
     units::radian_t maxAngle, bool addNoise,
-    const std::array<double, 1>& measurementStdDevs)
+    const std::array<double, 1>& measurementStdDevs, bool simulateGravity)
     : SingleJointedArmSim(LinearSystemId::SingleJointedArmSystem(motor, J, G),
                           motor, G, mass, armLength, minAngle, maxAngle,
-                          addNoise, measurementStdDevs) {}
+                          addNoise, measurementStdDevs, simulateGravity) {}
 
 SingleJointedArmSim::SingleJointedArmSim(
     const DCMotor& motor, double G, units::kilogram_t mass,
     units::meter_t armLength, units::radian_t minAngle,
     units::radian_t maxAngle, bool addNoise,
-    const std::array<double, 1>& measurementStdDevs)
+    const std::array<double, 1>& measurementStdDevs, bool simulateGravity)
     : SingleJointedArmSim(
           LinearSystemId::SingleJointedArmSystem(
               motor, 1.0 / 3.0 * mass * armLength * armLength, G),
           motor, G, mass, armLength, minAngle, maxAngle, addNoise,
-          measurementStdDevs) {}
+          measurementStdDevs, simulateGravity) {}
 
 bool SingleJointedArmSim::HasHitLowerLimit(
     const Eigen::Matrix<double, 2, 1>& x) const {
@@ -69,6 +70,14 @@ units::radians_per_second_t SingleJointedArmSim::GetVelocity() const {
   return units::radians_per_second_t{m_x(1)};
 }
 
+units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output
+  units::radians_per_second_t motorVelocity{m_x(1) * m_gearing};
+  return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
 Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
     const Eigen::Matrix<double, 2, 1>& currentXhat,
     const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
@@ -84,10 +93,15 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
 
   auto updatedXhat = RungeKutta(
       [&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
-        return m_plant.A() * x + m_plant.B() * u +
-               MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 /
-                                      (m_mass * m_r * m_r) * std::cos(x(0)))
-                                         .template to<double>());
+        Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;
+
+        if (m_simulateGravity) {
+          xdot += MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 /
+                                         (m_mass * m_r * m_r) * std::cos(x(0)))
+                                            .template to<double>());
+        }
+
+        return xdot;
       },
       currentXhat, u, dt);
 
@@ -99,11 +113,3 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
   }
   return updatedXhat;
 }
-
-units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
-  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
-  // is spinning 10x faster than the output
-  units::radians_per_second_t motorVelocity{m_x(1) * m_gearing};
-  return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
-         wpi::sgn(m_u(0));
-}

wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h

@@ -37,17 +37,19 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param addNoise           Whether the sim should automatically add some
    *                           encoder noise.
    * @param measurementStdDevs The standard deviation of the measurement noise.
+   * @param simulateGravity    If the affects of gravity should be simulated.
    */
   SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor,
                       double G, units::kilogram_t mass,
                       units::meter_t armLength, units::radian_t minAngle,
                       units::radian_t maxAngle, bool addNoise,
-                      const std::array<double, 1>& measurementStdDevs);
+                      const std::array<double, 1>& measurementStdDevs,
+                      bool simulateGravity);
   /**
    * Creates a simulated arm mechanism.
    *
    * @param motor              The type and number of motors on the arm gearbox.
-   * @param j                  The moment of inertia of the arm.
+   * @param J                  The moment of inertia of the arm.
    * @param G                  The gear ratio of the arm (numbers greater than 1
    *                           represent reductions).
    * @param mass               The mass of the arm.
@@ -57,12 +59,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param addNoise           Whether the sim should automatically add some
    *                           encoder noise.
    * @param measurementStdDevs The standard deviation of the measurement noise.
+   * @param simulateGravity    If the affects of gravity should be simulated.
    */
   SingleJointedArmSim(const DCMotor& motor, units::kilogram_square_meter_t J,
                       double G, units::kilogram_t mass,
                       units::meter_t armLength, units::radian_t minAngle,
                       units::radian_t maxAngle, bool addNoise,
-                      const std::array<double, 1>& measurementStdDevs);
+                      const std::array<double, 1>& measurementStdDevs,
+                      bool simulateGravity);
 
   /**
    * Creates a simulated arm mechanism.
@@ -79,11 +83,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param addNoise           Whether the sim should automatically add some
    *                           encoder noise.
    * @param measurementStdDevs The standard deviation of the measurement noise.
+   * @param simulateGravity    If the affects of gravity should be simulated.
    */
   SingleJointedArmSim(const DCMotor& motor, double G, units::kilogram_t mass,
                       units::meter_t armLength, units::radian_t minAngle,
                       units::radian_t maxAngle, bool addNoise,
-                      const std::array<double, 1>& measurementStdDevs);
+                      const std::array<double, 1>& measurementStdDevs,
+                      bool simulateGravity);
 
   /**
    * Returns whether the arm has hit the lower limit.
@@ -115,6 +121,9 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    */
   units::radians_per_second_t GetVelocity() const;
 
+  units::ampere_t GetCurrentDraw() const override;
+
+ protected:
   /**
    * Updates the state estimate of the arm.
    *
@@ -126,8 +135,6 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
       const Eigen::Matrix<double, 2, 1>& currentXhat,
       const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
 
-  units::ampere_t GetCurrentDraw() const override;
-
  private:
   units::meter_t m_r;
   units::radian_t m_minAngle;
@@ -135,5 +142,6 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
   units::kilogram_t m_mass;
   const DCMotor m_motor;
   double m_gearing;
+  bool m_simulateGravity;
 };
 }  // namespace frc::sim

wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp

@@ -12,7 +12,8 @@
 
 TEST(SingleJointedArmTest, Disabled) {
   frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 10_kg,
-                                    9.5_in, -180_deg, 0_deg, false, {0.0});
+                                    9.5_in, -180_deg, 0_deg, false, {0.0},
+                                    true);
   sim.ResetState(frc::MakeMatrix<2, 1>(0.0, 0.0));
 
   for (size_t i = 0; i < 12 / 0.02; ++i) {

wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp

@@ -55,7 +55,8 @@ class Robot : public frc::TimedRobot {
   // with a standard deviation of 0.5 degrees.
   frc::sim::SingleJointedArmSim m_armSim{
       m_armGearbox, 100.0, 5_kg, 30_in,
-      -180_deg,     0_deg, true, {(0.5_deg).to<double>()}};
+      -180_deg,     0_deg, true, {(0.5_deg).to<double>()},
+      true};
   frc::sim::EncoderSim m_encoderSim{m_encoder};
 
  public:

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

@@ -67,6 +67,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * the simulation and write simulated outputs to sensors.
    *
    * @param motor                DCMotor representing the motor driving the arm.
+   * @param jKgSquaredMeters     The moment of inertia of the arm.
    * @param G                    The gear ratio of the arm (numbers greater than 1
    *                             represent reductions).
    * @param armMassKg            The mass of the arm.
@@ -188,10 +189,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
     return m_x.get(1, 0);
   }
 
-  public double getInputVoltageVolts() {
-    return m_u.get(0, 0);
-  }
-
   @Override
   public double getCurrentDrawAmps() {
     // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is