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

[wpilib] Add physics simulation support with state-space (#2615)

Browse Source 
This includes physics simulation support for arms/elevator models, as well as differential drivetrains.

Swerve might be added at a later date.

Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2020-09-20 09:39:52 -07:00
committed by GitHub
parent 0503225928
commit b61f08d3fa
43 changed files with 3787 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
wpilibc/src/main/native/include/frc/simulation/BatterySim.h Normal file
View File

@@ -0,0 +1,55 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <numeric>
#include <units/current.h>
#include <units/impedance.h>
#include <units/voltage.h>
namespace frc::sim {
class BatterySim {
public:
/**
* Calculate the loaded battery voltage. Use this with
* {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
* voltage, which can then be retrieved with the {@link
* RobotController#getBatteryVoltage()} method.
*
* @param nominalVoltage The nominal battery voltage. Usually 12v.
* @param resistance The forward resistance of the battery. Most batteries
* are at or below 20 milliohms.
* @param currents The currents drawn from the battery.
* @return The battery's voltage under load.
*/
static units::volt_t Calculate(
units::volt_t nominalVoltage, units::ohm_t resistance,
std::initializer_list<units::ampere_t> currents) {
return nominalVoltage -
std::accumulate(currents.begin(), currents.end(), 0_A) * resistance;
}
/**
* Calculate the loaded battery voltage. Use this with
* {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
* voltage, which can then be retrieved with the {@link
* RobotController#getBatteryVoltage()} method. This function assumes a
* nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms)
*
* @param currents The currents drawn from the battery.
* @return The battery's voltage under load.
*/
static units::volt_t Calculate(
std::initializer_list<units::ampere_t> currents) {
return Calculate(12_V, 0.02_Ohm, currents);
}
};
} // namespace frc::sim

218
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h Normal file
View File

@@ -0,0 +1,218 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <Eigen/Core>
#include <units/length.h>
#include <units/moment_of_inertia.h>
#include <units/time.h>
#include <units/voltage.h>
namespace frc::sim {
class DifferentialDrivetrainSim {
public:
/**
* Create a SimDrivetrain.
*
* @param drivetrainPlant The LinearSystem representing the robot's
* drivetrain. This system can be created with
* LinearSystemId#createDrivetrainVelocitySystem or
* LinearSystemId#identifyDrivetrainSystem.
* @param trackWidth The robot's track width.
* @param driveMotor A {@link 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 drivetrainPlant.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain, in
* meters.
*/
DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant,
units::meter_t trackWidth, DCMotor driveMotor,
double gearingRatio, units::meter_t wheelRadius);
/**
* Create a SimDrivetrain.
*
* @param driveMotor A {@link 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 drivetrainPlant.
* @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 track width, or distance between left and
* right wheels.
*/
DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing,
units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth);
/**
* Set the applied voltage to the drivetrain. Note that positive voltage must
* make that side of the drivetrain travel forward (+X).
*
* @param leftVoltage The left voltage.
* @param rightVoltage The right voltage.
*/
void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage);
/**
* Set the gearing reduction on the drivetrain. This is commonly used for
* shifting drivetrains.
*
* @param newGearing The new gear ratio, as output over input.
*/
void SetGearing(double newGearing);
/**
* Updates the simulation.
*
* @param dt The time that's passed since the last {@link #update(double)}
* call.
*/
void Update(units::second_t dt);
/**
* Returns an element of the state vector.
*
* @param state The row of the state vector.
*/
double GetState(int state) const;
/**
* Get the current gearing reduction of the drivetrain, as output over input.
*/
double GetGearing() const;
/**
* Returns the current state vector x.
*/
Eigen::Matrix<double, 7, 1> GetState() const;
/**
* Get the estimated direction the robot is pointing. Note that this angle is
* counterclockwise-positive, while most gyros are clockwise positive.
*/
Rotation2d GetHeading() const;
/**
* Returns the current estimated position.
*/
Pose2d GetEstimatedPosition() const;
/**
* Returns the currently drawn current.
*/
units::ampere_t GetCurrentDraw() const;
Eigen::Matrix<double, 7, 1> Dynamics(const Eigen::Matrix<double, 7, 1>& x,
const Eigen::Matrix<double, 2, 1>& u);
class State {
public:
static constexpr int kX = 0;
static constexpr int kY = 1;
static constexpr int kHeading = 2;
static constexpr int kLeftVelocity = 3;
static constexpr int kRightVelocity = 4;
static constexpr int kLeftPosition = 5;
static constexpr int kRightPosition = 6;
};
/**
* Represents a gearing option of the Toughbox mini.
* 12.75:1 -- 14:50 and 14:50
* 10.71:1 -- 14:50 and 16:48
* 8.45:1 -- 14:50 and 19:45
* 7.31:1 -- 14:50 and 21:43
* 5.95:1 -- 14:50 and 24:40
*/
class KitbotGearing {
public:
static constexpr double k12p75 = 12.75;
static constexpr double k10p71 = 10.71;
static constexpr double k8p45 = 8.45;
static constexpr double k7p31 = 7.31;
static constexpr double k5p95 = 5.95;
};
class KitbotMotor {
public:
static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1);
static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2);
static constexpr frc::DCMotor SingleMiniCIMPerSide =
frc::DCMotor::MiniCIM(1);
static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
};
class KitbotWheelSize {
public:
static constexpr units::meter_t kSixInch = 6_in;
static constexpr units::meter_t kEightInch = 8_in;
static constexpr units::meter_t kTenInch = 10_in;
};
/**
* Create a sim for the standard FRC kitbot.
*
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
*/
static DifferentialDrivetrainSim createKitbotSim(frc::DCMotor motor,
double gearing,
units::meter_t wheelSize) {
// MOI estimation -- note that I = m r^2 for point masses
units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
2 // CIM plus toughbox per side
* (26_in / 2) * (26_in / 2);
return DifferentialDrivetrainSim{
motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in};
}
/**
* Create a sim for the standard FRC kitbot.
*
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param J The moment of inertia of the drivebase. This can be
* calculated using frc-characterization.
*/
static DifferentialDrivetrainSim createKitbotSim(
frc::DCMotor motor, double gearing, units::meter_t wheelSize,
units::kilogram_square_meter_t J) {
return DifferentialDrivetrainSim{motor, gearing, J,
25_kg, wheelSize / 2.0, 26_in};
}
private:
const LinearSystem<2, 2, 2>& m_plant;
units::meter_t m_rb;
units::meter_t m_wheelRadius;
DCMotor m_motor;
double m_originalGearing;
double m_currentGearing;
Eigen::Matrix<double, 7, 1> m_x;
Eigen::Matrix<double, 2, 1> m_u;
};
} // namespace frc::sim

