WPILibC++  2020.3.2-60-g3011ebe
Trajectory.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019-2020 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 {
27 class Trajectory {
28  public:
32  struct State {
33  // The time elapsed since the beginning of the trajectory.
34  units::second_t t = 0_s;
35 
36  // The speed at that point of the trajectory.
37  units::meters_per_second_t velocity = 0_mps;
38 
39  // The acceleration at that point of the trajectory.
40  units::meters_per_second_squared_t acceleration = 0_mps_sq;
41 
42  // The pose at that point of the trajectory.
43  Pose2d pose;
44 
45  // The curvature at that point of the trajectory.
46  units::curvature_t curvature{0.0};
47 
54  bool operator==(const State& other) const;
55 
62  bool operator!=(const State& other) const;
63 
72  State Interpolate(State endValue, double i) const;
73  };
74 
75  Trajectory() = default;
76 
80  explicit Trajectory(const std::vector<State>& states);
81 
86  units::second_t TotalTime() const { return m_totalTime; }
87 
92  const std::vector<State>& States() const { return m_states; }
93 
100  State Sample(units::second_t t) const;
101 
110  Trajectory TransformBy(const Transform2d& transform);
111 
121  Trajectory RelativeTo(const Pose2d& pose);
122 
128  Pose2d InitialPose() const { return Sample(0_s).pose; }
129 
130  private:
131  std::vector<State> m_states;
132  units::second_t m_totalTime = 0_s;
133 
143  template <typename T>
144  static T Lerp(const T& startValue, const T& endValue, const double t) {
145  return startValue + (endValue - startValue) * t;
146  }
147 };
148 
149 void to_json(wpi::json& json, const Trajectory::State& state);
150 
151 void from_json(const wpi::json& json, Trajectory::State& state);
152 
153 } // 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:128
frc::Trajectory::State
Represents one point on the trajectory.
Definition: Trajectory.h:32
frc::Trajectory::TotalTime
units::second_t TotalTime() const
Returns the overall duration of the trajectory.
Definition: Trajectory.h:86
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: Endian.h:31
frc::Trajectory::States
const std::vector< State > & States() const
Return the states of the trajectory.
Definition: Trajectory.h:92
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::Trajectory
Represents a time-parameterized trajectory.
Definition: Trajectory.h:27
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: PDPSim.h:16
frc::Trajectory::Sample
State Sample(units::second_t t) const
Sample the trajectory at a point in time.