Make sure trajectories have at least one split section (#858)

choreolib/py/choreo/__init__.py

@@ -94,6 +94,9 @@ def load_swerve_trajectory_string(trajectory_json_string: str) -> SwerveTrajecto
         for sample in data["trajectory"]["samples"]
     ]
     splits = [int(split) for split in data["trajectory"]["splits"]]
+    # Add 0 as the first split index
+    if len(splits) == 0 or splits[0] != 0:
+        splits.insert(0, 0)
     events = [
         EventMarker(
             float(event["from"]["offset"]["val"])

choreolib/py/choreo/test/choreolib_test.py

@@ -34,7 +34,7 @@
     {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":1.0, "x":0.5, "y":0.1, "heading":0.2, "vx":3.0, "vy":3.0, "omega":10.0, "ax":20.0, "ay":20.0, "alpha":30.0, "fx":[100.0,200.0,300.0,400.0], "fy":[-100.0,-200.0,-300.0,-400.0]}
   ],
-  "splits":[],
+  "splits":[0],
   "forcesAvailable":false
  },
  "events":[
@@ -75,7 +75,7 @@
             [-100.0, -200.0, -300.0, -400.0],
         ),
     ],
-    [],
+    [0],
     [choreo.EventMarker(0.0, "testEvent")],
 )
 

choreolib/src/main/java/choreo/Choreo.java

@@ -165,6 +165,12 @@ static Trajectory<? extends TrajectorySample<?>> loadTrajectoryString(
     EventMarker[] events = GSON.fromJson(wholeTrajectory.get("events"), EventMarker[].class);
     JsonObject trajectoryObj = wholeTrajectory.getAsJsonObject("trajectory");
     Integer[] splits = GSON.fromJson(trajectoryObj.get("splits"), Integer[].class);
+    if (splits.length == 0 || splits[0] != 0) {
+      Integer[] newArray = new Integer[splits.length + 1];
+      newArray[0] = 0;
+      System.arraycopy(splits, 0, newArray, 1, splits.length);
+      splits = newArray;
+    }
     if (projectFile.type.equals("Swerve")) {
       SwerveSample[] samples = GSON.fromJson(trajectoryObj.get("samples"), SwerveSample[].class);
       return new Trajectory<SwerveSample>(name, List.of(samples), List.of(splits), List.of(events));

choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp

@@ -23,6 +23,10 @@ void choreo::from_json(const wpi::json& json,
       json.at("trajectory").at("samples").get<std::vector<SwerveSample>>();
   trajectory.splits =
       json.at("trajectory").at("splits").get<std::vector<int>>();
+  // Add 0 as the first split index.
+  if (trajectory.splits.size() == 0 || trajectory.splits.at(0) != 0) {
+    trajectory.splits.insert(trajectory.splits.begin(), 0);
+  }
   trajectory.events = json.at("events").get<std::vector<EventMarker>>();
 }
 
@@ -41,5 +45,9 @@ void choreo::from_json(const wpi::json& json,
                            .get<std::vector<DifferentialSample>>();
   trajectory.splits =
       json.at("trajectory").at("splits").get<std::vector<int>>();
+  // Add 0 as the first split index.
+  if (trajectory.splits.size() == 0 || trajectory.splits.at(0) != 0) {
+    trajectory.splits.insert(trajectory.splits.begin(), 0);
+  }
   trajectory.events = json.at("events").get<std::vector<EventMarker>>();
 }

choreolib/src/test/java/choreo/ChoreoTests.java

@@ -47,7 +47,7 @@ public class ChoreoTests {
     {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":1.0, "x":0.5, "y":0.1, "heading":0.2, "vx":3.0, "vy":3.0, "omega":10.0, "ax":20.0, "ay":20.0, "alpha":30.0, "fx":[100.0,200.0,300.0,400.0], "fy":[-100.0,-200.0,-300.0,-400.0]}
   ],
-  "splits":[],
+  "splits":[0],
   "forcesAvailable":false
  },
  "events":[
@@ -167,7 +167,7 @@ public class ChoreoTests {
                   30.0,
                   new double[] {100.0, 200.0, 300.0, 400.0},
                   new double[] {-100.0, -200.0, -300.0, -400.0})),
-          List.of(),
+          List.of(0),
           List.of(new EventMarker(0.0, "testEvent")));
 
   @Test

choreolib/src/test/native/cpp/TrajectoryFileTests.cpp

@@ -43,7 +43,7 @@ constexpr std::string_view swerveTrajectoryString =
     {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":1.0, "x":0.5, "y":0.1, "heading":0.2, "vx":3.0, "vy":3.0, "omega":10.0, "ax":20.0, "ay":20.0, "alpha":30.0, "fx":[100.0,200.0,300.0,400.0], "fy":[-100.0,-200.0,-300.0,-400.0]}
   ],
-  "splits":[],
+  "splits":[0],
   "forcesAvailable":false
  },
  "events":[
@@ -79,7 +79,7 @@ const Trajectory<SwerveSample> correctSwerveTrajectory{
       30_rad_per_s_sq,
       {100_N, 200_N, 300_N, 400_N},
       {-100_N, -200_N, -300_N, -400_N}}},
-    {},
+    {0},
     {{0_s, "testEvent"}}};
 
 TEST(TrajectoryFileTest, DeserializeSwerveTrajectory) {

src-core/src/generation/transformers/mod.rs

@@ -233,7 +233,7 @@ fn postprocess(
             let total_intervals = interval;
             interval += pt.1.intervals;
             (
-                pt.1.split && !(pt.0 == 0 || pt.0 == snapshot.waypoints.len() - 1),
+                pt.0 == 0 || (pt.1.split && pt.0 != snapshot.waypoints.len() - 1),
                 total_intervals,
                 result.get(total_intervals).map_or(0.0, |s| match s {
                     Sample::Swerve { t, .. } => *t,

src-core/src/spec/trajectory.rs

@@ -396,8 +396,8 @@ pub struct Trajectory {
     /// The samples of the trajectory.
     pub samples: Vec<Sample>,
     /// The indices of samples which are associated with split waypoints.
-    /// First and last samples are never in this list even if the split toggle is set
-    /// for first or last waypoints
+    /// This includes 0, but the index of the last sample is never in this list even if the split toggle is set
+    /// for the last waypoint
     pub splits: Vec<usize>,
 }