From fc991cb59c7b268c43dc39b1ee9674dc7104952d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 27 Sep 2020 00:13:59 -0700 Subject: [PATCH] [wpilib] Clean up simulation physics API (#2739) Some vestigial functions were never removed, and C++ single-jointed arm sim was missing a flag for disabling gravity simulation. This is useful for mechanisms like turrets. Fixes #2738. --- .../cpp/simulation/SingleJointedArmSim.cpp | 42 +++++++++++-------- .../frc/simulation/SingleJointedArmSim.h | 20 ++++++--- .../simulation/SingleJointedArmSimTest.cpp | 3 +- .../cpp/examples/ArmSimulation/cpp/Robot.cpp | 3 +- .../simulation/SingleJointedArmSim.java | 5 +-- 5 files changed, 43 insertions(+), 30 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 156889c4ec..e89affa4f0 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -22,34 +22,35 @@ SingleJointedArmSim::SingleJointedArmSim( const LinearSystem<2, 1, 1>& system, const DCMotor motor, double G, units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs, bool simulateGravity) : LinearSystemSim<2, 1, 1>(system, addNoise, measurementStdDevs), m_r(armLength), m_minAngle(minAngle), m_maxAngle(maxAngle), m_mass(mass), m_motor(motor), - m_gearing(G) {} + m_gearing(G), + m_simulateGravity(simulateGravity) {} SingleJointedArmSim::SingleJointedArmSim( const DCMotor& motor, units::kilogram_square_meter_t J, double G, units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs, bool simulateGravity) : SingleJointedArmSim(LinearSystemId::SingleJointedArmSystem(motor, J, G), motor, G, mass, armLength, minAngle, maxAngle, - addNoise, measurementStdDevs) {} + addNoise, measurementStdDevs, simulateGravity) {} SingleJointedArmSim::SingleJointedArmSim( const DCMotor& motor, double G, units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs) + const std::array& measurementStdDevs, bool simulateGravity) : SingleJointedArmSim( LinearSystemId::SingleJointedArmSystem( motor, 1.0 / 3.0 * mass * armLength * armLength, G), motor, G, mass, armLength, minAngle, maxAngle, addNoise, - measurementStdDevs) {} + measurementStdDevs, simulateGravity) {} bool SingleJointedArmSim::HasHitLowerLimit( const Eigen::Matrix& x) const { @@ -69,6 +70,14 @@ units::radians_per_second_t SingleJointedArmSim::GetVelocity() const { return units::radians_per_second_t{m_x(1)}; } +units::ampere_t SingleJointedArmSim::GetCurrentDraw() const { + // Reductions are greater than 1, so a reduction of 10:1 would mean the motor + // is spinning 10x faster than the output + units::radians_per_second_t motorVelocity{m_x(1) * m_gearing}; + return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) * + wpi::sgn(m_u(0)); +} + Eigen::Matrix SingleJointedArmSim::UpdateX( const Eigen::Matrix& currentXhat, const Eigen::Matrix& u, units::second_t dt) { @@ -84,10 +93,15 @@ Eigen::Matrix SingleJointedArmSim::UpdateX( auto updatedXhat = RungeKutta( [&](const auto& x, const auto& u) -> Eigen::Matrix { - return m_plant.A() * x + m_plant.B() * u + - MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 / - (m_mass * m_r * m_r) * std::cos(x(0))) - .template to()); + Eigen::Matrix xdot = m_plant.A() * x + m_plant.B() * u; + + if (m_simulateGravity) { + xdot += MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 / + (m_mass * m_r * m_r) * std::cos(x(0))) + .template to()); + } + + return xdot; }, currentXhat, u, dt); @@ -99,11 +113,3 @@ Eigen::Matrix SingleJointedArmSim::UpdateX( } return updatedXhat; } - -units::ampere_t SingleJointedArmSim::GetCurrentDraw() const { - // Reductions are greater than 1, so a reduction of 10:1 would mean the motor - // is spinning 10x faster than the output - units::radians_per_second_t motorVelocity{m_x(1) * m_gearing}; - return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) * - wpi::sgn(m_u(0)); -} diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 51b519895f..8f6c315d3b 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -37,17 +37,19 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @param addNoise Whether the sim should automatically add some * encoder noise. * @param measurementStdDevs The standard deviation of the measurement noise. + * @param simulateGravity If the affects of gravity should be simulated. */ SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor, double G, units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs); + const std::array& measurementStdDevs, + bool simulateGravity); /** * Creates a simulated arm mechanism. * * @param motor The type and number of motors on the arm gearbox. - * @param j The moment of inertia of the arm. + * @param J The moment of inertia of the arm. * @param G The gear ratio of the arm (numbers greater than 1 * represent reductions). * @param mass The mass of the arm. @@ -57,12 +59,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @param addNoise Whether the sim should automatically add some * encoder noise. * @param measurementStdDevs The standard deviation of the measurement noise. + * @param simulateGravity If the affects of gravity should be simulated. */ SingleJointedArmSim(const DCMotor& motor, units::kilogram_square_meter_t J, double G, units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs); + const std::array& measurementStdDevs, + bool simulateGravity); /** * Creates a simulated arm mechanism. @@ -79,11 +83,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @param addNoise Whether the sim should automatically add some * encoder noise. * @param measurementStdDevs The standard deviation of the measurement noise. + * @param simulateGravity If the affects of gravity should be simulated. */ SingleJointedArmSim(const DCMotor& motor, double G, units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs); + const std::array& measurementStdDevs, + bool simulateGravity); /** * Returns whether the arm has hit the lower limit. @@ -115,6 +121,9 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { */ units::radians_per_second_t GetVelocity() const; + units::ampere_t GetCurrentDraw() const override; + + protected: /** * Updates the state estimate of the arm. * @@ -126,8 +135,6 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { const Eigen::Matrix& currentXhat, const Eigen::Matrix& u, units::second_t dt) override; - units::ampere_t GetCurrentDraw() const override; - private: units::meter_t m_r; units::radian_t m_minAngle; @@ -135,5 +142,6 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::kilogram_t m_mass; const DCMotor m_motor; double m_gearing; + bool m_simulateGravity; }; } // namespace frc::sim diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 0434f45e39..8c4c659b6d 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -12,7 +12,8 @@ TEST(SingleJointedArmTest, Disabled) { frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 10_kg, - 9.5_in, -180_deg, 0_deg, false, {0.0}); + 9.5_in, -180_deg, 0_deg, false, {0.0}, + true); sim.ResetState(frc::MakeMatrix<2, 1>(0.0, 0.0)); for (size_t i = 0; i < 12 / 0.02; ++i) { diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index cba1260b2e..ce53c0266e 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -55,7 +55,8 @@ class Robot : public frc::TimedRobot { // with a standard deviation of 0.5 degrees. frc::sim::SingleJointedArmSim m_armSim{ m_armGearbox, 100.0, 5_kg, 30_in, - -180_deg, 0_deg, true, {(0.5_deg).to()}}; + -180_deg, 0_deg, true, {(0.5_deg).to()}, + true}; frc::sim::EncoderSim m_encoderSim{m_encoder}; public: diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 7a371b89bb..e7568940a9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -67,6 +67,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * the simulation and write simulated outputs to sensors. * * @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. @@ -188,10 +189,6 @@ public class SingleJointedArmSim extends LinearSystemSim { 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