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

52
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java Normal file
View File

@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.RobotController;
public final class BatterySim {
private BatterySim() {
// Utility class
}
/**
* 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 resistanceOhms 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.
*/
public static double calculateLoadedBatteryVoltage(double nominalVoltage,
double resistanceOhms,
double... currents) {
double retval = nominalVoltage;
for (var current : currents) {
retval -= current * resistanceOhms;
}
return retval;
}
/**
* 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.
*/
public static double calculateDefaultBatteryLoadedVoltage(double... currents) {
return calculateLoadedBatteryVoltage(12, 0.02, currents);
}
}

303
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java Normal file
View File

@@ -0,0 +1,303 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
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.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.wpiutil.math.numbers.N7;
/**
* This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with
* {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation,
* and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to
* visualize their robot on the Sim GUI's field.
*
* <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
* 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.
*
*/
public class DifferentialDrivetrainSim {
private final DCMotor m_motor;
private final double m_originalGearing;
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;
private final double m_rb;
private final LinearSystem<N2, N2, N2> m_plant;
/**
* 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.
*/
public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing,
double jKgMetersSquared, double massKg,
double wheelRadiusMeters, double trackWidthMeters) {
this(LinearSystemId.createDrivetrainVelocitySystem(driveMotor, massKg, wheelRadiusMeters,
trackWidthMeters / 2.0, jKgMetersSquared, gearing),
driveMotor, gearing, trackWidthMeters, wheelRadiusMeters);
}
/**
* Create a SimDrivetrain
* .
* @param drivetrainPlant The {@link LinearSystem} representing the robot's drivetrain. This
* system can be created with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, double, double, double, double, double)}
* or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, double, double)}.
* @param driveMotor A {@link DCMotor} representing the drivetrain.
* @param gearing The gearingRatio ratio of the robot, as output over input.
* This must be the same ratio as the ratio used to identify or
* create the drivetrainPlant.
* @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.
*/
public DifferentialDrivetrainSim(LinearSystem<N2, N2, N2> drivetrainPlant,
DCMotor driveMotor, double gearing,
double trackWidthMeters,
double wheelRadiusMeters) {
this.m_plant = drivetrainPlant;
this.m_rb = trackWidthMeters / 2.0;
this.m_motor = driveMotor;
this.m_originalGearing = gearing;
m_wheelRadiusMeters = wheelRadiusMeters;
m_currentGearing = m_originalGearing;
m_x = new Matrix<>(Nat.N7(), Nat.N1());
}
/**
* Set the applied voltage to the drivetrain. Note that positive voltage must make that
* side of the drivetrain travel forward (+X).
*
* @param leftVoltageVolts The left voltage.
* @param rightVoltageVolts The right voltage.
*/
public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
m_u = VecBuilder.fill(leftVoltageVolts, rightVoltageVolts);
}
@SuppressWarnings("LocalVariableName")
public void update(double dtSeconds) {
// 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);
}
/**
* Get the full simulated state of the drivetrain.
*/
public Matrix<N7, N1> getState() {
return m_x;
}
/**
* Get the estimated direction the robot is pointing. Note that this angle is
* counterclockwise-positive, while most gyros are clockwise positive.
*/
public Rotation2d getHeading() {
return new Rotation2d(getState(State.kHeading));
}
/**
* Get the estimated position of the drivetrain.
*/
public Pose2d getEstimatedPosition() {
return new Pose2d(m_x.get(0, 0),
m_x.get(1, 0),
new Rotation2d(m_x.get(2, 0)));
}
public double getCurrentDrawAmps() {
var loadIleft = m_motor.getCurrent(
getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters,
m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
var loadIright = m_motor.getCurrent(
getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters,
m_u.get(1, 0)) * Math.signum(m_u.get(1, 0));
return loadIleft + loadIright;
}
public double getCurrentGearing() {
return m_currentGearing;
}
/**
* Set the gearing reduction on the drivetrain. This is commonly used for
* shifting drivetrains.
*
* @param newGearRatio The new gear ratio, as output over input.
*/
public void setCurrentGearing(double newGearRatio) {
this.m_currentGearing = newGearRatio;
}
@SuppressWarnings({"DuplicatedCode", "LocalVariableName"})
protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) {
// 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.
var B = new Matrix<>(Nat.N4(), Nat.N2());
B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing));
// 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.
var A = new Matrix<>(Nat.N4(), Nat.N4());
A.assignBlock(0, 0, m_plant.getA().times((this.m_currentGearing * this.m_currentGearing)
/ (this.m_originalGearing * this.m_originalGearing)));
A.assignBlock(2, 0, Matrix.eye(Nat.N2()));
var v = (x.get(State.kLeftVelocity.value, 0) + x.get(State.kRightVelocity.value, 0)) / 2.0;
var xdot = new Matrix<>(Nat.N7(), Nat.N1());
xdot.set(0, 0, v * Math.cos(x.get(State.kHeading.value, 0)));
xdot.set(1, 0, v * Math.sin(x.get(State.kHeading.value, 0)));
xdot.set(2, 0, (x.get(State.kRightVelocity.value, 0)
- x.get(State.kLeftVelocity.value, 0)) / (2.0 * m_rb));
xdot.assignBlock(3, 0,
A.times(x.block(Nat.N4(), Nat.N1(), 3, 0))
.plus(B.times(u)));
return xdot;
}
public enum State {
kX(0),
kY(1),
kHeading(2),
kLeftVelocity(3),
kRightVelocity(4),
kLeftPosition(5),
kRightPosition(6);
@SuppressWarnings("MemberName")
public final int value;
State(int i) {
this.value = i;
}
}
/**
* 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
*/
public enum KitbotGearing {
k12p75(12.75),
k10p71(10.71),
k8p45(8.45),
k7p31(7.31),
k5p95(5.95);
@SuppressWarnings("MemberName")
public final double value;
KitbotGearing(double i) {
this.value = i;
}
}
public enum KitbotMotor {
kSingleCIMPerSide(DCMotor.getCIM(1)),
kDualCIMPerSide(DCMotor.getCIM(2)),
kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)),
kDualMiniCIMPerSide(DCMotor.getMiniCIM(2));
@SuppressWarnings("MemberName")
public final DCMotor value;
KitbotMotor(DCMotor i) {
this.value = i;
}
}
public enum KitbotWheelSize {
SixInch(Units.inchesToMeters(6)),
EightInch(Units.inchesToMeters(8)),
TenInch(Units.inchesToMeters(10));
@SuppressWarnings("MemberName")
public final double value;
KitbotWheelSize(double i) {
this.value = i;
}
}
/**
* 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.
*/
public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
KitbotWheelSize wheelSize) {
// 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);
}
/**
* 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 jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using
* frc-characterization.
*/
public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
KitbotWheelSize wheelSize, double jKgMetersSquared) {
return new DifferentialDrivetrainSim(motor.value, gearing.value, jKgMetersSquared, 25 / 2.2,
wheelSize.value / 2.0, Units.inchesToMeters(26));
}
}

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java 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. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
private final DCMotor m_motor;
private final double m_drumRadiusMeters;
private final double m_minHeightMeters;
private final double m_maxHeightMeters;
private final double m_gearing;
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* * the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* * the simulation and write simulated outputs to sensors.
*
* @param elevatorGearbox The {@link DCMotor} representing the elevator motor.
* @param carriageMassKg The mass of the carriage.
* @param drumRadiusMeters The radius of the drum driving the elevator.
* @param gearingReduction The reduction between motor and drum, as output over input.
* In most cases this is greater than one.
* @param m_measurementStdDevs Standard deviations for position noise. Can be null
* if no added noise is desired.
*/
public ElevatorSim(DCMotor elevatorGearbox, double carriageMassKg,
double drumRadiusMeters, double gearingReduction,
double minHeightMeters, double maxHeightMeters,
Matrix<N1, N1> m_measurementStdDevs) {
super(LinearSystemId.createElevatorSystem(elevatorGearbox, carriageMassKg, drumRadiusMeters,
gearingReduction), m_measurementStdDevs);
this.m_motor = elevatorGearbox;
this.m_gearing = gearingReduction;
this.m_drumRadiusMeters = drumRadiusMeters;
this.m_minHeightMeters = minHeightMeters;
this.m_maxHeightMeters = maxHeightMeters;
}
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param elevatorPlant The elevator system being controlled. This system can be created
* with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, double, double)}
* or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyPositionSystem(double, double)}.
* @param m_measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false.
*/
public ElevatorSim(LinearSystem<N2, N1, N1> elevatorPlant,
Matrix<N1, N1> m_measurementStdDevs, DCMotor elevatorGearbox,
double gearingReduction, double drumRadiusMeters,
double minHeightMeters, double maxHeightMeters) {
super(elevatorPlant, m_measurementStdDevs);
this.m_motor = elevatorGearbox;
this.m_gearing = gearingReduction;
this.m_drumRadiusMeters = drumRadiusMeters;
this.m_minHeightMeters = minHeightMeters;
this.m_maxHeightMeters = maxHeightMeters;
}
public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
return x.get(0, 0) < this.m_minHeightMeters;
}
public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
return x.get(0, 0) > this.m_maxHeightMeters;
}
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
var updatedXhat = RungeKutta.rungeKutta(
(x, u_) -> (m_plant.getA().times(x)).plus(m_plant.getB().times(u_)).plus(VecBuilder.fill(0, -9.8)),
currentXhat, u, dtSeconds);
// We check for collision after updating xhat
if (hasHitLowerLimit(updatedXhat)) {
return VecBuilder.fill(m_minHeightMeters, 0);
} else if (hasHitUpperLimit(updatedXhat)) {
return VecBuilder.fill(m_maxHeightMeters, 0);
}
return updatedXhat;
}
public void setInputVoltage(double volts) {
setInput(volts);
}
public double getPositionMeters() {
return getOutput(0);
}
public double getVelocityMetersPerSecond() {
return m_x.get(0, 1);
}
@Override
public double getCurrentDrawAmps() {
// 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
double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadiusMeters * m_gearing;
var appliedVoltage = getInput(0);
return m_motor.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage);
}
}

