WPILibC++ 2023.4.3
Spline.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <utility>
8#include <vector>
9
10#include <wpi/array.h>
11
12#include "frc/EigenCore.h"
13#include "frc/geometry/Pose2d.h"
14#include "units/curvature.h"
15#include "units/length.h"
16
17namespace frc {
18/**
19 * Represents a two-dimensional parametric spline that interpolates between two
20 * points.
21 *
22 * @tparam Degree The degree of the spline.
23 */
24template <int Degree>
25class Spline {
26 public:
27 using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
28
29 Spline() = default;
30
31 Spline(const Spline&) = default;
32 Spline& operator=(const Spline&) = default;
33
34 Spline(Spline&&) = default;
35 Spline& operator=(Spline&&) = default;
36
37 virtual ~Spline() = default;
38
39 /**
40 * Represents a control vector for a spline.
41 *
42 * Each element in each array represents the value of the derivative at the
43 * index. For example, the value of x[2] is the second derivative in the x
44 * dimension.
45 */
47 wpi::array<double, (Degree + 1) / 2> x;
48 wpi::array<double, (Degree + 1) / 2> y;
49 };
50
51 /**
52 * Gets the pose and curvature at some point t on the spline.
53 *
54 * @param t The point t
55 * @return The pose and curvature at that point.
56 */
57 PoseWithCurvature GetPoint(double t) const {
58 Vectord<Degree + 1> polynomialBases;
59
60 // Populate the polynomial bases
61 for (int i = 0; i <= Degree; i++) {
62 polynomialBases(i) = std::pow(t, Degree - i);
63 }
64
65 // This simply multiplies by the coefficients. We need to divide out t some
66 // n number of times where n is the derivative we want to take.
67 Vectord<6> combined = Coefficients() * polynomialBases;
68
69 double dx, dy, ddx, ddy;
70
71 // If t = 0, all other terms in the equation cancel out to zero. We can use
72 // the last x^0 term in the equation.
73 if (t == 0.0) {
74 dx = Coefficients()(2, Degree - 1);
75 dy = Coefficients()(3, Degree - 1);
76 ddx = Coefficients()(4, Degree - 2);
77 ddy = Coefficients()(5, Degree - 2);
78 } else {
79 // Divide out t for first derivative.
80 dx = combined(2) / t;
81 dy = combined(3) / t;
82
83 // Divide out t for second derivative.
84 ddx = combined(4) / t / t;
85 ddy = combined(5) / t / t;
86 }
87
88 // Find the curvature.
89 const auto curvature =
90 (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
91
92 return {
93 {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
94 units::curvature_t{curvature}};
95 }
96
97 protected:
98 /**
99 * Returns the coefficients of the spline.
100 *
101 * @return The coefficients of the spline.
102 */
104
105 /**
106 * Converts a Translation2d into a vector that is compatible with Eigen.
107 *
108 * @param translation The Translation2d to convert.
109 * @return The vector.
110 */
111 static Eigen::Vector2d ToVector(const Translation2d& translation) {
112 return Eigen::Vector2d{translation.X().value(), translation.Y().value()};
113 }
114
115 /**
116 * Converts an Eigen vector into a Translation2d.
117 *
118 * @param vector The vector to convert.
119 * @return The Translation2d.
120 */
121 static Translation2d FromVector(const Eigen::Vector2d& vector) {
122 return Translation2d{units::meter_t{vector(0)}, units::meter_t{vector(1)}};
123 }
124};
125} // namespace frc
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:180
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:26
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:25
virtual ~Spline()=default
virtual Matrixd< 6, Degree+1 > Coefficients() const =0
Returns the coefficients of the spline.
static Translation2d FromVector(const Eigen::Vector2d &vector)
Converts an Eigen vector into a Translation2d.
Definition: Spline.h:121
Spline()=default
PoseWithCurvature GetPoint(double t) const
Gets the pose and curvature at some point t on the spline.
Definition: Spline.h:57
Spline & operator=(const Spline &)=default
static Eigen::Vector2d ToVector(const Translation2d &translation)
Converts a Translation2d into a vector that is compatible with Eigen.
Definition: Spline.h:111
Spline(const Spline &)=default
std::pair< Pose2d, units::curvature_t > PoseWithCurvature
Definition: Spline.h:27
Spline(Spline &&)=default
Spline & operator=(Spline &&)=default
Represents a translation in 2D space.
Definition: Translation2d.h:29
constexpr units::meter_t X() const
Returns the X component of the translation.
Definition: Translation2d.h:70
constexpr units::meter_t Y() const
Returns the Y component of the translation.
Definition: Translation2d.h:77
This class is a wrapper around std::array that does compile time size checking.
Definition: array.h:25
constexpr common_return_t< T1, T2 > hypot(const T1 x, const T2 y) noexcept
Compile-time Pythagorean addition function.
Definition: hypot.hpp:84
Definition: AprilTagFieldLayout.h:22
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
constexpr common_t< T1, T2 > pow(const T1 base, const T2 exp_term) noexcept
Compile-time power function.
Definition: pow.hpp:76
Represents a control vector for a spline.
Definition: Spline.h:46
wpi::array< double,(Degree+1)/2 > x
Definition: Spline.h:47
wpi::array< double,(Degree+1)/2 > y
Definition: Spline.h:48