[wpimath] Add DifferentialDriveAccelerationLimiter (#4091)

wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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 "frc/controller/DifferentialDriveAccelerationLimiter.h"
+
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
+    LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+    units::meters_per_second_squared_t maxLinearAccel,
+    units::radians_per_second_squared_t maxAngularAccel)
+    : m_system{std::move(system)},
+      m_trackwidth{trackwidth},
+      m_maxLinearAccel{maxLinearAccel},
+      m_maxAngularAccel{maxAngularAccel} {}
+
+DifferentialDriveAccelerationLimiter::WheelVoltages
+DifferentialDriveAccelerationLimiter::Calculate(
+    units::meters_per_second_t leftVelocity,
+    units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
+    units::volt_t rightVoltage) {
+  Eigen::Vector<double, 2> u{leftVoltage.value(), rightVoltage.value()};
+
+  // Find unconstrained wheel accelerations
+  Eigen::Vector<double, 2> x{leftVelocity.value(), rightVelocity.value()};
+  Eigen::Vector<double, 2> dxdt = m_system.A() * x + m_system.B() * u;
+
+  // Converts from wheel accelerations to linear and angular acceleration
+  // a = (dxdt(0) + dxdt(1)) / 2.0
+  // alpha = (dxdt(1) - dxdt(0)) / trackwidth
+  Eigen::Matrix<double, 2, 2> M{
+      {0.5, 0.5}, {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
+
+  // Convert to linear and angular accelerations, constrain them, then convert
+  // back
+  Eigen::Vector<double, 2> accels = M * dxdt;
+  if (accels(0) > m_maxLinearAccel.value()) {
+    accels(0) = m_maxLinearAccel.value();
+  } else if (accels(0) < -m_maxLinearAccel.value()) {
+    accels(0) = -m_maxLinearAccel.value();
+  }
+  if (accels(1) > m_maxAngularAccel.value()) {
+    accels(1) = m_maxAngularAccel.value();
+  } else if (accels(1) < -m_maxAngularAccel.value()) {
+    accels(1) = -m_maxAngularAccel.value();
+  }
+  dxdt = M.householderQr().solve(accels);
+
+  // Find voltages for the given wheel accelerations
+  // dx/dt = Ax + Bu
+  // u = B⁻¹(dx/dt - Ax)
+  u = m_system.B().householderQr().solve(dxdt - m_system.A() * x);
+
+  return {units::volt_t{u(0)}, units::volt_t{u(1)}};
+}

wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "Eigen/Core"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/length.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and
+ * angular acceleration.
+ *
+ * The differential drive model can be created via the functions in
+ * LinearSystemId.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
+ public:
+  /**
+   * Motor voltages for a differential drive.
+   */
+  struct WheelVoltages {
+    units::volt_t left = 0_V;
+    units::volt_t right = 0_V;
+  };
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The trackwidth.
+   * @param maxLinearAccel The maximum linear acceleration.
+   * @param maxAngularAccel The maximum angular acceleration.
+   */
+  DifferentialDriveAccelerationLimiter(
+      LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+      units::meters_per_second_squared_t maxLinearAccel,
+      units::radians_per_second_squared_t maxAngularAccel);
+
+  /**
+   * Returns the next voltage pair subject to acceleraiton constraints.
+   *
+   * @param leftVelocity The left wheel velocity.
+   * @param rightVelocity The right wheel velocity.
+   * @param leftVoltage The unconstrained left motor voltage.
+   * @param rightVoltage The unconstrained right motor voltage.
+   * @return The constrained wheel voltages.
+   */
+  WheelVoltages Calculate(units::meters_per_second_t leftVelocity,
+                          units::meters_per_second_t rightVelocity,
+                          units::volt_t leftVoltage,
+                          units::volt_t rightVoltage);
+
+ private:
+  LinearSystem<2, 2, 2> m_system;
+  units::meter_t m_trackwidth;
+  units::meters_per_second_squared_t m_maxLinearAccel;
+  units::radians_per_second_squared_t m_maxAngularAccel;
+};
+
+}  // namespace frc