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

121
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/simulation/DifferentialDrivetrainSim.h"
#include <frc/system/plant/LinearSystemId.h>
#include "frc/system/RungeKutta.h"
using namespace frc;
using namespace frc::sim;
DifferentialDrivetrainSim::DifferentialDrivetrainSim(
const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth,
DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius)
: m_plant(plant),
m_rb(trackWidth / 2.0),
m_wheelRadius(wheelRadius),
m_motor(driveMotor),
m_originalGearing(gearRatio),
m_currentGearing(gearRatio) {
m_x.setZero();
m_u.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)
: DifferentialDrivetrainSim(
frc::LinearSystemId::DrivetrainVelocitySystem(
driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
trackWidth, driveMotor, gearing, wheelRadius) {}
void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
units::volt_t rightVoltage) {
m_u << leftVoltage.to<double>(), rightVoltage.to<double>();
}
void DifferentialDrivetrainSim::SetGearing(double newGearing) {
m_currentGearing = 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);
}
double DifferentialDrivetrainSim::GetGearing() const {
return m_currentGearing;
}
Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
return m_x;
}
Rotation2d DifferentialDrivetrainSim::GetHeading() const {
return Rotation2d(units::radian_t(m_x(State::kHeading)));
}
Pose2d DifferentialDrivetrainSim::GetEstimatedPosition() 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))));
}
units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
auto loadIleft =
m_motor.Current(units::radians_per_second_t(m_x(State::kLeftVelocity) *
m_currentGearing /
m_wheelRadius.to<double>()),
units::volt_t(m_u(0))) *
wpi::sgn(m_u(0));
auto loadIRight =
m_motor.Current(units::radians_per_second_t(m_x(State::kRightVelocity) *
m_currentGearing /
m_wheelRadius.to<double>()),
units::volt_t(m_u(1))) *
wpi::sgn(m_u(1));
return loadIleft + loadIRight;
}
Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
const Eigen::Matrix<double, 7, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
// Because G^2 can be factored out of A, we can divide by the old ratio
// squared and multiply by the new ratio squared to get a new drivetrain
// model.
Eigen::Matrix<double, 4, 2> B;
B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
m_originalGearing / m_originalGearing;
B.block<2, 2>(2, 0).setZero();
// Because G can be factored out of B, we can divide by the old ratio and
// multiply by the new ratio to get a new drivetrain model.
Eigen::Matrix<double, 4, 4> A;
A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
A.block<2, 2>(2, 0).setIdentity();
A.block<4, 2>(0, 2).setZero();
double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
Eigen::Matrix<double, 7, 1> xdot;
xdot(0) = v * std::cos(x(State::kHeading));
xdot(1) = v * std::sin(x(State::kHeading));
xdot(2) =
((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb))
.to<double>();
xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u;
return xdot;
}

79
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp Normal file
View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/simulation/ElevatorSim.h"
#include <wpi/MathExtras.h>
#include "frc/StateSpaceUtil.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
using namespace frc::sim;
ElevatorSim::ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass,
double gearing, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool addNoise,
const std::array<double, 1>& m_measurementStdDevs)
: LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
addNoise, m_measurementStdDevs),
m_motor(gearbox),
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing) {}
bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) < m_minHeight.to<double>();
}
bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxHeight.to<double>();
}
units::meter_t ElevatorSim::GetPosition() const { return units::meter_t{Y(0)}; }
units::meters_per_second_t ElevatorSim::GetVelocity() const {
return units::meters_per_second_t{m_x(1)};
}
units::ampere_t ElevatorSim::GetCurrentDraw() const {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output.
// v = r w, so w = v / r
units::meters_per_second_t velocity{m_x(1)};
units::radians_per_second_t motorVelocity =
velocity / m_drumRadius * m_gearing * 1_rad;
// Perform calculation and return.
return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}
Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
auto updatedXhat = RungeKutta(
[&](const Eigen::Matrix<double, 2, 1>& x,
const Eigen::Matrix<double, 1, 1>& u_)
-> Eigen::Matrix<double, 2, 1> {
return m_plant.A() * x + m_plant.B() * u_ + MakeMatrix<2, 1>(0.0, -9.8);
},
currentXhat, u, dt);
// Check for collision after updating x-hat.
if (HasHitLowerLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_minHeight.to<double>(), 0.0);
} else if (HasHitUpperLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
}
return updatedXhat;
}

41
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/simulation/FlywheelSim.h"
#include <wpi/MathExtras.h>
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
using namespace frc::sim;
FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant,
const DCMotor& gearbox, double gearing, bool addNoise,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<1, 1, 1>(plant, addNoise, measurementStdDevs),
m_motor(gearbox),
m_gearing(gearing) {}
FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi, bool addNoise,
const std::array<double, 1>& measurementStdDevs)
: FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
gearbox, gearing, addNoise, measurementStdDevs) {}
units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
return units::radians_per_second_t{Y(0)};
}
units::ampere_t FlywheelSim::GetCurrentDraw() const {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output.
return m_motor.Current(GetAngularVelocity() * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

109
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp Normal file
View File

@@ -0,0 +1,109 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/simulation/SingleJointedArmSim.h"
#include <cmath>
#include <units/voltage.h>
#include <wpi/MathExtras.h>
#include "frc/system/RungeKutta.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
using namespace frc::sim;
SingleJointedArmSim::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)
: LinearSystemSim<2, 1, 1>(system, addNoise, measurementStdDevs),
m_r(armLength),
m_minAngle(minAngle),
m_maxAngle(maxAngle),
m_mass(mass),
m_motor(motor),
m_gearing(G) {}
SingleJointedArmSim::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)
: SingleJointedArmSim(LinearSystemId::SingleJointedArmSystem(motor, J, G),
motor, G, mass, armLength, minAngle, maxAngle,
addNoise, measurementStdDevs) {}
SingleJointedArmSim::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)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(
motor, 1.0 / 3.0 * mass * armLength * armLength, G),
motor, G, mass, armLength, minAngle, maxAngle, addNoise,
measurementStdDevs) {}
bool SingleJointedArmSim::HasHitLowerLimit(
const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) < m_minAngle.to<double>();
}
bool SingleJointedArmSim::HasHitUpperLimit(
const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxAngle.to<double>();
}
units::radian_t SingleJointedArmSim::GetAngle() const {
return units::radian_t{m_x(0)};
}
units::radians_per_second_t SingleJointedArmSim::GetVelocity() const {
return units::radians_per_second_t{m_x(1)};
}
Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
// Horizontal case:
// Torque = F * r = I * alpha
// alpha = F * r / I
// Since F = mg,
// alpha = m * g * r / I
// Finally, multiply RHS by cos(theta) to account for the arm angle
// This acceleration is added to the linear system dynamics x-dot = Ax + Bu
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// std::cos(theta)]]
auto updatedXhat = RungeKutta(
[&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
return m_plant.A() * x + m_plant.B() * u +
MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 /
(m_mass * m_r * m_r) * std::cos(x(0)))
.template to<double>());
},
currentXhat, u, dt);
// Check for collisions.
if (HasHitLowerLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_minAngle.to<double>(), 0.0);
} else if (HasHitUpperLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 0.0);
}
return updatedXhat;
}
units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output
units::radians_per_second_t motorVelocity{m_x(1) * m_gearing};
return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

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