From 9d20ab30247faec4e355dbf47e853237693c1a22 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 24 Apr 2022 07:19:18 -0700 Subject: [PATCH] [wpilib] Allow disabling ElevatorSim gravity (#4145) Closes #4144. --- .../native/cpp/simulation/ElevatorSim.cpp | 18 ++++--- .../cpp/simulation/SingleJointedArmSim.cpp | 10 ++-- .../include/frc/simulation/ElevatorSim.h | 5 ++ .../frc/simulation/SingleJointedArmSim.h | 10 ++-- .../native/cpp/simulation/ElevatorSimTest.cpp | 39 +++++++-------- .../examples/ElevatorSimulation/cpp/Robot.cpp | 1 + .../first/wpilibj/simulation/ElevatorSim.java | 47 +++++++++++++++---- .../wpilibj/simulation/ElevatorSimTest.java | 20 ++++---- .../examples/elevatorsimulation/Robot.java | 1 + 9 files changed, 97 insertions(+), 54 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index a2d5c663df..64c7aceaea 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -15,19 +15,20 @@ using namespace frc::sim; 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, + units::meter_t maxHeight, bool simulateGravity, const std::array& measurementStdDevs) : LinearSystemSim(plant, measurementStdDevs), m_gearbox(gearbox), m_drumRadius(drumRadius), m_minHeight(minHeight), m_maxHeight(maxHeight), - m_gearing(gearing) {} + m_gearing(gearing), + m_simulateGravity(simulateGravity) {} ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, units::kilogram_t carriageMass, units::meter_t drumRadius, units::meter_t minHeight, - units::meter_t maxHeight, + units::meter_t maxHeight, bool simulateGravity, const std::array& measurementStdDevs) : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, drumRadius, gearing), @@ -36,7 +37,8 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, m_drumRadius(drumRadius), m_minHeight(minHeight), m_maxHeight(maxHeight), - m_gearing(gearing) {} + m_gearing(gearing), + m_simulateGravity(simulateGravity) {} bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const { return elevatorHeight < m_minHeight; @@ -87,8 +89,12 @@ Eigen::Vector ElevatorSim::UpdateX( auto updatedXhat = RKDP( [&](const Eigen::Vector& x, const Eigen::Vector& u_) -> Eigen::Vector { - return m_plant.A() * x + m_plant.B() * u_ + - Eigen::Vector{0.0, -9.8}; + Eigen::Vector xdot = m_plant.A() * x + m_plant.B() * u; + + if (m_simulateGravity) { + xdot += Eigen::Vector{0.0, -9.8}; + } + return xdot; }, currentXhat, u, dt); // Check for collision after updating x-hat. diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index db56434ffe..a6fad8f975 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -18,13 +18,13 @@ using namespace frc::sim; 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, units::kilogram_t mass, bool simulateGravity, + units::radian_t maxAngle, units::kilogram_t armMass, bool simulateGravity, const std::array& measurementStdDevs) : LinearSystemSim<2, 1, 1>(system, measurementStdDevs), m_r(armLength), m_minAngle(minAngle), m_maxAngle(maxAngle), - m_mass(mass), + m_armMass(armMass), m_gearbox(gearbox), m_gearing(gearing), m_simulateGravity(simulateGravity) {} @@ -32,11 +32,11 @@ SingleJointedArmSim::SingleJointedArmSim( 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, units::kilogram_t mass, bool simulateGravity, + units::radian_t maxAngle, units::kilogram_t armMass, bool simulateGravity, const std::array& measurementStdDevs) : SingleJointedArmSim( LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing), - gearbox, gearing, armLength, minAngle, maxAngle, mass, + gearbox, gearing, armLength, minAngle, maxAngle, armMass, simulateGravity, measurementStdDevs) {} bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const { @@ -94,7 +94,7 @@ Eigen::Vector SingleJointedArmSim::UpdateX( if (m_simulateGravity) { xdot += Eigen::Vector{ - 0.0, (m_mass * m_r * -9.8 * 3.0 / (m_mass * m_r * m_r) * + 0.0, (m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * std::cos(x(0))) .value()}; } diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 14b6d2df29..0965fdd4fb 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -31,11 +31,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * wrapped around. * @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 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, const std::array& measurementStdDevs = {0.0}); /** @@ -50,11 +52,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * wrapped around. * @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 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, const std::array& measurementStdDevs = {0.0}); /** @@ -133,5 +137,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { units::meter_t m_minHeight; units::meter_t m_maxHeight; double m_gearing; + bool m_simulateGravity; }; } // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 4ff29cc0b5..6c5a34ba87 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -30,14 +30,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @param armLength The length of the arm. * @param minAngle The minimum angle that the arm is capable of. * @param maxAngle The maximum angle that the arm is capable of. - * @param mass The mass of the arm. - * @param measurementStdDevs The standard deviations of the measurements. + * @param armMass The mass of the arm. * @param simulateGravity Whether gravity should be simulated or not. + * @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, units::kilogram_t mass, + units::radian_t maxAngle, units::kilogram_t armMass, bool simulateGravity, const std::array& measurementStdDevs = {0.0}); /** @@ -52,8 +52,8 @@ 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 mass The mass of the arm. - * @param measurementStdDevs The standard deviation of the measurement noise. * @param simulateGravity Whether gravity should be simulated or not. + * @param measurementStdDevs The standard deviation of the measurement noise. */ SingleJointedArmSim(const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi, @@ -150,7 +150,7 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::meter_t m_r; units::radian_t m_minAngle; units::radian_t m_maxAngle; - units::kilogram_t m_mass; + units::kilogram_t m_armMass; const DCMotor m_gearbox; double m_gearing; bool m_simulateGravity; diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 03022abf21..9d623c0464 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -2,6 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include #include #include "frc/Encoder.h" @@ -15,10 +16,13 @@ #include "frc/system/plant/LinearSystemId.h" #include "gtest/gtest.h" +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + TEST(ElevatorSimTest, StateSpaceSim) { frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m, - {0.01}); + true, {0.01}); frc2::PIDController controller(10, 0.0, 0.0); frc::PWMVictorSPX motor(0); @@ -45,7 +49,7 @@ TEST(ElevatorSimTest, StateSpaceSim) { TEST(ElevatorSimTest, MinMax) { frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m, - {0.01}); + true, {0.01}); for (size_t i = 0; i < 100; ++i) { sim.SetInput(Eigen::Vector{0.0}); sim.Update(20_ms); @@ -64,26 +68,19 @@ TEST(ElevatorSimTest, MinMax) { } TEST(ElevatorSimTest, Stability) { - static constexpr double kElevatorGearing = 100.0; - static constexpr units::meter_t kElevatorDrumRadius = 0.5_in; - static constexpr units::kilogram_t kCarriageMass = 4.0_kg; - frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); + frc::sim::ElevatorSim sim{ + frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, true}; - frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem( - m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing); - - Eigen::Vector x0{0.0, 0.0}; - Eigen::Vector u0{12.0}; - - Eigen::Vector x1{0.0, 0.0}; - for (size_t i = 0; i < 50; i++) { - x1 = frc::RKDP( - [&](const Eigen::Vector& x, - const Eigen::Vector& u) -> Eigen::Vector { - return system.A() * x + system.B() * u; - }, - x1, u0, 0.020_s); + sim.SetState(Eigen::Vector{0.0, 0.0}); + sim.SetInput(Eigen::Vector{12.0}); + for (int i = 0; i < 50; ++i) { + sim.Update(20_ms); } - EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1); + frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem( + frc::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100); + EXPECT_NEAR_UNITS(units::meter_t{system.CalculateX( + Eigen::Vector{0.0, 0.0}, + Eigen::Vector{12.0}, 20_ms * 50)(0)}, + sim.GetPosition(), 1_cm); } diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 4b2891defa..34609eddfa 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -64,6 +64,7 @@ class Robot : public frc::TimedRobot { kElevatorDrumRadius, kMinElevatorHeight, kMaxElevatorHeight, + true, {0.01}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index ecd237961e..f4b2b607a0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -30,6 +30,9 @@ public class ElevatorSim extends LinearSystemSim { // The max allowable height for the elevator. private final double m_maxHeight; + // Whether the simulator should simulate gravity. + private final boolean m_simulateGravity; + /** * Creates a simulated elevator mechanism. * @@ -39,6 +42,7 @@ public class ElevatorSim extends LinearSystemSim { * @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( LinearSystem plant, @@ -46,8 +50,17 @@ public class ElevatorSim extends LinearSystemSim { double gearing, double drumRadiusMeters, double minHeightMeters, - double maxHeightMeters) { - this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null); + double maxHeightMeters, + boolean simulateGravity) { + this( + plant, + gearbox, + gearing, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + null); } /** @@ -59,6 +72,7 @@ public class ElevatorSim extends LinearSystemSim { * @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 measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( @@ -68,6 +82,7 @@ public class ElevatorSim extends LinearSystemSim { double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, + boolean simulateGravity, Matrix measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; @@ -75,6 +90,7 @@ public class ElevatorSim extends LinearSystemSim { m_drumRadius = drumRadiusMeters; m_minHeight = minHeightMeters; m_maxHeight = maxHeightMeters; + m_simulateGravity = simulateGravity; } /** @@ -86,6 +102,7 @@ public class ElevatorSim extends LinearSystemSim { * @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, @@ -93,9 +110,17 @@ public class ElevatorSim extends LinearSystemSim { double carriageMassKg, double drumRadiusMeters, double minHeightMeters, - double maxHeightMeters) { + double maxHeightMeters, + boolean simulateGravity) { this( - gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, null); + gearbox, + gearing, + carriageMassKg, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + null); } /** @@ -107,6 +132,7 @@ public class ElevatorSim extends LinearSystemSim { * @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 measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( @@ -116,6 +142,7 @@ public class ElevatorSim extends LinearSystemSim { double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, + boolean simulateGravity, Matrix measurementStdDevs) { super( LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), @@ -125,6 +152,7 @@ public class ElevatorSim extends LinearSystemSim { m_drumRadius = drumRadiusMeters; m_minHeight = minHeightMeters; m_maxHeight = maxHeightMeters; + m_simulateGravity = simulateGravity; } /** @@ -222,10 +250,13 @@ public class ElevatorSim extends LinearSystemSim { // Calculate updated x-hat from Runge-Kutta. var updatedXhat = NumericalIntegration.rkdp( - (x, u_) -> - (m_plant.getA().times(x)) - .plus(m_plant.getB().times(u_)) - .plus(VecBuilder.fill(0, -9.8)), + (x, u_) -> { + Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_)); + if (m_simulateGravity) { + xdot = xdot.plus(VecBuilder.fill(0, -9.8)); + } + return xdot; + }, currentXhat, u, dtSeconds); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index a2e0ef4657..47c1f666ca 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -33,6 +33,7 @@ class ElevatorSimTest { 0.75 * 25.4 / 1000.0, 0.0, 3.0, + true, VecBuilder.fill(0.01)); try (var motor = new PWMVictorSPX(0); @@ -62,18 +63,15 @@ class ElevatorSimTest { @Test void testMinMax() { - var plant = - LinearSystemId.createElevatorSystem( - DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67); - var sim = new ElevatorSim( - plant, DCMotor.getVex775Pro(4), 14.67, + 8.0, 0.75 * 25.4 / 1000.0, 0.0, 1.0, + true, VecBuilder.fill(0.01)); for (int i = 0; i < 100; i++) { @@ -93,17 +91,21 @@ class ElevatorSimTest { @Test void testStability() { - var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10); + var sim = + new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, true); sim.setState(VecBuilder.fill(0, 0)); sim.setInput(12); - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; ++i) { sim.update(0.02); } + var system = + LinearSystemId.createElevatorSystem( + DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100); assertEquals( - sim.m_plant.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0), + system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0), sim.getPositionMeters(), - 0.1); + 0.01); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java index 3f2bae81ec..7e42a1ac27 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -59,6 +59,7 @@ public class Robot extends TimedRobot { kElevatorDrumRadius, kMinElevatorHeight, kMaxElevatorHeight, + true, VecBuilder.fill(0.01)); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);