WPILibC++  2020.3.2
SplineParameterizer.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 /*
9  * MIT License
10  *
11  * Copyright (c) 2018 Team 254
12  *
13  * Permission is hereby granted, free of charge, to any person obtaining a copy
14  * of this software and associated documentation files (the "Software"), to deal
15  * in the Software without restriction, including without limitation the rights
16  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17  * copies of the Software, and to permit persons to whom the Software is
18  * furnished to do so, subject to the following conditions:
19  *
20  * The above copyright notice and this permission notice shall be included in
21  * all copies or substantial portions of the Software.
22  *
23  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
24  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
25  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
26  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
27  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
28  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
29  * SOFTWARE.
30  */
31 
32 #pragma once
33 
34 #include <frc/spline/Spline.h>
35 
36 #include <stack>
37 #include <string>
38 #include <utility>
39 #include <vector>
40 
41 #include <units/units.h>
42 #include <wpi/Twine.h>
43 
44 namespace frc {
45 
50  public:
51  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
52 
53  struct MalformedSplineException : public std::runtime_error {
54  explicit MalformedSplineException(const char* what_arg)
55  : runtime_error(what_arg) {}
56  };
57 
71  template <int Dim>
72  static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
73  double t0 = 0.0,
74  double t1 = 1.0) {
75  std::vector<PoseWithCurvature> splinePoints;
76 
77  // The parameterization does not add the initial point. Let's add that.
78  splinePoints.push_back(spline.GetPoint(t0));
79 
80  // We use an "explicit stack" to simulate recursion, instead of a recursive
81  // function call This give us greater control, instead of a stack overflow
82  std::stack<StackContents> stack;
83  stack.emplace(StackContents{t0, t1});
84 
85  StackContents current;
86  PoseWithCurvature start;
87  PoseWithCurvature end;
88  int iterations = 0;
89 
90  while (!stack.empty()) {
91  current = stack.top();
92  stack.pop();
93  start = spline.GetPoint(current.t0);
94  end = spline.GetPoint(current.t1);
95 
96  const auto twist = start.first.Log(end.first);
97 
98  if (units::math::abs(twist.dy) > kMaxDy ||
99  units::math::abs(twist.dx) > kMaxDx ||
100  units::math::abs(twist.dtheta) > kMaxDtheta) {
101  stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
102  stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
103  } else {
104  splinePoints.push_back(spline.GetPoint(current.t1));
105  }
106 
107  if (iterations++ >= kMaxIterations) {
109  "Could not parameterize a malformed spline. "
110  "This means that you probably had two or more adjacent "
111  "waypoints that were very close together with headings "
112  "in opposing directions.");
113  }
114  }
115 
116  return splinePoints;
117  }
118 
119  private:
120  // Constraints for spline parameterization.
121  static constexpr units::meter_t kMaxDx = 5_in;
122  static constexpr units::meter_t kMaxDy = 0.05_in;
123  static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
124 
125  struct StackContents {
126  double t0;
127  double t1;
128  };
129 
137  static constexpr int kMaxIterations = 5000;
138 
139  friend class CubicHermiteSplineTest;
140  friend class QuinticHermiteSplineTest;
141 };
142 } // namespace frc
frc::Spline
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:33
frc::SplineParameterizer
Class used to parameterize a spline by its arc length.
Definition: SplineParameterizer.h:49
frc::SplineParameterizer::MalformedSplineException
Definition: SplineParameterizer.h:53
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::Spline::GetPoint
PoseWithCurvature GetPoint(double t) const
Gets the pose and curvature at some point t on the spline.
Definition: Spline.h:65
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16