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

[wpilib] Add noise to Differential Drive simulator (#2903)

Browse Source 
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
This commit is contained in:
Matt
2020-12-04 18:46:50 -08:00
committed by GitHub
parent 387f56cb7b
commit 963ad5c255
10 changed files with 253 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -36,10 +36,17 @@ class DifferentialDrivetrainSim {
* create the drivetrainPlant.
* @param wheelRadiusMeters 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]^T. 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(const LinearSystem<2, 2, 2>& plant,
units::meter_t trackWidth, DCMotor driveMotor,
double gearingRatio, units::meter_t wheelRadius);
DifferentialDrivetrainSim(
const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth,
DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius,
const std::array<double, 7>& measurementStdDevs = {});
/**
* Create a SimDrivetrain.
@@ -55,11 +62,18 @@ class DifferentialDrivetrainSim {
* @param wheelRadius The radius of the wheels on the drivetrain.
* @param trackWidth The robot's track width, 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]^T. 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(frc::DCMotor driveMotor, double gearing,
units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth);
DifferentialDrivetrainSim(
frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth,
const std::array<double, 7>& measurementStdDevs = {});
/**
* Sets the applied voltage to the drivetrain. Note that positive voltage must
@@ -86,24 +100,12 @@ class DifferentialDrivetrainSim {
*/
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;
/**
* Returns 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;
/**
* Returns the direction the robot is pointing.
*
@@ -117,6 +119,38 @@ class DifferentialDrivetrainSim {
*/
Pose2d GetPose() const;
/**
* Get the right encoder position in meters.
* @return The encoder position.
*/
units::meter_t GetRightPosition() const {
return units::meter_t{GetOutput(State::kRightPosition)};
}
/**
* Get the right encoder velocity in meters per second.
* @return The encoder velocity.
*/
units::meters_per_second_t GetRightVelocity() const {
return units::meters_per_second_t{GetOutput(State::kRightVelocity)};
}
/**
* Get the left encoder position in meters.
* @return The encoder position.
*/
units::meter_t GetLeftPosition() const {
return units::meter_t{GetOutput(State::kLeftPosition)};
}
/**
* Get the left encoder velocity in meters per second.
* @return The encoder velocity.
*/
units::meters_per_second_t GetLeftVelocity() const {
return units::meters_per_second_t{GetOutput(State::kLeftVelocity)};
}
/**
* Returns the currently drawn current.
*/
@@ -189,10 +223,16 @@ class DifferentialDrivetrainSim {
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]^T. 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.
*/
static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor,
double gearing,
units::meter_t wheelSize) {
static DifferentialDrivetrainSim CreateKitbotSim(
frc::DCMotor motor, double gearing, units::meter_t wheelSize,
const std::array<double, 7>& measurementStdDevs = {}) {
// 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) *
@@ -200,7 +240,8 @@ class DifferentialDrivetrainSim {
* (26_in / 2) * (26_in / 2);
return DifferentialDrivetrainSim{
motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in};
motor, gearing, batteryMoi + gearboxMoi, 25_kg,
wheelSize / 2.0, 26_in, measurementStdDevs};
}
/**
@@ -211,15 +252,47 @@ class DifferentialDrivetrainSim {
* @param wheelSize The wheel size.
* @param J The moment of inertia of the drivebase. This can be
* calculated using frc-characterization.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]^T. 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.
*/
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};
units::kilogram_square_meter_t J,
const std::array<double, 7>& measurementStdDevs = {}) {
return DifferentialDrivetrainSim{
motor, gearing, J, 25_kg, wheelSize / 2.0, 26_in, measurementStdDevs};
}
private:
/**
* Returns an element of the state vector.
*
* @param output The row of the output vector.
*/
double GetOutput(int output) const;
/**
* Returns the current output vector y.
*/
Eigen::Matrix<double, 7, 1> GetOutput() const;
/**
* Returns an element of the state vector. Note that this will not include
* noise!
*
* @param output The row of the output vector.
*/
double GetState(int state) const;
/**
* Returns the current state vector x. Note that this will not include noise!
*/
Eigen::Matrix<double, 7, 1> GetState() const;
LinearSystem<2, 2, 2> m_plant;
units::meter_t m_rb;
units::meter_t m_wheelRadius;
@@ -231,5 +304,7 @@ class DifferentialDrivetrainSim {
Eigen::Matrix<double, 7, 1> m_x;
Eigen::Matrix<double, 2, 1> m_u;
Eigen::Matrix<double, 7, 1> m_y;
std::array<double, 7> m_measurementStdDevs;
};
} // namespace frc::sim

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

@@ -39,8 +39,7 @@ class LinearSystemSim {
* @param measurementStdDevs The standard deviations of the measurements.
*/
LinearSystemSim(const LinearSystem<States, Inputs, Outputs>& system,
const std::array<double, Outputs>& measurementStdDevs =
std::array<double, Outputs>{})
const std::array<double, Outputs>& measurementStdDevs = {})
: m_plant(system), m_measurementStdDevs(measurementStdDevs) {
m_x = Eigen::Matrix<double, States, 1>::Zero();
m_y = Eigen::Matrix<double, Outputs, 1>::Zero();