[wpimath] Add LTV controllers (#4094)

This adds a unicycle controller that's a drop-in replacement for Ramsete
and a differential drive controller that controls the full pose and
outputs voltages. The main benefit is LQR-like tuning knobs using a
system model.

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

@@ -0,0 +1,128 @@
+// 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.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVDifferentialDriveControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  /** States of the drivetrain system. */
+  static class State {
+    /// X position in global coordinate frame.
+    public static final int kX = 0;
+
+    /// Y position in global coordinate frame.
+    public static final int kY = 1;
+
+    /// Heading in global coordinate frame.
+    public static final int kHeading = 2;
+
+    /// Left encoder velocity.
+    public static final int kLeftVelocity = 3;
+
+    /// Right encoder velocity.
+    public static final int kRightVelocity = 4;
+  }
+
+  private static final double kLinearV = 3.02; // V/(m/s)
+  private static final double kLinearA = 0.642; // V/(m/s²)
+  private static final double kAngularV = 1.382; // V/(m/s)
+  private static final double kAngularA = 0.08495; // V/(m/s²)
+  private static final LinearSystem<N2, N2, N2> plant =
+      LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
+  private static final double kTrackwidth = 0.9;
+
+  private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    double v = (x.get(State.kLeftVelocity, 0) + x.get(State.kRightVelocity, 0)) / 2.0;
+
+    var xdot = new Matrix<>(Nat.N5(), Nat.N1());
+    xdot.set(0, 0, v * Math.cos(x.get(State.kHeading, 0)));
+    xdot.set(1, 0, v * Math.sin(x.get(State.kHeading, 0)));
+    xdot.set(2, 0, (x.get(State.kRightVelocity, 0) - x.get(State.kLeftVelocity, 0)) / kTrackwidth);
+    xdot.assignBlock(
+        3, 0, plant.getA().times(x.block(Nat.N2(), Nat.N1(), 3, 0)).plus(plant.getB().times(u)));
+    return xdot;
+  }
+
+  @Test
+  void testReachesReference() {
+    final double kDt = 0.02;
+
+    final var controller =
+        new LTVDifferentialDriveController(
+            plant,
+            kTrackwidth,
+            VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95),
+            VecBuilder.fill(12.0, 12.0),
+            kDt);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    var x =
+        new MatBuilder<>(Nat.N5(), Nat.N1())
+            .fill(
+                robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0);
+
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+      robotPose =
+          new Pose2d(
+              x.get(State.kX, 0), x.get(State.kY, 0), new Rotation2d(x.get(State.kHeading, 0)));
+      final var output =
+          controller.calculate(
+              robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state);
+
+      x =
+          NumericalIntegration.rkdp(
+              LTVDifferentialDriveControllerTest::dynamics,
+              x,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right),
+              kDt);
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}

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

@@ -0,0 +1,65 @@
+// 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.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVUnicycleControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  void testReachesReference() {
+    final double kDt = 0.02;
+
+    final var controller =
+        new LTVUnicycleController(
+            VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+
+      var output = controller.calculate(robotPose, state);
+      robotPose =
+          robotPose.exp(
+              new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}

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

@@ -0,0 +1,101 @@
+// 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 <cmath>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/LTVDifferentialDriveController.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/math.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+                                                   180.0};
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+  /// X position in global coordinate frame.
+  static constexpr int kX = 0;
+
+  /// Y position in global coordinate frame.
+  static constexpr int kY = 1;
+
+  /// Heading in global coordinate frame.
+  static constexpr int kHeading = 2;
+
+  /// Left encoder velocity.
+  static constexpr int kLeftVelocity = 3;
+
+  /// Right encoder velocity.
+  static constexpr int kRightVelocity = 4;
+};
+
+static constexpr auto kLinearV = 3.02_V / 1_mps;
+static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
+static constexpr auto kAngularV = 1.382_V / 1_mps;
+static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
+static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem(
+    kLinearV, kLinearA, kAngularV, kAngularA);
+static constexpr auto kTrackwidth = 0.9_m;
+
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
+  double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
+
+  frc::Vectord<5> 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)) / kTrackwidth)
+                .value();
+  xdot.block<2, 1>(3, 0) = plant.A() * x.block<2, 1>(3, 0) + plant.B() * u;
+  return xdot;
+}
+
+TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
+  constexpr auto kDt = 0.02_s;
+
+  frc::LTVDifferentialDriveController controller{
+      plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
+  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  frc::Vectord<5> x = frc::Vectord<5>::Zero();
+  x(State::kX) = robotPose.X().value();
+  x(State::kY) = robotPose.Y().value();
+  x(State::kHeading) = robotPose.Rotation().Radians().value();
+
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    robotPose =
+        frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)},
+                    units::radian_t{x(State::kHeading)}};
+    auto [leftVoltage, rightVoltage] = controller.Calculate(
+        robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
+        units::meters_per_second_t{x(State::kRightVelocity)}, state);
+
+    x = frc::RKDP(&Dynamics, x,
+                  frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
+                  kDt);
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+                                      robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}

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

@@ -0,0 +1,44 @@
+// 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/MathUtil.h"
+#include "frc/controller/LTVUnicycleController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/math.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+                                                   180.0};
+
+TEST(LTVUnicycleControllerTest, ReachesReference) {
+  constexpr auto kDt = 0.02_s;
+
+  frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
+  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+    static_cast<void>(vy);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+                                      robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}