Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Clean up physics simulation class APIs (#2763)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-10-16 00:00:45 -04:00
committed by GitHub
parent 8f3e5794b3
commit 061432147d
19 changed files with 746 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

@@ -25,24 +25,40 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
/**
* Constructs a simulated elevator mechanism.
*
* @param gearbox The type of and number of motors in your elevator
* gearbox.
* @param carriageMass The mass of the elevator carriage.
* @param gearing The gearing of the elevator (numbers greater than
* 1 represent reductions).
* @param drumRadius The radius of the drum that your cable is wrapped
* around.
* @param plant The linear system that represents 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 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 addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass,
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 addNoise = false,
const std::array<double, 1>& m_measurementStdDevs = {0.0});
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* 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 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,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Returns whether the elevator has hit the lower limit.
@@ -56,7 +72,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* Returns whether the elevator has hit the upper limit.
*
* @param x The current elevator state.
* @return Whether the elevator has hit the uppwer limit.
* @return Whether the elevator has hit the upper limit.
*/
bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
@@ -81,6 +97,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
*/
units::ampere_t GetCurrentDraw() const override;
/**
* Sets the input voltage for the elevator.
*
* @param voltage The input voltage.
*/
void SetInputVoltage(units::volt_t voltage);
protected:
/**
* Updates the state estimate of the elevator.
@@ -94,7 +117,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
private:
DCMotor m_motor;
DCMotor m_gearbox;
units::meter_t m_drumRadius;
units::meter_t m_minHeight;
units::meter_t m_maxHeight;

17
wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
View File

@@ -28,12 +28,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
* gearbox.
* @param gearing The gearing of the flywheel (numbers greater than
* 1 represent reductions).
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox,
double gearing, bool addNoise = false,
double gearing,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
@@ -44,12 +42,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
* @param gearing The gearing of the flywheel (numbers greater than
* 1 represent reductions).
* @param moi The moment of inertia of the flywheel.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
FlywheelSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi, bool addNoise = false,
units::kilogram_square_meter_t moi,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
@@ -66,8 +62,15 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
*/
units::ampere_t GetCurrentDraw() const override;
/**
* Sets the input voltage for the flywheel.
*
* @param voltage The input voltage.
*/
void SetInputVoltage(units::volt_t voltage);
private:
DCMotor m_motor;
DCMotor m_gearbox;
double m_gearing;
};
} // namespace frc::sim

103
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
View File

