[wpimath] Add SimpleMotorFeedforward::Calculate(velocity, nextVelocity) overload (#3183)

This is often more convenient than using the overload with velocity and
acceleration.

Fixes #3160.

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

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
 /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
 @SuppressWarnings("MemberName")
 public class SimpleMotorFeedforward {
@@ -47,6 +51,24 @@ public class SimpleMotorFeedforward {
     return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @param dtSeconds Time between velocity setpoints in seconds.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
+    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+
+    return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below:
 

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

@@ -6,6 +6,9 @@
 
 #include <wpi/MathExtras.h>
 
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "units/time.h"
 #include "units/voltage.h"
 
@@ -52,6 +55,31 @@ class SimpleMotorFeedforward {
     return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint, in distance per
+   *                        second.
+   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Eigen::Matrix<double, 1, 1> r;
+    r << currentVelocity.template to<double>();
+
+    Eigen::Matrix<double, 1, 1> nextR;
+    nextR << nextVelocity.template to<double>();
+
+    return kS * wpi::sgn(currentVelocity.template to<double>()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below:
 

wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java

@@ -0,0 +1,46 @@
+// 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class SimpleMotorFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    double Ks = 0.5;
+    double Kv = 3.0;
+    double Ka = 0.6;
+    double dt = 0.02;
+
+    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka);
+    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka);
+
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+    var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+
+    assertEquals(37.524995834325161 + 0.5, simpleMotor.calculate(2.0, 3.0, dt), 0.002);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + Ks,
+        simpleMotor.calculate(2.0, 3.0, dt),
+        0.002);
+
+    // These won't match exactly. It's just an approximation to make sure they're
+    // in the same ballpark.
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + Ks,
+        simpleMotor.calculate(2.0, 1.0 / dt),
+        2.0);
+  }
+}

wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp

@@ -0,0 +1,51 @@
+// 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 <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(SimpleMotorFeedforwardTest, Calculate) {
+  double Ks = 0.5;
+  double Kv = 3.0;
+  double Ka = 0.6;
+  auto dt = 0.02_s;
+
+  Eigen::Matrix<double, 1, 1> A;
+  A << -Kv / Ka;
+
+  Eigen::Matrix<double, 1, 1> B;
+  B << 1.0 / Ka;
+
+  frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+  frc::SimpleMotorFeedforward<units::meter> simpleMotor{
+      units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
+      units::volt_t{Ka} / 1_mps_sq};
+
+  Eigen::Matrix<double, 1, 1> r;
+  r << 2;
+  Eigen::Matrix<double, 1, 1> nextR;
+  nextR << 3;
+
+  EXPECT_NEAR(37.524995834325161 + Ks,
+              simpleMotor.Calculate(2_mps, 3_mps, dt).to<double>(), 0.002);
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
+              simpleMotor.Calculate(2_mps, 3_mps, dt).to<double>(), 0.002);
+
+  // These won't match exactly. It's just an approximation to make sure they're
+  // in the same ballpark.
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
+              simpleMotor.Calculate(2_mps, 1_mps / dt).to<double>(), 2.0);
+}
+
+}  // namespace frc