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

[wpilib] Allow disabling ElevatorSim gravity (#4145)

Browse Source 
Closes #4144.
This commit is contained in:
Tyler Veness
2022-04-24 07:19:18 -07:00
committed by GitHub
parent aaa69f6717
commit 9d20ab3024
9 changed files with 97 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -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<double, 1>& 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<double, 1>& 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<double, 2> ElevatorSim::UpdateX(
auto updatedXhat = RKDP(
[&](const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
return m_plant.A() * x + m_plant.B() * u_ +
Eigen::Vector<double, 2>{0.0, -9.8};
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{0.0, -9.8};
}
return xdot;
},
currentXhat, u, dt);
// Check for collision after updating x-hat.

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

@@ -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<double, 1>& 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<double, 1>& 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<double, 2> SingleJointedArmSim::UpdateX(
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{
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()};
}

5
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

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

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

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

39
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -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 <units/math.h>
#include <units/time.h>
#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<double, 1>{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<double, 2> x0{0.0, 0.0};
Eigen::Vector<double, 1> u0{12.0};
Eigen::Vector<double, 2> x1{0.0, 0.0};
for (size_t i = 0; i < 50; i++) {
x1 = frc::RKDP(
[&](const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
return system.A() * x + system.B() * u;
},
x1, u0, 0.020_s);
sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
sim.SetInput(Eigen::Vector<double, 1>{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<double, 2>{0.0, 0.0},
Eigen::Vector<double, 1>{12.0}, 20_ms * 50)(0)},
sim.GetPosition(), 1_cm);
}

1
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
View File

@@ -64,6 +64,7 @@ class Robot : public frc::TimedRobot {
kElevatorDrumRadius,
kMinElevatorHeight,
kMaxElevatorHeight,
true,
{0.01}};
frc::sim::EncoderSim m_encoderSim{m_encoder};

47
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -30,6 +30,9 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
// 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<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 simulateGravity Whether gravity should be simulated or not.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
@@ -46,8 +50,17 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
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<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 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<N2, N1, N1> {
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
@@ -75,6 +90,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
m_drumRadius = drumRadiusMeters;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
}
/**
@@ -86,6 +102,7 @@ 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 simulateGravity Whether gravity should be simulated or not.
*/
public ElevatorSim(
DCMotor gearbox,
@@ -93,9 +110,17 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
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<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 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<N2, N1, N1> {
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
Matrix<N1, N1> measurementStdDevs) {
super(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
@@ -125,6 +152,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
m_drumRadius = drumRadiusMeters;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
}
/**
@@ -222,10 +250,13 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
// 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<N2, N1> 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);

20
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View File

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

1
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
View File

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