[wpilib] Fix trajectory sampling in DifferentialDriveSim test (#3821)

Also rename C++ test file to match class name. Fixes #3818.
2026-06-19 00:41:43 +00:00 · 2021-12-22 22:27:51 -08:00
parent 6f91f37cd0
commit c3fd20db59
2 changed files with 6 additions and 7 deletions
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -0,0 +1,115 @@
+// 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 <units/current.h>
+#include <units/math.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/simulation/DifferentialDrivetrainSim.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+
+TEST(DifferentialDrivetrainSimTest, Convergence) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{
+      plant, 24_in, motor,
+      1.0,   2_in,  {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
+
+  frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
+  frc::RamseteController ramsete;
+
+  feedforward.Reset(Eigen::Vector<double, 2>{0.0, 0.0});
+
+  // Ground truth.
+  Eigen::Vector<double, 7> groundTruthX = Eigen::Vector<double, 7>::Zero();
+
+  frc::TrajectoryConfig config{1_mps, 1_mps_sq};
+  config.AddConstraint(
+      frc::DifferentialDriveKinematicsConstraint(kinematics, 1_mps));
+
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config);
+
+  for (auto t = 0_s; t < trajectory.TotalTime(); t += 20_ms) {
+    auto state = trajectory.Sample(t);
+    auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
+
+    auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
+    auto voltages =
+        feedforward.Calculate(Eigen::Vector<double, 2>{l.value(), r.value()});
+
+    // Sim periodic code.
+    sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0)));
+    sim.Update(20_ms);
+
+    // Update ground truth.
+    groundTruthX = frc::RK4(
+        [&sim](const auto& x, const auto& u) -> Eigen::Vector<double, 7> {
+          return sim.Dynamics(x, u);
+        },
+        groundTruthX, voltages, 20_ms);
+  }
+
+  // 2 inch tolerance is OK since our ground truth is an approximation of the
+  // ODE solution using RK4 anyway
+  EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05);
+  EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05);
+  EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
+}
+
+TEST(DifferentialDrivetrainSimTest, Current) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  sim.SetInputs(-12_V, 12_V);
+  for (int i = 0; i < 10; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+
+  sim.SetInputs(12_V, 12_V);
+  for (int i = 0; i < 20; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+
+  sim.SetInputs(-12_V, 12_V);
+  for (int i = 0; i < 30; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+}
+
+TEST(DifferentialDrivetrainSimTest, ModelStability) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  sim.SetInputs(2_V, 4_V);
+
+  // 10 seconds should be enough time to verify stability
+  for (int i = 0; i < 500; ++i) {
+    sim.Update(20_ms);
+  }
+
+  EXPECT_LT(units::math::abs(sim.GetPose().Translation().Norm()), 100_m);
+}