Improve trajectory generation in tight curves

lib/path/path_point.dart

@@ -7,14 +7,14 @@ class PathPoint {
   final Point position;
   RotationTarget? rotationTarget;
   final PathConstraints constraints;
-  final num distanceAlongPath;
+  num distanceAlongPath = 0.0;
   num maxV = double.infinity;
-  num waypointPos = 0.0;
+  final num waypointPos;
 
   PathPoint({
     required this.position,
     required this.rotationTarget,
     required this.constraints,
-    required this.distanceAlongPath,
+    required this.waypointPos,
   });
 }

lib/path/pathplanner_path.dart

@@ -362,16 +362,15 @@ class PathPlannerPath {
       position: samplePath(0.0),
       rotationTarget: null,
       constraints: _constraintsForPos(0.0),
-      distanceAlongPath: 0.0,
+      waypointPos: 0.0,
     ));
-    pathPoints.last.waypointPos = 0.0;
 
     double pos = targetIncrement;
     while (pos < waypoints.length - 1) {
       var position = samplePath(pos);
 
       num distance = pathPoints.last.position.distanceTo(position);
-      if (distance == 0.0) {
+      if (distance <= 0.01) {
         pos = min(pos + targetIncrement, waypoints.length - 1);
         continue;
       }
@@ -395,7 +394,6 @@ class PathPlannerPath {
         }
       } else if (delta < -targetSpacing * 0.25) {
         // Points are too close, increment waypoint relative pos by correct amount
-
         double correctIncrement = (targetSpacing * targetIncrement) / distance;
         pos = pos - targetIncrement + correctIncrement;
 
@@ -423,10 +421,8 @@ class PathPlannerPath {
         position: position,
         rotationTarget: null,
         constraints: _constraintsForPos(pos),
-        distanceAlongPath: pathPoints.last.distanceAlongPath +
-            pathPoints.last.position.distanceTo(position),
+        waypointPos: pos,
       ));
-      pathPoints.last.waypointPos = pos;
       pos = min(pos + targetIncrement, waypoints.length - 1);
     }
 
@@ -438,7 +434,7 @@ class PathPlannerPath {
       var position = samplePath(pos);
 
       num distance = pathPoints.last.position.distanceTo(position);
-      if (distance == 0.0) {
+      if (distance <= 0.01) {
         invalid = false;
         break;
       }
@@ -479,17 +475,90 @@ class PathPlannerPath {
         position: position,
         rotationTarget: null,
         constraints: _constraintsForPos(pos),
-        distanceAlongPath: pathPoints.last.distanceAlongPath +
-            pathPoints.last.position.distanceTo(position),
+        waypointPos: pos,
       ));
-      pathPoints.last.waypointPos = pos;
       pos = waypoints.length - 1;
     }
 
     pathPoints.last.rotationTarget = RotationTarget(
         rotationDegrees: goalEndState.rotation,
         waypointRelativePos: waypoints.length - 1);
 
+    for (int i = 1; i < pathPoints.length - 1; i++) {
+      num curveRadius = _calculateRadius(pathPoints[i - 1].position,
+          pathPoints[i].position, pathPoints[i + 1].position);
+
+      if (!curveRadius.isFinite) {
+        continue;
+      }
+
+      if (curveRadius.abs() < 0.25) {
+        // Curve radius is too tight for default spacing, insert 4 more points
+        num before1WaypointPos = GeometryUtil.numLerp(
+            pathPoints[i - 1].waypointPos, pathPoints[i].waypointPos, 0.33);
+        num before2WaypointPos = GeometryUtil.numLerp(
+            pathPoints[i - 1].waypointPos, pathPoints[i].waypointPos, 0.67);
+        num after1WaypointPos = GeometryUtil.numLerp(
+            pathPoints[i].waypointPos, pathPoints[i + 1].waypointPos, 0.33);
+        num after2WaypointPos = GeometryUtil.numLerp(
+            pathPoints[i].waypointPos, pathPoints[i + 1].waypointPos, 0.67);
+
+        PathPoint before1 = PathPoint(
+          position: samplePath(before1WaypointPos),
+          rotationTarget: null,
+          constraints: pathPoints[i].constraints,
+          waypointPos: before1WaypointPos,
+        );
+        PathPoint before2 = PathPoint(
+          position: samplePath(before2WaypointPos),
+          rotationTarget: null,
+          constraints: pathPoints[i].constraints,
+          waypointPos: before2WaypointPos,
+        );
+        PathPoint after1 = PathPoint(
+          position: samplePath(after1WaypointPos),
+          rotationTarget: null,
+          constraints: pathPoints[i].constraints,
+          waypointPos: after1WaypointPos,
+        );
+        PathPoint after2 = PathPoint(
+          position: samplePath(after2WaypointPos),
+          rotationTarget: null,
+          constraints: pathPoints[i].constraints,
+          waypointPos: after2WaypointPos,
+        );
+
+        pathPoints.insert(i, before2);
+        pathPoints.insert(i, before1);
+        pathPoints.insert(i + 3, after2);
+        pathPoints.insert(i + 3, after1);
+        i += 4;
+      } else if (curveRadius.abs() < 0.5) {
+        // Curve radius is too tight for default spacing, insert 2 more points
+        num beforeWaypointPos = GeometryUtil.numLerp(
+            pathPoints[i - 1].waypointPos, pathPoints[i].waypointPos, 0.5);
+        num afterWaypointPos = GeometryUtil.numLerp(
+            pathPoints[i].waypointPos, pathPoints[i + 1].waypointPos, 0.5);
+
+        PathPoint before = PathPoint(
+          position: samplePath(beforeWaypointPos),
+          rotationTarget: null,
+          constraints: pathPoints[i].constraints,
+          waypointPos: beforeWaypointPos,
+        );
+        PathPoint after = PathPoint(
+          position: samplePath(afterWaypointPos),
+          rotationTarget: null,
+          constraints: pathPoints[i].constraints,
+          waypointPos: afterWaypointPos,
+        );
+
+        pathPoints.insert(i, before);
+        pathPoints.insert(i + 2, after);
+        i += 2;
+      }
+    }
+
     for (int i = 0; i < pathPoints.length; i++) {
       num curveRadius = _getCurveRadiusAtPoint(i).abs();
 
@@ -500,6 +569,11 @@ class PathPlannerPath {
       } else {
         pathPoints[i].maxV = pathPoints[i].constraints.maxVelocity;
       }
+
+      if (i > 0) {
+        pathPoints[i].distanceAlongPath = pathPoints[i - 1].distanceAlongPath +
+            pathPoints[i].position.distanceTo(pathPoints[i - 1].position);
+      }
     }
 
     pathPoints.last.maxV = goalEndState.velocity;

