[wpilib] Add mechanism specific SetState overloads to physics sims (#5534)

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.simulation;
 
 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.LinearSystem;
@@ -84,6 +85,16 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
     m_gearing = gearing;
   }
 
+  /**
+   * Sets the state of the DC motor.
+   *
+   * @param position The new position
+   * @param velocity The new velocity
+   */
+  void setState(double angularPositionRad, double angularVelocityRadPerSec) {
+    setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
+  }
+
   /**
    * Returns the DC motor position.
    *

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

@@ -67,8 +67,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
     m_maxHeight = maxHeightMeters;
     m_simulateGravity = simulateGravity;
 
-    setState(
-        VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0));
+    setState(startingHeightMeters, 0);
   }
 
   /**
@@ -174,6 +173,19 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
         null);
   }
 
+  /**
+   * Sets the elevator's state. The new position will be limited between the minimum and maximum
+   * allowed heights.
+   *
+   * @param positionMeters The new position in meters.
+   * @param velocityMetersPerSecond New velocity in meters per second.
+   */
+  public void setState(double positionMeters, double velocityMetersPerSecond) {
+    setState(
+        VecBuilder.fill(
+            MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
+  }
+
   /**
    * Returns whether the elevator would hit the lower limit.
    *

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -84,6 +85,15 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
     m_gearing = gearing;
   }
 
+  /**
+   * Sets the flywheel's state.
+   *
+   * @param velocityRadPerSec The new velocity in radians per second.
+   */
+  public void setState(double velocityRadPerSec) {
+    setState(VecBuilder.fill(velocityRadPerSec));
+  }
+
   /**
    * Returns the flywheel velocity.
    *

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

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.numbers.N1;
@@ -66,7 +67,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
     m_maxAngle = maxAngleRads;
     m_simulateGravity = simulateGravity;
 
-    setState(VecBuilder.fill(startingAngleRads, 0));
+    setState(startingAngleRads, 0.0);
   }
 
   /**
@@ -172,6 +173,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
         measurementStdDevs);
   }
 
+  /**
+   * Sets the arm's state. The new angle will be limited between the minimum and maximum allowed
+   * limits.
+   *
+   * @param angleRadians The new angle.
+   * @param velocityRadPerSec The new angular velocity.
+   */
+  void setState(double angleRadians, double velocityRadPerSec) {
+    setState(
+        VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
+  }
+
   /**
    * Returns whether the arm would hit the lower limit.
    *