1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <concepts>
#include <memory>
#include <utility>
#include <vector>
#include <wpi/SymbolExports.h>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/acceleration.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents the configuration for generating a trajectory. This class stores
* the start velocity, end velocity, max velocity, max acceleration, custom
* constraints, and the reversed flag.
*
* The class must be constructed with a max velocity and max acceleration.
* The other parameters (start velocity, end velocity, constraints, reversed)
* have been defaulted to reasonable values (0, 0, {}, false). These values can
* be changed via the SetXXX methods.
*/
class WPILIB_DLLEXPORT TrajectoryConfig {
public:
/**
* Constructs a config object.
* @param maxVelocity The max velocity of the trajectory.
* @param maxAcceleration The max acceleration of the trajectory.
*/
TrajectoryConfig(units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration)
: m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
TrajectoryConfig(const TrajectoryConfig&) = delete;
TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
TrajectoryConfig(TrajectoryConfig&&) = default;
TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
/**
* Sets the start velocity of the trajectory.
* @param startVelocity The start velocity of the trajectory.
*/
void SetStartVelocity(units::meters_per_second_t startVelocity) {
m_startVelocity = startVelocity;
}
/**
* Sets the end velocity of the trajectory.
* @param endVelocity The end velocity of the trajectory.
*/
void SetEndVelocity(units::meters_per_second_t endVelocity) {
m_endVelocity = endVelocity;
}
/**
* Sets the reversed flag of the trajectory.
* @param reversed Whether the trajectory should be reversed or not.
*/
void SetReversed(bool reversed) { m_reversed = reversed; }
/**
* Adds a user-defined constraint to the trajectory.
* @param constraint The user-defined constraint.
*/
template <std::derived_from<TrajectoryConstraint> Constraint>
void AddConstraint(Constraint constraint) {
m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
}
/**
* Adds a differential drive kinematics constraint to ensure that
* no wheel velocity of a differential drive goes above the max velocity.
*
* @param kinematics The differential drive kinematics.
*/
void SetKinematics(const DifferentialDriveKinematics& kinematics) {
AddConstraint(
DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Adds a mecanum drive kinematics constraint to ensure that
* no wheel velocity of a mecanum drive goes above the max velocity.
*
* @param kinematics The mecanum drive kinematics.
*/
void SetKinematics(MecanumDriveKinematics kinematics) {
AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Adds a swerve drive kinematics constraint to ensure that
* no wheel velocity of a swerve drive goes above the max velocity.
*
* @param kinematics The swerve drive kinematics.
*/
template <size_t NumModules>
void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Returns the starting velocity of the trajectory.
* @return The starting velocity of the trajectory.
*/
units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
/**
* Returns the ending velocity of the trajectory.
* @return The ending velocity of the trajectory.
*/
units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
/**
* Returns the maximum velocity of the trajectory.
* @return The maximum velocity of the trajectory.
*/
units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
/**
* Returns the maximum acceleration of the trajectory.
* @return The maximum acceleration of the trajectory.
*/
units::meters_per_second_squared_t MaxAcceleration() const {
return m_maxAcceleration;
}
/**
* Returns the user-defined constraints of the trajectory.
* @return The user-defined constraints of the trajectory.
*/
const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
const {
return m_constraints;
}
/**
* Returns whether the trajectory is reversed or not.
* @return whether the trajectory is reversed or not.
*/
bool IsReversed() const { return m_reversed; }
private:
units::meters_per_second_t m_startVelocity = 0_mps;
units::meters_per_second_t m_endVelocity = 0_mps;
units::meters_per_second_t m_maxVelocity;
units::meters_per_second_squared_t m_maxAcceleration;
std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
bool m_reversed = false;
};
} // namespace frc