79
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java 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. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
private final DCMotor m_motor;
private final double m_gearing;
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param flywheelPlant The flywheel system being controlled. This system can be created
* using {@link LinearSystemId#createFlywheelSystem(DCMotor, double, double)}
* or {@link LinearSystemId#identifyVelocitySystem(double, double)}
* @param flywheelGearbox The DCMotor representing the motor driving the flywheel.
* @param gearing The reduction between motor and drum, as output over input.
* In most cases this is greater than one.
* @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false.
*/
public FlywheelSim(LinearSystem<N1, N1, N1> flywheelPlant, DCMotor flywheelGearbox,
double gearing, Matrix<N1, N1> measurementStdDevs) {
super(flywheelPlant, measurementStdDevs);
this.m_motor = flywheelGearbox;
this.m_gearing = gearing;
}
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param flywheelGearbox The DCMotor representing the motor driving the flywheel.
* @param gearing The reduction between motor and drum, as output over input.
* In most cases this is greater than one.
* @param jKgMetersSquared The moment of inertia J of the flywheel. This can be
* calculated with CAD. If J is unknown, {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
* using FRC-Characterization's kV and kA may be better.
* @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false.
*/
public FlywheelSim(DCMotor flywheelGearbox, double gearing,
double jKgMetersSquared, Matrix<N1, N1> measurementStdDevs) {
super(LinearSystemId.createFlywheelSystem(flywheelGearbox, jKgMetersSquared, gearing),
measurementStdDevs);
this.m_motor = flywheelGearbox;
this.m_gearing = gearing;
}
public double getAngularVelocityRadPerSec() {
return getOutput(0);
}
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getOutput(0));
}
@Override
public double getCurrentDrawAmps() {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the flywheel
return m_motor.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
}
}

