[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>

wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp

@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DifferentialDrivetrainSim.h"
+
+#include <frc/system/plant/LinearSystemId.h>
+
+#include "frc/system/RungeKutta.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DifferentialDrivetrainSim::DifferentialDrivetrainSim(
+    const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth,
+    DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius)
+    : m_plant(plant),
+      m_rb(trackWidth / 2.0),
+      m_wheelRadius(wheelRadius),
+      m_motor(driveMotor),
+      m_originalGearing(gearRatio),
+      m_currentGearing(gearRatio) {
+  m_x.setZero();
+  m_u.setZero();
+}
+
+DifferentialDrivetrainSim::DifferentialDrivetrainSim(
+    frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
+    units::kilogram_t mass, units::meter_t wheelRadius,
+    units::meter_t trackWidth)
+    : DifferentialDrivetrainSim(
+          frc::LinearSystemId::DrivetrainVelocitySystem(
+              driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
+          trackWidth, driveMotor, gearing, wheelRadius) {}
+
+void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
+                                          units::volt_t rightVoltage) {
+  m_u << leftVoltage.to<double>(), rightVoltage.to<double>();
+}
+
+void DifferentialDrivetrainSim::SetGearing(double newGearing) {
+  m_currentGearing = newGearing;
+}
+
+void DifferentialDrivetrainSim::Update(units::second_t dt) {
+  m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
+                   m_u, dt);
+}
+
+double DifferentialDrivetrainSim::GetState(int state) const {
+  return m_x(state);
+}
+
+double DifferentialDrivetrainSim::GetGearing() const {
+  return m_currentGearing;
+}
+
+Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
+  return m_x;
+}
+
+Rotation2d DifferentialDrivetrainSim::GetHeading() const {
+  return Rotation2d(units::radian_t(m_x(State::kHeading)));
+}
+
+Pose2d DifferentialDrivetrainSim::GetEstimatedPosition() const {
+  return Pose2d(units::meter_t(m_x(State::kX)), units::meter_t(m_x(State::kY)),
+                Rotation2d(units::radian_t(m_x(State::kHeading))));
+}
+
+units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
+  auto loadIleft =
+      m_motor.Current(units::radians_per_second_t(m_x(State::kLeftVelocity) *
+                                                  m_currentGearing /
+                                                  m_wheelRadius.to<double>()),
+                      units::volt_t(m_u(0))) *
+      wpi::sgn(m_u(0));
+
+  auto loadIRight =
+      m_motor.Current(units::radians_per_second_t(m_x(State::kRightVelocity) *
+                                                  m_currentGearing /
+                                                  m_wheelRadius.to<double>()),
+                      units::volt_t(m_u(1))) *
+      wpi::sgn(m_u(1));
+
+  return loadIleft + loadIRight;
+}
+
+Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
+    const Eigen::Matrix<double, 7, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  // Because G^2 can be factored out of A, we can divide by the old ratio
+  // squared and multiply by the new ratio squared to get a new drivetrain
+  // model.
+  Eigen::Matrix<double, 4, 2> B;
+  B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
+                        m_originalGearing / m_originalGearing;
+  B.block<2, 2>(2, 0).setZero();
+
+  // Because G can be factored out of B, we can divide by the old ratio and
+  // multiply by the new ratio to get a new drivetrain model.
+  Eigen::Matrix<double, 4, 4> A;
+  A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
+
+  A.block<2, 2>(2, 0).setIdentity();
+  A.block<4, 2>(0, 2).setZero();
+
+  double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
+
+  Eigen::Matrix<double, 7, 1> xdot;
+  xdot(0) = v * std::cos(x(State::kHeading));
+  xdot(1) = v * std::sin(x(State::kHeading));
+  xdot(2) =
+      ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb))
+          .to<double>();
+  xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u;
+  return xdot;
+}

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

wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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/FlywheelSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant,
+                         const DCMotor& gearbox, double gearing, bool addNoise,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim<1, 1, 1>(plant, addNoise, measurementStdDevs),
+      m_motor(gearbox),
+      m_gearing(gearing) {}
+
+FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
+                         units::kilogram_square_meter_t moi, bool addNoise,
+                         const std::array<double, 1>& measurementStdDevs)
+    : FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
+                  gearbox, gearing, addNoise, measurementStdDevs) {}
+
+units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
+  return units::radians_per_second_t{Y(0)};
+}
+
+units::ampere_t FlywheelSim::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.
+  return m_motor.Current(GetAngularVelocity() * m_gearing,
+                         units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}

wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp

@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SingleJointedArmSim.h"
+
+#include <cmath>
+
+#include <units/voltage.h>
+#include <wpi/MathExtras.h>
+
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+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)
+    : 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) {}
+
+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)
+    : SingleJointedArmSim(LinearSystemId::SingleJointedArmSystem(motor, J, G),
+                          motor, G, mass, armLength, minAngle, maxAngle,
+                          addNoise, measurementStdDevs) {}
+
+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)
+    : SingleJointedArmSim(
+          LinearSystemId::SingleJointedArmSystem(
+              motor, 1.0 / 3.0 * mass * armLength * armLength, G),
+          motor, G, mass, armLength, minAngle, maxAngle, addNoise,
+          measurementStdDevs) {}
+
+bool SingleJointedArmSim::HasHitLowerLimit(
+    const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) < m_minAngle.to<double>();
+}
+
+bool SingleJointedArmSim::HasHitUpperLimit(
+    const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) > m_maxAngle.to<double>();
+}
+
+units::radian_t SingleJointedArmSim::GetAngle() const {
+  return units::radian_t{m_x(0)};
+}
+
+units::radians_per_second_t SingleJointedArmSim::GetVelocity() const {
+  return units::radians_per_second_t{m_x(1)};
+}
+
+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) {
+  // 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)]]
+
+  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>());
+      },
+      currentXhat, u, dt);
+
+  // Check for collisions.
+  if (HasHitLowerLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_minAngle.to<double>(), 0.0);
+  } else if (HasHitUpperLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 0.0);
+  }
+  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));
+}