[wpimath] Reorganize LinearSystem factories

docs/build.gradle

@@ -179,8 +179,6 @@ task generateJavaDocs(type: Javadoc) {
             "-org.wpilib.math.spline.struct," +
             "-org.wpilib.math.system.proto," +
             "-org.wpilib.math.system.struct," +
-            "-org.wpilib.math.system.plant.proto," +
-            "-org.wpilib.math.system.plant.struct," +
             "-org.wpilib.math.trajectory.proto," +
             "-org.wpilib.math.trajectory.struct," +
             "-org.wpilib.command3.proto," +

tools/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp

@@ -8,21 +8,22 @@
 
 #include "wpi/math/controller/LinearQuadraticRegulator.hpp"
 #include "wpi/math/system/LinearSystem.hpp"
-#include "wpi/math/system/plant/LinearSystemId.hpp"
+#include "wpi/math/system/Models.hpp"
 #include "wpi/sysid/analysis/FeedbackControllerPreset.hpp"
 #include "wpi/units/acceleration.hpp"
 #include "wpi/units/velocity.hpp"
 #include "wpi/units/voltage.hpp"
 
 using namespace sysid;
 
-using Kv_t = decltype(1_V / 1_mps);
-using Ka_t = decltype(1_V / 1_mps_sq);
 using Matrix1d = Eigen::Matrix<double, 1, 1>;
 
 FeedbackGains sysid::CalculatePositionFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka) {
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+
   if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
     return {0.0, 0.0};
   }
@@ -43,9 +44,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
     return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
   }
 
-  auto system =
-      wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meters>(
-          Kv_t{Kv}, Ka_t{Ka});
+  auto system = wpi::math::Models::ElevatorFromSysId(Kv_t{Kv}, Ka_t{Ka});
 
   wpi::math::LinearQuadraticRegulator<2, 1> controller{
       system, {params.qp, params.qv}, {params.r}, preset.period};
@@ -61,6 +60,9 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
 FeedbackGains sysid::CalculateVelocityFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka, double encFactor) {
+  using Kv_t = decltype(1_V / 1_rad_per_s);
+  using Ka_t = decltype(1_V / 1_rad_per_s_sq);
+
   if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
     return {0.0, 0.0};
   }
@@ -72,9 +74,7 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
     return {0.0, 0.0};
   }
 
-  auto system =
-      wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
-          Kv_t{Kv}, Ka_t{Ka});
+  auto system = wpi::math::Models::FlywheelFromSysId(Kv_t{Kv}, Ka_t{Ka});
   wpi::math::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qv}, {params.r}, preset.period};
   controller.LatencyCompensate(system, preset.period, preset.measurementDelay);

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

@@ -7,8 +7,8 @@
 #include <utility>
 
 #include "wpi/math/random/Normal.hpp"
+#include "wpi/math/system/Models.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
-#include "wpi/math/system/plant/LinearSystemId.hpp"
 #include "wpi/math/util/StateSpaceUtil.hpp"
 #include "wpi/system/RobotController.hpp"
 #include "wpi/util/MathExtras.hpp"
