WPILibC++  2020.3.2
QuinticHermiteSpline.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 
12 #include <Eigen/Core>
13 
14 #include "frc/spline/Spline.h"
15 
16 namespace frc {
20 class QuinticHermiteSpline : public Spline<5> {
21  public:
36  QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
37  std::array<double, 3> xFinalControlVector,
38  std::array<double, 3> yInitialControlVector,
39  std::array<double, 3> yFinalControlVector);
40 
41  protected:
46  Eigen::Matrix<double, 6, 6> Coefficients() const override {
47  return m_coefficients;
48  }
49 
50  private:
51  Eigen::Matrix<double, 6, 6> m_coefficients =
52  Eigen::Matrix<double, 6, 6>::Zero();
53 
58  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
59  // clang-format off
60  static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
61  -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
62  +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
63  -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
64  +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
65  +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
66  +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
67  // clang-format on
68  return basis;
69  }
70 
80  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
81  std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
82  return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
83  initialVector[2], finalVector[0], finalVector[1], finalVector[2])
84  .finished();
85  }
86 };
87 } // namespace frc
frc::Spline
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:33
frc::QuinticHermiteSpline::QuinticHermiteSpline
QuinticHermiteSpline(std::array< double, 3 > xInitialControlVector, std::array< double, 3 > xFinalControlVector, std::array< double, 3 > yInitialControlVector, std::array< double, 3 > yFinalControlVector)
Constructs a quintic hermite spline with the specified control vectors.
frc::QuinticHermiteSpline
Represents a hermite spline of degree 5.
Definition: QuinticHermiteSpline.h:20
frc::QuinticHermiteSpline::Coefficients
Eigen::Matrix< double, 6, 6 > Coefficients() const override
Returns the coefficients matrix.
Definition: QuinticHermiteSpline.h:46
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16