Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Clean up simulation physics API (#2739)

Browse Source 
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.
This commit is contained in:
Tyler Veness
2020-09-27 00:13:59 -07:00
committed by GitHub
parent 17d3d2f754
commit fc991cb59c
5 changed files with 43 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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<double, 1>& measurementStdDevs)
const std::array<double, 1>& 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<double, 1>& measurementStdDevs)
const std::array<double, 1>& 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<double, 1>& measurementStdDevs)
const std::array<double, 1>& 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<double, 2, 1>& 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<double, 2, 1> SingleJointedArmSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
@@ -84,10 +93,15 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
auto updatedXhat = RungeKutta(
[&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
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<double>());
Eigen::Matrix<double, 2, 1> 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<double>());
}
return xdot;
},
currentXhat, u, dt);
@@ -99,11 +113,3 @@ Eigen::Matrix<double, 2, 1> 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));
}