[choreolib] Add utility methods to convert choreo trajectories to wpi…

…lib trajectories

choreolib/src/main/java/com/choreo/lib/ChoreoTrajectory.java

@@ -3,6 +3,7 @@
 package com.choreo.lib;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.trajectory.Trajectory;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -174,4 +175,25 @@ public ChoreoTrajectory flipped() {
     }
     return new ChoreoTrajectory(flippedStates);
   }
+
+  /**
+   * Converts the trajectory to a WPILib {@link Trajectory}.
+   *
+   * @return The converted Trajectory.
+   */
+  public Trajectory toTrajectory() {
+    var trajectory = new Trajectory();
+    var lastVel = 0.0;
+    var lastTime = 0.0;
+    for (var state : samples) {
+      var vel = Math.hypot(state.velocityX, state.velocityY);
+      trajectory
+          .getStates()
+          .add(state.toTrajectoryState((vel - lastVel) / (state.timestamp - lastTime)));
+      lastVel = vel;
+      lastTime = state.timestamp;
+    }
+
+    return trajectory;
+  }
 }

choreolib/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
 import java.util.Arrays;
 
 /** A single robot state in a ChoreoTrajectory. */
@@ -147,4 +148,15 @@ public ChoreoTrajectoryState flipped() {
         Arrays.stream(this.moduleForcesX).map(x -> -x).toArray(),
         this.moduleForcesY);
   }
+
+  /**
+   * Converts the state to a WPILib {@link Trajectory.State}.
+   *
+   * @return The WPILib {@link Trajectory.State}.
+   */
+  public Trajectory.State toTrajectoryState(double accelerationMetersPerSecondSq) {
+    var vel = Math.hypot(velocityX, velocityY);
+    return new Trajectory.State(
+        timestamp, vel, accelerationMetersPerSecondSq, getPose(), angularVelocity / vel);
+  }
 }