Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Reorganize LinearSystem factories (#8468)

Browse Source
This commit is contained in:
Tyler Veness
2026-01-12 19:09:35 -08:00
committed by GitHub
parent 89d0759ef2
commit 00fa8361dd
108 changed files with 1808 additions and 2138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -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) {}

10
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -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) {

11
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -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) {

13
wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp
View File

@@ -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,

65
wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp
View File

@@ -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,

58
wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp
View File

@@ -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,

12
wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp
View File

@@ -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,

43
wpilibc/src/main/native/include/wpi/simulation/SingleJointedArmSim.hpp
View File

@@ -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/angle.hpp"
#include "wpi/units/length.hpp"
@@ -22,17 +22,16 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
/**
* Creates a simulated arm mechanism.
*
* @param system The system representing this arm. This system can
* be created with
* wpi::math::LinearSystemId::SingleJointedArmSystem().
* @param gearbox The type and number of motors on the arm gearbox.
* @param gearing The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param armLength The length of the arm.
* @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 system The system representing this arm. This system can be created
* with wpi::math::Models::SingleJointedArmFromPhysicalConstants().
* @param gearbox The type and number of motors on the arm gearbox.
* @param gearing The gear ratio of the arm (numbers greater than 1 represent
* reductions).
* @param armLength The length of the arm.
* @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 wpi::math::LinearSystem<2, 1, 2>& system,
@@ -46,16 +45,16 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
/**
* Creates a simulated arm mechanism.
*
* @param gearbox The type and number of motors on the arm gearbox.
* @param gearing The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param moi The moment of inertia of the arm. This can be
* calculated from CAD software.
* @param armLength The length of the arm.
* @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 gearbox The type and number of motors on the arm gearbox.
* @param gearing The gear ratio of the arm (numbers greater than 1 represent
* reductions).
* @param moi The moment of inertia of the arm. This can be calculated from
* CAD software.
* @param armLength The length of the arm.
* @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(