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