@@ -18,7 +18,16 @@
namespace frc::sim {
/**
* Represents a simulated generic linear system.
* This class helps simulate linear systems. To use this class, do the following
* in the simulationPeriodic() method.
*
* Call the SetInput() method with the inputs to your system (generally
* voltage). Call the Update() method to update the simulation. Set simulated
* sensor readings with the simulated positions in the GetOutput() method.
*
* @tparam States The number of states of the system.
* @tparam Inputs The number of inputs to the system.
* @tparam Outputs The number of outputs of the system.
*/
template <int States, int Inputs, int Outputs>
class LinearSystemSim {
@@ -27,53 +36,19 @@ class LinearSystemSim {
* Creates a simulated generic linear system.
*
* @param system The system to simulate.
* @param addNoise Whether the sim should automatically add some
* measurement noise.
* @param measurementStdDevs The standard deviations of the measurement noise.
* @param measurementStdDevs The standard deviations of the measurements.
*/
LinearSystemSim(const LinearSystem<States, Inputs, Outputs>& system,
bool addNoise = false,
const std::array<double, Outputs>& measurementStdDevs = {0.0})
: m_plant(system),
m_shouldAddNoise(addNoise),
m_measurementStdDevs(measurementStdDevs) {
const std::array<double, Outputs>& measurementStdDevs =
std::array<double, Outputs>{})
: m_plant(system), m_measurementStdDevs(measurementStdDevs) {
m_x = Eigen::Matrix<double, States, 1>::Zero();
m_y = Eigen::Matrix<double, Outputs, 1>::Zero();
m_u = Eigen::Matrix<double, Inputs, 1>::Zero();
}
/**
* Returns whether the sim should add noise to the measurements.
*
* @return Whether the sim should add noise to the measurements.
*/
bool ShouldAddNoise() const { return m_shouldAddNoise; }
/**
* Returns the current output of the plant.
*
* @return The current output of the plant.
*/
const Eigen::Matrix<double, Outputs, 1>& Y() const { return m_y; }
/**
* Returns an element of the current output of the plant.
*
* @param row The row to return.
*/
double Y(int i) const { return m_y(i); }
/**
* Sets whether the sim should add noise to measurements.
*
* @param shouldAddNoise Whether the sim should add noise to measurements.
*/
void SetShouldAddNoise(bool shouldAddNoise) {
m_shouldAddNoise = shouldAddNoise;
}
/**
* Updates the linear system sim.
* Updates the simulation.
*
* @param dt The time between updates.
*/
@@ -85,12 +60,27 @@ class LinearSystemSim {
// y = Cx + Du
m_y = m_plant.CalculateY(m_x, m_u);
// Add noise if needed.
if (m_shouldAddNoise) {
m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
}
// Add noise. If the user did not pass a noise vector to the
// constructor, then this method will not do anything because
// the standard deviations default to zero.
m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
}
/**
* Returns the current output of the plant.
*
* @return The current output of the plant.
*/
const Eigen::Matrix<double, Outputs, 1>& GetOutput() const { return m_y; }
/**
* Returns an element of the current output of the plant.
*
* @param row The row to return.
* @return An element of the current output of the plant.
*/
double GetOutput(int row) const { return m_y(row); }
/**
* Sets the system inputs (usually voltages).
*
@@ -98,13 +88,31 @@ class LinearSystemSim {
*/
void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
/*
* Sets the system inputs.
*
* @param row The row in the input matrix to set.
* @param value The value to set the row to.
*/
void SetInput(int row, double value) { m_u(row, 0) = value; }
/**
* Sets the system state.
*
* @param state The state.
* @param state The new state.
*/
void SetState(const Eigen::Matrix<double, States, 1>& state) { m_x = state; }
/**
* Returns the current drawn by this simulated system. Override this method to
* add a custom current calculation.
*
* @return The current drawn by this simulated mechanism.
*/
virtual units::ampere_t GetCurrentDraw() const {
return units::ampere_t(0.0);
}
protected:
/**
* Updates the state estimate of the system.
@@ -119,12 +127,7 @@ class LinearSystemSim {
return m_plant.CalculateX(currentXhat, u, dt);
}
virtual units::ampere_t GetCurrentDraw() const {
return units::ampere_t(0.0);
}
LinearSystem<States, Inputs, Outputs> m_plant;
bool m_shouldAddNoise;
Eigen::Matrix<double, States, 1> m_x;
Eigen::Matrix<double, Outputs, 1> m_y;

102
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -27,69 +27,43 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* Creates a simulated arm mechanism.
*
* @param system The system representing this arm.
* @param motor The type and number of motors on the arm gearbox.
* @param G The gear ratio of the arm (numbers greater than 1
* @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 mass The mass of the arm.
* @param armLength The length of the arm.
* @param minAngle The minimum allowed angle for the arm.
* @param maxAngle The maximum allowed angle for the arm.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
* @param simulateGravity If the affects of gravity should be simulated.
* @param minAngle The minimum angle that the arm is capable of.
* @param maxAngle The maximum angle that the arm is capable of.
* @param mass The mass of the arm.
* @param measurementStdDevs The standard deviations of the measurements.
* @param simulateGravity Whether gravity should be simulated or not.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor,
double G, units::kilogram_t mass,
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 addNoise,
const std::array<double, 1>& measurementStdDevs,
bool simulateGravity);
units::radian_t maxAngle, units::kilogram_t mass,
bool simulateGravity,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Creates a simulated arm mechanism.
*
* @param motor The type and number of motors on the arm gearbox.
* @param J The moment of inertia of the arm.
* @param G The gear ratio of the arm (numbers greater than 1
* @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 mass The mass of the arm.
* @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 allowed angle for the arm.
* @param maxAngle The maximum allowed angle for the arm.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
* @param simulateGravity If the affects of gravity should be simulated.
*/
SingleJointedArmSim(const DCMotor& motor, units::kilogram_square_meter_t J,
double G, units::kilogram_t mass,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs,
bool simulateGravity);
/**
* Creates a simulated arm mechanism.
*
* @param motor The type and number of motors on the arm gearbox.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param minAngle The minimum angle that the arm is capable of.
* @param maxAngle The maximum angle that the arm is capable of.
* @param mass The mass of the arm.
* @param armLength The length of the arm.
* @param minAngle The minimum allowed angle for the arm. This is
* measured from horizontal, with straight out being 0.
* @param maxAngle The maximum allowed angle for the arm. This is
* measured from horizontal, with straight out being 0.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
* @param simulateGravity If the affects of gravity should be simulated.
* @param simulateGravity Whether gravity should be simulated or not.
*/
SingleJointedArmSim(const DCMotor& motor, double G, units::kilogram_t mass,
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 addNoise,
const std::array<double, 1>& measurementStdDevs,
bool simulateGravity);
units::radian_t maxAngle, units::kilogram_t mass,
bool simulateGravity,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Returns whether the arm has hit the lower limit.
@@ -121,8 +95,34 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
*/
units::radians_per_second_t GetVelocity() const;
/**
* Returns the arm current draw.
*
* @return The arm current draw.
*/
units::ampere_t GetCurrentDraw() const override;
/**
* Sets the input voltage for the elevator.
*
* @param voltage The input voltage.
*/
void SetInputVoltage(units::volt_t voltage);
/**
* Calculates a rough estimate of the moment of inertia of an arm given its
* length and mass.
*
* @param length The length of the arm.
* @param mass The mass of the arm.
*
* @return The calculated moment of inertia.
*/
static constexpr units::kilogram_square_meter_t EstimateMOI(
units::meter_t length, units::kilogram_t mass) {
return 1.0 / 3.0 * mass * length * length;
}
protected:
/**
* Updates the state estimate of the arm.
@@ -140,7 +140,7 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
units::radian_t m_minAngle;
units::radian_t m_maxAngle;
units::kilogram_t m_mass;
const DCMotor m_motor;
const DCMotor m_gearbox;
double m_gearing;
bool m_simulateGravity;
};