Expose holomonic preview path in PathPlannerPath

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java

@@ -27,6 +27,7 @@ public class PathPlannerPath {
   private GoalEndState goalEndState;
   private List<PathPoint> allPoints;
   private boolean reversed;
+  private Rotation2d previewStartingRotation;
 
   /**
    * Create a new path planner path
@@ -38,6 +39,7 @@ public class PathPlannerPath {
    * @param globalConstraints The global constraints of the path
    * @param goalEndState The goal end state of the path
    * @param reversed Should the robot follow the path reversed (differential drive only)
+   * @param previewStartingRotation The settings used for previews in the UI
    */
   public PathPlannerPath(
       List<Translation2d> bezierPoints,
@@ -46,7 +48,8 @@ public PathPlannerPath(
       List<EventMarker> eventMarkers,
       PathConstraints globalConstraints,
       GoalEndState goalEndState,
-      boolean reversed) {
+      boolean reversed,
+      Rotation2d previewStartingRotation) {
     this.bezierPoints = bezierPoints;
     this.rotationTargets = holonomicRotations;
     this.constraintZones = constraintZones;
@@ -55,10 +58,41 @@ public PathPlannerPath(
     this.goalEndState = goalEndState;
     this.reversed = reversed;
     this.allPoints = createPath(this.bezierPoints, this.rotationTargets, this.constraintZones);
+    this.previewStartingRotation = previewStartingRotation;
 
     precalcValues();
   }
 
+  /**
+   * Create a new path planner path
+   *
+   * @param bezierPoints List of points representing the cubic Bezier curve of the path
+   * @param holonomicRotations List of rotation targets along the path
+   * @param constraintZones List of constraint zones along the path
+   * @param eventMarkers List of event markers along the path
+   * @param globalConstraints The global constraints of the path
+   * @param goalEndState The goal end state of the path
+   * @param reversed Should the robot follow the path reversed (differential drive only)
+   */
+  public PathPlannerPath(
+      List<Translation2d> bezierPoints,
+      List<RotationTarget> holonomicRotations,
+      List<ConstraintsZone> constraintZones,
+      List<EventMarker> eventMarkers,
+      PathConstraints globalConstraints,
+      GoalEndState goalEndState,
+      boolean reversed) {
+    this(
+        bezierPoints,
+        holonomicRotations,
+        constraintZones,
+        eventMarkers,
+        globalConstraints,
+        goalEndState,
+        reversed,
+        Rotation2d.fromDegrees(0));
+  }
+
   /**
    * Simplified constructor to create a path with no rotation targets, constraint zones, or event
    * markers.
@@ -82,7 +116,8 @@ public PathPlannerPath(
         Collections.emptyList(),
         constraints,
         goalEndState,
-        reversed);
+        reversed,
+        Rotation2d.fromDegrees(0));
   }
 
   /**
@@ -109,6 +144,7 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS
     this.goalEndState = goalEndState;
     this.reversed = false;
     this.allPoints = new ArrayList<>();
+    this.previewStartingRotation = Rotation2d.fromDegrees(0);
   }
 
   /**
@@ -204,6 +240,7 @@ public void hotReload(JSONObject pathJson) {
     this.goalEndState = updatedPath.goalEndState;
     this.allPoints = updatedPath.allPoints;
     this.reversed = updatedPath.reversed;
+    this.previewStartingRotation = updatedPath.previewStartingRotation;
   }
 
   /**
@@ -259,14 +296,25 @@ private static PathPlannerPath fromJson(JSONObject pathJson) {
       eventMarkers.add(EventMarker.fromJson((JSONObject) markerJson));
     }
 
+    Rotation2d previewStartingRotation = Rotation2d.fromDegrees(0);
+    if (pathJson.containsKey("previewStartingState")) {
+      JSONObject previewStartingStateJson = (JSONObject) pathJson.get("previewStartingState");
+      if (previewStartingStateJson != null) {
+        previewStartingRotation =
+            Rotation2d.fromDegrees(
+                ((Number) previewStartingStateJson.get("rotation")).doubleValue());
+      }
+    }
+
     return new PathPlannerPath(
         bezierPoints,
         rotationTargets,
         constraintZones,
         eventMarkers,
         globalConstraints,
         goalEndState,
-        reversed);
+        reversed,
+        previewStartingRotation);
   }
 
   private static List<Translation2d> bezierPointsFromWaypointsJson(JSONArray waypointsJson) {
@@ -317,6 +365,22 @@ public Pose2d getStartingDifferentialPose() {
     return new Pose2d(startPos, heading);
   }
 
+  /**
+   * Get the starting pose for the holomonic path based on the preview settings.
+   *
+   * <p>NOTE: This should only be used for the first path you are running, and only if you are not
+   * using an auto mode file. Using this pose to reset the robots pose between sequential paths will
+   * cause a loss of accuracy.
+   *
+   * @return Pose at the path's starting point
+   */
+  public Pose2d getPreviewStartingHolonomicPose() {
+    Rotation2d heading =
+        previewStartingRotation == null ? Rotation2d.fromDegrees(0) : previewStartingRotation;
+
+    return new Pose2d(getPoint(0).position, heading);
+  }
+
   /**
    * Get the constraints for a point along the path
    *
@@ -566,7 +630,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
           Collections.emptyList(),
           globalConstraints,
           goalEndState,
-          reversed);
+          reversed,
+          previewStartingRotation);
     } else if ((closestPointIdx == 0 && robotNextControl == null)
         || (Math.abs(closestDist - startingPose.getTranslation().getDistance(getPoint(0).position))
                 <= 0.25
@@ -627,7 +692,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
                 .collect(Collectors.toList()),
             globalConstraints,
             goalEndState,
-            reversed);
+            reversed,
+            previewStartingRotation);
       }
     }
 
@@ -660,7 +726,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
           Collections.emptyList(),
           globalConstraints,
           goalEndState,
-          reversed);
+          reversed,
+          previewStartingRotation);
     }
 
     if (bezierPoints.isEmpty()) {
@@ -796,7 +863,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
         mappedMarkers,
         globalConstraints,
         goalEndState,
-        reversed);
+        reversed,
+        previewStartingRotation);
   }
 
   /**

pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp

@@ -14,10 +14,11 @@ PathPlannerPath::PathPlannerPath(std::vector<frc::Translation2d> bezierPoints,
 		std::vector<ConstraintsZone> constraintZones,
 		std::vector<EventMarker> eventMarkers,
 		PathConstraints globalConstraints, GoalEndState goalEndState,
-		bool reversed) : m_bezierPoints(bezierPoints), m_rotationTargets(
-		rotationTargets), m_constraintZones(constraintZones), m_eventMarkers(
-		eventMarkers), m_globalConstraints(globalConstraints), m_goalEndState(
-		goalEndState), m_reversed(reversed) {
+		bool reversed, frc::Rotation2d previewStartingRotation) : m_bezierPoints(
+		bezierPoints), m_rotationTargets(rotationTargets), m_constraintZones(
+		constraintZones), m_eventMarkers(eventMarkers), m_globalConstraints(
+		globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingRotation(
+		previewStartingRotation) {
 	m_allPoints = PathPlannerPath::createPath(m_bezierPoints, m_rotationTargets,
 			m_constraintZones);
 
@@ -41,6 +42,7 @@ void PathPlannerPath::hotReload(const wpi::json &json) {
 	m_goalEndState = updatedPath.m_goalEndState;
 	m_reversed = updatedPath.m_reversed;
 	m_allPoints = updatedPath.m_allPoints;
+	m_previewStartingRotation = updatedPath.m_previewStartingRotation;
 }
 
 std::vector<frc::Translation2d> PathPlannerPath::bezierFromPoses(
@@ -127,8 +129,18 @@ PathPlannerPath PathPlannerPath::fromJson(const wpi::json &json) {
 		eventMarkers.push_back(EventMarker::fromJson(markerJson));
 	}
 
+	frc::Rotation2d previewStartingRotation;
+	if (json.contains("previewStartingState")
+			&& !json.at("previewStartingState").is_null()) {
+		auto jsonStartingState = json.at("previewStartingState");
+		previewStartingRotation = frc::Rotation2d(
+				units::degree_t(
+						jsonStartingState.at("rotation").get<double>()));
+	}
+
 	return PathPlannerPath(bezierPoints, rotationTargets, constraintZones,
-			eventMarkers, globalConstraints, goalEndState, reversed);
+			eventMarkers, globalConstraints, goalEndState, reversed,
+			previewStartingRotation);
 }
 
 std::vector<frc::Translation2d> PathPlannerPath::bezierPointsFromWaypointsJson(
@@ -238,6 +250,10 @@ frc::Pose2d PathPlannerPath::getStartingDifferentialPose() {
 	return frc::Pose2d(startPos, heading);
 }
 
+frc::Pose2d PathPlannerPath::getPreviewStartingHolonomicPose() {
+	return frc::Pose2d(getPoint(0).position, m_previewStartingRotation);
+}
+
 void PathPlannerPath::precalcValues() {
 	if (numPoints() > 0) {
 		for (size_t i = 0; i < m_allPoints.size(); i++) {

pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h

@@ -35,7 +35,8 @@ class PathPlannerPath {
 			std::vector<ConstraintsZone> constraintZones,
 			std::vector<EventMarker> eventMarkers,
 			PathConstraints globalConstraints, GoalEndState goalEndState,
-			bool reversed);
+			bool reversed, frc::Rotation2d previewStartingRotation =
+					frc::Rotation2d());
 
 	/**
 	 * Simplified constructor to create a path with no rotation targets, constraint zones, or event
@@ -81,6 +82,15 @@ class PathPlannerPath {
 	 */
 	frc::Pose2d getStartingDifferentialPose();
 
+	/**
+	 * Get the starting pose for the holomonic path based on the preview settings.
+	 *
+	 * NOTE: This should only be used for the first path you are running, and only if you are not using an auto mode file. Using this pose to reset the robots pose between sequential paths will cause a loss of accuracy.
+	 *
+	 * @return Pose at the path's starting point
+	 */
+	frc::Pose2d getPreviewStartingHolonomicPose();
+
 	/**
 	 * Get the constraints for a point along the path
 	 *
@@ -234,5 +244,6 @@ class PathPlannerPath {
 	GoalEndState m_goalEndState;
 	std::vector<PathPoint> m_allPoints;
 	bool m_reversed;
+	frc::Rotation2d m_previewStartingRotation;
 };
 }

pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java

@@ -0,0 +1,85 @@
+package com.pathplanner.lib.path;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+public class PathPlannerPathTest {
+  private static final double EPSILON = 1e-6;
+
+  @Test
+  public void testDifferentialStartingPose() {
+    PathPlannerPath path =
+        new PathPlannerPath(
+            List.of(
+                new Translation2d(2, 1),
+                new Translation2d(3.12, 1),
+                new Translation2d(3.67, 1.00),
+                new Translation2d(5.19, 1.00)),
+            new ArrayList<>(),
+            new ArrayList<>(),
+            new ArrayList<>(),
+            new PathConstraints(1, 2, 3, 4),
+            new GoalEndState(0, Rotation2d.fromDegrees(0)),
+            true,
+            null);
+
+    Pose2d initialPose = path.getStartingDifferentialPose();
+    assertNotNull(initialPose);
+    assertEquals(2, initialPose.getX(), EPSILON);
+    assertEquals(1, initialPose.getY(), EPSILON);
+    assertEquals(180, initialPose.getRotation().getDegrees(), EPSILON);
+  }
+
+  @Test
+  public void testHolomonicStartingPoseSet() {
+    PathPlannerPath path =
+        new PathPlannerPath(
+            List.of(
+                new Translation2d(2, 1),
+                new Translation2d(3.12, 1),
+                new Translation2d(3.67, 1.00),
+                new Translation2d(5.19, 1.00)),
+            new ArrayList<>(),
+            new ArrayList<>(),
+            new ArrayList<>(),
+            new PathConstraints(1, 2, 3, 4),
+            new GoalEndState(0, Rotation2d.fromDegrees(0)),
+            true,
+            Rotation2d.fromDegrees(90));
+    Pose2d initialPose = path.getPreviewStartingHolonomicPose();
+    assertNotNull(initialPose);
+    assertEquals(2, initialPose.getX(), EPSILON);
+    assertEquals(1, initialPose.getY(), EPSILON);
+    assertEquals(90, initialPose.getRotation().getDegrees(), EPSILON);
+  }
+
+  @Test
+  public void testHolomonicStartingPoseNotSet() {
+    PathPlannerPath path =
+        new PathPlannerPath(
+            List.of(
+                new Translation2d(2, 1),
+                new Translation2d(3.12, 1),
+                new Translation2d(3.67, 1.00),
+                new Translation2d(5.19, 1.00)),
+            new ArrayList<>(),
+            new ArrayList<>(),
+            new ArrayList<>(),
+            new PathConstraints(1, 2, 3, 4),
+            new GoalEndState(0, Rotation2d.fromDegrees(0)),
+            true,
+            null);
+    Pose2d initialPose = path.getPreviewStartingHolonomicPose();
+    assertNotNull(initialPose);
+    assertEquals(2, initialPose.getX(), EPSILON);
+    assertEquals(1, initialPose.getY(), EPSILON);
+    assertEquals(0, initialPose.getRotation().getDegrees(), EPSILON);
+  }
+}