Add "rotate fast" option to rotation targets and end state (#482)

.github/workflows/format-command.yml

@@ -36,7 +36,7 @@ jobs:
       - name: Setup flutter
         uses: subosito/flutter-action@v2
         with:
-          flutter-version: 3.13.9
+          flutter-version: 3.16.0
           cache: true
           cache-path: ${{ runner.tool_cache }}/flutter/linux
 

.github/workflows/pathplanner-ci.yaml

@@ -14,7 +14,7 @@ on:
         required: true
 
 env:
-  FLUTTER_VERSION: 3.13.9
+  FLUTTER_VERSION: 3.16.0
 
 jobs:
   formatting-analysis:
@@ -85,7 +85,7 @@ jobs:
         include:
           - os: windows-2022
             build-option: "windows"
-            artifact-path: "build/windows/runner/Release"
+            artifact-path: "build/windows/x64/runner/Release"
             artifact-name: PathPlanner-Windows
           - os: macos-13
             build-option: "macos"
@@ -145,9 +145,9 @@ jobs:
       - name: Copy redist
         if: ${{ matrix.build-option == 'windows' }}
         run: |
-          cp windows/redist/msvcp140.dll build/windows/runner/Release
-          cp windows/redist/vcruntime140.dll build/windows/runner/Release
-          cp windows/redist/vcruntime140_1.dll build/windows/runner/Release
+          cp windows/redist/msvcp140.dll build/windows/x64/runner/Release
+          cp windows/redist/vcruntime140.dll build/windows/x64/runner/Release
+          cp windows/redist/vcruntime140_1.dll build/windows/x64/runner/Release
       
       - name: Zip release
         if: ${{ matrix.build-option == 'macos' }}

lib/path/goal_end_state.dart

@@ -1,34 +1,40 @@
 class GoalEndState {
   num velocity;
   num rotation;
+  bool rotateFast;
 
   GoalEndState({
     this.velocity = 0,
     this.rotation = 0,
+    this.rotateFast = false,
   });
 
   GoalEndState.fromJson(Map<String, dynamic> json)
       : velocity = json['velocity'] ?? 0,
-        rotation = json['rotation'] ?? 0;
+        rotation = json['rotation'] ?? 0,
+        rotateFast = json['rotateFast'] ?? false;
 
   Map<String, dynamic> toJson() {
     return {
       'velocity': velocity,
       'rotation': rotation,
+      'rotateFast': rotateFast,
     };
   }
 
   GoalEndState clone() {
-    return GoalEndState(velocity: velocity, rotation: rotation);
+    return GoalEndState(
+        velocity: velocity, rotation: rotation, rotateFast: rotateFast);
   }
 
   @override
   bool operator ==(Object other) =>
       other is GoalEndState &&
       other.runtimeType == runtimeType &&
       other.velocity == velocity &&
-      other.rotation == rotation;
+      other.rotation == rotation &&
+      other.rotateFast == rotateFast;
 
   @override
-  int get hashCode => Object.hash(velocity, rotation);
+  int get hashCode => Object.hash(velocity, rotation, rotateFast);
 }

lib/path/path_point.dart

@@ -1,17 +1,18 @@
 import 'dart:math';
 
 import 'package:pathplanner/path/path_constraints.dart';
+import 'package:pathplanner/path/rotation_target.dart';
 
 class PathPoint {
   final Point position;
-  final num? holonomicRotation;
+  final RotationTarget? rotationTarget;
   final PathConstraints constraints;
   final num distanceAlongPath;
   num maxV = double.infinity;
 
   PathPoint({
     required this.position,
-    required this.holonomicRotation,
+    required this.rotationTarget,
     required this.constraints,
     required this.distanceAlongPath,
   });

lib/path/pathplanner_path.dart

@@ -342,7 +342,7 @@ class PathPlannerPath {
     for (int i = 0; i < waypoints.length - 1; i++) {
       for (double t = 0; t < 1.0; t += pathResolution) {
         num actualWaypointPos = i + t;
-        num? rotation;
+        RotationTarget? rotation;
 
         if (unaddedTargets.isNotEmpty) {
           if ((unaddedTargets[0].waypointRelativePos - actualWaypointPos)
@@ -351,7 +351,7 @@ class PathPlannerPath {
                       min(actualWaypointPos + pathResolution,
                           waypoints.length - 1))
                   .abs()) {
-            rotation = unaddedTargets.removeAt(0).rotationDegrees;
+            rotation = unaddedTargets.removeAt(0);
           }
         }
 
@@ -378,7 +378,7 @@ class PathPlannerPath {
         pathPoints.add(
           PathPoint(
             position: position,
-            holonomicRotation: rotation,
+            rotationTarget: rotation,
             constraints: constraints ?? globalConstraints,
             distanceAlongPath: dist,
           ),
@@ -388,7 +388,10 @@ class PathPlannerPath {
       if (i == waypoints.length - 2) {
         pathPoints.add(PathPoint(
           position: waypoints[waypoints.length - 1].anchor,
-          holonomicRotation: goalEndState.rotation,
+          rotationTarget: RotationTarget(
+              rotationDegrees: goalEndState.rotation,
+              waypointRelativePos: waypoints.length - 1,
+              rotateFast: goalEndState.rotateFast),
           constraints: globalConstraints,
           distanceAlongPath: pathPoints.last.distanceAlongPath +
               (pathPoints.last.position

lib/path/rotation_target.dart

@@ -1,27 +1,32 @@
 class RotationTarget {
   num waypointRelativePos;
   num rotationDegrees;
+  bool rotateFast;
 
   RotationTarget({
     this.waypointRelativePos = 0.5,
     this.rotationDegrees = 0,
+    this.rotateFast = false,
   });
 
   RotationTarget.fromJson(Map<String, dynamic> json)
       : waypointRelativePos = json['waypointRelativePos'] ?? 0.5,
-        rotationDegrees = json['rotationDegrees'] ?? 0;
+        rotationDegrees = json['rotationDegrees'] ?? 0,
+        rotateFast = json['rotateFast'] ?? false;
 
   Map<String, dynamic> toJson() {
     return {
       'waypointRelativePos': waypointRelativePos,
       'rotationDegrees': rotationDegrees,
+      'rotateFast': rotateFast,
     };
   }
 
   RotationTarget clone() {
     return RotationTarget(
       waypointRelativePos: waypointRelativePos,
       rotationDegrees: rotationDegrees,
+      rotateFast: rotateFast,
     );
   }
 
@@ -30,8 +35,10 @@ class RotationTarget {
       other is RotationTarget &&
       other.runtimeType == runtimeType &&
       other.waypointRelativePos == waypointRelativePos &&
-      other.rotationDegrees == rotationDegrees;
+      other.rotationDegrees == rotationDegrees &&
+      other.rotateFast == rotateFast;
 
   @override
-  int get hashCode => Object.hash(waypointRelativePos, rotationDegrees);
+  int get hashCode =>
+      Object.hash(waypointRelativePos, rotationDegrees, rotateFast);
 }

lib/services/simulator/trajectory_generator.dart

@@ -389,7 +389,7 @@ class Trajectory {
     num startingRotationRadians, {
     num maxModuleSpeed = 4.5,
     num driveBaseRadius = 0.425,
-  }) : states = _generateStates(path, linearVel) {
+  }) : states = _generateStates(path, linearVel, startingRotationRadians) {
     _simulateRotation(startingRotationRadians, maxModuleSpeed, driveBaseRadius);
   }
 
@@ -453,7 +453,7 @@ class Trajectory {
     int idx = path.pathPoints.length - 1;
 
     for (int i = startingIndex; i < path.pathPoints.length - 2; i++) {
-      if (path.pathPoints[i].holonomicRotation != null) {
+      if (path.pathPoints[i].rotationTarget != null) {
         idx = i;
         break;
       }
@@ -463,12 +463,16 @@ class Trajectory {
   }
 
   static List<TrajectoryState> _generateStates(
-      PathPlannerPath path, num linearVel) {
+      PathPlannerPath path, num linearVel, num currentRotationRadians) {
     List<TrajectoryState> states = [];
 
     num startVel = linearVel.abs();
 
+    num prevRotationTargetDist = 0;
+    num prevRotationTargetRot = currentRotationRadians;
     int nextRotationTargetIdx = _getNextRotationTargetIdx(path, 0);
+    num distBetweenTargets =
+        path.pathPoints[nextRotationTargetIdx].distanceAlongPath;
 
     // Initial pass. Creates all states and handles linear acceleration
     for (int i = 0; i < path.pathPoints.length; i++) {
@@ -478,11 +482,37 @@ class Trajectory {
       state.constraints = constraints;
 
       if (i > nextRotationTargetIdx) {
+        prevRotationTargetDist =
+            path.pathPoints[nextRotationTargetIdx].distanceAlongPath;
+        prevRotationTargetRot = GeometryUtil.toRadians(path
+            .pathPoints[nextRotationTargetIdx].rotationTarget!.rotationDegrees);
         nextRotationTargetIdx = _getNextRotationTargetIdx(path, i);
+        distBetweenTargets =
+            path.pathPoints[nextRotationTargetIdx].distanceAlongPath -
+                prevRotationTargetDist;
       }
 
-      state.holonomicRotationRadians = GeometryUtil.toRadians(
-          path.pathPoints[nextRotationTargetIdx].holonomicRotation!);
+      RotationTarget nextTarget =
+          path.pathPoints[nextRotationTargetIdx].rotationTarget!;
+
+      if (nextTarget.rotateFast) {
+        state.holonomicRotationRadians =
+            GeometryUtil.toRadians(nextTarget.rotationDegrees);
+      } else {
+        double t =
+            (path.pathPoints[i].distanceAlongPath - prevRotationTargetDist) /
+                distBetweenTargets;
+        t = min(max(0.0, t), 1.0);
+        if (!t.isFinite) {
+          t = 0;
+        }
+
+        state.holonomicRotationRadians = GeometryUtil.rotationLerp(
+            prevRotationTargetRot,
+            GeometryUtil.toRadians(nextTarget.rotationDegrees),
+            t,
+            pi);
+      }
 
       state.position = path.pathPoints[i].position;
 

lib/widgets/editor/tree_widgets/goal_end_state_tree.dart

@@ -66,6 +66,24 @@ class GoalEndStateTree extends StatelessWidget {
             ],
           ),
         ),
+        if (holonomicMode) const SizedBox(height: 12),
+        if (holonomicMode)
+          Row(
+            children: [
+              Checkbox(
+                value: path.goalEndState.rotateFast,
+                onChanged: (value) {
+                  _addChange(
+                      () => path.goalEndState.rotateFast = value ?? false);
+                },
+              ),
+              const SizedBox(width: 4),
+              const Text(
+                'Rotate as Fast as Possible',
+                style: TextStyle(fontSize: 18),
+              ),
+            ],
+          ),
       ],
     );
   }

lib/widgets/editor/tree_widgets/rotation_targets_tree.dart

@@ -194,6 +194,32 @@ class _RotationTargetsTreeState extends State<RotationTargetsTree> {
           ),
         ),
         const SizedBox(height: 12),
+        Row(
+          children: [
+            Checkbox(
+              value: rotations[targetIdx].rotateFast,
+              onChanged: (value) {
+                widget.undoStack.add(Change(
+                  rotations[targetIdx].clone(),
+                  () {
+                    rotations[targetIdx].rotateFast = value ?? false;
+                    widget.onPathChanged?.call();
+                  },
+                  (oldValue) {
+                    rotations[targetIdx].rotateFast = oldValue.rotateFast;
+                    widget.onPathChanged?.call();
+                  },
+                ));
+              },
+            ),
+            const SizedBox(width: 4),
+            const Text(
+              'Reach as Fast as Possible',
+              style: TextStyle(fontSize: 18),
+            ),
+          ],
+        ),
+        const SizedBox(height: 12),
         Slider(
           value: rotations[targetIdx].waypointRelativePos.toDouble(),
           min: 0.0,

pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java

@@ -69,7 +69,8 @@ public void initialize() {
                 >= 0.25)) {
       replanPath(currentPose, currentSpeeds);
     } else {
-      generatedTrajectory = new PathPlannerTrajectory(path, currentSpeeds);
+      generatedTrajectory =
+          new PathPlannerTrajectory(path, currentSpeeds, currentPose.getRotation());
       PathPlannerLogging.logActivePath(path);
       PPLibTelemetry.setCurrentPath(path);
     }
@@ -146,7 +147,8 @@ public void end(boolean interrupted) {
 
   private void replanPath(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
     PathPlannerPath replanned = path.replan(currentPose, currentSpeeds);
-    generatedTrajectory = new PathPlannerTrajectory(replanned, currentSpeeds);
+    generatedTrajectory =
+        new PathPlannerTrajectory(replanned, currentSpeeds, currentPose.getRotation());
     PathPlannerLogging.logActivePath(replanned);
     PPLibTelemetry.setCurrentPath(replanned);
   }