lib/trajectory/trajectory.dart

@@ -228,17 +228,22 @@ class PathPlannerTrajectory {
       // Go over the modules again to make sure they take the same amount of time to reach the next
       // state
       num maxDT = 0.0;
+      num realMaxDT = 0.0;
       for (int m = 0; m < numModules; m++) {
         Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
             states[i - 1].moduleStates[m].angle;
-        if (prevRotDelta.getDegrees().abs() >= 45) {
-          continue;
-        }
 
         num modVel = states[i].moduleStates[m].speedMetersPerSecond;
 
         num dt = states[i + 1].moduleStates[m].deltaPos / modVel;
-        maxDT = max(dt, maxDT);
+        realMaxDT = max(dt, realMaxDT);
+
+        if (prevRotDelta.getDegrees().abs() < 60) {
+          maxDT = max(dt, maxDT);
+        }
+      }
+      if (maxDT == 0.0) {
+        maxDT = realMaxDT;
       }
 
       // Recalculate all module velocities with the allowed DT
@@ -359,24 +364,28 @@ class PathPlannerTrajectory {
       // Go over the modules again to make sure they take the same amount of time to reach the next
       // state
       num maxDT = 0.0;
+      num realMaxDT = 0.0;
       for (int m = 0; m < numModules; m++) {
         Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
             states[i - 1].moduleStates[m].angle;
-        if (prevRotDelta.getDegrees().abs() >= 45) {
-          continue;
-        }
         num modVel = states[i].moduleStates[m].speedMetersPerSecond;
 
         num dt = states[i + 1].moduleStates[m].deltaPos / modVel;
-        maxDT = max(dt, maxDT);
+        realMaxDT = max(dt, realMaxDT);
+
+        if (prevRotDelta.getDegrees().abs() < 60) {
+          maxDT = max(dt, maxDT);
+        }
+      }
+      if (maxDT == 0.0) {
+        maxDT = realMaxDT;
       }
 
       // Recalculate all module velocities with the allowed DT
       for (int m = 0; m < numModules; m++) {
         Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
             states[i - 1].moduleStates[m].angle;
         if (prevRotDelta.getDegrees().abs() >= 60) {
-          // print('v: ${states[i].moduleStates[m].speedMetersPerSecond}');
           continue;
         }
 

lib/widgets/editor/split_auto_editor.dart

@@ -311,7 +311,7 @@ class _SplitAutoEditorState extends State<SplitAutoEditor>
           'Failed to generate trajectory for auto: ${widget.auto.name}');
       ScaffoldMessenger.of(context).showSnackBar(const SnackBar(
         content: Text(
-            'Failed to generate trajectory. Try adjusting the path shape or the positions of rotation targets.'),
+            'Failed to generate trajectory. Please open an issue on the pathplanner github and include the failing path file.'),
       ));
     }
   }

lib/widgets/editor/split_path_editor.dart

@@ -750,7 +750,7 @@ class _SplitPathEditorState extends State<SplitPathEditor>
             'Failed to generate trajectory for path: ${widget.path.name}');
         ScaffoldMessenger.of(context).showSnackBar(const SnackBar(
           content: Text(
-              'Failed to generate trajectory. Try adjusting the path shape or the positions of rotation targets.'),
+              'Failed to generate trajectory. Please open an issue on the pathplanner github and include this path file.'),
         ));
       }
     }