[wpimath] Replace Trajectory API with new one

wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp

@@ -53,11 +53,13 @@ void FieldObject2d::SetPoses(std::initializer_list<wpi::math::Pose2d> poses) {
   SetPoses({poses.begin(), poses.end()});
 }
 
-void FieldObject2d::SetTrajectory(const wpi::math::Trajectory& trajectory) {
+template <typename SampleType>
+void FieldObject2d::SetTrajectory(
+    const wpi::math::Trajectory<SampleType>& trajectory) {
   std::scoped_lock lock(m_mutex);
   m_poses.clear();
-  m_poses.reserve(trajectory.States().size());
-  for (auto&& state : trajectory.States()) {
+  m_poses.reserve(trajectory.Samples().size());
+  for (auto&& state : trajectory.Samples()) {
     m_poses.push_back(state.pose);
   }
   UpdateEntry();

wpilibc/src/main/native/include/wpi/smartdashboard/FieldObject2d.hpp

@@ -18,6 +18,7 @@
 #include "wpi/util/mutex.hpp"
 
 namespace wpi::math {
+template <typename SampleType>
 class Trajectory;
 }
 
@@ -83,7 +84,8 @@ class FieldObject2d {
    *
    * @param trajectory The trajectory from which poses should be added.
    */
-  void SetTrajectory(const wpi::math::Trajectory& trajectory);
+  template <typename SampleType>
+  void SetTrajectory(const wpi::math::Trajectory<SampleType>& trajectory);
 
   /**
    * Get multiple poses.

wpilibc/src/main/python/semiwrap/FieldObject2d.yml

@@ -1,4 +1,7 @@
 extra_includes:
+- wpi/math/trajectory/DifferentialTrajectory.hpp
+- wpi/math/trajectory/SplineTrajectory.hpp
+- wpi/math/trajectory/HolonomicTrajectory.hpp
 - wpi/math/trajectory/Trajectory.hpp
 
 classes:
@@ -18,6 +21,10 @@ classes:
           std::initializer_list<wpi::math::Pose2d>:
             ignore: true
       SetTrajectory:
+        template_impls:
+        - [wpi::math::SplineSample]
+        - [wpi::math::DifferentialSample]
+        - [wpi::math::TrajectorySample]
       GetPoses:
         overloads:
           '[const]':

wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp

@@ -44,7 +44,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
       wpi::math::Pose2d{}, {}, wpi::math::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 state = trajectory.SampleAt(t);
     auto feedbackOut = feedback.Calculate(sim.GetPose(), state);
 
     auto [l, r] = kinematics.ToWheelSpeeds(feedbackOut);

wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp

@@ -12,12 +12,11 @@
 
 class Robot : public wpi::TimedRobot {
  public:
-  Robot() {
-    m_trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
-        wpi::math::Pose2d{2_m, 2_m, 0_rad}, {},
-        wpi::math::Pose2d{6_m, 4_m, 0_rad},
-        wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
-  }
+  Robot()
+      : m_trajectory(wpi::math::TrajectoryGenerator::GenerateTrajectory(
+            wpi::math::Pose2d{2_m, 2_m, 0_rad}, {},
+            wpi::math::Pose2d{6_m, 4_m, 0_rad},
+            wpi::math::TrajectoryConfig(2_mps, 2_mps_sq))) {}
 
   void RobotPeriodic() override { m_drive.Periodic(); }
 
@@ -28,7 +27,7 @@ class Robot : public wpi::TimedRobot {
 
   void AutonomousPeriodic() override {
     auto elapsed = m_timer.Get();
-    auto reference = m_trajectory.Sample(elapsed);
+    auto reference = m_trajectory.SampleAt(elapsed);
     auto speeds = m_feedback.Calculate(m_drive.GetPose(), reference);
     m_drive.Drive(speeds.vx, speeds.omega);
   }
@@ -60,7 +59,7 @@ class Robot : public wpi::TimedRobot {
   wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
 
   Drivetrain m_drive;
-  wpi::math::Trajectory m_trajectory;
+  wpi::math::SplineTrajectory m_trajectory;
   wpi::math::LTVUnicycleController m_feedback{20_ms};
   wpi::Timer m_timer;
 };

wpilibj/src/main/java/org/wpilib/smartdashboard/FieldObject2d.java

@@ -13,6 +13,7 @@
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.trajectory.Trajectory;
+import org.wpilib.math.trajectory.TrajectorySample;
 import org.wpilib.networktables.DoubleArrayEntry;
 import org.wpilib.units.measure.Distance;
 
@@ -105,9 +106,10 @@ public synchronized void setPoses(Pose2d... poses) {
    *
    * @param trajectory The trajectory from which the poses should be added.
    */
-  public synchronized void setTrajectory(Trajectory trajectory) {
+  public synchronized <SampleType extends TrajectorySample> void setTrajectory(
+      Trajectory<SampleType> trajectory) {
     m_poses.clear();
-    for (Trajectory.State state : trajectory.getStates()) {
+    for (SampleType state : trajectory.getSamples()) {
       m_poses.add(state.pose);
     }
     updateEntry();

wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java

@@ -61,8 +61,8 @@ void testConvergence() {
             new TrajectoryConfig(1, 1)
                 .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
 
-    for (double t = 0; t < traj.getTotalTime(); t += 0.020) {
-      var state = traj.sample(t);
+    for (double t = 0; t < traj.duration; t += 0.020) {
+      var state = traj.sampleAt(t);
       var feedbackOut = feedback.calculate(sim.getPose(), state);
 
       var wheelSpeeds = kinematics.toWheelSpeeds(feedbackOut);

wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Robot.java

@@ -12,6 +12,7 @@
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.trajectory.SplineSample;
 import org.wpilib.math.trajectory.Trajectory;
 import org.wpilib.math.trajectory.TrajectoryConfig;
 import org.wpilib.math.trajectory.TrajectoryGenerator;
@@ -28,7 +29,7 @@ public class Robot extends TimedRobot {
   private final Drivetrain m_drive = new Drivetrain();
   private final LTVUnicycleController m_feedback = new LTVUnicycleController(0.020);
   private final Timer m_timer = new Timer();
-  private final Trajectory m_trajectory;
+  private final Trajectory<SplineSample> m_trajectory;
 
   /** Called once at the beginning of the robot program. */
   public Robot() {
@@ -48,13 +49,13 @@ public void robotPeriodic() {
   @Override
   public void autonomousInit() {
     m_timer.restart();
-    m_drive.resetOdometry(m_trajectory.getInitialPose());
+    m_drive.resetOdometry(m_trajectory.start().pose);
   }
 
   @Override
   public void autonomousPeriodic() {
     double elapsed = m_timer.get();
-    Trajectory.State reference = m_trajectory.sample(elapsed);
+    SplineSample reference = m_trajectory.sampleAt(elapsed);
     ChassisSpeeds speeds = m_feedback.calculate(m_drive.getPose(), reference);
     m_drive.drive(speeds.vx, speeds.omega);
   }

wpimath/BUILD.bazel

@@ -287,6 +287,7 @@ wpilib_java_junit5_test(
         ":wpimath-java",
         "//wpiunits:wpiunits-java",
         "//wpiutil:wpiutil-java",
+        "@maven//:com_fasterxml_jackson_core_jackson_databind",
         "@maven//:org_ejml_ejml_core",
         "@maven//:org_ejml_ejml_simple",
         "@maven//:us_hebi_quickbuf_quickbuf_runtime",