[wpimath] Move controller tests to wpimath (#3541)

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

@@ -0,0 +1,64 @@
+// 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 <wpi/numbers>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/HolonomicDriveController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/math.h"
+#include "units/time.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(HolonomicDriveControllerTest, ReachesReference) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+      frc::ProfiledPIDController<units::radian>{
+          1.0, 0.0, 0.0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              6.28_rad_per_s, 3.14_rad_per_s / 1_s}}};
+
+  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.0_mps, 4.0_mps_sq});
+
+  constexpr auto kDt = 0.02_s;
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, 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(robotPose.Rotation().Radians()), 0_rad,
+                    kAngularTolerance);
+}
+
+TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+      frc::ProfiledPIDController<units::radian>{
+          1, 0, 0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              4_rad_per_s, 2_rad_per_s / 1_s}}};
+
+  frc::ChassisSpeeds speeds = controller.Calculate(
+      frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+
+  EXPECT_EQ(0, speeds.omega.to<double>());
+}

wpimath/src/test/native/cpp/controller/PIDInputOutputTest.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 "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+class PIDInputOutputTest : public testing::Test {
+ protected:
+  frc2::PIDController* controller;
+
+  void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(PIDInputOutputTest, ContinuousInputTest) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180, 180);
+  EXPECT_DOUBLE_EQ(controller->Calculate(-179, 179), -2);
+
+  controller->EnableContinuousInput(0, 360);
+  EXPECT_DOUBLE_EQ(controller->Calculate(1, 359), -2);
+}
+
+TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
+}
+
+TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025, 0);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0, 0);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025, 0));
+}

wpimath/src/test/native/cpp/controller/PIDToleranceTest.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 "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+static constexpr double kSetpoint = 50.0;
+static constexpr double kRange = 200;
+static constexpr double kTolerance = 10.0;
+
+TEST(PIDToleranceTest, InitialTolerance) {
+  frc2::PIDController controller{0.5, 0.0, 0.0};
+  controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+  EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST(PIDToleranceTest, AbsoluteTolerance) {
+  frc2::PIDController controller{0.5, 0.0, 0.0};
+  controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+  EXPECT_TRUE(controller.AtSetpoint())
+      << "Error was not in tolerance when it should have been. Error was "
+      << controller.GetPositionError();
+
+  controller.SetTolerance(kTolerance);
+  controller.SetSetpoint(kSetpoint);
+
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+
+  controller.Calculate(0.0);
+
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+
+  controller.Calculate(kSetpoint + kTolerance / 2);
+
+  EXPECT_TRUE(controller.AtSetpoint())
+      << "Error was not in tolerance when it should have been. Error was "
+      << controller.GetPositionError();
+
+  controller.Calculate(kSetpoint + 10 * kTolerance);
+
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+}

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

@@ -0,0 +1,117 @@
+// 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 <wpi/numbers>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+
+class ProfiledPIDInputOutputTest : public testing::Test {
+ protected:
+  frc::ProfiledPIDController<units::degrees>* controller;
+
+  void SetUp() override {
+    controller = new frc::ProfiledPIDController<units::degrees>(
+        0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq});
+  }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  static constexpr units::degree_t kSetpoint{-179.0};
+  static constexpr units::degree_t kMeasurement{-179.0};
+  static constexpr units::degree_t kGoal{179.0};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            180_deg);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
+                                    units::radian_t{wpi::numbers::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.4826633343199735};
+  static constexpr units::radian_t kMeasurement{-3.1352207333939606};
+  static constexpr units::radian_t kGoal{-3.534162788601621};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
+                                    units::radian_t{wpi::numbers::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.5176604690006377};
+  static constexpr units::radian_t kMeasurement{3.1191729343822456};
+  static constexpr units::radian_t kGoal{2.709680418117445};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest4) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(0_rad,
+                                    units::radian_t{2.0 * wpi::numbers::pi});
+
+  static constexpr units::radian_t kSetpoint{2.78};
+  static constexpr units::radian_t kMeasurement{3.12};
+  static constexpr units::radian_t kGoal{2.71};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025_deg, 0_deg);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0_deg, 0_deg);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025_deg, 0_deg));
+}

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

@@ -0,0 +1,43 @@
+// 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/RamseteController.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(RamseteControllerTest, ReachesReference) {
+  frc::RamseteController controller{2.0, 0.7};
+  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});
+
+  constexpr auto kDt = 0.02_s;
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++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);
+}