[wpimath] Add ImplicitModelFollower (#4056)

2026-06-25 01:41:43 +00:00 · 2022-03-20 00:36:12 -07:00
parent 78108c2aba
commit 8d79dc8738
4 changed files with 493 additions and 0 deletions
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
@@ -0,0 +1,109 @@
+// 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 static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class ImplicitModelFollowerTest {
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  void testSameModel() {
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant, dt);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+
+    // Backward
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+
+    // Rotate CCW
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  void testSlowerRefModel() {
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    // Linear acceleration is slower, but angular acceleration is the same
+    var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
+
+    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef, dt);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertTrue(x.get(0, 0) >= xImf.get(0, 0));
+      assertTrue(x.get(1, 0) >= xImf.get(1, 0));
+    }
+
+    // Backward
+    x.fill(0.0);
+    xImf.fill(0.0);
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertTrue(x.get(0, 0) <= xImf.get(0, 0));
+      assertTrue(x.get(1, 0) <= xImf.get(1, 0));
+    }
+
+    // Rotate CCW
+    x.fill(0.0);
+    xImf.fill(0.0);
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5);
+    }
+  }
+}
--- a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
@@ -0,0 +1,109 @@
+// 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 "frc/controller/ImplicitModelFollower.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+namespace frc {
+
+TEST(ImplicitModelFollowerTest, SameModel) {
+  constexpr auto dt = 5_ms;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  ImplicitModelFollower<2, 2> imf{plant, plant, dt};
+
+  Eigen::Vector<double, 2> x{0.0, 0.0};
+  Eigen::Vector<double, 2> xImf{0.0, 0.0};
+
+  // Forward
+  Eigen::Vector<double, 2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xImf(0));
+    EXPECT_DOUBLE_EQ(x(1), xImf(1));
+  }
+
+  // Backward
+  u = Eigen::Vector<double, 2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xImf(0));
+    EXPECT_DOUBLE_EQ(x(1), xImf(1));
+  }
+
+  // Rotate CCW
+  u = Eigen::Vector<double, 2>{-12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xImf(0));
+    EXPECT_DOUBLE_EQ(x(1), xImf(1));
+  }
+}
+
+TEST(ImplicitModelFollowerTest, SlowerRefModel) {
+  constexpr auto dt = 5_ms;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  // Linear acceleration is slower, but angular acceleration is the same
+  auto plantRef = LinearSystemId::IdentifyDrivetrainSystem(
+      Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0});
+
+  ImplicitModelFollower<2, 2> imf{plant, plantRef, dt};
+
+  Eigen::Vector<double, 2> x{0.0, 0.0};
+  Eigen::Vector<double, 2> xImf{0.0, 0.0};
+
+  // Forward
+  Eigen::Vector<double, 2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_GE(x(0), xImf(0));
+    EXPECT_GE(x(1), xImf(1));
+  }
+
+  // Backward
+  x.setZero();
+  xImf.setZero();
+  u = Eigen::Vector<double, 2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_LE(x(0), xImf(0));
+    EXPECT_LE(x(1), xImf(1));
+  }
+
+  // Rotate CCW
+  x.setZero();
+  xImf.setZero();
+  u = Eigen::Vector<double, 2>{-12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_NEAR(x(0), xImf(0), 1e-5);
+    EXPECT_NEAR(x(1), xImf(1), 1e-5);
+  }
+}
+
+}  // namespace frc