103
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h Normal file
View File

@@ -0,0 +1,103 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include <units/length.h>
#include <units/mass.h>
#include <units/velocity.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/plant/DCMotor.h"
namespace frc::sim {
/**
* Represents a simulated elevator mechanism.
*/
class ElevatorSim : public LinearSystemSim<2, 1, 1> {
public:
/**
* 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 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.
*/
ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass,
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});
/**
* Returns whether the elevator has hit the lower limit.
*
* @param x The current elevator state.
* @return Whether the elevator has hit the lower limit.
*/
bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
/**
* Returns whether the elevator has hit the upper limit.
*
* @param x The current elevator state.
* @return Whether the elevator has hit the uppwer limit.
*/
bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
/**
* Returns the position of the elevator.
*
* @return The position of the elevator.
*/
units::meter_t GetPosition() const;
/**
* Returns the velocity of the elevator.
*
* @return The velocity of the elevator.
*/
units::meters_per_second_t GetVelocity() const;
/**
* Returns the elevator current draw.
*
* @return The elevator current draw.
*/
units::ampere_t GetCurrentDraw() const override;
protected:
/**
* 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.
*/
Eigen::Matrix<double, 2, 1> UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
private:
DCMotor m_motor;
units::meter_t m_drumRadius;
units::meter_t m_minHeight;
units::meter_t m_maxHeight;
double m_gearing;
};
} // namespace frc::sim

