diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h index 96c2c6e7d1..fa237368d9 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h @@ -105,6 +105,13 @@ class Trajectory { */ State Sample(units::second_t t) const; + /** + * Returns the initial pose of the trajectory. + * + * @return The initial pose of the trajectory. + */ + Pose2d InitialPose() const { return Sample(0_s).pose; } + private: std::vector m_states; units::second_t m_totalTime; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java index 4f49244015..1e810cb14f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java @@ -60,6 +60,15 @@ public class Trajectory { return startValue.plus((endValue.minus(startValue)).times(t)); } + /** + * Returns the initial pose of the trajectory. + * + * @return The initial pose of the trajectory. + */ + public Pose2d getInitialPose() { + return sample(0).poseMeters; + } + /** * Returns the overall duration of the trajectory. *