Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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

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

@@ -16,25 +16,28 @@ using namespace frc::sim;
DifferentialDrivetrainSim::DifferentialDrivetrainSim(
const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth,
DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius)
DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius,
const std::array<double, 7>& measurementStdDevs)
: m_plant(plant),
m_rb(trackWidth / 2.0),
m_wheelRadius(wheelRadius),
m_motor(driveMotor),
m_originalGearing(gearRatio),
m_currentGearing(gearRatio) {
m_currentGearing(gearRatio),
m_measurementStdDevs(measurementStdDevs) {
m_x.setZero();
m_u.setZero();
m_y.setZero();
}
DifferentialDrivetrainSim::DifferentialDrivetrainSim(
frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth)
units::meter_t trackWidth, const std::array<double, 7>& measurementStdDevs)
: DifferentialDrivetrainSim(
frc::LinearSystemId::DrivetrainVelocitySystem(
driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
trackWidth, driveMotor, gearing, wheelRadius) {}
trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
units::volt_t rightVoltage) {
@@ -48,27 +51,36 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
void DifferentialDrivetrainSim::Update(units::second_t dt) {
m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
m_u, dt);
}
double DifferentialDrivetrainSim::GetState(int state) const {
return m_x(state);
m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
}
double DifferentialDrivetrainSim::GetGearing() const {
return m_currentGearing;
}
Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetOutput() const {
return m_y;
}
Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
return m_x;
}
double DifferentialDrivetrainSim::GetOutput(int output) const {
return m_y(output);
}
double DifferentialDrivetrainSim::GetState(int state) const {
return m_x(state);
}
Rotation2d DifferentialDrivetrainSim::GetHeading() const {
return Rotation2d(units::radian_t(m_x(State::kHeading)));
return Rotation2d(units::radian_t(GetOutput(State::kHeading)));
}
Pose2d DifferentialDrivetrainSim::GetPose() const {
return Pose2d(units::meter_t(m_x(State::kX)), units::meter_t(m_x(State::kY)),
Rotation2d(units::radian_t(m_x(State::kHeading))));
return Pose2d(units::meter_t(GetOutput(State::kX)),
units::meter_t(GetOutput(State::kY)), GetHeading());
}
units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {

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();

11
wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
View File

@@ -26,7 +26,9 @@ TEST(DifferentialDriveSim, Convergence) {
motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
frc::DifferentialDriveKinematics kinematics{24_in};
frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
frc::sim::DifferentialDrivetrainSim sim{
plant, 24_in, motor,
1.0, 2_in, {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
frc::RamseteController ramsete;
@@ -64,9 +66,10 @@ TEST(DifferentialDriveSim, Convergence) {
groundTruthX, voltages, 20_ms);
}
EXPECT_NEAR(groundTruthX(0, 0), sim.GetState(0), 1E-6);
EXPECT_NEAR(groundTruthX(1, 0), sim.GetState(1), 1E-6);
EXPECT_NEAR(groundTruthX(2, 0), sim.GetState(2), 1E-6);
EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().to<double>(), 0.01);
EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().to<double>(), 0.01);
EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().to<double>(),
0.01);
}
TEST(DifferentialDriveSim, Current) {

16
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -42,14 +42,14 @@ void DriveSubsystem::SimulationPeriodic() {
frc::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
frc::sim::DifferentialDrivetrainSim::State::kLeftPosition));
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState(
frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity));
m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
frc::sim::DifferentialDrivetrainSim::State::kRightPosition));
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState(
frc::sim::DifferentialDrivetrainSim::State::kRightVelocity));
m_leftEncoderSim.SetDistance(
m_drivetrainSimulator.GetLeftPosition().to<double>());
m_leftEncoderSim.SetRate(
m_drivetrainSimulator.GetLeftVelocity().to<double>());
m_rightEncoderSim.SetDistance(
m_drivetrainSimulator.GetRightPosition().to<double>());
m_rightEncoderSim.SetRate(
m_drivetrainSimulator.GetRightVelocity().to<double>());
m_gyroAngleSim.SetAngle(
-m_drivetrainSimulator.GetHeading().Degrees().to<double>());

9
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
View File