73
wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h Normal file
View File

@@ -0,0 +1,73 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/angular_velocity.h>
#include <units/moment_of_inertia.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
namespace frc::sim {
/**
* Represents a simulated flywheel mechanism.
*/
class FlywheelSim : public LinearSystemSim<1, 1, 1> {
public:
/**
* Creates a simulated flywhel mechanism.
*
* @param plant The linear system representing the flywheel.
* @param gearbox The type of and number of motors in the flywheel
* 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,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Creates a simulated flywhel mechanism.
*
* @param gearbox The type of and number of motors in the flywheel
* gearbox.
* @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,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Returns the flywheel velocity.
*
* @return The flywheel velocity.
*/
units::radians_per_second_t GetAngularVelocity() const;
/**
* Returns the flywheel current draw.
*
* @return The flywheel current draw.
*/
units::ampere_t GetCurrentDraw() const override;
private:
DCMotor m_motor;
double m_gearing;
};
} // namespace frc::sim

136
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h Normal file
View File

@@ -0,0 +1,136 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include <Eigen/Core>
#include <units/current.h>
#include <units/time.h>
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
namespace frc::sim {
/**
* Represents a simulated generic linear system.
*/
template <int States, int Inputs, int Outputs>
class LinearSystemSim {
public:
/**
* 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.
*/
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) {
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.
*
* @param dt The time between updates.
*/
void Update(units::second_t dt) {
// Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
// Bu_k
m_x = UpdateX(m_x, m_u, dt);
// 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);
}
}
/**
* Sets the system inputs (usually voltages).
*
* @param u The system inputs.
*/
void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
/**
* Resets the system state.
*
* @param state The state to reset to.
*/
void ResetState(const Eigen::Matrix<double, States, 1>& state) {
m_x = state;
}
protected:
/**
* Updates the state estimate of the system.
*
* @param currentXhat The current state estimate.
* @param u The system inputs (usually voltage).
* @param dt The time difference between controller updates.
*/
virtual Eigen::Matrix<double, States, 1> UpdateX(
const Eigen::Matrix<double, States, 1>& currentXhat,
const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
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;
Eigen::Matrix<double, Inputs, 1> m_u;
std::array<double, Outputs> m_measurementStdDevs;
};
} // namespace frc::sim

139
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h Normal file
View File

@@ -0,0 +1,139 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include <units/angle.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/moment_of_inertia.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/plant/DCMotor.h"
namespace frc::sim {
/**
* Represents a simulated arm mechanism.
*/
class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
public:
/**
* 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
* 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.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor,
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);
/**
* 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
* 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.
*/
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);
/**
* 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 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.
*/
SingleJointedArmSim(const DCMotor& motor, 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);
/**
* Returns whether the arm has hit the lower limit.
*
* @param x The current arm state.
* @return Whether the arm has hit the lower limit.
*/
bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
/**
* Returns whether the arm has hit the upper limit.
*
* @param x The current arm state.
* @return Whether the arm has hit the upper limit.
*/
bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
/**
* Returns the current arm angle.
*
* @return The current arm angle.
*/
units::radian_t GetAngle() const;
/**
* Returns the current arm velocity.
*
* @return The current arm velocity.
*/
units::radians_per_second_t GetVelocity() const;
/**
* Updates the state estimate of the arm.
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Eigen::Matrix<double, 2, 1> UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
units::ampere_t GetCurrentDraw() const override;
private:
units::meter_t m_r;
units::radian_t m_minAngle;
units::radian_t m_maxAngle;
units::kilogram_t m_mass;
const DCMotor m_motor;
double m_gearing;
};
} // namespace frc::sim