WPILibC++  2020.3.2
SplineHelper.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 "frc/spline/CubicHermiteSpline.h"
15 #include "frc/spline/QuinticHermiteSpline.h"
16 
17 namespace frc {
22 class SplineHelper {
23  public:
33  static std::array<Spline<3>::ControlVector, 2>
35  const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
36  const Pose2d& end);
37 
44  static std::vector<Spline<5>::ControlVector>
45  QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
46 
64  static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
65  const Spline<3>::ControlVector& start,
66  std::vector<Translation2d> waypoints,
67  const Spline<3>::ControlVector& end);
68 
78  static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
79  const std::vector<Spline<5>::ControlVector>& controlVectors);
80 
81  private:
82  static Spline<3>::ControlVector CubicControlVector(double scalar,
83  const Pose2d& point) {
84  return {
85  {point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
86  {point.Translation().Y().to<double>(),
87  scalar * point.Rotation().Sin()}};
88  }
89 
90  static Spline<5>::ControlVector QuinticControlVector(double scalar,
91  const Pose2d& point) {
92  return {{point.Translation().X().to<double>(),
93  scalar * point.Rotation().Cos(), 0.0},
94  {point.Translation().Y().to<double>(),
95  scalar * point.Rotation().Sin(), 0.0}};
96  }
97 
107  static void ThomasAlgorithm(const std::vector<double>& a,
108  const std::vector<double>& b,
109  const std::vector<double>& c,
110  const std::vector<double>& d,
111  std::vector<double>* solutionVector);
112 };
113 } // namespace frc
frc::Spline
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:33
frc::SplineHelper::CubicSplinesFromControlVectors
static std::vector< CubicHermiteSpline > CubicSplinesFromControlVectors(const Spline< 3 >::ControlVector &start, std::vector< Translation2d > waypoints, const Spline< 3 >::ControlVector &end)
Returns a set of cubic splines corresponding to the provided control vectors.
frc::Rotation2d::Cos
double Cos() const
Returns the cosine of the rotation.
Definition: Rotation2d.h:160
frc::Translation2d::X
units::meter_t X() const
Returns the X component of the translation.
Definition: Translation2d.h:61
frc::Pose2d::Translation
const Translation2d & Translation() const
Returns the underlying translation.
Definition: Pose2d.h:104
frc::SplineHelper
Helper class that is used to generate cubic and quintic splines from user provided waypoints.
Definition: SplineHelper.h:22
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::Rotation2d::Sin
double Sin() const
Returns the sine of the rotation.
Definition: Rotation2d.h:167
frc::Pose2d::Rotation
const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:111
frc::SplineHelper::CubicControlVectorsFromWaypoints
static std::array< Spline< 3 >::ControlVector, 2 > CubicControlVectorsFromWaypoints(const Pose2d &start, const std::vector< Translation2d > &interiorWaypoints, const Pose2d &end)
Returns 2 cubic control vectors from a set of exterior waypoints and interior translations.
frc::SplineHelper::QuinticControlVectorsFromWaypoints
static std::vector< Spline< 5 >::ControlVector > QuinticControlVectorsFromWaypoints(const std::vector< Pose2d > &waypoints)
Returns quintic control vectors from a set of waypoints.
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::SplineHelper::QuinticSplinesFromControlVectors
static std::vector< QuinticHermiteSpline > QuinticSplinesFromControlVectors(const std::vector< Spline< 5 >::ControlVector > &controlVectors)
Returns a set of quintic splines corresponding to the provided control vectors.