Add method to get holonomic path start pose if ideal starting state is present

pathplannerlib-python/pathplannerlib/path.py

@@ -769,6 +769,21 @@ def getStartingDifferentialPose(self) -> Pose2d:
 
         return Pose2d(startPos, heading)
 
+    def getStartingHolonomicPose(self) -> Union[Pose2d, None]:
+        """
+        Get the holonomic pose for the start point of this path. If the path does not have an ideal
+        starting state, this will return None.
+
+        :return: The ideal starting pose if an ideal starting state is present, None otherwise
+        """
+        if self._idealStartingState is None:
+            return None
+
+        startPos = self.getPoint(0).position
+        rotation = self._idealStartingState.rotation
+
+        return Pose2d(startPos, rotation)
+
     def isChoreoPath(self) -> bool:
         """
         Check if this path is loaded from a Choreo trajectory

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

@@ -671,7 +671,7 @@ public Rotation2d getInitialHeading() {
   }
 
   /**
-   * Get the differential pose for the start point of this path
+   * Get the differential pose for the start point of this path.
    *
    * @return Pose at the path's starting point
    */
@@ -687,6 +687,23 @@ public Pose2d getStartingDifferentialPose() {
     return new Pose2d(startPos, heading);
   }
 
+  /**
+   * Get the holonomic pose for the start point of this path. If the path does not have an ideal
+   * starting state, this will return an empty optional.
+   *
+   * @return The ideal starting pose if an ideal starting state is present, empty optional otherwise
+   */
+  public Optional<Pose2d> getStartingHolonomicPose() {
+    if (idealStartingState == null) {
+      return Optional.empty();
+    }
+
+    Translation2d startPos = getPoint(0).position;
+    Rotation2d rotation = idealStartingState.rotation();
+
+    return Optional.of(new Pose2d(startPos, rotation));
+  }
+
   private PathConstraints constraintsForWaypointPos(double pos) {
     for (ConstraintsZone z : constraintZones) {
       if (pos >= z.minPosition() && pos <= z.maxPosition()) {

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

@@ -627,6 +627,17 @@ frc::Pose2d PathPlannerPath::getStartingDifferentialPose() {
 	return frc::Pose2d(startPos, heading);
 }
 
+std::optional<frc::Pose2d> PathPlannerPath::getStartingHolonomicPose() {
+	if (!m_idealStartingState.has_value()) {
+		return std::nullopt;
+	}
+
+	frc::Translation2d startPos = getPoint(0).position;
+	frc::Rotation2d rotation = m_idealStartingState.value().getRotation();
+
+	return frc::Pose2d(startPos, rotation);
+}
+
 std::optional<PathPlannerTrajectory> PathPlannerPath::getIdealTrajectory(
 		RobotConfig robotConfig) {
 	if (!m_idealTrajectory.has_value() && m_idealStartingState.has_value()) {

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

@@ -146,6 +146,14 @@ class PathPlannerPath: public std::enable_shared_from_this<PathPlannerPath> {
 	 */
 	frc::Pose2d getStartingDifferentialPose();
 
+	/**
+	 * Get the holonomic pose for the start point of this path. If the path does not have an ideal
+	 * starting state, this will return nullopt.
+	 *
+	 * @return The ideal starting pose if an ideal starting state is present, nullopt otherwise
+	 */
+	std::optional<frc::Pose2d> getStartingHolonomicPose();
+
 	/**
 	 * Create a path planner path from pre-generated path points. This is used internally, and you
 	 * likely should not use this