diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index efe02fa1..6fc85e29 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/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 diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 706848c3..0ee5dd0f 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/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 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()) { diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index e3f739a4..4dabee12 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/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 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 PathPlannerPath::getIdealTrajectory( RobotConfig robotConfig) { if (!m_idealTrajectory.has_value() && m_idealStartingState.has_value()) { diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h index 089d5f4c..ebf5956a 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h @@ -146,6 +146,14 @@ class PathPlannerPath: public std::enable_shared_from_this { */ 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 getStartingHolonomicPose(); + /** * Create a path planner path from pre-generated path points. This is used internally, and you * likely should not use this