[wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288)

- Add a constructor parameter to configure the initial angle of the arm
- Also reorganizes cascading constructors for Java
This commit is contained in:
Sriman Achanta
2023-07-18 16:00:27 -04:00
committed by GitHub
parent 14f30752ab
commit 335e7dd89d
16 changed files with 151 additions and 96 deletions

View File

@@ -16,6 +16,7 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
@@ -23,22 +24,21 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
m_simulateGravity(simulateGravity) {
SetState(
frc::Vectord<2>{std::clamp(startingHeight, minHeight, maxHeight), 0.0});
}
ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
units::kilogram_t carriageMass,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
measurementStdDevs),
m_gearbox(gearbox),
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
: ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
gearbox, gearing, drumRadius, minHeight, maxHeight,
simulateGravity, startingHeight, measurementStdDevs) {}
bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
return elevatorHeight <= m_minHeight;

View File

@@ -19,6 +19,7 @@ SingleJointedArmSim::SingleJointedArmSim(
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
m_armLen(armLength),
@@ -26,17 +27,20 @@ SingleJointedArmSim::SingleJointedArmSim(
m_maxAngle(maxAngle),
m_gearbox(gearbox),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
m_simulateGravity(simulateGravity) {
SetState(frc::Vectord<2>{startingAngle, 0.0});
}
SingleJointedArmSim::SingleJointedArmSim(
const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,
measurementStdDevs) {}
startingAngle, measurementStdDevs) {}
bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
return armAngle <= m_minAngle;

View File

@@ -32,12 +32,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
double gearing, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
@@ -53,12 +54,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const DCMotor& gearbox, double gearing,
units::kilogram_t carriageMass, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**

View File

@@ -31,12 +31,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* @param minAngle The minimum angle that the arm is capable of.
* @param maxAngle The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngle The initial position of the arm.
* @param measurementStdDevs The standard deviations of the measurements.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Creates a simulated arm mechanism.
@@ -50,12 +52,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* @param minAngle The minimum angle that the arm is capable of.
* @param maxAngle The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngle The initial position of the arm.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
SingleJointedArmSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
/**

View File

@@ -21,7 +21,7 @@
TEST(ElevatorSimTest, StateSpaceSim) {
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
0_m, 3_m, true, {0.01});
0_m, 3_m, true, 0_m, {0.01});
frc2::PIDController controller(10, 0.0, 0.0);
frc::PWMVictorSPX motor(0);
@@ -46,7 +46,7 @@ TEST(ElevatorSimTest, StateSpaceSim) {
TEST(ElevatorSimTest, MinMax) {
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
0_m, 1_m, true, {0.01});
0_m, 1_m, true, 0_m, {0.01});
for (size_t i = 0; i < 100; ++i) {
sim.SetInput(frc::Vectord<1>{0.0});
sim.Update(20_ms);
@@ -66,7 +66,7 @@ TEST(ElevatorSimTest, MinMax) {
TEST(ElevatorSimTest, Stability) {
frc::sim::ElevatorSim sim{
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false};
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false, 0_m};
sim.SetState(frc::Vectord<2>{0.0, 0.0});
sim.SetInput(frc::Vectord<1>{12.0});

View File

@@ -9,7 +9,7 @@
TEST(SingleJointedArmTest, Disabled) {
frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 300, 3_kg_sq_m,
30_in, -180_deg, 0_deg, true);
30_in, -180_deg, 0_deg, true, 90_deg);
sim.SetState(frc::Vectord<2>{0.0, 0.0});
for (size_t i = 0; i < 12 / 0.02; ++i) {

View File

@@ -53,6 +53,7 @@ class Arm {
kMinAngle,
kMaxAngle,
true,
0_deg,
{kArmEncoderDistPerPulse}};
frc::sim::EncoderSim m_encoderSim{m_encoder};

View File

@@ -56,6 +56,7 @@ class Elevator {
Constants::kMinElevatorHeight,
Constants::kMaxElevatorHeight,
true,
0_m,
{0.01}};
frc::sim::EncoderSim m_encoderSim{m_encoder};

View File

@@ -38,7 +38,8 @@ class PotentiometerPIDTest : public testing::Test {
kElevatorDrumRadius,
0.0_m,
Robot::kFullHeight,
true};
true,
0.0_m};
frc::sim::PWMSim m_motorSim{Robot::kMotorChannel};
frc::sim::AnalogInputSim m_analogSim{Robot::kPotChannel};
frc::sim::JoystickSim m_joystickSim{Robot::kJoystickChannel};

View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
@@ -43,6 +44,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
@@ -51,16 +54,19 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity) {
this(
plant,
gearbox,
gearing,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
null);
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_drumRadius = drumRadiusMeters;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
setState(
VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0));
}
/**
@@ -72,8 +78,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @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 startingHeightMeters The starting height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
@@ -83,43 +89,16 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_drumRadius = drumRadiusMeters;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
}
/**
* 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 simulateGravity Whether gravity should be simulated or not.
*/
public ElevatorSim(
DCMotor gearbox,
double gearing,
double carriageMassKg,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity) {
double startingHeightMeters) {
this(
plant,
gearbox,
gearing,
carriageMassKg,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
null);
}
@@ -133,6 +112,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public ElevatorSim(
@@ -143,16 +123,51 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
super(
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
gearbox,
gearing,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_drumRadius = drumRadiusMeters;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
}
/**
* 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 simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
*/
public ElevatorSim(
DCMotor gearbox,
double gearing,
double carriageMassKg,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters) {
this(
gearbox,
gearing,
carriageMassKg,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
null);
}
/**

View File

@@ -43,6 +43,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngleRads The initial position of the Arm simulation in radians.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public SingleJointedArmSim(
LinearSystem<N2, N1, N1> plant,
@@ -51,16 +53,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double armLengthMeters,
double minAngleRads,
double maxAngleRads,
boolean simulateGravity) {
this(
plant,
gearbox,
gearing,
armLengthMeters,
minAngleRads,
maxAngleRads,
simulateGravity,
null);
boolean simulateGravity,
double startingAngleRads,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_armLenMeters = armLengthMeters;
m_minAngle = minAngleRads;
m_maxAngle = maxAngleRads;
m_simulateGravity = simulateGravity;
setState(VecBuilder.fill(startingAngleRads, 0));
}
/**
@@ -73,7 +77,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param measurementStdDevs The standard deviations of the measurements.
* @param startingAngleRads The initial position of the Arm simulation in radians.
*/
public SingleJointedArmSim(
LinearSystem<N2, N1, N1> plant,
@@ -83,14 +87,17 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double minAngleRads,
double maxAngleRads,
boolean simulateGravity,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_armLenMeters = armLengthMeters;
m_minAngle = minAngleRads;
m_maxAngle = maxAngleRads;
m_simulateGravity = simulateGravity;
double startingAngleRads) {
this(
plant,
gearbox,
gearing,
armLengthMeters,
minAngleRads,
maxAngleRads,
simulateGravity,
startingAngleRads,
null);
}
/**
@@ -103,6 +110,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngleRads The initial position of the Arm simulation in radians.
*/
public SingleJointedArmSim(
DCMotor gearbox,
@@ -111,7 +119,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double armLengthMeters,
double minAngleRads,
double maxAngleRads,
boolean simulateGravity) {
boolean simulateGravity,
double startingAngleRads) {
this(
gearbox,
gearing,
@@ -120,6 +129,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
minAngleRads,
maxAngleRads,
simulateGravity,
startingAngleRads,
null);
}
@@ -133,6 +143,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingAngleRads The initial position of the Arm simulation in radians.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public SingleJointedArmSim(
@@ -143,16 +154,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double minAngleRads,
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N1, N1> measurementStdDevs) {
super(
this(
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
gearbox,
gearing,
armLengthMeters,
minAngleRads,
maxAngleRads,
simulateGravity,
startingAngleRads,
measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_armLenMeters = armLengthMeters;
m_minAngle = minAngleRads;
m_maxAngle = maxAngleRads;
m_simulateGravity = simulateGravity;
}
/**

View File

@@ -33,6 +33,7 @@ class ElevatorSimTest {
0.0,
3.0,
true,
0.0,
VecBuilder.fill(0.01));
try (var motor = new PWMVictorSPX(0);
@@ -71,6 +72,7 @@ class ElevatorSimTest {
0.0,
1.0,
true,
0.0,
VecBuilder.fill(0.01));
for (int i = 0; i < 100; i++) {
@@ -91,7 +93,8 @@ class ElevatorSimTest {
@Test
void testStability() {
var sim =
new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false);
new ElevatorSim(
DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0);
sim.setState(VecBuilder.fill(0, 0));
sim.setInput(12);

View File

@@ -14,10 +14,18 @@ import org.junit.jupiter.api.Test;
class SingleJointedArmSimTest {
SingleJointedArmSim m_sim =
new SingleJointedArmSim(
DCMotor.getVex775Pro(2), 300, 3.0, Units.inchesToMeters(30.0), -Math.PI, 0.0, true);
DCMotor.getVex775Pro(2),
300,
3.0,
Units.inchesToMeters(30.0),
-Math.PI,
0.0,
true,
Math.PI / 2.0);
@Test
void testArmDisabled() {
// Reset Arm angle to 0
m_sim.setState(VecBuilder.fill(0.0, 0.0));
for (int i = 0; i < 12 / 0.02; i++) {

View File

@@ -50,6 +50,7 @@ public class Arm implements AutoCloseable {
Constants.kMinAngleRads,
Constants.kMaxAngleRads,
true,
0,
VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
);
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);

View File

@@ -54,6 +54,7 @@ public class Elevator implements AutoCloseable {
Constants.kMinElevatorHeightMeters,
Constants.kMaxElevatorHeightMeters,
true,
0,
VecBuilder.fill(0.01));
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final PWMSim m_motorSim = new PWMSim(m_motor);

View File

@@ -55,6 +55,7 @@ class PotentiometerPIDTest {
0.0,
Robot.kFullHeightMeters,
true,
0,
null);
m_analogSim = new AnalogInputSim(Robot.kPotChannel);
m_motorSim = new PWMSim(Robot.kMotorChannel);