36 units::second_t t = 0_s;
39 units::meters_per_second_t velocity = 0_mps;
42 units::meters_per_second_squared_t acceleration = 0_mps_sq;
69 const auto deltaT = newT - t;
77 const auto reversing =
83 const units::meters_per_second_t newV =
84 velocity + (acceleration * deltaT);
88 const units::meter_t newS =
89 (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
90 (reversing ? -1.0 : 1.0);
96 const double interpolationFrac =
99 return {newT, newV, acceleration,
112 explicit Trajectory(
const std::vector<State>& states) : m_states(states) {
113 if (m_states.empty()) {
114 throw std::invalid_argument(
115 "Trajectory manually initialized with no states.");
118 m_totalTime = states.back().t;
125 units::second_t
TotalTime()
const {
return m_totalTime; }
132 const std::vector<State>&
States()
const {
return m_states; }
142 if (m_states.empty()) {
143 throw std::runtime_error(
144 "Trajectory cannot be sampled if it has no states.");
147 if (t <= m_states.front().t) {
148 return m_states.front();
150 if (t >= m_totalTime) {
151 return m_states.back();
158 std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t,
159 [](
const auto& a,
const auto& b) { return a.t < b; });
161 auto prevSample = sample - 1;
173 return prevSample->Interpolate(
174 *sample, (t - prevSample->t) / (sample->t - prevSample->t));
186 auto& firstState = m_states[0];
187 auto& firstPose = firstState.pose;
190 auto newFirstPose = firstPose + transform;
191 auto newStates = m_states;
192 newStates[0].pose = newFirstPose;
194 for (
unsigned int i = 1; i < newStates.size(); i++) {
195 auto& state = newStates[i];
198 state.pose = newFirstPose + (state.pose - firstPose);
214 auto newStates = m_states;
215 for (
auto& state : newStates) {
216 state.pose = state.pose.RelativeTo(pose);
232 if (m_states.empty()) {
236 auto states = m_states;
237 auto otherStates = other.
States();
238 for (
auto& otherState : otherStates) {
239 otherState.t += m_totalTime;
246 states.insert(states.end(), otherStates.begin() + 1, otherStates.end());
263 std::vector<State> m_states;
264 units::second_t m_totalTime = 0_s;
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
namespace for Niels Lohmann
Definition json.h:99
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Translation2d & Translation() const
Returns the underlying translation.
Definition Pose2d.h:107
Represents a time-parameterized trajectory.
Definition Trajectory.h:29
units::second_t TotalTime() const
Returns the overall duration of the trajectory.
Definition Trajectory.h:125
Trajectory operator+(const Trajectory &other) const
Concatenates another trajectory to the current trajectory.
Definition Trajectory.h:229
Trajectory TransformBy(const Transform2d &transform)
Transforms all poses in the trajectory by the given transform.
Definition Trajectory.h:185
Trajectory RelativeTo(const Pose2d &pose)
Transforms all poses in the trajectory so that they are relative to the given pose.
Definition Trajectory.h:213
bool operator==(const Trajectory &) const =default
Checks equality between this Trajectory and another object.
Trajectory(const std::vector< State > &states)
Constructs a trajectory from a vector of states.
Definition Trajectory.h:112
State Sample(units::second_t t) const
Sample the trajectory at a point in time.
Definition Trajectory.h:141
Pose2d InitialPose() const
Returns the initial pose of the trajectory.
Definition Trajectory.h:255
const std::vector< State > & States() const
Return the states of the trajectory.
Definition Trajectory.h:132
constexpr units::meter_t Distance(const Translation2d &other) const
Calculates the distance between two translations in 2D space.
Definition Translation2d.h:74
constexpr UnitType abs(const UnitType x) noexcept
Compute absolute value.
Definition math.h:726
WPILIB_DLLEXPORT void to_json(wpi::json &json, const Rotation3d &rotation)
WPILIB_DLLEXPORT void from_json(const wpi::json &json, Rotation3d &rotation)
constexpr T Lerp(const T &startValue, const T &endValue, double t)
Linearly interpolates between two values.
Definition MathExtras.h:772
Represents one point on the trajectory.
Definition Trajectory.h:34
Pose2d pose
The pose at that point of the trajectory.
Definition Trajectory.h:45
constexpr bool operator==(const State &) const =default
Checks equality between this State and another object.
units::second_t t
The time elapsed since the beginning of the trajectory.
Definition Trajectory.h:36
units::curvature_t curvature
The curvature at that point of the trajectory.
Definition Trajectory.h:48
constexpr State Interpolate(State endValue, double i) const
Interpolates between two States.
Definition Trajectory.h:63