[wpimath] Add DifferentialDriveFeedforward classes which wrap LinearPlantInversionFeedforward (#4598)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java

@@ -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.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
+public class DifferentialDriveFeedforward {
+  private final LinearSystem<N2, N2, N2> m_plant;
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
+   * @param trackwidth The distance between the differential drive's left and right wheels, in
+   *     meters.
+   */
+  public DifferentialDriveFeedforward(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+    m_plant =
+        LinearSystemId.identifyDrivetrainSystem(
+            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+  }
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
+   */
+  public DifferentialDriveFeedforward(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+    m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+  }
+
+  /**
+   * Calculates the differential drive feedforward inputs given velocity setpoints.
+   *
+   * @param currentLeftVelocity The current left velocity of the differential drive in
+   *     meters/second.
+   * @param nextLeftVelocity The next left velocity of the differential drive in meters/second.
+   * @param currentRightVelocity The current right velocity of the differential drive in
+   *     meters/second.
+   * @param nextRightVelocity The next right velocity of the differential drive in meters/second.
+   * @param dtSeconds Discretization timestep.
+   * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      double currentLeftVelocity,
+      double nextLeftVelocity,
+      double currentRightVelocity,
+      double nextRightVelocity,
+      double dtSeconds) {
+    var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds);
+    var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
+    var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
+    var u = feedforward.calculate(r, nextR);
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+}

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

@@ -0,0 +1,37 @@
+// 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/DifferentialDriveFeedforward.h"
+
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+
+DifferentialDriveFeedforward::DifferentialDriveFeedforward(
+    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+    decltype(1_V / 1_rad_per_s) kVAngular,
+    decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
+    : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {}
+
+DifferentialDriveFeedforward::DifferentialDriveFeedforward(
+    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+    decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
+    : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kVLinear, kALinear, kVAngular, kAAngular)} {}
+
+DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
+    units::meters_per_second_t currentLeftVelocity,
+    units::meters_per_second_t nextLeftVelocity,
+    units::meters_per_second_t currentRightVelocity,
+    units::meters_per_second_t nextRightVelocity, units::second_t dt) {
+  frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
+
+  frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
+  frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
+  auto u = feedforward.Calculate(r, nextR);
+  return {units::volt_t{u(0)}, units::volt_t{u(1)}};
+}

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

@@ -0,0 +1,83 @@
+// 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 "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class which computes the feedforward outputs for a differential
+ * drive drivetrain.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
+  frc::LinearSystem<2, 2, 2> m_plant;
+
+ public:
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per
+   * second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per
+   * second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per
+   * second squared).
+   * @param trackwidth The distance between the differential drive's left and
+   * right wheels, in meters.
+   */
+  DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+                               decltype(1_V / 1_mps_sq) kALinear,
+                               decltype(1_V / 1_rad_per_s) kVAngular,
+                               decltype(1_V / 1_rad_per_s_sq) kAAngular,
+                               units::meter_t trackwidth);
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per
+   * second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per
+   * second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per
+   * second squared).
+   */
+  DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+                               decltype(1_V / 1_mps_sq) kALinear,
+                               decltype(1_V / 1_mps) kVAngular,
+                               decltype(1_V / 1_mps_sq) kAAngular);
+
+  /**
+   * Calculates the differential drive feedforward inputs given velocity
+   * setpoints.
+   *
+   * @param currentLeftVelocity The current left velocity of the differential
+   * drive in meters/second.
+   * @param nextLeftVelocity The next left velocity of the differential drive in
+   * meters/second.
+   * @param currentRightVelocity The current right velocity of the differential
+   * drive in meters/second.
+   * @param nextRightVelocity The next right velocity of the differential drive
+   * in meters/second.
+   * @param dt Discretization timestep.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      units::meters_per_second_t currentLeftVelocity,
+      units::meters_per_second_t nextLeftVelocity,
+      units::meters_per_second_t currentRightVelocity,
+      units::meters_per_second_t nextRightVelocity, units::second_t dt);
+};
+}  // namespace frc