diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 88e45c6186..58392d6709 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -18,13 +18,12 @@ 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 armMass, bool simulateGravity, + units::radian_t maxAngle, bool simulateGravity, const std::array& measurementStdDevs) : LinearSystemSim<2, 1, 1>(system, measurementStdDevs), - m_r(armLength), + m_armLen(armLength), m_minAngle(minAngle), m_maxAngle(maxAngle), - m_armMass(armMass), m_gearbox(gearbox), m_gearing(gearing), m_simulateGravity(simulateGravity) {} @@ -32,12 +31,12 @@ 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 armMass, bool simulateGravity, + units::radian_t maxAngle, bool simulateGravity, const std::array& measurementStdDevs) : SingleJointedArmSim( LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing), - gearbox, gearing, armLength, minAngle, maxAngle, armMass, - simulateGravity, measurementStdDevs) {} + gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity, + measurementStdDevs) {} bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const { return armAngle <= m_minAngle; @@ -78,24 +77,37 @@ void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) { Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u, units::second_t dt) { - // 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 * - // std::cos(theta)]] + // The torque on the arm is given by τ = F⋅r, where F is the force applied by + // gravity and r the distance from pivot to center of mass. Recall from + // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is + // torque on the arm, J is the mass-moment of inertia about the pivot axis, + // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J + // + // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal: + // + // α = (m⋅g⋅cos(θ))⋅r/J + // + // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the + // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a + // rod rotating about it's end, where L is the overall rod length. The mass + // distribution is assumed to be uniform. Substitute r=L/2 to find: + // + // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²) + // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²) + // α = 3/2⋅g⋅cos(θ)/L + // + // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu + // + // f(x, u) = Ax + Bu + [0 α]ᵀ + // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ Vectord<2> updatedXhat = RKDP( [&](const auto& x, const auto& u) -> Vectord<2> { Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u; if (m_simulateGravity) { - xdot += Vectord<2>{0.0, (m_armMass * m_r * -9.8 * 3.0 / - (m_armMass * m_r * m_r) * std::cos(x(0))) - .value()}; + xdot += Vectord<2>{ + 0.0, (3.0 / 2.0 * -9.8 / m_armLen * std::cos(x(0))).value()}; } return xdot; }, diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 57ad6b1a36..bf0de42bdf 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -30,15 +30,13 @@ 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 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 armMass, - bool simulateGravity, + units::radian_t maxAngle, bool simulateGravity, const std::array& measurementStdDevs = {0.0}); /** * Creates a simulated arm mechanism. @@ -51,15 +49,13 @@ 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 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, units::meter_t armLength, units::radian_t minAngle, - units::radian_t maxAngle, units::kilogram_t mass, - bool simulateGravity, + units::radian_t maxAngle, bool simulateGravity, const std::array& measurementStdDevs = {0.0}); /** @@ -146,10 +142,9 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::second_t dt) override; private: - units::meter_t m_r; + units::meter_t m_armLen; units::radian_t m_minAngle; units::radian_t m_maxAngle; - units::kilogram_t m_armMass; const DCMotor m_gearbox; double m_gearing; bool m_simulateGravity; diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 2e4c793e56..44264308da 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -8,8 +8,8 @@ #include "gtest/gtest.h" TEST(SingleJointedArmTest, Disabled) { - frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m, - 9.5_in, -180_deg, 0_deg, 10_lb, true); + frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 300, 3_kg_sq_m, + 30_in, -180_deg, 0_deg, true); sim.SetState(frc::Vectord<2>{0.0, 0.0}); for (size_t i = 0; i < 12 / 0.02; ++i) { diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp index 7456adb1ff..f93fa7aa9b 100644 --- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -18,7 +18,6 @@ #include "frc/simulation/LinearSystemSim.h" #include "frc/simulation/PWMSim.h" #include "frc/simulation/RoboRioSim.h" -#include "frc/simulation/SingleJointedArmSim.h" #include "frc/system/plant/LinearSystemId.h" #include "gtest/gtest.h" diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index 2027d255ef..8e51ccbcc9 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -63,12 +63,11 @@ class Robot : public frc::TimedRobot { // with a standard deviation of 1 encoder tick. frc::sim::SingleJointedArmSim m_armSim{ m_armGearbox, - 600.0, - frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 5_kg), + 200.0, + frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 8_kg), 30_in, -75_deg, 255_deg, - 5_kg, true, {kArmEncoderDistPerPulse}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; 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 b6a5e85dfb..26a41bd479 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 @@ -22,7 +22,7 @@ public class SingleJointedArmSim extends LinearSystemSim { private final double m_gearing; // The length of the arm. - private final double m_r; + private final double m_armLenMeters; // The minimum angle that the arm is capable of. private final double m_minAngle; @@ -30,9 +30,6 @@ public class SingleJointedArmSim extends LinearSystemSim { // The maximum angle that the arm is capable of. private final double m_maxAngle; - // The mass of the arm. - private final double m_armMass; - // Whether the simulator should simulate gravity. private final boolean m_simulateGravity; @@ -45,7 +42,6 @@ public class SingleJointedArmSim extends LinearSystemSim { * @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( @@ -55,7 +51,6 @@ public class SingleJointedArmSim extends LinearSystemSim { double armLengthMeters, double minAngleRads, double maxAngleRads, - double armMassKg, boolean simulateGravity) { this( plant, @@ -64,7 +59,6 @@ public class SingleJointedArmSim extends LinearSystemSim { armLengthMeters, minAngleRads, maxAngleRads, - armMassKg, simulateGravity, null); } @@ -78,7 +72,6 @@ public class SingleJointedArmSim extends LinearSystemSim { * @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. */ @@ -89,16 +82,14 @@ public class SingleJointedArmSim extends LinearSystemSim { double armLengthMeters, double minAngleRads, double maxAngleRads, - double armMassKg, boolean simulateGravity, Matrix measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; - m_r = armLengthMeters; + m_armLenMeters = armLengthMeters; m_minAngle = minAngleRads; m_maxAngle = maxAngleRads; - m_armMass = armMassKg; m_simulateGravity = simulateGravity; } @@ -111,7 +102,6 @@ public class SingleJointedArmSim extends LinearSystemSim { * @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( @@ -121,7 +111,6 @@ public class SingleJointedArmSim extends LinearSystemSim { double armLengthMeters, double minAngleRads, double maxAngleRads, - double armMassKg, boolean simulateGravity) { this( gearbox, @@ -130,7 +119,6 @@ public class SingleJointedArmSim extends LinearSystemSim { armLengthMeters, minAngleRads, maxAngleRads, - armMassKg, simulateGravity, null); } @@ -144,7 +132,6 @@ public class SingleJointedArmSim extends LinearSystemSim { * @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. */ @@ -155,7 +142,6 @@ public class SingleJointedArmSim extends LinearSystemSim { double armLengthMeters, double minAngleRads, double maxAngleRads, - double armMassKg, boolean simulateGravity, Matrix measurementStdDevs) { super( @@ -163,10 +149,9 @@ public class SingleJointedArmSim extends LinearSystemSim { measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; - m_r = armLengthMeters; + m_armLenMeters = armLengthMeters; m_minAngle = minAngleRads; m_maxAngle = maxAngleRads; - m_armMass = armMassKg; m_simulateGravity = simulateGravity; } @@ -268,30 +253,37 @@ public class SingleJointedArmSim extends LinearSystemSim { */ @Override protected Matrix updateX(Matrix currentXhat, Matrix 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)]] + // The torque on the arm is given by τ = F⋅r, where F is the force applied by + // gravity and r the distance from pivot to center of mass. Recall from + // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is + // torque on the arm, J is the mass-moment of inertia about the pivot axis, + // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J + // + // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal: + // + // α = (m⋅g⋅cos(θ))⋅r/J + // + // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the + // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a + // rod rotating about it's end, where L is the overall rod length. The mass + // distribution is assumed to be uniform. Substitute r=L/2 to find: + // + // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²) + // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²) + // α = 3/2⋅g⋅cos(θ)/L + // + // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu + // + // f(x, u) = Ax + Bu + [0 α]ᵀ + // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ + Matrix updatedXhat = NumericalIntegration.rkdp( (Matrix x, Matrix _u) -> { Matrix 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)))); + double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters; + xdot = xdot.plus(VecBuilder.fill(0, alphaGrav)); } return xdot; }, diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java index 1be29a9da4..f14bcd177f 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java @@ -14,14 +14,7 @@ import org.junit.jupiter.api.Test; class SingleJointedArmSimTest { SingleJointedArmSim m_sim = new SingleJointedArmSim( - DCMotor.getVex775Pro(2), - 100, - 3.0, - Units.inchesToMeters(19.0 / 2.0), - -Math.PI, - 0.0, - 10.0 / 2.2, - true); + DCMotor.getVex775Pro(2), 300, 3.0, Units.inchesToMeters(30.0), -Math.PI, 0.0, true); @Test void testArmDisabled() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java index c84d9228bc..a46be5bb03 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java @@ -54,8 +54,8 @@ public class Robot extends TimedRobot { private final Joystick m_joystick = new Joystick(kJoystickPort); // Simulation classes help us simulate what's going on, including gravity. - private static final double m_armReduction = 600; - private static final double m_armMass = 5.0; // Kilograms + private static final double m_armReduction = 200; + private static final double m_armMass = 8.0; // Kilograms private static final double m_armLength = Units.inchesToMeters(30); // This arm sim represents an arm that can travel from -75 degrees (rotated down front) // to 255 degrees (rotated down in the back). @@ -67,7 +67,6 @@ public class Robot extends TimedRobot { m_armLength, Units.degreesToRadians(-75), Units.degreesToRadians(255), - m_armMass, true, VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick );