12 #include <units/units.h>
14 #include "frc/geometry/Pose2d.h"
15 #include "frc/geometry/Transform2d.h"
27 units::compound_unit<units::radian, units::inverse<units::meter>>>;
41 units::second_t t = 0_s;
44 units::meters_per_second_t velocity = 0_mps;
47 units::meters_per_second_squared_t acceleration = 0_mps_sq;
87 explicit Trajectory(
const std::vector<State>& states);
93 units::second_t
TotalTime()
const {
return m_totalTime; }
99 const std::vector<State>&
States()
const {
return m_states; }
107 State
Sample(units::second_t t)
const;
138 std::vector<State> m_states;
139 units::second_t m_totalTime;
150 template <
typename T>
151 static T Lerp(
const T& startValue,
const T& endValue,
const double t) {
152 return startValue + (endValue - startValue) * t;
156 void to_json(
wpi::json& json,
const Trajectory::State& state);
158 void from_json(
const wpi::json& json, Trajectory::State& state);
a class to store JSON values
Definition: json.h:2655
Pose2d InitialPose() const
Returns the initial pose of the trajectory.
Definition: Trajectory.h:135
Represents one point on the trajectory.
Definition: Trajectory.h:39
units::second_t TotalTime() const
Returns the overall duration of the trajectory.
Definition: Trajectory.h:93
bool operator==(const State &other) const
Checks equality between this State and another object.
Trajectory RelativeTo(const Pose2d &pose)
Transforms all poses in the trajectory so that they are relative to the given pose.
Trajectory TransformBy(const Transform2d &transform)
Transforms all poses in the trajectory by the given transform.
WPILib C++ utilities (wpiutil) namespace.
Definition: EventLoopRunner.h:17
const std::vector< State > & States() const
Return the states of the trajectory.
Definition: Trajectory.h:99
bool operator!=(const State &other) const
Checks inequality between this State and another object.
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
units::unit_t< units::compound_unit< units::radian, units::inverse< units::meter > >> curvature_t
Define a unit for curvature.
Definition: Trajectory.h:27
Represents a time-parameterized trajectory.
Definition: Trajectory.h:34
State Interpolate(State endValue, double i) const
Interpolates between two States.
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
State Sample(units::second_t t) const
Sample the trajectory at a point in time.