mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpilib] Clean up physics simulation class APIs (#2763)
This commit is contained in:
Prateek Machiraju
committed by
GitHub
GitHub
parent
8f3e5794b3
commit
061432147d
@@ -16,106 +16,186 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
/**
|
||||
* Represents a simulated elevator mechanism.
|
||||
*/
|
||||
public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
// Gearbox for the elevator.
|
||||
private final DCMotor m_gearbox;
|
||||
|
||||
private final DCMotor m_motor;
|
||||
private final double m_drumRadiusMeters;
|
||||
private final double m_minHeightMeters;
|
||||
private final double m_maxHeightMeters;
|
||||
// Gearing between the motors and the output.
|
||||
private final double m_gearing;
|
||||
|
||||
// The radius of the drum that the elevator spool is wrapped around.
|
||||
private final double m_drumRadius;
|
||||
|
||||
// The min allowable height for the elevator.
|
||||
private final double m_minHeight;
|
||||
|
||||
// The max allowable height for the elevator.
|
||||
private final double m_maxHeight;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* Creates a simulated elevator mechanism.
|
||||
*
|
||||
* @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.
|
||||
* @param plant The linear system that represents the elevator.
|
||||
* @param gearbox The type of and number of motors in the elevator gearbox.
|
||||
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
|
||||
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
|
||||
* @param minHeightMeters The min allowable height of the elevator.
|
||||
* @param maxHeightMeters The max allowable height of the elevator.
|
||||
*/
|
||||
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;
|
||||
public ElevatorSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
|
||||
double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) {
|
||||
this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* Creates a simulated elevator mechanism.
|
||||
*
|
||||
* @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.
|
||||
* @param plant The linear system that represents the elevator.
|
||||
* @param gearbox The type of and number of motors in the elevator gearbox.
|
||||
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
|
||||
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
|
||||
* @param minHeightMeters The min allowable height of the elevator.
|
||||
* @param maxHeightMeters The max allowable height of the elevator.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
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 ElevatorSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
|
||||
double drumRadiusMeters, double minHeightMeters, double maxHeightMeters,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_drumRadius = drumRadiusMeters;
|
||||
m_minHeight = minHeightMeters;
|
||||
m_maxHeight = maxHeightMeters;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a simulated elevator mechanism.
|
||||
*
|
||||
* @param gearbox The type of and number of motors in the elevator gearbox.
|
||||
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
|
||||
* @param carriageMassKg The mass of the elevator carriage.
|
||||
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
|
||||
* @param minHeightMeters The min allowable height of the elevator.
|
||||
* @param maxHeightMeters The max allowable height of the elevator.
|
||||
*/
|
||||
public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg,
|
||||
double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) {
|
||||
this(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters,
|
||||
null);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a simulated elevator mechanism.
|
||||
*
|
||||
* @param gearbox The type of and number of motors in the elevator gearbox.
|
||||
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
|
||||
* @param carriageMassKg The mass of the elevator carriage.
|
||||
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
|
||||
* @param minHeightMeters The min allowable height of the elevator.
|
||||
* @param maxHeightMeters The max allowable height of the elevator.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg,
|
||||
double drumRadiusMeters, double minHeightMeters, double maxHeightMeters,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
|
||||
measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_drumRadius = drumRadiusMeters;
|
||||
m_minHeight = minHeightMeters;
|
||||
m_maxHeight = maxHeightMeters;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the elevator has hit the lower limit.
|
||||
*
|
||||
* @param x The current elevator state.
|
||||
* @return Whether the elevator has hit the lower limit.
|
||||
*/
|
||||
public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
|
||||
return x.get(0, 0) < this.m_minHeightMeters;
|
||||
return x.get(0, 0) < this.m_minHeight;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the elevator has hit the upper limit.
|
||||
*
|
||||
* @param x The current elevator state.
|
||||
* @return Whether the elevator has hit the upper limit.
|
||||
*/
|
||||
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);
|
||||
return x.get(0, 0) > this.m_maxHeight;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the position of the elevator.
|
||||
*
|
||||
* @return The position of the elevator.
|
||||
*/
|
||||
public double getPositionMeters() {
|
||||
return getOutput(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the velocity of the elevator.
|
||||
*
|
||||
* @return The velocity of the elevator.
|
||||
*/
|
||||
public double getVelocityMetersPerSecond() {
|
||||
return m_x.get(0, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the elevator current draw.
|
||||
*
|
||||
* @return The elevator current draw.
|
||||
*/
|
||||
@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);
|
||||
double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
|
||||
var appliedVoltage = m_u.get(0, 0);
|
||||
return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the input voltage for the elevator.
|
||||
*
|
||||
* @param volts The input voltage.
|
||||
*/
|
||||
public void setInputVoltage(double volts) {
|
||||
setInput(volts);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the state of the elevator.
|
||||
*
|
||||
* @param currentXhat The current state estimate.
|
||||
* @param u The system inputs (voltage).
|
||||
* @param dtSeconds The time difference between controller updates.
|
||||
*/
|
||||
@Override
|
||||
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat,
|
||||
Matrix<N1, N1> u, double dtSeconds) {
|
||||
// Calculate updated x-hat from Runge-Kutta.
|
||||
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 collisions after updating x-hat.
|
||||
if (hasHitLowerLimit(updatedXhat)) {
|
||||
return VecBuilder.fill(m_minHeight, 0);
|
||||
}
|
||||
if (hasHitUpperLimit(updatedXhat)) {
|
||||
return VecBuilder.fill(m_maxHeight, 0);
|
||||
}
|
||||
return updatedXhat;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -14,66 +14,121 @@ import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
/**
|
||||
* Represents a simulated flywheel mechanism.
|
||||
*/
|
||||
public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
|
||||
// Gearbox for the flywheel.
|
||||
private final DCMotor m_gearbox;
|
||||
|
||||
private final DCMotor m_motor;
|
||||
// The gearing from the motors to the output.
|
||||
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.
|
||||
* Creates a simulated flywheel mechanism.
|
||||
*
|
||||
* @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.
|
||||
* @param plant The linear system that represents 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).
|
||||
*/
|
||||
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;
|
||||
public FlywheelSim(LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double gearing) {
|
||||
super(plant);
|
||||
m_gearbox = gearbox;
|
||||
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.
|
||||
* Creates a simulated flywheel mechanism.
|
||||
*
|
||||
* @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.
|
||||
* @param plant The linear system that represents 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 measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
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 FlywheelSim(LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double gearing,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a simulated flywheel 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 jKgMetersSquared The moment of inertia of the flywheel. If this is unknown,
|
||||
* use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
|
||||
* constructor.
|
||||
*/
|
||||
public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
|
||||
super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared));
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a simulated flywheel 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 jKgMetersSquared The moment of inertia of the flywheel. If this is unknown,
|
||||
* use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
|
||||
* constructor.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared),
|
||||
measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the flywheel velocity.
|
||||
*
|
||||
* @return The flywheel velocity.
|
||||
*/
|
||||
public double getAngularVelocityRadPerSec() {
|
||||
return getOutput(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the flywheel velocity in RPM.
|
||||
*
|
||||
* @return The flywheel velocity in RPM.
|
||||
*/
|
||||
public double getAngularVelocityRPM() {
|
||||
return Units.radiansPerSecondToRotationsPerMinute(getOutput(0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the flywheel current draw.
|
||||
*
|
||||
* @return The flywheel current draw.
|
||||
*/
|
||||
@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))
|
||||
return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
|
||||
* Math.signum(m_u.get(0, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the input voltage for the flywheel.
|
||||
*
|
||||
* @param volts The input voltage.
|
||||
*/
|
||||
public void setInputVoltage(double volts) {
|
||||
setInput(volts);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,13 +7,14 @@
|
||||
|
||||
package edu.wpi.first.wpilibj.simulation;
|
||||
|
||||
import org.ejml.MatrixDimensionException;
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
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
|
||||
@@ -23,35 +24,46 @@ import org.ejml.simple.SimpleMatrix;
|
||||
*
|
||||
* <p>Call {@link #update} to update the simulation.
|
||||
*
|
||||
* <p>Set simulated sensor readings with the simulated positions in {@link #getOutput(int)}
|
||||
* <p>Set simulated sensor readings with the simulated positions in {@link #getOutput()}
|
||||
*
|
||||
* @param <States> The number of states of the system.
|
||||
* @param <Inputs> The number of inputs to the system.
|
||||
* @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> {
|
||||
|
||||
public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
// The plant that represents the linear system.
|
||||
protected final LinearSystem<States, Inputs, Outputs> m_plant;
|
||||
|
||||
// Variables for state, output, and input.
|
||||
@SuppressWarnings("MemberName")
|
||||
protected Matrix<States, N1> m_x;
|
||||
@SuppressWarnings("MemberName")
|
||||
protected Matrix<Outputs, N1> m_y;
|
||||
@SuppressWarnings("MemberName")
|
||||
protected Matrix<Inputs, N1> m_u;
|
||||
|
||||
// The standard deviations of measurements, used for adding noise
|
||||
// to the measurements.
|
||||
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.
|
||||
* Creates a simulated generic linear system.
|
||||
*
|
||||
* @param system The system to simulate.
|
||||
*/
|
||||
public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system) {
|
||||
this(system, null);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a simulated generic linear system with measurement noise.
|
||||
*
|
||||
* @param system The system being controlled.
|
||||
* @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false.
|
||||
* @param measurementStdDevs Standard deviations of measurements.
|
||||
*/
|
||||
public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system, Matrix<Outputs, N1> measurementStdDevs) {
|
||||
public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system,
|
||||
Matrix<Outputs, N1> measurementStdDevs) {
|
||||
this.m_plant = system;
|
||||
this.m_measurementStdDevs = measurementStdDevs;
|
||||
|
||||
@@ -60,14 +72,10 @@ public class LinearSystemSim<States extends Num, Inputs extends Num,
|
||||
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.
|
||||
* @param dtSeconds The time between updates.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void update(double dtSeconds) {
|
||||
@@ -76,67 +84,93 @@ public class LinearSystemSim<States extends Num, Inputs extends Num,
|
||||
|
||||
// y = cx + du
|
||||
m_y = m_plant.calculateY(m_x, m_u);
|
||||
|
||||
// Add measurement noise.
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current output of the plant.
|
||||
*
|
||||
* @return The current output of the plant.
|
||||
*/
|
||||
public Matrix<Outputs, N1> getOutput() {
|
||||
return m_y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the current output of the plant.
|
||||
*
|
||||
* @param row The row to return.
|
||||
* @return An element of the current output of the plant.
|
||||
*/
|
||||
public double getOutput(int row) {
|
||||
return m_y.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the system inputs (usually voltages).
|
||||
*
|
||||
* @param u The system inputs.
|
||||
*/
|
||||
public void setInput(Matrix<Inputs, N1> u) {
|
||||
this.m_u = u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the system inputs.
|
||||
*
|
||||
* @param row The row in the input matrix to set.
|
||||
* @param value The value to set the row to.
|
||||
*/
|
||||
public void setInput(int row, double value) {
|
||||
m_u.set(row, 0, value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the system inputs.
|
||||
*
|
||||
* @param u An array of doubles that represent the inputs of the system.
|
||||
*/
|
||||
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));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the system state.
|
||||
*
|
||||
* @param state The state.
|
||||
* @param state The new state.
|
||||
*/
|
||||
public void setState(Matrix<States, N1> state) {
|
||||
m_x = state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the current drawn by this simulated system. Override this method to add current
|
||||
* calculation.
|
||||
* Returns the current drawn by this simulated system. Override this method to add a custom
|
||||
* current calculation.
|
||||
*
|
||||
* @return The currently drawn current.
|
||||
* @return The current drawn by this simulated mechanism.
|
||||
*/
|
||||
public double getCurrentDrawAmps() {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the state estimate of the system.
|
||||
*
|
||||
* @param currentXhat The current state estimate.
|
||||
* @param u The system inputs (usually voltage).
|
||||
* @param dtSeconds The time difference between controller updates.
|
||||
* @return The new state.
|
||||
*/
|
||||
protected Matrix<States, N1> updateX(Matrix<States, N1> currentXhat,
|
||||
Matrix<Inputs, N1> u, double dtSeconds) {
|
||||
return m_plant.calculateX(currentXhat, u, dtSeconds);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -16,195 +16,235 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
/**
|
||||
* Represents a simulated single jointed arm mechanism.
|
||||
*/
|
||||
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
// The gearbox for the arm.
|
||||
private final DCMotor m_gearbox;
|
||||
|
||||
// The gearing between the motors and the output.
|
||||
private final double m_gearing;
|
||||
|
||||
// The length of the arm.
|
||||
@SuppressWarnings("MemberName")
|
||||
private final double m_r;
|
||||
private final double m_maxAngleRads;
|
||||
private final double m_minAngleRads;
|
||||
|
||||
// The minimum angle that the arm is capable of.
|
||||
private final double m_minAngle;
|
||||
|
||||
// The maximum angle that the arm is capable of.
|
||||
private final double m_maxAngle;
|
||||
|
||||
// The mass of the arm.
|
||||
private final double m_armMass;
|
||||
private final DCMotor m_motor;
|
||||
private final double m_gearing;
|
||||
|
||||
// Whether the simulator should simulate gravity.
|
||||
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.
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @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.
|
||||
* @param plant The linear system that represents the arm.
|
||||
* @param gearbox The type of and number of motors in the arm gearbox.
|
||||
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
|
||||
* @param armLengthMeters The length of the arm.
|
||||
* @param minAngleRads The minimum angle that the arm is capable of.
|
||||
* @param maxAngleRads The maximum angle that the arm is capable of.
|
||||
* @param armMassKg The mass of the arm.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
*/
|
||||
public SingleJointedArmSim(LinearSystem<N2, N1, N1> armSystem,
|
||||
DCMotor motor, double G,
|
||||
double armMassKg,
|
||||
public SingleJointedArmSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
|
||||
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;
|
||||
double armMassKg, boolean simulateGravity) {
|
||||
this(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, armMassKg,
|
||||
simulateGravity, null);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @param motor DCMotor representing the motor driving the arm.
|
||||
* @param jKgSquaredMeters The moment of inertia of 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.
|
||||
* @param plant The linear system that represents the arm.
|
||||
* @param gearbox The type of and number of motors in the arm gearbox.
|
||||
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
|
||||
* @param armLengthMeters The length of the arm.
|
||||
* @param minAngleRads The minimum angle that the arm is capable of.
|
||||
* @param maxAngleRads The maximum angle that the arm is capable of.
|
||||
* @param armMassKg The mass of the arm.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
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);
|
||||
public SingleJointedArmSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
|
||||
double armLengthMeters, double minAngleRads, double maxAngleRads,
|
||||
double armMassKg, boolean simulateGravity,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_r = armLengthMeters;
|
||||
m_minAngle = minAngleRads;
|
||||
m_maxAngle = maxAngleRads;
|
||||
m_armMass = armMassKg;
|
||||
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.
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @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.
|
||||
* @param gearbox The type of and number of motors in the arm gearbox.
|
||||
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
|
||||
* @param jKgMetersSquared The moment of inertia of the arm. This can be calculated from CAD software.
|
||||
* @param armLengthMeters The length of the arm.
|
||||
* @param minAngleRads The minimum angle that the arm is capable of.
|
||||
* @param maxAngleRads The maximum angle that the arm is capable of.
|
||||
* @param armMassKg The mass of the arm.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
*/
|
||||
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);
|
||||
public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
|
||||
double armLengthMeters, double minAngleRads, double maxAngleRads,
|
||||
double armMassKg, boolean simulateGravity) {
|
||||
this(gearbox, gearing, jKgMetersSquared, armLengthMeters, minAngleRads, maxAngleRads,
|
||||
armMassKg, simulateGravity, null);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @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 gearbox The type of and number of motors in the arm gearbox.
|
||||
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
|
||||
* @param jKgMetersSquared The moment of inertia of the arm. This can be calculated from CAD software.
|
||||
* @param armLengthMeters The length of the arm.
|
||||
* @param minAngleRads The minimum angle that the arm is capable of.
|
||||
* @param maxAngleRads The maximum angle that the arm is capable of.
|
||||
* @param armMassKg The mass of the arm.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
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 SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
|
||||
double armLengthMeters, double minAngleRads, double maxAngleRads,
|
||||
double armMassKg, boolean simulateGravity,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
|
||||
measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_r = armLengthMeters;
|
||||
m_minAngle = minAngleRads;
|
||||
m_maxAngle = maxAngleRads;
|
||||
m_armMass = armMassKg;
|
||||
m_simulateGravity = simulateGravity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the arm has hit the lower limit.
|
||||
*
|
||||
* @param x The current arm state.
|
||||
* @return Whether the arm has hit the lower limit.
|
||||
*/
|
||||
public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
|
||||
return x.get(0, 0) < this.m_minAngleRads;
|
||||
return x.get(0, 0) < this.m_minAngle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the arm has hit the upper limit.
|
||||
*
|
||||
* @param x The current arm state.
|
||||
* @return Whether the arm has hit the upper limit.
|
||||
*/
|
||||
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;
|
||||
return x.get(0, 0) > this.m_maxAngle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current arm angle.
|
||||
*
|
||||
* @return The current arm angle.
|
||||
*/
|
||||
public double getAngleRads() {
|
||||
return m_x.get(0, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current arm velocity.
|
||||
*
|
||||
* @return The current arm velocity.
|
||||
*/
|
||||
public double getVelocityRadPerSec() {
|
||||
return m_x.get(1, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the arm current draw.
|
||||
*
|
||||
* @return The aram current draw.
|
||||
*/
|
||||
@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));
|
||||
return m_gearbox.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_));
|
||||
/**
|
||||
* Sets the input voltage for the elevator.
|
||||
*
|
||||
* @param volts The input voltage.
|
||||
*/
|
||||
public void setInputVoltage(double volts) {
|
||||
setInput(volts);
|
||||
}
|
||||
|
||||
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))));
|
||||
/**
|
||||
* Calculates a rough estimate of the moment of inertia of an arm given its
|
||||
* length and mass.
|
||||
*
|
||||
* @param lengthMeters The length of the arm.
|
||||
* @param massKg The mass of the arm.
|
||||
*
|
||||
* @return The calculated moment of inertia.
|
||||
*/
|
||||
public static double estimateMOI(double lengthMeters, double massKg) {
|
||||
return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the state of the arm.
|
||||
*
|
||||
* @param currentXhat The current state estimate.
|
||||
* @param u The system inputs (voltage).
|
||||
* @param dtSeconds The time difference between controller updates.
|
||||
*/
|
||||
@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
|
||||
// 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 *
|
||||
// cos(theta)]]
|
||||
Matrix<N2, N1> updatedXhat = RungeKutta.rungeKutta(
|
||||
(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;
|
||||
},
|
||||
currentXhat, u, dtSeconds);
|
||||
|
||||
// We check for collision after updating xhat
|
||||
if (hasHitLowerLimit(updatedXhat)) {
|
||||
return VecBuilder.fill(m_minAngle, 0);
|
||||
}
|
||||
return xdot;
|
||||
if (hasHitUpperLimit(updatedXhat)) {
|
||||
return VecBuilder.fill(m_maxAngle, 0);
|
||||
}
|
||||
return updatedXhat;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user