WPILibC++  2020.3.2
Trajectory.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <vector>
11 
12 #include <units/units.h>
13 
14 #include "frc/geometry/Pose2d.h"
15 #include "frc/geometry/Transform2d.h"
16 
17 namespace wpi {
18 class json;
19 } // namespace wpi
20 
21 namespace frc {
22 
26 using curvature_t = units::unit_t<
27  units::compound_unit<units::radian, units::inverse<units::meter>>>;
28 
34 class Trajectory {
35  public:
39  struct State {
40  // The time elapsed since the beginning of the trajectory.
41  units::second_t t = 0_s;
42 
43  // The speed at that point of the trajectory.
44  units::meters_per_second_t velocity = 0_mps;
45 
46  // The acceleration at that point of the trajectory.
47  units::meters_per_second_squared_t acceleration = 0_mps_sq;
48 
49  // The pose at that point of the trajectory.
50  Pose2d pose;
51 
52  // The curvature at that point of the trajectory.
53  curvature_t curvature{0.0};
54 
61  bool operator==(const State& other) const;
62 
69  bool operator!=(const State& other) const;
70 
79  State Interpolate(State endValue, double i) const;
80  };
81 
82  Trajectory() = default;
83 
87  explicit Trajectory(const std::vector<State>& states);
88 
93  units::second_t TotalTime() const { return m_totalTime; }
94 
99  const std::vector<State>& States() const { return m_states; }
100 
107  State Sample(units::second_t t) const;
108 
117  Trajectory TransformBy(const Transform2d& transform);
118 
128  Trajectory RelativeTo(const Pose2d& pose);
129 
135  Pose2d InitialPose() const { return Sample(0_s).pose; }
136 
137  private:
138  std::vector<State> m_states;
139  units::second_t m_totalTime;
140 
150  template <typename T>
151  static T Lerp(const T& startValue, const T& endValue, const double t) {
152  return startValue + (endValue - startValue) * t;
153  }
154 };
155 
156 void to_json(wpi::json& json, const Trajectory::State& state);
157 
158 void from_json(const wpi::json& json, Trajectory::State& state);
159 
160 } // namespace frc
wpi::json
a class to store JSON values
Definition: json.h:2655
frc::Trajectory::InitialPose
Pose2d InitialPose() const
Returns the initial pose of the trajectory.
Definition: Trajectory.h:135
frc::Trajectory::State
Represents one point on the trajectory.
Definition: Trajectory.h:39
frc::Trajectory::TotalTime
units::second_t TotalTime() const
Returns the overall duration of the trajectory.
Definition: Trajectory.h:93
frc::Trajectory::State::operator==
bool operator==(const State &other) const
Checks equality between this State and another object.
frc::Trajectory::RelativeTo
Trajectory RelativeTo(const Pose2d &pose)
Transforms all poses in the trajectory so that they are relative to the given pose.
frc::Transform2d
Represents a transformation for a Pose2d.
Definition: Transform2d.h:19
frc::Trajectory::TransformBy
Trajectory TransformBy(const Transform2d &transform)
Transforms all poses in the trajectory by the given transform.
wpi
WPILib C++ utilities (wpiutil) namespace.
Definition: EventLoopRunner.h:17
frc::Trajectory::States
const std::vector< State > & States() const
Return the states of the trajectory.
Definition: Trajectory.h:99
frc::Trajectory::State::operator!=
bool operator!=(const State &other) const
Checks inequality between this State and another object.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::curvature_t
units::unit_t< units::compound_unit< units::radian, units::inverse< units::meter > >> curvature_t
Define a unit for curvature.
Definition: Trajectory.h:27
frc::Trajectory
Represents a time-parameterized trajectory.
Definition: Trajectory.h:34
frc::Trajectory::State::Interpolate
State Interpolate(State endValue, double i) const
Interpolates between two States.
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::Trajectory::Sample
State Sample(units::second_t t) const
Sample the trajectory at a point in time.