Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Fix MOI calculation error in SingleJointedArmSim (#4968)

Browse Source 
Previous calculation derivation mixed up length and distance to CG.
This commit is contained in:
Matt
2023-01-18 23:40:39 -05:00
committed by GitHub
parent 42c997a3c4
commit 27ba096ea1
8 changed files with 69 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -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<double, 1>& 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<double, 1>& 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;
},

11
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -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<double, 1>& 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<double, 1>& 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;

4
wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
View File

@@ -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) {

1
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
View File

@@ -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"

5
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
View File

@@ -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};

66
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -22,7 +22,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
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<N2, N1, N1> {
// 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<N2, N1, N1> {
* @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<N2, N1, N1> {
double armLengthMeters,
double minAngleRads,
double maxAngleRads,
double armMassKg,
boolean simulateGravity) {
this(
plant,
@@ -64,7 +59,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
armLengthMeters,
minAngleRads,
maxAngleRads,
armMassKg,
simulateGravity,
null);
}
@@ -78,7 +72,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @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<N2, N1, N1> {
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_armLenMeters = armLengthMeters;
m_minAngle = minAngleRads;
m_maxAngle = maxAngleRads;
m_armMass = armMassKg;
m_simulateGravity = simulateGravity;
}
@@ -111,7 +102,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @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<N2, N1, N1> {
double armLengthMeters,
double minAngleRads,
double maxAngleRads,
double armMassKg,
boolean simulateGravity) {
this(
gearbox,
@@ -130,7 +119,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
armLengthMeters,
minAngleRads,
maxAngleRads,
armMassKg,
simulateGravity,
null);
}
@@ -144,7 +132,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @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<N2, N1, N1> {
double armLengthMeters,
double minAngleRads,
double maxAngleRads,
double armMassKg,
boolean simulateGravity,
Matrix<N1, N1> measurementStdDevs) {
super(
@@ -163,10 +149,9 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
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<N2, N1, N1> {
*/
@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)]]
// 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<N2, N1> updatedXhat =
NumericalIntegration.rkdp(
(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))));
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;
},

9
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
View File

@@ -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() {

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
View File

@@ -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
);