Curves
A library for curves generation.
polynomial_splines_traits.hpp
Go to the documentation of this file.
1 /*
2  * polynomial_splines.hpp
3  *
4  * Created on: Mar 7, 2017
5  * Author: Dario Bellicoso
6  */
7 
8 #pragma once
9 
10 // stl
11 #include <array>
12 #include <iostream>
13 
14 // eigen
15 #include <Eigen/Core>
16 #include <Eigen/QR>
17 
18 // boost
19 #include <boost/math/special_functions/pow.hpp>
20 
21 namespace curves {
22 
23 struct SplineOptions {
24 
26  : tf_(0.0),
27  pos0_(0.0), posT_(0.0),
28  vel0_(0.0), velT_(0.0),
29  acc0_(0.0), accT_(0.0)
30  {
31 
32  }
33 
34  constexpr SplineOptions(double tf, double pos0, double posT, double vel0,
35  double velT, double acc0, double accT)
36  : tf_(tf),
37  pos0_(pos0), posT_(posT),
38  vel0_(vel0), velT_(velT),
39  acc0_(acc0), accT_(accT)
40  {
41 
42  }
43 
44  SplineOptions (SplineOptions&&) = default;
46 
48  double tf_;
49 
51  double pos0_;
52 
54  double posT_;
55 
57  double vel0_;
58 
60  double velT_;
61 
63  double acc0_;
64 
66  double accT_;
67 };
68 
69 namespace spline_traits {
70 
71 // Main struct template.
72 template<typename Core_, int SplineOrder_>
73 struct spline_rep {
74 
75  static constexpr unsigned int splineOrder = SplineOrder_;
76  static constexpr unsigned int numCoefficients = SplineOrder_+1;
77 
78  using TimeVectorType = std::array<Core_, numCoefficients>;
79  using SplineCoefficients = std::array<Core_, numCoefficients>;
80 
81  static inline TimeVectorType tau(Core_ tk) noexcept;
82  static inline TimeVectorType dtau(Core_ tk) noexcept;
83  static inline TimeVectorType ddtau(Core_ tk) noexcept;
84 
85  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients);
86 };
87 
88 // Specialization for linear splines.
89 template<>
90 struct spline_rep<double, 1> {
91 
92  static constexpr unsigned int splineOrder = 1;
93  static constexpr unsigned int numCoefficients = splineOrder+1;
94 
95  using TimeVectorType = std::array<double, numCoefficients>;
96  using SplineCoefficients = std::array<double, numCoefficients>;
97 
98  static inline TimeVectorType tau(double tk) noexcept {
99  return { tk, 1.0 };
100  }
101 
102  static inline TimeVectorType dtau(double tk) noexcept {
103  return { 1.0, 0.0 };
104  }
105 
106  static inline TimeVectorType ddtau(double tk) noexcept {
107  return { 0.0, 0.0 };
108  }
109 
110  static constexpr TimeVectorType tauZero{{ 0.0, 1.0 }};
111  static constexpr TimeVectorType dtauZero{{ 1.0, 0.0 }};
112  static constexpr TimeVectorType ddtauZero{{ 0.0, 0.0 }};
113 
115  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
116  coefficients[1] = opts.pos0_; //a0
117  coefficients[0] = (opts.posT_ - opts.pos0_) / opts.tf_; //a1
118 
119  return true;
120  }
121 };
122 
123 // Specialization for quadratic splines.
124 template<>
125 struct spline_rep<double, 2> {
126 
127  static constexpr unsigned int splineOrder = 2;
128  static constexpr unsigned int numCoefficients = splineOrder+1;
129 
130  using TimeVectorType = std::array<double, numCoefficients>;
131  using SplineCoefficients = std::array<double, numCoefficients>;
132 
133  static inline TimeVectorType tau(double tk) noexcept {
134  return { boost::math::pow<2>(tk), tk, 1.0 };
135  }
136 
137  static inline TimeVectorType dtau(double tk) noexcept {
138  return { 2.0*tk, 1.0, 0.0 };
139  }
140 
141  static inline TimeVectorType ddtau(double tk) noexcept {
142  return { 2.0, 0.0, 0.0 };
143  }
144 
145  static constexpr TimeVectorType tauZero{{ 0.0, 0.0, 1.0 }};
146  static constexpr TimeVectorType dtauZero{{ 0.0, 1.0, 0.0 }};
147  static constexpr TimeVectorType ddtauZero{{ 2.0, 0.0, 0.0 }};
148 
150  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
151 
152  coefficients[2] = opts.pos0_; // a0
153  coefficients[1] = opts.vel0_; // a1
154  coefficients[0] = (opts.posT_ - opts.pos0_ - opts.vel0_*opts.tf_) / boost::math::pow<2>(opts.tf_); // a2
155 
156  return true;
157  }
158 };
159 
160 // Specialization for third order splines (cubic).
161 template<>
162 struct spline_rep<double, 3> {
163 
164  static constexpr unsigned int splineOrder = 3;
165  static constexpr unsigned int numCoefficients = splineOrder+1;
166 
167  using TimeVectorType = std::array<double, numCoefficients>;
168  using SplineCoefficients = std::array<double, numCoefficients>;
169 
170  static inline TimeVectorType tau(double tk) noexcept {
171  return { boost::math::pow<3>(tk), boost::math::pow<2>(tk), tk, 1.0 };
172  }
173 
174  static inline TimeVectorType dtau(double tk) noexcept {
175  return { 3.0*boost::math::pow<2>(tk), 2.0*tk, 1.0, 0.0 };
176  }
177 
178  static inline TimeVectorType ddtau(double tk) noexcept {
179  return { 6.0*tk, 2.0, 0.0, 0.0 };
180  }
181 
182  static constexpr TimeVectorType tauZero{{ 0.0, 0.0, 0.0, 1.0 }};
183  static constexpr TimeVectorType dtauZero{{ 0.0, 0.0, 1.0, 0.0 }};
184  static constexpr TimeVectorType ddtauZero{{ 0.0, 2.0, 0.0, 0.0 }};
185 
187  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
188 
189  Eigen::Matrix<double, numCoefficients, 1> b;
190  b << opts.pos0_, opts.vel0_, opts.posT_, opts.velT_;
191 
192  Eigen::Matrix<double, numCoefficients, numCoefficients> A;
193  A << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
194  Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
195  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
196  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data());
197 
198  Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) = A.colPivHouseholderQr().solve(b);
199 
200  return true;
201  }
202 };
203 
204 // Specialization for fourth order (quartic) splines.
205 template<>
206 struct spline_rep<double, 4> {
207 
208  static constexpr unsigned int splineOrder = 4;
209  static constexpr unsigned int numCoefficients = splineOrder+1;
210 
211  using TimeVectorType = std::array<double, numCoefficients>;
212  using SplineCoefficients = std::array<double, numCoefficients>;
213 
214  static inline TimeVectorType tau(double tk) noexcept {
215  return { boost::math::pow<4>(tk), boost::math::pow<3>(tk), boost::math::pow<2>(tk), tk, 1.0};
216  }
217 
218  static inline TimeVectorType dtau(double tk) noexcept {
219  return { 4.0*boost::math::pow<3>(tk), 3.0*boost::math::pow<2>(tk), 2.0*tk, 1.0, 0.0};
220  }
221 
222  static inline TimeVectorType ddtau(double tk) noexcept {
223  return { 12.0*boost::math::pow<2>(tk), 6.0*tk, 2.0, 0.0, 0.0};
224  }
225 
226  static constexpr TimeVectorType tauZero{{ 0.0, 0.0, 0.0, 0.0, 1.0 }};
227  static constexpr TimeVectorType dtauZero{{ 0.0, 0.0, 0.0, 1.0, 0.0 }};
228  static constexpr TimeVectorType ddtauZero{{ 0.0, 0.0, 2.0, 0.0, 0.0 }};
229 
231  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
232  Eigen::Matrix<double, numCoefficients, 1> b;
233  b << opts.pos0_, opts.vel0_, opts.acc0_, opts.posT_, opts.velT_;
234 
235  Eigen::Matrix<double, numCoefficients, numCoefficients> A;
236  A << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
237  Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
238  Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(ddtauZero.data()),
239  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
240  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data());
241 
242  Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) = A.colPivHouseholderQr().solve(b);
243 
244  return true;
245  }
246 
247 };
248 
249 
250 // Specialization for fifth order (quintic) splines.
251 template<>
252 struct spline_rep<double, 5> {
253 
254  static constexpr unsigned int splineOrder = 5;
255  static constexpr unsigned int numCoefficients = splineOrder+1;
256 
257  using TimeVectorType = std::array<double, numCoefficients>;
258  using SplineCoefficients = std::array<double, numCoefficients>;
259 
260  static inline TimeVectorType tau(double tk) noexcept {
261  return { boost::math::pow<5>(tk), boost::math::pow<4>(tk), boost::math::pow<3>(tk),
262  boost::math::pow<2>(tk), tk, 1.0};
263  }
264 
265  static inline TimeVectorType dtau(double tk) noexcept {
266  return { 5.0*boost::math::pow<4>(tk), 4.0*boost::math::pow<3>(tk), 3.0*boost::math::pow<2>(tk),
267  2.0*tk, 1.0, 0.0};
268  }
269 
270  static inline TimeVectorType ddtau(double tk) noexcept {
271  return { 20.0*boost::math::pow<3>(tk), 12.0*boost::math::pow<2>(tk), 6.0*tk,
272  2.0, 0.0, 0.0};
273  }
274 
275  static constexpr TimeVectorType tauZero{{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 }};
276  static constexpr TimeVectorType dtauZero{{ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 }};
277  static constexpr TimeVectorType ddtauZero{{ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0 }};
278 
280  static inline bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
281  Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) =
282  (Eigen::Matrix<double, numCoefficients, numCoefficients>() << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
283  Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
284  Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(ddtauZero.data()),
285  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
286  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data()),
287  Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((ddtau(opts.tf_)).data())).finished().
288  colPivHouseholderQr().solve(
289  (Eigen::Matrix<double, numCoefficients, 1>() << opts.pos0_, opts.vel0_, opts.acc0_, opts.posT_, opts.velT_, opts.accT_).finished());
290 
291  return true;
292  }
293 
294 };
295 
296 }
297 
298 }
double vel0_
The scalar velocity at time 0.
std::array< double, numCoefficients > SplineCoefficients
std::array< double, numCoefficients > TimeVectorType
double accT_
The scalar acceleration at time tf.
double acc0_
The scalar acceleration at time 0.
static bool compute(const SplineOptions &opts, SplineCoefficients &coefficients)
Map initial pos/vel/accel and final pos/vel to spline coefficients.
static TimeVectorType tau(double tk) noexcept
static TimeVectorType dtau(double tk) noexcept
constexpr SplineOptions(double tf, double pos0, double posT, double vel0, double velT, double acc0, double accT)
std::array< double, numCoefficients > TimeVectorType
std::array< double, numCoefficients > TimeVectorType
double posT_
The scalar position at time tf.
static TimeVectorType tau(double tk) noexcept
static TimeVectorType dtau(double tk) noexcept
double pos0_
The scalar position at time 0.
double tf_
The total duration of the spline in seconds.
static TimeVectorType dtau(double tk) noexcept
static TimeVectorType ddtau(double tk) noexcept
static TimeVectorType ddtau(double tk) noexcept
std::array< Core_, numCoefficients > SplineCoefficients
static TimeVectorType dtau(double tk) noexcept
std::array< double, numCoefficients > TimeVectorType
static bool compute(const SplineOptions &opts, SplineCoefficients &coefficients)
Map initial pos, initial vel and final pos to spline coefficients.
static TimeVectorType tau(double tk) noexcept
static TimeVectorType ddtau(double tk) noexcept
static TimeVectorType ddtau(double tk) noexcept
static bool compute(const SplineOptions &opts, SplineCoefficients &coefficients)
Map initial pos and final pos to spline coefficients.
std::array< double, numCoefficients > SplineCoefficients
static TimeVectorType tau(double tk) noexcept
static bool compute(const SplineOptions &opts, SplineCoefficients &coefficients)
Map initial pos/vel/accel and final pos/vel/accel to spline coefficients.
static bool compute(const SplineOptions &opts, SplineCoefficients &coefficients)
Map initial pos, initial vel, final pos and final vel to spline coefficients.
std::array< double, numCoefficients > SplineCoefficients
static TimeVectorType dtau(double tk) noexcept
static TimeVectorType ddtau(double tk) noexcept
double velT_
The scalar velocity at time tf.
std::array< double, numCoefficients > TimeVectorType
SplineOptions & operator=(SplineOptions &&)=default
static TimeVectorType tau(double tk) noexcept
std::array< double, numCoefficients > SplineCoefficients
std::array< double, numCoefficients > SplineCoefficients
std::array< Core_, numCoefficients > TimeVectorType