Curves
A library for curves generation.
PolynomialSpline.hpp
Go to the documentation of this file.
1 /*
2  * PolynomialSpline.hpp
3  *
4  * Created on: Mar 7, 2017
5  * Author: Dario Bellicoso
6  */
7 
8 #pragma once
9 
10 // eigen
11 #include <Eigen/Core>
12 
13 // curves
15 
16 // stl
17 #include <numeric>
18 
19 namespace curves {
20 
35 template <int splineOrder_>
37  public:
38 
39  static constexpr unsigned int splineOrder = splineOrder_;
40  static constexpr unsigned int coefficientCount = splineOrder + 1;
41 
44  using EigenTimeVectorType = Eigen::Matrix<double, 1, coefficientCount>;
45  using EigenCoefficientVectorType = Eigen::Matrix<double, coefficientCount, 1>;
46 
48  duration_(0.0),
49  didEvaluateCoeffs_(false),
51  {
52 
53  }
54 
55  template<typename SplineCoeff_>
56  PolynomialSpline(SplineCoeff_&& coefficients, double duration) :
57  duration_(duration),
58  didEvaluateCoeffs_(true),
59  coefficients_(std::forward<SplineCoeff_>(coefficients))
60  {
61 
62  }
63 
64  explicit PolynomialSpline(const SplineOptions& options) : duration_(options.tf_) {
65  computeCoefficients(options);
66  }
67 
68  explicit PolynomialSpline(SplineOptions&& options) : duration_(options.tf_) {
69  computeCoefficients(std::move(options));
70  }
71 
72  virtual ~PolynomialSpline() = default;
73 
74  PolynomialSpline(PolynomialSpline &&) = default;
76 
77  PolynomialSpline(const PolynomialSpline&) = default;
78  PolynomialSpline& operator=(const PolynomialSpline&) = default;
79 
82  return coefficients_;
83  }
84 
86  template<typename SplineOptionsType_>
87  bool computeCoefficients(SplineOptionsType_&& options) {
88  duration_ = options.tf_;
89  return SplineImplementation::compute(std::forward<SplineOptionsType_>(options), coefficients_);
90  }
91 
93  void setCoefficientsAndDuration(const SplineCoefficients& coefficients, double duration) {
94  coefficients_ = coefficients;
95  duration_ = duration;
96  }
97 
99  constexpr double getPositionAtTime(double tk) const {
100  return std::inner_product(coefficients_.begin(), coefficients_.end(),
101  SplineImplementation::tau(std::max(0.0, std::min(tk, duration_))).begin(), 0.0);
102  }
103 
105  constexpr double getVelocityAtTime(double tk) const {
106  return std::inner_product(coefficients_.begin(), coefficients_.end(),
107  SplineImplementation::dtau(std::max(0.0, std::min(tk, duration_))).begin(), 0.0);
108  }
109 
111  constexpr double getAccelerationAtTime(double tk) const {
112  return std::inner_product(coefficients_.begin(), coefficients_.end(),
113  SplineImplementation::ddtau(std::max(0.0, std::min(tk, duration_))).begin(), 0.0);
114  }
115 
116 
117 
118 
120  static inline void getTimeVector(Eigen::Ref<EigenTimeVectorType> timeVec, const double tk) {
121  timeVec = Eigen::Map<EigenTimeVectorType>(SplineImplementation::tau(tk).data());
122  }
123 
125  template<typename Derived>
126  static inline void getTimeVector(Eigen::MatrixBase<Derived> const & timeVec, const double tk) {
127  assert(timeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
128  timeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
129  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
130  const_cast<Eigen::MatrixBase<Derived>&>(timeVec) =
131  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::tau(tk)).data());
132  }
133 
135  template<typename Derived>
136  static inline void addTimeVector(Eigen::MatrixBase<Derived> const & timeVec, const double tk) {
137  assert(timeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
138  timeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
139  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
140  const_cast<Eigen::MatrixBase<Derived>&>(timeVec) +=
141  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::tau(tk)).data());
142  }
143 
144 
145 
146 
148  static inline void getDTimeVector(Eigen::Ref<EigenTimeVectorType> dtimeVec, const double tk) {
149  dtimeVec = Eigen::Map<EigenTimeVectorType>(SplineImplementation::dtau(tk).data());
150  }
151 
153  template<typename Derived>
154  static inline void getDiffTimeVector(Eigen::MatrixBase<Derived> const & dtimeVec, const double tk) {
155  assert(dtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
156  dtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
157  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
158  const_cast<Eigen::MatrixBase<Derived>&>(dtimeVec) =
159  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtau(tk)).data());
160  }
161 
163  template<typename Derived>
164  static inline void addDiffTimeVector(Eigen::MatrixBase<Derived> const & dtimeVec, const double tk) {
165  assert(dtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
166  dtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
167  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
168  const_cast<Eigen::MatrixBase<Derived>&>(dtimeVec) +=
169  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtau(tk)).data());
170  }
171 
172 
173 
175  static inline void getDDTimeVector(Eigen::Ref<EigenTimeVectorType> ddtimeVec, const double tk) {
176  ddtimeVec = Eigen::Map<EigenTimeVectorType>(SplineImplementation::ddtau(tk).data());
177  }
178 
180  template<typename Derived>
181  static inline void getDDiffTimeVector(Eigen::MatrixBase<Derived> const & ddtimeVec, const double tk) {
182  assert(ddtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
183  ddtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
184  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
185  const_cast<Eigen::MatrixBase<Derived>&>(ddtimeVec) =
186  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtau(tk)).data());
187  }
188 
190  template<typename Derived>
191  static inline void addDDiffTimeVector(Eigen::MatrixBase<Derived> const & ddtimeVec, const double tk) {
192  assert(ddtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
193  ddtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
194  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
195  const_cast<Eigen::MatrixBase<Derived>&>(ddtimeVec) +=
196  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::ddtau(tk)).data());
197  }
198 
199 
200 
202  static inline void getTimeVectorAtZero(Eigen::Ref<EigenTimeVectorType> timeVec) {
203  timeVec = Eigen::Map<const EigenTimeVectorType>((SplineImplementation::tauZero).data());
204  }
205 
207  template<typename Derived>
208  static inline void getTimeVectorAtZero(
209  Eigen::MatrixBase<Derived> const & timeVec) {
210  assert(timeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
211  timeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
212  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
213  const_cast<Eigen::MatrixBase<Derived>&>(timeVec) =
214  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::tauZero).data());
215  }
216 
218  template<typename Derived>
219  static inline void addTimeVectorAtZero(
220  Eigen::MatrixBase<Derived> const & timeVec) {
221  assert(timeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
222  timeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
223  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
224  const_cast<Eigen::MatrixBase<Derived>&>(timeVec) +=
225  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::tauZero).data());
226  }
227 
228 
229 
230 
232  static inline void getDTimeVectorAtZero(Eigen::Ref<EigenTimeVectorType> dtimeVec) {
233  dtimeVec = Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtauZero).data());
234  }
235 
237  template<typename Derived>
238  static inline void getDiffTimeVectorAtZero(
239  Eigen::MatrixBase<Derived> const & dtimeVec) {
240  assert(dtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
241  dtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
242  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
243  const_cast<Eigen::MatrixBase<Derived>&>(dtimeVec) =
244  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtauZero).data());
245  }
246 
248  template<typename Derived>
249  static inline void addDiffTimeVectorAtZero(
250  Eigen::MatrixBase<Derived> const & dtimeVec) {
251  assert(dtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
252  dtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
253  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
254  const_cast<Eigen::MatrixBase<Derived>&>(dtimeVec) +=
255  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtauZero).data());
256  }
257 
258 
260  static inline void getDDTimeVectorAtZero(Eigen::Ref<EigenTimeVectorType> ddtimeVec) {
261  ddtimeVec = Eigen::Map<const EigenTimeVectorType>((SplineImplementation::ddtauZero).data());
262  }
263 
265  template<typename Derived>
266  static inline void getDDiffTimeVectorAtZero(
267  Eigen::MatrixBase<Derived> const & ddtimeVec) {
268  assert(ddtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
269  ddtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
270  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
271  const_cast<Eigen::MatrixBase<Derived>&>(ddtimeVec) =
272  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::ddtauZero).data());
273  }
274 
276  template<typename Derived>
277  static inline void addDDiffTimeVectorAtZero(
278  Eigen::MatrixBase<Derived> const & ddtimeVec) {
279  assert(ddtimeVec.rows() == EigenTimeVectorType::RowsAtCompileTime &&
280  ddtimeVec.cols() == EigenTimeVectorType::ColsAtCompileTime);
281  // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
282  const_cast<Eigen::MatrixBase<Derived>&>(ddtimeVec) +=
283  Eigen::Map<const EigenTimeVectorType>((SplineImplementation::ddtauZero).data());
284  }
285 
287  double getSplineDuration() const {
288  return duration_;
289  }
290 
291  protected:
293  double duration_;
294 
297 
298  /*
299  * s(t) = an*t^n + ... + a1*t + a0
300  * splineCoeff_ = [an ... a1 a0]
301  */
303 };
304 
305 } /* namespace */
static constexpr unsigned int splineOrder
constexpr double getAccelerationAtTime(double tk) const
Get the second derivative of the spline evaluated at time tk.
static void getTimeVectorAtZero(Eigen::Ref< EigenTimeVectorType > timeVec)
Get the time vector evaluated at zero.
static void getDDiffTimeVectorAtZero(Eigen::MatrixBase< Derived > const &ddtimeVec)
Get the time vector evaluated at zero.
static void getDiffTimeVector(Eigen::MatrixBase< Derived > const &dtimeVec, const double tk)
Get the first derivative of the time vector evaluated at time tk.
static void addTimeVector(Eigen::MatrixBase< Derived > const &timeVec, const double tk)
Get the time vector evaluated at time tk and add it to the input vector.
static TimeVectorType dtau(Core_ tk) noexcept
static void addDDiffTimeVectorAtZero(Eigen::MatrixBase< Derived > const &ddtimeVec)
Get the time vector evaluated at zero.
PolynomialSpline(SplineOptions &&options)
typename SplineImplementation::SplineCoefficients SplineCoefficients
constexpr double getVelocityAtTime(double tk) const
Get the first derivative of the spline evaluated at time tk.
STL namespace.
static void getDDTimeVectorAtZero(Eigen::Ref< EigenTimeVectorType > ddtimeVec)
Get the second derivative of the time vector evaluated at zero.
double getSplineDuration() const
Get the duration of the spline in seconds.
Eigen::Matrix< double, coefficientCount, 1 > EigenCoefficientVectorType
static void addTimeVectorAtZero(Eigen::MatrixBase< Derived > const &timeVec)
Get the time vector evaluated at zero.
static void getDTimeVectorAtZero(Eigen::Ref< EigenTimeVectorType > dtimeVec)
Get the first derivative of the time vector evaluated at zero.
static void getTimeVector(Eigen::Ref< EigenTimeVectorType > timeVec, const double tk)
Get the time vector evaluated at time tk.
constexpr double getPositionAtTime(double tk) const
Get the spline evaluated at time tk.
static constexpr unsigned int coefficientCount
static TimeVectorType ddtau(Core_ tk) noexcept
std::array< Core_, numCoefficients > SplineCoefficients
void setCoefficientsAndDuration(const SplineCoefficients &coefficients, double duration)
Set the coefficients and the duration of the spline.
double duration_
The duration of the spline in seconds.
static void getDiffTimeVectorAtZero(Eigen::MatrixBase< Derived > const &dtimeVec)
Get the time vector evaluated at zero.
const SplineCoefficients & getCoefficients() const
Get the coefficients of the spline.
PolynomialSpline(SplineCoeff_ &&coefficients, double duration)
PolynomialSpline & operator=(PolynomialSpline &&)=default
bool didEvaluateCoeffs_
True if the coefficents were computed at least once.
static void getDDiffTimeVector(Eigen::MatrixBase< Derived > const &ddtimeVec, const double tk)
Get the second derivative of the time vector evaluated at time tk.
static bool compute(const SplineOptions &opts, SplineCoefficients &coefficients)
SplineCoefficients coefficients_
static void addDiffTimeVector(Eigen::MatrixBase< Derived > const &dtimeVec, const double tk)
Get the first derivative of the time vector evaluated at time tk and add it to the input vector...
bool computeCoefficients(SplineOptionsType_ &&options)
Compute the coefficients of the spline.
static void getDDTimeVector(Eigen::Ref< EigenTimeVectorType > ddtimeVec, const double tk)
Get the second derivative of the time vector evaluated at time tk.
PolynomialSpline(const SplineOptions &options)
static TimeVectorType tau(Core_ tk) noexcept
static void addDDiffTimeVector(Eigen::MatrixBase< Derived > const &ddtimeVec, const double tk)
Get the second derivative of the time vector evaluated at time tk and add it to the input vector...
static void getDTimeVector(Eigen::Ref< EigenTimeVectorType > dtimeVec, const double tk)
Get the first derivative of the time vector evaluated at time tk.
static void getTimeVector(Eigen::MatrixBase< Derived > const &timeVec, const double tk)
Get the time vector evaluated at time tk.
Eigen::Matrix< double, 1, coefficientCount > EigenTimeVectorType
static void getTimeVectorAtZero(Eigen::MatrixBase< Derived > const &timeVec)
Get the time vector evaluated at zero.
static void addDiffTimeVectorAtZero(Eigen::MatrixBase< Derived > const &dtimeVec)
Get the time vector evaluated at zero.
virtual ~PolynomialSpline()=default