@@ -39,7 +39,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
     wpi::units::meter_t wheelRadius, wpi::units::meter_t trackwidth,
     const std::array<double, 7>& measurementStdDevs)
     : DifferentialDrivetrainSim(
-          wpi::math::LinearSystemId::DrivetrainVelocitySystem(
+          wpi::math::Models::DifferentialDriveFromPhysicalConstants(
               driveMotor, mass, wheelRadius, trackwidth / 2.0, J, gearing),
           trackwidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
 

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

@@ -4,8 +4,8 @@
 
 #include "wpi/simulation/ElevatorSim.hpp"
 
+#include "wpi/math/system/Models.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
-#include "wpi/math/system/plant/LinearSystemId.hpp"
 #include "wpi/system/RobotController.hpp"
 #include "wpi/util/MathExtras.hpp"
 
@@ -33,7 +33,7 @@ ElevatorSim::ElevatorSim(const wpi::math::DCMotor& gearbox, double gearing,
                          wpi::units::meter_t maxHeight, bool simulateGravity,
                          wpi::units::meter_t startingHeight,
                          const std::array<double, 2>& measurementStdDevs)
-    : ElevatorSim(wpi::math::LinearSystemId::ElevatorSystem(
+    : ElevatorSim(wpi::math::Models::ElevatorFromPhysicalConstants(
                       gearbox, carriageMass, drumRadius, gearing),
                   gearbox, minHeight, maxHeight, simulateGravity,
                   startingHeight, measurementStdDevs) {}
@@ -48,9 +48,9 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
                          wpi::units::meter_t maxHeight, bool simulateGravity,
                          wpi::units::meter_t startingHeight,
                          const std::array<double, 2>& measurementStdDevs)
-    : ElevatorSim(wpi::math::LinearSystemId::IdentifyPositionSystem(kV, kA),
-                  gearbox, minHeight, maxHeight, simulateGravity,
-                  startingHeight, measurementStdDevs) {}
+    : ElevatorSim(wpi::math::Models::ElevatorFromSysId(kV, kA), gearbox,
+                  minHeight, maxHeight, simulateGravity, startingHeight,
+                  measurementStdDevs) {}
 
 void ElevatorSim::SetState(wpi::units::meter_t position,
                            wpi::units::meters_per_second_t velocity) {

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

@@ -6,8 +6,8 @@
 
 #include <cmath>
 
+#include "wpi/math/system/Models.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
-#include "wpi/math/system/plant/LinearSystemId.hpp"
 #include "wpi/system/RobotController.hpp"
 #include "wpi/units/voltage.hpp"
 #include "wpi/util/MathExtras.hpp"
@@ -38,10 +38,11 @@ SingleJointedArmSim::SingleJointedArmSim(
     wpi::units::radian_t minAngle, wpi::units::radian_t maxAngle,
     bool simulateGravity, wpi::units::radian_t startingAngle,
     const std::array<double, 2>& measurementStdDevs)
-    : SingleJointedArmSim(wpi::math::LinearSystemId::SingleJointedArmSystem(
-                              gearbox, moi, gearing),
-                          gearbox, gearing, armLength, minAngle, maxAngle,
-                          simulateGravity, startingAngle, measurementStdDevs) {}
+    : SingleJointedArmSim(
+          wpi::math::Models::SingleJointedArmFromPhysicalConstants(gearbox, moi,
+                                                                   gearing),
+          gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,
+          startingAngle, measurementStdDevs) {}
 
 void SingleJointedArmSim::SetState(wpi::units::radian_t angle,
                                    wpi::units::radians_per_second_t velocity) {

wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp

@@ -4,8 +4,8 @@
 
 #pragma once
 
+#include "wpi/math/system/DCMotor.hpp"
 #include "wpi/math/system/LinearSystem.hpp"
-#include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/simulation/LinearSystemSim.hpp"
 #include "wpi/units/angle.hpp"
 #include "wpi/units/angular_acceleration.hpp"
@@ -22,12 +22,11 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
   /**
    * Creates a simulated DC motor mechanism.
    *
-   * @param plant              The linear system representing the DC motor. This
-   * system can be created with wpi::math::LinearSystemId::DCMotorSystem(). If
-   * wpi::math::LinearSystemId::DCMotorSystem(kV, kA) is used, the distance unit
-   * must be radians.
-   * @param gearbox            The type of and number of motors in the DC motor
-   * gearbox.
+   * @param plant The linear system representing the DC motor. This system can
+   *     be created with wpi::math::Models::ElevatorFromPhysicalConstants(). If
+   *     wpi::math::Models::ElevatorFromSysId(kV, kA) is used, the distance unit
+   *     must be radians.
+   * @param gearbox The type of and number of motors in the DC motor gearbox.
    * @param measurementStdDevs The standard deviation of the measurement noise.
    */
   DCMotorSim(const wpi::math::LinearSystem<2, 1, 2>& plant,

wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp

@@ -4,13 +4,15 @@
 
 #pragma once
 
-#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
+#include "wpi/math/geometry/Pose2d.hpp"
+#include "wpi/math/geometry/Rotation2d.hpp"
 #include "wpi/math/linalg/EigenCore.hpp"
+#include "wpi/math/system/DCMotor.hpp"
 #include "wpi/math/system/LinearSystem.hpp"
-#include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/units/length.hpp"
 #include "wpi/units/moment_of_inertia.hpp"
 #include "wpi/units/time.hpp"
+#include "wpi/units/velocity.hpp"
 #include "wpi/units/voltage.hpp"
 
 namespace wpi::sim {
@@ -21,24 +23,22 @@ class DifferentialDrivetrainSim {
    * Creates a simulated differential drivetrain.
    *
    * @param plant The wpi::math::LinearSystem representing the robot's
-   * drivetrain. This system can be created with
-   *              wpi::math::LinearSystemId::DrivetrainVelocitySystem() or
-   *              wpi::math::LinearSystemId::IdentifyDrivetrainSystem().
-   * @param trackwidth   The robot's trackwidth.
-   * @param driveMotor   A wpi::math::DCMotor representing the left side of the
-   * drivetrain.
+   *     drivetrain. This system can be created with
+   *     wpi::math::Models::DifferentialDriveFromPhysicalConstants() or
+   *     wpi::math::Models::DifferentialDriveFromSysId().
+   * @param trackwidth The robot's trackwidth.
+   * @param driveMotor A wpi::math::DCMotor representing the left side of the
+   *     drivetrain.
    * @param gearingRatio The gearingRatio ratio of the left side, as output over
-   *                     input. This must be the same ratio as the ratio used to
-   *                     identify or create the plant.
+   *     input. This must be the same ratio as the ratio used to identify or
+   *     create the plant.
    * @param wheelRadius  The radius of the wheels on the drivetrain, in meters.
    * @param measurementStdDevs Standard deviations for measurements, in the form
-   *                           [x, y, heading, left velocity, right velocity,
-   *                           left distance, right distance]ᵀ. Can be omitted
-   *                           if no noise is desired. Gyro standard deviations
-   *                           of 0.0001 radians, velocity standard deviations
-   *                           of 0.05 m/s, and position measurement standard
-   *                           deviations of 0.005 meters are a reasonable
-   *                           starting point.
+   *     [x, y, heading, left velocity, right velocity, left distance, right
+   *     distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
+   *     deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s,
+   *     and position measurement standard deviations of 0.005 meters are a
+   *     reasonable starting point.
    */
   DifferentialDrivetrainSim(
       wpi::math::LinearSystem<2, 2, 2> plant, wpi::units::meter_t trackwidth,
@@ -49,25 +49,22 @@ class DifferentialDrivetrainSim {
   /**
    * Creates a simulated differential drivetrain.
    *
-   * @param driveMotor  A wpi::math::DCMotor representing the left side of the
-   * drivetrain.
-   * @param gearing     The gearing on the drive between motor and wheel, as
-   *                    output over input. This must be the same ratio as the
-   *                    ratio used to identify or create the plant.
-   * @param J           The moment of inertia of the drivetrain about its
-   *                    center.
-   * @param mass        The mass of the drivebase.
+   * @param driveMotor A wpi::math::DCMotor representing the left side of the
+   *     drivetrain.
+   * @param gearing The gearing on the drive between motor and wheel, as output
+   *     over input. This must be the same ratio as the ratio used to identify
+   *     or create the plant.
+   * @param J The moment of inertia of the drivetrain about its center.
+   * @param mass The mass of the drivebase.
    * @param wheelRadius The radius of the wheels on the drivetrain.
-   * @param trackwidth  The robot's trackwidth, or distance between left and
-   *                    right wheels.
+   * @param trackwidth The robot's trackwidth, or distance between left and
+   *     right wheels.
    * @param measurementStdDevs Standard deviations for measurements, in the form
-   *                           [x, y, heading, left velocity, right velocity,
-   *                           left distance, right distance]ᵀ. Can be omitted
-   *                           if no noise is desired. Gyro standard deviations
-   *                           of 0.0001 radians, velocity standard deviations
-   *                           of 0.05 m/s, and position measurement standard
-   *                           deviations of 0.005 meters are a reasonable
-   *                           starting point.
+   *     [x, y, heading, left velocity, right velocity, left distance, right
+   *     distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
+   *     deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s,
+   *     and position measurement standard deviations of 0.005 meters are a
+   *     reasonable starting point.
    */
   DifferentialDrivetrainSim(
       wpi::math::DCMotor driveMotor, double gearing,

wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp

@@ -6,7 +6,7 @@
 
 #include <array>
 
-#include "wpi/math/system/plant/DCMotor.hpp"
+#include "wpi/math/system/DCMotor.hpp"
 #include "wpi/simulation/LinearSystemSim.hpp"
 #include "wpi/units/length.hpp"
 #include "wpi/units/mass.hpp"
@@ -31,15 +31,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
   /**
    * Constructs a simulated elevator mechanism.
    *
-   * @param plant              The linear system that represents the elevator.
-   *                           This system can be created with
-   *                           wpi::math::LinearSystemId::ElevatorSystem().
-   * @param gearbox            The type of and number of motors in your
-   *                           elevator gearbox.
-   * @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 plant The linear system that represents the elevator. This system
+   *     can be created with wpi::math::Models::ElevatorFromPhysicalConstants().
+   * @param gearbox The type of and number of motors in your elevator gearbox.
+   * @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 wpi::math::LinearSystem<2, 1, 2>& plant,
@@ -51,17 +49,15 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
   /**
    * Constructs a simulated elevator mechanism.
    *
-   * @param gearbox            The type of and number of motors in your
-   *                           elevator gearbox.
-   * @param gearing            The gearing of the elevator (numbers greater
-   *                           than 1 represent reductions).
-   * @param carriageMass       The mass of the elevator carriage.
-   * @param drumRadius         The radius of the drum that your cable is
-   *                           wrapped around.
-   * @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 gearbox The type of and number of motors in your elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1
+   *     represent reductions).
+   * @param carriageMass The mass of the elevator carriage.
+   * @param drumRadius The radius of the drum that your cable is wrapped around.
+   * @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 wpi::math::DCMotor& gearbox, double gearing,
@@ -74,14 +70,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
   /**
    * Constructs a simulated elevator mechanism.
    *
-   * @param kV                 The velocity gain.
-   * @param kA                 The acceleration gain.
-   * @param gearbox            The type of and number of motors in your
-   *                           elevator gearbox.
-   * @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 kV The velocity gain.
+   * @param kA The acceleration gain.
+   * @param gearbox The type of and number of motors in your elevator gearbox.
+   * @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.
    */
   template <typename Distance>
@@ -98,6 +93,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
   /**
    * Sets the elevator's state. The new position will be limited between the
    * minimum and maximum allowed heights.
+   *
    * @param position The new position
    * @param velocity The new velocity
    */
@@ -167,8 +163,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
    * Updates the state estimate of the elevator.
    *
    * @param currentXhat The current state estimate.
-   * @param u           The system inputs (voltage).
-   * @param dt          The time difference between controller updates.
+   * @param u The system inputs (voltage).
+   * @param dt The time difference between controller updates.
    */
   wpi::math::Vectord<2> UpdateX(const wpi::math::Vectord<2>& currentXhat,
                                 const wpi::math::Vectord<1>& u,

wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp

@@ -4,8 +4,8 @@
 
 #pragma once
 
+#include "wpi/math/system/DCMotor.hpp"
 #include "wpi/math/system/LinearSystem.hpp"
-#include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/simulation/LinearSystemSim.hpp"
 #include "wpi/units/angular_acceleration.hpp"
 #include "wpi/units/angular_velocity.hpp"
@@ -21,12 +21,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
   /**
    * Creates a simulated flywheel mechanism.
    *
-   * @param plant              The linear system representing the flywheel. This
-   *                           system can be created with
-   *                           wpi::math::LinearSystemId::FlywheelSystem() or
-   * wpi::math::LinearSystemId::IdentifyVelocitySystem().
-   * @param gearbox            The type of and number of motors in the flywheel
-   *                           gearbox.
+   * @param plant The linear system representing the flywheel. This system can
+   *     be created with wpi::math::Models::FlywheelFromPhysicalConstants() or
+   *     wpi::math::Models::FlywheelFromSysId().
+   * @param gearbox The type of and number of motors in the flywheel gearbox.
    * @param measurementStdDevs The standard deviation of the measurement noise.
    */
   FlywheelSim(const wpi::math::LinearSystem<1, 1, 1>& plant,