WPILibC++  2020.3.2
Spline.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 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 <array>
11 #include <utility>
12 #include <vector>
13 
14 #include <Eigen/Core>
15 
16 #include "frc/geometry/Pose2d.h"
17 
18 namespace frc {
19 
23 using curvature_t = units::unit_t<
24  units::compound_unit<units::radian, units::inverse<units::meter>>>;
25 
32 template <int Degree>
33 class Spline {
34  public:
35  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
36 
37  Spline() = default;
38 
39  Spline(const Spline&) = default;
40  Spline& operator=(const Spline&) = default;
41 
42  Spline(Spline&&) = default;
43  Spline& operator=(Spline&&) = default;
44 
45  virtual ~Spline() = default;
46 
54  struct ControlVector {
55  std::array<double, (Degree + 1) / 2> x;
56  std::array<double, (Degree + 1) / 2> y;
57  };
58 
65  PoseWithCurvature GetPoint(double t) const {
66  Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
67 
68  // Populate the polynomial bases
69  for (int i = 0; i <= Degree; i++) {
70  polynomialBases(i) = std::pow(t, Degree - i);
71  }
72 
73  // This simply multiplies by the coefficients. We need to divide out t some
74  // n number of times where n is the derivative we want to take.
75  Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
76 
77  double dx, dy, ddx, ddy;
78 
79  // If t = 0, all other terms in the equation cancel out to zero. We can use
80  // the last x^0 term in the equation.
81  if (t == 0.0) {
82  dx = Coefficients()(2, Degree - 1);
83  dy = Coefficients()(3, Degree - 1);
84  ddx = Coefficients()(4, Degree - 2);
85  ddy = Coefficients()(5, Degree - 2);
86  } else {
87  // Divide out t for first derivative.
88  dx = combined(2) / t;
89  dy = combined(3) / t;
90 
91  // Divide out t for second derivative.
92  ddx = combined(4) / t / t;
93  ddy = combined(5) / t / t;
94  }
95 
96  // Find the curvature.
97  const auto curvature =
98  (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
99 
100  return {
101  {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
102  curvature_t(curvature)};
103  }
104 
105  protected:
111  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
112 
119  static Eigen::Vector2d ToVector(const Translation2d& translation) {
120  return (Eigen::Vector2d() << translation.X().to<double>(),
121  translation.Y().to<double>())
122  .finished();
123  }
124 
131  static Translation2d FromVector(const Eigen::Vector2d& vector) {
132  return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
133  }
134 };
135 } // namespace frc
frc::Spline
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:33
frc::Spline::ToVector
static Eigen::Vector2d ToVector(const Translation2d &translation)
Converts a Translation2d into a vector that is compatible with Eigen.
Definition: Spline.h:119
frc::Spline::ControlVector
Represents a control vector for a spline.
Definition: Spline.h:54
frc::Translation2d::X
units::meter_t X() const
Returns the X component of the translation.
Definition: Translation2d.h:61
frc::Rotation2d
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
frc::Translation2d
Represents a translation in 2d space.
Definition: Translation2d.h:28
frc::curvature_t
units::unit_t< units::compound_unit< units::radian, units::inverse< units::meter > >> curvature_t
Define a unit for curvature.
Definition: Trajectory.h:27
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
frc::Translation2d::Y
units::meter_t Y() const
Returns the Y component of the translation.
Definition: Translation2d.h:68
frc::Spline::Coefficients
virtual Eigen::Matrix< double, 6, Degree+1 > Coefficients() const =0
Returns the coefficients of the spline.
frc::Spline::FromVector
static Translation2d FromVector(const Eigen::Vector2d &vector)
Converts an Eigen vector into a Translation2d.
Definition: Spline.h:131