WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
Trajectory.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <algorithm>
8#include <stdexcept>
9#include <vector>
10
11#include <wpi/MathExtras.h>
12#include <wpi/SymbolExports.h>
13#include <wpi/json_fwd.h>
14
15#include "frc/geometry/Pose2d.h"
17#include "units/acceleration.h"
18#include "units/curvature.h"
19#include "units/math.h"
20#include "units/time.h"
21#include "units/velocity.h"
22
23namespace frc {
24/**
25 * Represents a time-parameterized trajectory. The trajectory contains of
26 * various States that represent the pose, curvature, time elapsed, velocity,
27 * and acceleration at that point.
28 */
30 public:
31 /**
32 * Represents one point on the trajectory.
33 */
35 /// The time elapsed since the beginning of the trajectory.
36 units::second_t t = 0_s;
37
38 /// The speed at that point of the trajectory.
39 units::meters_per_second_t velocity = 0_mps;
40
41 /// The acceleration at that point of the trajectory.
42 units::meters_per_second_squared_t acceleration = 0_mps_sq;
43
44 /// The pose at that point of the trajectory.
46
47 /// The curvature at that point of the trajectory.
48 units::curvature_t curvature{0.0};
49
50 /**
51 * Checks equality between this State and another object.
52 */
53 constexpr bool operator==(const State&) const = default;
54
55 /**
56 * Interpolates between two States.
57 *
58 * @param endValue The end value for the interpolation.
59 * @param i The interpolant (fraction).
60 *
61 * @return The interpolated state.
62 */
63 constexpr State Interpolate(State endValue, double i) const {
64 // Find the new [t] value.
65 const auto newT = wpi::Lerp(t, endValue.t, i);
66
67 // Find the delta time between the current state and the interpolated
68 // state.
69 const auto deltaT = newT - t;
70
71 // If delta time is negative, flip the order of interpolation.
72 if (deltaT < 0_s) {
73 return endValue.Interpolate(*this, 1.0 - i);
74 }
75
76 // Check whether the robot is reversing at this stage.
77 const auto reversing =
78 velocity < 0_mps ||
79 (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
80
81 // Calculate the new velocity.
82 // v = v_0 + at
83 const units::meters_per_second_t newV =
84 velocity + (acceleration * deltaT);
85
86 // Calculate the change in position.
87 // delta_s = v_0 t + 0.5at²
88 const units::meter_t newS =
89 (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
90 (reversing ? -1.0 : 1.0);
91
92 // Return the new state. To find the new position for the new state, we
93 // need to interpolate between the two endpoint poses. The fraction for
94 // interpolation is the change in position (delta s) divided by the total
95 // distance between the two endpoints.
96 const double interpolationFrac =
97 newS / endValue.pose.Translation().Distance(pose.Translation());
98
99 return {newT, newV, acceleration,
100 wpi::Lerp(pose, endValue.pose, interpolationFrac),
101 wpi::Lerp(curvature, endValue.curvature, interpolationFrac)};
102 }
103 };
104
105 Trajectory() = default;
106
107 /**
108 * Constructs a trajectory from a vector of states.
109 *
110 * @throws std::invalid_argument if the vector of states is empty.
111 */
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.");
116 }
117
118 m_totalTime = states.back().t;
119 }
120
121 /**
122 * Returns the overall duration of the trajectory.
123 * @return The duration of the trajectory.
124 */
125 units::second_t TotalTime() const { return m_totalTime; }
126
127 /**
128 * Return the states of the trajectory.
129 *
130 * @return The states of the trajectory.
131 */
132 const std::vector<State>& States() const { return m_states; }
133
134 /**
135 * Sample the trajectory at a point in time.
136 *
137 * @param t The point in time since the beginning of the trajectory to sample.
138 * @return The state at that point in time.
139 * @throws std::runtime_error if the trajectory has no states.
140 */
141 State Sample(units::second_t t) const {
142 if (m_states.empty()) {
143 throw std::runtime_error(
144 "Trajectory cannot be sampled if it has no states.");
145 }
146
147 if (t <= m_states.front().t) {
148 return m_states.front();
149 }
150 if (t >= m_totalTime) {
151 return m_states.back();
152 }
153
154 // Use binary search to get the element with a timestamp no less than the
155 // requested timestamp. This starts at 1 because we use the previous state
156 // later on for interpolation.
157 auto sample =
158 std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t,
159 [](const auto& a, const auto& b) { return a.t < b; });
160
161 auto prevSample = sample - 1;
162
163 // The sample's timestamp is now greater than or equal to the requested
164 // timestamp. If it is greater, we need to interpolate between the
165 // previous state and the current state to get the exact state that we
166 // want.
167
168 // If the difference in states is negligible, then we are spot on!
169 if (units::math::abs(sample->t - prevSample->t) < 1E-9_s) {
170 return *sample;
171 }
172 // Interpolate between the two states for the state that we want.
173 return prevSample->Interpolate(
174 *sample, (t - prevSample->t) / (sample->t - prevSample->t));
175 }
176
177 /**
178 * Transforms all poses in the trajectory by the given transform. This is
179 * useful for converting a robot-relative trajectory into a field-relative
180 * trajectory. This works with respect to the first pose in the trajectory.
181 *
182 * @param transform The transform to transform the trajectory by.
183 * @return The transformed trajectory.
184 */
186 auto& firstState = m_states[0];
187 auto& firstPose = firstState.pose;
188
189 // Calculate the transformed first pose.
190 auto newFirstPose = firstPose + transform;
191 auto newStates = m_states;
192 newStates[0].pose = newFirstPose;
193
194 for (unsigned int i = 1; i < newStates.size(); i++) {
195 auto& state = newStates[i];
196 // We are transforming relative to the coordinate frame of the new initial
197 // pose.
198 state.pose = newFirstPose + (state.pose - firstPose);
199 }
200
201 return Trajectory(newStates);
202 }
203
204 /**
205 * Transforms all poses in the trajectory so that they are relative to the
206 * given pose. This is useful for converting a field-relative trajectory
207 * into a robot-relative trajectory.
208 *
209 * @param pose The pose that is the origin of the coordinate frame that
210 * the current trajectory will be transformed into.
211 * @return The transformed trajectory.
212 */
214 auto newStates = m_states;
215 for (auto& state : newStates) {
216 state.pose = state.pose.RelativeTo(pose);
217 }
218 return Trajectory(newStates);
219 }
220
221 /**
222 * Concatenates another trajectory to the current trajectory. The user is
223 * responsible for making sure that the end pose of this trajectory and the
224 * start pose of the other trajectory match (if that is the desired behavior).
225 *
226 * @param other The trajectory to concatenate.
227 * @return The concatenated trajectory.
228 */
229 Trajectory operator+(const Trajectory& other) const {
230 // If this is a default constructed trajectory with no states, then we can
231 // simply return the rhs trajectory.
232 if (m_states.empty()) {
233 return other;
234 }
235
236 auto states = m_states;
237 auto otherStates = other.States();
238 for (auto& otherState : otherStates) {
239 otherState.t += m_totalTime;
240 }
241
242 // Here we omit the first state of the other trajectory because we don't
243 // want two time points with different states. Sample() will automatically
244 // interpolate between the end of this trajectory and the second state of
245 // the other trajectory.
246 states.insert(states.end(), otherStates.begin() + 1, otherStates.end());
247 return Trajectory(states);
248 }
249
250 /**
251 * Returns the initial pose of the trajectory.
252 *
253 * @return The initial pose of the trajectory.
254 */
255 Pose2d InitialPose() const { return Sample(0_s).pose; }
256
257 /**
258 * Checks equality between this Trajectory and another object.
259 */
260 bool operator==(const Trajectory&) const = default;
261
262 private:
263 std::vector<State> m_states;
264 units::second_t m_totalTime = 0_s;
265};
266
268void to_json(wpi::json& json, const Trajectory::State& state);
269
271void from_json(const wpi::json& json, Trajectory::State& state);
272
273} // namespace frc
274
#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
Trajectory()=default
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
Represents a transformation for a Pose2d in the pose's frame.
Definition Transform2d.h:21
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
Definition CAN.h:11
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