@@ -161,9 +161,12 @@ class DriveSubsystem : public frc2::SubsystemBase {
// These classes help simulate our drivetrain.
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
DriveConstants::kDrivetrainPlant, DriveConstants::kTrackwidth,
DriveConstants::kDrivetrainGearbox, DriveConstants::kDrivetrainGearing,
DriveConstants::kWheelDiameter / 2};
DriveConstants::kDrivetrainPlant,
DriveConstants::kTrackwidth,
DriveConstants::kDrivetrainGearbox,
DriveConstants::kDrivetrainGearing,
DriveConstants::kWheelDiameter / 2,
{0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};

126
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
@@ -29,25 +30,26 @@ import edu.wpi.first.wpiutil.math.numbers.N7;
*
* <p>Our state-space system is:
*
* <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l, voltError_r, headingError]]^T
* <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]^T
* in the field coordinate system (dist_* are wheel distances.)
*
* <p>u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep
* from a LTVDiffDriveController.
*
* <p>y = [[x, y, theta]]^T from a global measurement source(ex. vision),
* or y = [[dist_l, dist_r, theta]] from encoders and gyro.
*
* <p>y = x
*/
public class DifferentialDrivetrainSim {
private final DCMotor m_motor;
private final double m_originalGearing;
private final Matrix<N7, N1> m_measurementStdDevs;
private double m_currentGearing;
private final double m_wheelRadiusMeters;
@SuppressWarnings("MemberName")
private Matrix<N2, N1> m_u;
@SuppressWarnings("MemberName")
private Matrix<N7, N1> m_x;
@SuppressWarnings("MemberName")
private Matrix<N7, N1> m_y;
private final double m_rb;
private final LinearSystem<N2, N2, N2> m_plant;
@@ -55,21 +57,27 @@ public class DifferentialDrivetrainSim {
/**
* 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 jKgMetersSquared The moment of inertia of the drivetrain about its center.
* @param massKg The mass of the drivebase.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain.
* @param trackWidthMeters The robot's track width, or distance between left and right wheels.
* @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 jKgMetersSquared The moment of inertia of the drivetrain about its center.
* @param massKg The mass of the drivebase.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain.
* @param trackWidthMeters 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 null 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.
*/
public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing,
double jKgMetersSquared, double massKg,
double wheelRadiusMeters, double trackWidthMeters) {
double wheelRadiusMeters, double trackWidthMeters,
Matrix<N7, N1> measurementStdDevs) {
this(LinearSystemId.createDrivetrainVelocitySystem(driveMotor, massKg, wheelRadiusMeters,
trackWidthMeters / 2.0, jKgMetersSquared, gearing),
driveMotor, gearing, trackWidthMeters, wheelRadiusMeters);
driveMotor, gearing, trackWidthMeters, wheelRadiusMeters, measurementStdDevs);
}
/**
@@ -85,20 +93,27 @@ public class DifferentialDrivetrainSim {
* @param trackWidthMeters The distance between the two sides of the drivetrian. Can be
* found with frc-characterization.
* @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 null 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.
*/
public DifferentialDrivetrainSim(LinearSystem<N2, N2, N2> drivetrainPlant,
DCMotor driveMotor, double gearing,
double trackWidthMeters,
double wheelRadiusMeters) {
double trackWidthMeters, double wheelRadiusMeters,
Matrix<N7, N1> measurementStdDevs) {
this.m_plant = drivetrainPlant;
this.m_rb = trackWidthMeters / 2.0;
this.m_motor = driveMotor;
this.m_originalGearing = gearing;
this.m_measurementStdDevs = measurementStdDevs;
m_wheelRadiusMeters = wheelRadiusMeters;
m_currentGearing = m_originalGearing;
m_x = new Matrix<>(Nat.N7(), Nat.N1());
m_u = VecBuilder.fill(0, 0);
m_y = new Matrix<>(Nat.N7(), Nat.N1());
}
/**
@@ -117,19 +132,27 @@ public class DifferentialDrivetrainSim {
// Update state estimate with RK4
m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds);
}
public double getState(State state) {
return m_x.get(state.value, 0);
m_y = m_x;
if (m_measurementStdDevs != null) {
m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
}
}
/**
* Returns the full simulated state of the drivetrain.
*/
public Matrix<N7, N1> getState() {
Matrix<N7, N1> getState() {
return m_x;
}
double getState(State state) {
return m_x.get(state.value, 0);
}
private double getOutput(State output) {
return m_y.get(output.value, 0);
}
/**
* Returns the direction the robot is pointing.
*
@@ -137,16 +160,47 @@ public class DifferentialDrivetrainSim {
* clockwise positive.
*/
public Rotation2d getHeading() {
return new Rotation2d(getState(State.kHeading));
return new Rotation2d(getOutput(State.kHeading));
}
/**
* Returns the current pose.
*/
public Pose2d getPose() {
return new Pose2d(m_x.get(0, 0),
m_x.get(1, 0),
new Rotation2d(m_x.get(2, 0)));
return new Pose2d(getOutput(State.kX), getOutput(State.kY),
getHeading());
}
/**
* Get the right encoder position in meters.
* @return The encoder position.
*/
public double getRightPositionMeters() {
return getOutput(State.kRightPosition);
}
/**
* Get the right encoder velocity in meters per second.
* @return The encoder velocity.
*/
public double getRightVelocityMetersPerSecond() {
return getOutput(State.kRightVelocity);
}
/**
* Get the left encoder position in meters.
* @return The encoder position.
*/
public double getLeftPositionMeters() {
return getOutput(State.kLeftPosition);
}
/**
* Get the left encoder velocity in meters per second.
* @return The encoder velocity.
*/
public double getLeftVelocityMetersPerSecond() {
return getOutput(State.kLeftVelocity);
}
public double getCurrentDrawAmps() {
@@ -227,7 +281,7 @@ public class DifferentialDrivetrainSim {
return xdot;
}
public enum State {
enum State {
kX(0),
kY(1),
kHeading(2),
@@ -300,15 +354,21 @@ public 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 null 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.
*/
public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
KitbotWheelSize wheelSize) {
KitbotWheelSize wheelSize,
Matrix<N7, N1> measurementStdDevs) {
// MOI estimation -- note that I = m r^2 for point masses
var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2);
var gearboxMoi = (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */)
* Math.pow(Units.inchesToMeters(26.0 / 2.0), 2);
return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi);
return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi, measurementStdDevs);
}
/**
@@ -319,10 +379,16 @@ public class DifferentialDrivetrainSim {
* @param wheelSize The wheel size.
* @param jKgMetersSquared 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 null 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.
*/
public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
KitbotWheelSize wheelSize, double jKgMetersSquared) {
KitbotWheelSize wheelSize, double jKgMetersSquared,
Matrix<N7, N1> measurementStdDevs) {
return new DifferentialDrivetrainSim(motor.value, gearing.value, jKgMetersSquared, 25 / 2.2,
wheelSize.value / 2.0, Units.inchesToMeters(26));
wheelSize.value / 2.0, Units.inchesToMeters(26), measurementStdDevs);
}
}

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -60,7 +60,7 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
* Creates a simulated generic linear system with measurement noise.
*
* @param system The system being controlled.
* @param measurementStdDevs Standard deviations of measurements.
* @param measurementStdDevs Standard deviations of measurements. Can be null if no noise is desired.
*/
public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system,
Matrix<Outputs, N1> measurementStdDevs) {

6
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -47,7 +47,7 @@ class DifferentialDrivetrainSimTest {
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
var sim = new DifferentialDrivetrainSim(plant,
motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020);
var ramsete = new RamseteController();
@@ -99,7 +99,7 @@ class DifferentialDrivetrainSimTest {
0.5,
1.0);
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), null);
sim.setInputs(-12, -12);
for (int i = 0; i < 10; i++) {
@@ -132,7 +132,7 @@ class DifferentialDrivetrainSimTest {
5.0);
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2));
var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
sim.setInputs(2, 4);

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
View File

@@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpiutil.math.VecBuilder;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
@@ -87,7 +88,8 @@ public class DriveSubsystem extends SubsystemBase {
Constants.DriveConstants.kDriveGearbox,
Constants.DriveConstants.kDriveGearing,
Constants.DriveConstants.kTrackwidthMeters,
Constants.DriveConstants.kWheelDiameterMeters / 2.0);
Constants.DriveConstants.kWheelDiameterMeters / 2.0,
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
// The encoder and gyro angle sims let us set simulated sensor readings
m_leftEncoderSim = new EncoderSim(m_leftEncoder);
@@ -118,10 +120,10 @@ public class DriveSubsystem extends SubsystemBase {
-m_rightMotors.get() * RobotController.getBatteryVoltage());
m_drivetrainSimulator.update(0.020);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftPosition));
m_leftEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftVelocity));
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightPosition));
m_rightEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightVelocity));
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees());
m_fieldSim.setRobotPose(getPose());