137
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java Normal file
View File

@@ -0,0 +1,137 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.ejml.MatrixDimensionException;
import org.ejml.simple.SimpleMatrix;
/**
* This class helps simulate linear systems. To use this class, do the following in the
* {@link edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method.
*
* <p>Call {@link #setInput(double...)} with the inputs to the system (usually voltage).
*
* <p>Call {@link #update} to update the simulation.
*
* <p>Set simulated sensor readings with the simulated positions in {@link #getOutput(int)}
*
* @param <States> The number of states of the system.
* @param <Inputs> The number of inputs to the system.
* @param <Outputs> The number of outputs of the system.
*/
@SuppressWarnings("ClassTypeParameterName")
public class LinearSystemSim<States extends Num, Inputs extends Num,
Outputs extends Num> {
protected final LinearSystem<States, Inputs, Outputs> m_plant;
@SuppressWarnings("MemberName")
protected Matrix<States, N1> m_x;
@SuppressWarnings("MemberName")
protected Matrix<Outputs, N1> m_y;
@SuppressWarnings("MemberName")
protected Matrix<Inputs, N1> m_u;
protected final Matrix<Outputs, N1> m_measurementStdDevs;
/**
* Create a SimLinearSystem. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param system The system being controlled.
* @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false.
*/
public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system, Matrix<Outputs, N1> measurementStdDevs) {
this.m_plant = system;
this.m_measurementStdDevs = measurementStdDevs;
m_x = new Matrix<>(new SimpleMatrix(system.getA().getNumRows(), 1));
m_u = new Matrix<>(new SimpleMatrix(system.getB().getNumCols(), 1));
m_y = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1));
}
protected Matrix<States, N1> updateX(Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dtSeconds) {
return m_plant.calculateX(currentXhat, u, dtSeconds);
}
/**
* Updates the simulation.
*
* @param dtSeconds The time that's passed since the last {@link #update(double)} call.
*/
@SuppressWarnings("LocalVariableName")
public void update(double dtSeconds) {
// Update X. By default, this is the linear system dynamics X = Ax + Bu
m_x = updateX(m_x, m_u, dtSeconds);
// y = cx + du
m_y = m_plant.calculateY(m_x, m_u);
if (m_measurementStdDevs != null) {
m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
}
}
public Matrix<Outputs, N1> getY() {
return m_y;
}
public double getY(int row) {
return m_y.get(row, 0);
}
public void setInput(Matrix<Inputs, N1> u) {
this.m_u = u;
}
public void setInput(int row, double value) {
m_u.set(row, 0, value);
}
public void setInput(double... u) {
if (u.length != m_u.getNumRows()) {
throw new MatrixDimensionException("Malformed input! Got " + u.length
+ " elements instead of " + m_u.getNumRows());
}
m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u));
}
public double getInput(int row) {
return m_u.get(row, 0);
}
public Matrix<Inputs, N1> getInput() {
return m_u;
}
public Matrix<Outputs, N1> getOutput() {
return m_y;
}
public double getOutput(int row) {
return m_y.get(row, 0);
}
public void resetState(Matrix<States, N1> state) {
m_x = state;
}
/**
* Get the current drawn by this simulated system. Override this method to add current
* calculation.
*
* @return The currently drawn current.
*/
public double getCurrentDrawAmps() {
return 0.0;
}
}

