WPILibC++  2020.3.2
TrajectoryGenerator.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 <functional>
11 #include <memory>
12 #include <utility>
13 #include <vector>
14 
15 #include "frc/spline/SplineParameterizer.h"
16 #include "frc/trajectory/Trajectory.h"
17 #include "frc/trajectory/TrajectoryConfig.h"
18 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
19 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
20 
21 namespace frc {
26  public:
27  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
28 
43  const std::vector<Translation2d>& interiorWaypoints,
44  Spline<3>::ControlVector end, const TrajectoryConfig& config);
45 
59  const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
60  const Pose2d& end, const TrajectoryConfig& config);
61 
73  std::vector<Spline<5>::ControlVector> controlVectors,
74  const TrajectoryConfig& config);
75 
85  static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
86  const TrajectoryConfig& config);
87 
96  template <typename Spline>
97  static std::vector<PoseWithCurvature> SplinePointsFromSplines(
98  const std::vector<Spline>& splines) {
99  // Create the vector of spline points.
100  std::vector<PoseWithCurvature> splinePoints;
101 
102  // Add the first point to the vector.
103  splinePoints.push_back(splines.front().GetPoint(0.0));
104 
105  // Iterate through the vector and parameterize each spline, adding the
106  // parameterized points to the final vector.
107  for (auto&& spline : splines) {
108  auto points = SplineParameterizer::Parameterize(spline);
109  // Append the array of poses to the vector. We are removing the first
110  // point because it's a duplicate of the last point from the previous
111  // spline.
112  splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
113  std::end(points));
114  }
115  return splinePoints;
116  }
117 
123  static void SetErrorHandler(std::function<void(const char*)> func) {
124  s_errorFunc = std::move(func);
125  }
126 
127  private:
128  static void ReportError(const char* error);
129 
130  static const Trajectory kDoNothingTrajectory;
131  static std::function<void(const char*)> s_errorFunc;
132 };
133 } // namespace frc
frc::Spline
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:33
frc::TrajectoryGenerator
Helper class used to generate trajectories with various constraints.
Definition: TrajectoryGenerator.h:25
frc::TrajectoryGenerator::SetErrorHandler
static void SetErrorHandler(std::function< void(const char *)> func)
Set error reporting function.
Definition: TrajectoryGenerator.h:123
frc::TrajectoryGenerator::GenerateTrajectory
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 control vectors and config.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::SplineParameterizer::Parameterize
static std::vector< PoseWithCurvature > Parameterize(const Spline< Dim > &spline, double t0=0.0, double t1=1.0)
Parameterizes the spline.
Definition: SplineParameterizer.h:72
frc::Trajectory
Represents a time-parameterized trajectory.
Definition: Trajectory.h:34
frc::TrajectoryGenerator::SplinePointsFromSplines
static std::vector< PoseWithCurvature > SplinePointsFromSplines(const std::vector< Spline > &splines)
Generate spline points from a vector of splines by parameterizing the splines.
Definition: TrajectoryGenerator.h:97
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::TrajectoryConfig
Represents the configuration for generating a trajectory.
Definition: TrajectoryConfig.h:35