[wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288)

- Add a constructor parameter to configure the initial angle of the arm
- Also reorganizes cascading constructors for Java

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.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;
@@ -43,6 +44,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @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 startingHeightMeters The starting height of the elevator.
+   * @param measurementStdDevs The standard deviations of the measurements.
    */
   public ElevatorSim(
       LinearSystem<N2, N1, N1> plant,
@@ -51,16 +54,19 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double drumRadiusMeters,
       double minHeightMeters,
       double maxHeightMeters,
-      boolean simulateGravity) {
-    this(
-        plant,
-        gearbox,
-        gearing,
-        drumRadiusMeters,
-        minHeightMeters,
-        maxHeightMeters,
-        simulateGravity,
-        null);
+      boolean simulateGravity,
+      double startingHeightMeters,
+      Matrix<N1, N1> measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_drumRadius = drumRadiusMeters;
+    m_minHeight = minHeightMeters;
+    m_maxHeight = maxHeightMeters;
+    m_simulateGravity = simulateGravity;
+
+    setState(
+        VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0));
   }
 
   /**
@@ -72,8 +78,8 @@ 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 startingHeightMeters The starting height of the elevator.
    * @param simulateGravity Whether gravity should be simulated or not.
-   * @param measurementStdDevs The standard deviations of the measurements.
    */
   public ElevatorSim(
       LinearSystem<N2, N1, N1> plant,
@@ -83,43 +89,16 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double minHeightMeters,
       double maxHeightMeters,
       boolean simulateGravity,
-      Matrix<N1, N1> measurementStdDevs) {
-    super(plant, measurementStdDevs);
-    m_gearbox = gearbox;
-    m_gearing = gearing;
-    m_drumRadius = drumRadiusMeters;
-    m_minHeight = minHeightMeters;
-    m_maxHeight = maxHeightMeters;
-    m_simulateGravity = simulateGravity;
-  }
-
-  /**
-   * Creates a simulated elevator mechanism.
-   *
-   * @param gearbox The type of and number of motors in the elevator gearbox.
-   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
-   * @param carriageMassKg The mass of the elevator carriage.
-   * @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,
-      double gearing,
-      double carriageMassKg,
-      double drumRadiusMeters,
-      double minHeightMeters,
-      double maxHeightMeters,
-      boolean simulateGravity) {
+      double startingHeightMeters) {
     this(
+        plant,
         gearbox,
         gearing,
-        carriageMassKg,
         drumRadiusMeters,
         minHeightMeters,
         maxHeightMeters,
         simulateGravity,
+        startingHeightMeters,
         null);
   }
 
@@ -133,6 +112,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @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 startingHeightMeters The starting height of the elevator.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
   public ElevatorSim(
@@ -143,16 +123,51 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
       double minHeightMeters,
       double maxHeightMeters,
       boolean simulateGravity,
+      double startingHeightMeters,
       Matrix<N1, N1> measurementStdDevs) {
-    super(
+    this(
         LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
+        gearbox,
+        gearing,
+        drumRadiusMeters,
+        minHeightMeters,
+        maxHeightMeters,
+        simulateGravity,
+        startingHeightMeters,
         measurementStdDevs);
-    m_gearbox = gearbox;
-    m_gearing = gearing;
-    m_drumRadius = drumRadiusMeters;
-    m_minHeight = minHeightMeters;
-    m_maxHeight = maxHeightMeters;
-    m_simulateGravity = simulateGravity;
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param carriageMassKg The mass of the elevator carriage.
+   * @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 startingHeightMeters The starting height of the elevator.
+   */
+  public ElevatorSim(
+      DCMotor gearbox,
+      double gearing,
+      double carriageMassKg,
+      double drumRadiusMeters,
+      double minHeightMeters,
+      double maxHeightMeters,
+      boolean simulateGravity,
+      double startingHeightMeters) {
+    this(
+        gearbox,
+        gearing,
+        carriageMassKg,
+        drumRadiusMeters,
+        minHeightMeters,
+        maxHeightMeters,
+        simulateGravity,
+        startingHeightMeters,
+        null);
   }
 
   /**

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

@@ -43,6 +43,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * @param minAngleRads The minimum angle that the arm is capable of.
    * @param maxAngleRads The maximum angle that the arm is capable of.
    * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param measurementStdDevs The standard deviations of the measurements.
    */
   public SingleJointedArmSim(
       LinearSystem<N2, N1, N1> plant,
@@ -51,16 +53,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
       double armLengthMeters,
       double minAngleRads,
       double maxAngleRads,
-      boolean simulateGravity) {
-    this(
-        plant,
-        gearbox,
-        gearing,
-        armLengthMeters,
-        minAngleRads,
-        maxAngleRads,
-        simulateGravity,
-        null);
+      boolean simulateGravity,
+      double startingAngleRads,
+      Matrix<N1, N1> measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_armLenMeters = armLengthMeters;
+    m_minAngle = minAngleRads;
+    m_maxAngle = maxAngleRads;
+    m_simulateGravity = simulateGravity;
+
+    setState(VecBuilder.fill(startingAngleRads, 0));
   }
 
   /**
@@ -73,7 +77,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * @param minAngleRads The minimum angle that the arm is capable of.
    * @param maxAngleRads The maximum angle that the arm is capable of.
    * @param simulateGravity Whether gravity should be simulated or not.
-   * @param measurementStdDevs The standard deviations of the measurements.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
    */
   public SingleJointedArmSim(
       LinearSystem<N2, N1, N1> plant,
@@ -83,14 +87,17 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
       double minAngleRads,
       double maxAngleRads,
       boolean simulateGravity,
-      Matrix<N1, N1> measurementStdDevs) {
-    super(plant, measurementStdDevs);
-    m_gearbox = gearbox;
-    m_gearing = gearing;
-    m_armLenMeters = armLengthMeters;
-    m_minAngle = minAngleRads;
-    m_maxAngle = maxAngleRads;
-    m_simulateGravity = simulateGravity;
+      double startingAngleRads) {
+    this(
+        plant,
+        gearbox,
+        gearing,
+        armLengthMeters,
+        minAngleRads,
+        maxAngleRads,
+        simulateGravity,
+        startingAngleRads,
+        null);
   }
 
   /**
@@ -103,6 +110,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * @param minAngleRads The minimum angle that the arm is capable of.
    * @param maxAngleRads The maximum angle that the arm is capable of.
    * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
    */
   public SingleJointedArmSim(
       DCMotor gearbox,
@@ -111,7 +119,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
       double armLengthMeters,
       double minAngleRads,
       double maxAngleRads,
-      boolean simulateGravity) {
+      boolean simulateGravity,
+      double startingAngleRads) {
     this(
         gearbox,
         gearing,
@@ -120,6 +129,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
         minAngleRads,
         maxAngleRads,
         simulateGravity,
+        startingAngleRads,
         null);
   }
 
@@ -133,6 +143,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * @param minAngleRads The minimum angle that the arm is capable of.
    * @param maxAngleRads The maximum angle that the arm is capable of.
    * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
   public SingleJointedArmSim(
@@ -143,16 +154,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
       double minAngleRads,
       double maxAngleRads,
       boolean simulateGravity,
+      double startingAngleRads,
       Matrix<N1, N1> measurementStdDevs) {
-    super(
+    this(
         LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
+        gearbox,
+        gearing,
+        armLengthMeters,
+        minAngleRads,
+        maxAngleRads,
+        simulateGravity,
+        startingAngleRads,
         measurementStdDevs);
-    m_gearbox = gearbox;
-    m_gearing = gearing;
-    m_armLenMeters = armLengthMeters;
-    m_minAngle = minAngleRads;
-    m_maxAngle = maxAngleRads;
-    m_simulateGravity = simulateGravity;
   }
 
   /**