[wpilib] Add physics simulation support with state-space (#2615)

This includes physics simulation support for arms/elevator models, as well as differential drivetrains. Swerve might be added at a later date. Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com> Co-authored-by: Tyler Veness <calcmogul@gmail.com>
2026-06-20 00:51:42 +00:00 · 2020-09-20 09:39:52 -07:00
parent 0503225928
commit b61f08d3fa
43 changed files with 3787 additions and 31 deletions
--- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/ElevatorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+ElevatorSim::ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass,
+                         double gearing, units::meter_t drumRadius,
+                         units::meter_t minHeight, units::meter_t maxHeight,
+                         bool addNoise,
+                         const std::array<double, 1>& m_measurementStdDevs)
+    : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
+                                                     drumRadius, gearing),
+                      addNoise, m_measurementStdDevs),
+      m_motor(gearbox),
+      m_drumRadius(drumRadius),
+      m_minHeight(minHeight),
+      m_maxHeight(maxHeight),
+      m_gearing(gearing) {}
+
+bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) < m_minHeight.to<double>();
+}
+
+bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) > m_maxHeight.to<double>();
+}
+
+units::meter_t ElevatorSim::GetPosition() const { return units::meter_t{Y(0)}; }
+
+units::meters_per_second_t ElevatorSim::GetVelocity() const {
+  return units::meters_per_second_t{m_x(1)};
+}
+
+units::ampere_t ElevatorSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+
+  // v = r w, so w = v / r
+  units::meters_per_second_t velocity{m_x(1)};
+  units::radians_per_second_t motorVelocity =
+      velocity / m_drumRadius * m_gearing * 1_rad;
+
+  // Perform calculation and return.
+  return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
+    const Eigen::Matrix<double, 2, 1>& currentXhat,
+    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+  auto updatedXhat = RungeKutta(
+      [&](const Eigen::Matrix<double, 2, 1>& x,
+          const Eigen::Matrix<double, 1, 1>& u_)
+          -> Eigen::Matrix<double, 2, 1> {
+        return m_plant.A() * x + m_plant.B() * u_ + MakeMatrix<2, 1>(0.0, -9.8);
+      },
+      currentXhat, u, dt);
+  // Check for collision after updating x-hat.
+  if (HasHitLowerLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_minHeight.to<double>(), 0.0);
+  } else if (HasHitUpperLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
+  }
+  return updatedXhat;
+}