[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

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

@@ -16,6 +16,7 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
                          const DCMotor& gearbox, double gearing,
                          units::meter_t drumRadius, units::meter_t minHeight,
                          units::meter_t maxHeight, bool simulateGravity,
+                         units::meter_t startingHeight,
                          const std::array<double, 1>& measurementStdDevs)
     : LinearSystemSim(plant, measurementStdDevs),
       m_gearbox(gearbox),
@@ -23,22 +24,21 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
       m_minHeight(minHeight),
       m_maxHeight(maxHeight),
       m_gearing(gearing),
-      m_simulateGravity(simulateGravity) {}
+      m_simulateGravity(simulateGravity) {
+  SetState(
+      frc::Vectord<2>{std::clamp(startingHeight, minHeight, maxHeight), 0.0});
+}
 
 ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
                          units::kilogram_t carriageMass,
                          units::meter_t drumRadius, units::meter_t minHeight,
                          units::meter_t maxHeight, bool simulateGravity,
+                         units::meter_t startingHeight,
                          const std::array<double, 1>& measurementStdDevs)
-    : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
-                                                     drumRadius, gearing),
-                      measurementStdDevs),
-      m_gearbox(gearbox),
-      m_drumRadius(drumRadius),
-      m_minHeight(minHeight),
-      m_maxHeight(maxHeight),
-      m_gearing(gearing),
-      m_simulateGravity(simulateGravity) {}
+    : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
+                                                 drumRadius, gearing),
+                  gearbox, gearing, drumRadius, minHeight, maxHeight,
+                  simulateGravity, startingHeight, measurementStdDevs) {}
 
 bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
   return elevatorHeight <= m_minHeight;

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

@@ -19,6 +19,7 @@ SingleJointedArmSim::SingleJointedArmSim(
     const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
     units::meter_t armLength, units::radian_t minAngle,
     units::radian_t maxAngle, bool simulateGravity,
+    units::radian_t startingAngle,
     const std::array<double, 1>& measurementStdDevs)
     : LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
       m_armLen(armLength),
@@ -26,17 +27,20 @@ SingleJointedArmSim::SingleJointedArmSim(
       m_maxAngle(maxAngle),
       m_gearbox(gearbox),
       m_gearing(gearing),
-      m_simulateGravity(simulateGravity) {}
+      m_simulateGravity(simulateGravity) {
+  SetState(frc::Vectord<2>{startingAngle, 0.0});
+}
 
 SingleJointedArmSim::SingleJointedArmSim(
     const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
     units::meter_t armLength, units::radian_t minAngle,
     units::radian_t maxAngle, bool simulateGravity,
+    units::radian_t startingAngle,
     const std::array<double, 1>& measurementStdDevs)
     : SingleJointedArmSim(
           LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
           gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,
-          measurementStdDevs) {}
+          startingAngle, measurementStdDevs) {}
 
 bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
   return armAngle <= m_minAngle;

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

@@ -32,12 +32,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
    * @param minHeight          The minimum allowed height of the elevator.
    * @param maxHeight          The maximum allowed height of the elevator.
    * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingHeight     The starting height of the elevator.
    * @param measurementStdDevs The standard deviation of the measurements.
    */
   ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
               double gearing, units::meter_t drumRadius,
               units::meter_t minHeight, units::meter_t maxHeight,
-              bool simulateGravity,
+              bool simulateGravity, units::meter_t startingHeight,
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**
@@ -53,12 +54,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
    * @param minHeight          The minimum allowed height of the elevator.
    * @param maxHeight          The maximum allowed height of the elevator.
    * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingHeight     The starting height of the elevator.
    * @param measurementStdDevs The standard deviation of the measurements.
    */
   ElevatorSim(const DCMotor& gearbox, double gearing,
               units::kilogram_t carriageMass, units::meter_t drumRadius,
               units::meter_t minHeight, units::meter_t maxHeight,
-              bool simulateGravity,
+              bool simulateGravity, units::meter_t startingHeight,
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**

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

@@ -31,12 +31,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param minAngle           The minimum angle that the arm is capable of.
    * @param maxAngle           The maximum angle that the arm is capable of.
    * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingAngle      The initial position of the arm.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
   SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
                       const DCMotor& gearbox, double gearing,
                       units::meter_t armLength, units::radian_t minAngle,
                       units::radian_t maxAngle, bool simulateGravity,
+                      units::radian_t startingAngle,
                       const std::array<double, 1>& measurementStdDevs = {0.0});
   /**
    * Creates a simulated arm mechanism.
@@ -50,12 +52,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
    * @param minAngle           The minimum angle that the arm is capable of.
    * @param maxAngle           The maximum angle that the arm is capable of.
    * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingAngle      The initial position of the arm.
    * @param measurementStdDevs The standard deviation of the measurement noise.
    */
   SingleJointedArmSim(const DCMotor& gearbox, double gearing,
                       units::kilogram_square_meter_t moi,
                       units::meter_t armLength, units::radian_t minAngle,
                       units::radian_t maxAngle, bool simulateGravity,
+                      units::radian_t startingAngle,
                       const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**