[wpimath] Make TrajectorySample only contain time

This allows reusing the trajectory infrastructure for non-drivetrain
trajectories.

Author: calcmogul

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

@@ -1,6 +1,6 @@
 extra_includes:
 - wpi/math/trajectory/DifferentialTrajectory.hpp
-- wpi/math/trajectory/SplineTrajectory.hpp
+- wpi/math/trajectory/DrivetrainSplineTrajectory.hpp
 - wpi/math/trajectory/HolonomicTrajectory.hpp
 - wpi/math/trajectory/Trajectory.hpp
 
@@ -22,8 +22,8 @@ classes:
             ignore: true
       SetTrajectory:
         template_impls:
-        - [wpi::math::SplineSample]
         - [wpi::math::DifferentialSample]
+        - [wpi::math::DrivetrainSplineSample]
         - [wpi::math::TrajectorySample]
       GetPoses:
         overloads:

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

@@ -59,7 +59,7 @@ class Robot : public wpi::TimedRobot {
   wpi::math::SlewRateLimiter<wpi::units::scalar> rotLimiter{3 / 1_s};
 
   Drivetrain drive;
-  wpi::math::SplineTrajectory trajectory;
+  wpi::math::DrivetrainSplineTrajectory trajectory;
   wpi::math::LTVUnicycleController feedback{20_ms};
   wpi::Timer timer;
 };

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

@@ -12,8 +12,8 @@
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.trajectory.HolonomicSample;
 import org.wpilib.math.trajectory.Trajectory;
-import org.wpilib.math.trajectory.TrajectorySample;
 import org.wpilib.networktables.DoubleArrayEntry;
 import org.wpilib.units.measure.Distance;
 
@@ -107,7 +107,7 @@ public synchronized void setPoses(Pose2d... poses) {
    * @param <SampleType> The type of the trajectory sample.
    * @param trajectory The trajectory from which the poses should be added.
    */
-  public synchronized <SampleType extends TrajectorySample> void setTrajectory(
+  public synchronized <SampleType extends HolonomicSample> void setTrajectory(
       Trajectory<SampleType> trajectory) {
     m_poses.clear();
     for (SampleType state : trajectory.getSamples()) {

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

@@ -12,7 +12,7 @@
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.kinematics.ChassisVelocities;
-import org.wpilib.math.trajectory.SplineSample;
+import org.wpilib.math.trajectory.DrivetrainSplineSample;
 import org.wpilib.math.trajectory.Trajectory;
 import org.wpilib.math.trajectory.TrajectoryConfig;
 import org.wpilib.math.trajectory.TrajectoryGenerator;
@@ -29,7 +29,7 @@ public class Robot extends TimedRobot {
   private final Drivetrain drive = new Drivetrain();
   private final LTVUnicycleController feedback = new LTVUnicycleController(0.020);
   private final Timer timer = new Timer();
-  private final Trajectory<SplineSample> trajectory;
+  private final Trajectory<DrivetrainSplineSample> trajectory;
 
   /** Called once at the beginning of the robot program. */
   public Robot() {
@@ -55,7 +55,7 @@ public void autonomousInit() {
   @Override
   public void autonomousPeriodic() {
     double elapsed = timer.get();
-    SplineSample reference = trajectory.sampleAt(elapsed);
+    DrivetrainSplineSample reference = trajectory.sampleAt(elapsed);
     ChassisVelocities velocities = feedback.calculate(drive.getPose(), reference);
     drive.drive(velocities.vx, velocities.omega);
   }