213
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java Normal file
View File

@@ -0,0 +1,213 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.RungeKutta;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
@SuppressWarnings("MemberName")
private final double m_r;
private final double m_maxAngleRads;
private final double m_minAngleRads;
private final double m_armMass;
private final DCMotor m_motor;
private final double m_gearing;
private final boolean m_simulateGravity;
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param armSystem The arm system being controlled.
* @param motor DCMotor representing the motor driving the arm.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param armMassKg The mass of the arm.
* @param armLengthMeters The distance from the pivot of the arm to its center of mass.
* This number is not the same as the overall length of the arm.
* @param minAngleRads The min angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param maxAngleRads The max angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param posMeasurementStdDev Standard deviations of noise in the arm's position measurement.
* @param simulateGravity If the affects of gravity should be simulated.
*/
public SingleJointedArmSim(LinearSystem<N2, N1, N1> armSystem,
DCMotor motor, double G,
double armMassKg,
double armLengthMeters, double minAngleRads, double maxAngleRads,
double posMeasurementStdDev, boolean simulateGravity) {
super(armSystem, VecBuilder.fill(posMeasurementStdDev));
this.m_armMass = armMassKg;
this.m_r = armLengthMeters;
this.m_minAngleRads = minAngleRads;
this.m_maxAngleRads = maxAngleRads;
this.m_motor = motor;
this.m_gearing = G;
this.m_simulateGravity = simulateGravity;
}
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param motor DCMotor representing the motor driving the arm.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param armMassKg The mass of the arm.
* @param armLengthMeters The distance from the pivot of the arm to its center of mass.
* @param minAngleRads The min angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param maxAngleRads The max angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param posMeasurementStdDev Standard deviations of noise in the arm's position measurement.
* @param simulateGravity If the affects of gravity should be simulated.
*/
public SingleJointedArmSim(DCMotor motor, double jKgSquaredMeters,
double G, double armMassKg, double armLengthMeters,
double minAngleRads, double maxAngleRads,
double posMeasurementStdDev, boolean simulateGravity) {
this(LinearSystemId.createSingleJointedArmSystem(motor, jKgSquaredMeters, G),
motor, G,
armMassKg, armLengthMeters, minAngleRads, maxAngleRads,
posMeasurementStdDev, simulateGravity);
}
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param motor DCMotor representing the motor driving the arm.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param armMassKg The mass of the arm.
* @param armLengthMeters The distance from the pivot of the arm to its center of mass.
* @param minAngleRads The min angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param maxAngleRads The max angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param positionMeasurementStdDevs Standard deviations of noise in the arm's position measurement.
* @param simulateGravity If the affects of gravity should be simulated.
*/
public SingleJointedArmSim(DCMotor motor,
double G, double armMassKg, double armLengthMeters,
double minAngleRads, double maxAngleRads,
double positionMeasurementStdDevs, boolean simulateGravity) {
this(LinearSystemId.createSingleJointedArmSystem(motor,
1.0 / 3.0 * armMassKg * armLengthMeters * armLengthMeters, G),
motor, G,
armMassKg, armLengthMeters, minAngleRads, maxAngleRads,
positionMeasurementStdDevs, simulateGravity);
}
/**
* Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate
* the state of the system. In simulationPeriodic, users should first set inputs from motors, update
* the simulation and write simulated outputs to sensors.
*
* @param motor DCMotor representing the motor driving the arm.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param armMassKg The mass of the arm.
* @param armLengthMeters The distance from the pivot of the arm to its center of mass.
* @param minAngleRads The min angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param maxAngleRads The max angle the arm can reach, with 0 being measured from
* horizontal. The simulation will not allow motion past this.
* @param positionMeasurementStdDevs Standard deviations of noise in the arm's position measurement.
*/
public SingleJointedArmSim(DCMotor motor,
double G, double armMassKg, double armLengthMeters,
double minAngleRads, double maxAngleRads,
double positionMeasurementStdDevs) {
this(LinearSystemId.createSingleJointedArmSystem(motor,
1.0 / 3.0 * armMassKg * armLengthMeters * armLengthMeters, G),
motor, G,
armMassKg, armLengthMeters, minAngleRads, maxAngleRads,
positionMeasurementStdDevs, true);
}
public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
return x.get(0, 0) < this.m_minAngleRads;
}
public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
return x.get(0, 0) > this.m_maxAngleRads;
}
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
/*
Horizontal case:
Torque = f * r = I * alpha
alpha = f * r / I
since f=mg,
alpha = m g r / I
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]]
*/
Matrix<N2, N1> updatedXhat = RungeKutta.rungeKutta(
this::dynamics,
currentXhat, u, dtSeconds);
// We check for collision after updating xhat
if (hasHitLowerLimit(updatedXhat)) {
return VecBuilder.fill(m_minAngleRads, 0);
} else if (hasHitUpperLimit(updatedXhat)) {
return VecBuilder.fill(m_maxAngleRads, 0);
}
return updatedXhat;
}
public double getAngleRads() {
return m_x.get(0, 0);
}
public double getVelocityRadPerSec() {
return m_x.get(1, 0);
}
public double getInputVoltageVolts() {
return m_u.get(0, 0);
}
@Override
public double getCurrentDrawAmps() {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
// spinning 10x faster than the output
var motorVelocity = m_x.get(1, 0) * m_gearing;
return m_motor.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
}
private Matrix<N2, N1> dynamics(Matrix<N2, N1> x, Matrix<N1, N1> u_) {
Matrix<N2, N1> xdot = (m_plant.getA().times(x))
.plus(m_plant.getB().times(u_));
if (m_simulateGravity) {
xdot = xdot.plus(VecBuilder.fill(0,
m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * Math.cos(x.get(0, 0))));
}
return xdot;
}
}