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
// 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 <functional>
#include <memory>
#include <utility>
#include <vector>
#include <wpi/SymbolExports.h>
#include "frc/spline/SplineParameterizer.h"
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* Helper class used to generate trajectories with various constraints.
*/
class WPILIB_DLLEXPORT TrajectoryGenerator {
public:
using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
/**
* Generates a trajectory from the given control vectors and config. This
* method uses clamped cubic splines -- a method in which the exterior control
* vectors and interior waypoints are provided. The headings are automatically
* determined at the interior points to ensure continuous curvature.
*
* @param initial The initial control vector.
* @param interiorWaypoints The interior waypoints.
* @param end The ending control vector.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
Spline<3>::ControlVector end, const TrajectoryConfig& config);
/**
* Generates a trajectory from the given waypoints and config. This method
* uses clamped cubic splines -- a method in which the initial pose, final
* pose, and interior waypoints are provided. The headings are automatically
* determined at the interior points to ensure continuous curvature.
*
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end, const TrajectoryConfig& config);
/**
* Generates a trajectory from the given quintic control vectors and config.
* This method uses quintic hermite splines -- therefore, all points must be
* represented by control vectors. Continuous curvature is guaranteed in this
* method.
*
* @param controlVectors List of quintic control vectors.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
std::vector<Spline<5>::ControlVector> controlVectors,
const TrajectoryConfig& config);
/**
* Generates a trajectory from the given waypoints and config. This method
* uses quintic hermite splines -- therefore, all points must be represented
* by Pose2d objects. Continuous curvature is guaranteed in this method.
*
* @param waypoints List of waypoints..
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
const TrajectoryConfig& config);
/**
* Generate spline points from a vector of splines by parameterizing the
* splines.
*
* @param splines The splines to parameterize.
*
* @return The spline points for use in time parameterization of a trajectory.
*/
template <typename Spline>
static std::vector<PoseWithCurvature> SplinePointsFromSplines(
const std::vector<Spline>& splines) {
// Create the vector of spline points.
std::vector<PoseWithCurvature> splinePoints;
// Add the first point to the vector.
splinePoints.push_back(splines.front().GetPoint(0.0).value());
// Iterate through the vector and parameterize each spline, adding the
// parameterized points to the final vector.
for (auto&& spline : splines) {
auto points = SplineParameterizer::Parameterize(spline);
// Append the array of poses to the vector. We are removing the first
// point because it's a duplicate of the last point from the previous
// spline.
splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
std::end(points));
}
return splinePoints;
}
/**
* Set error reporting function. By default, it is output to stderr.
*
* @param func Error reporting function.
*/
static void SetErrorHandler(std::function<void(const char*)> func);
private:
static void ReportError(const char* error);
static const Trajectory kDoNothingTrajectory;
static std::function<void(const char*)> s_errorFunc;
};
} // namespace frc