Curves
A library for curves generation.
PolynomialSplineContainer.hpp
Go to the documentation of this file.
1 /*
2  * PolynomialSplineContainer.hpp
3  *
4  * Created on: Dec 8, 2014
5  * Author: C. Dario Bellicoso, Peter Fankhauser
6  */
7 
8 #pragma once
9 
10 // Eigen
12 #include <Eigen/Core>
13 #include <Eigen/Dense>
14 
15 // std
16 #include <iostream>
17 #include <memory>
18 #include <limits>
19 
20 // boost
21 #include <boost/math/special_functions/pow.hpp>
22 
23 namespace curves {
24 
25 template <int splineOrder_>
27  public:
29  using SplineList = std::vector<SplineType>;
30 
31  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 
34  virtual ~PolynomialSplineContainer() = default;
35 
37  SplineType* getSpline(int splineIndex);
38 
40  const SplineList& getSplines() const;
41 
43  bool advance(double dt);
44 
46  void setContainerTime(double t);
47 
49  template<typename SplineType_>
50  bool addSpline(SplineType_&& spline) {
51  containerDuration_ += spline.getSplineDuration();
52  splines_.emplace_back(std::forward<SplineType_>(spline));
53  return true;
54  }
55 
57  bool reserveSplines(const unsigned int numSplines);
58 
60  bool reset();
61 
63  bool resetTime();
64 
66  double getContainerDuration() const;
67 
69  double getContainerTime() const;
70 
72  bool isEmpty() const;
73 
75  double getPosition() const;
76 
78  double getVelocity() const;
79 
81  double getAcceleration() const;
82 
86  int getActiveSplineIndexAtTime(double t, double& timeOffset) const;
87 
89  int getActiveSplineIndex() const;
90 
92  double getPositionAtTime(double t) const;
93 
95  double getVelocityAtTime(double t) const;
96 
98  double getAccelerationAtTime(double t) const;
99 
101  double getEndPosition() const;
102 
104  double getEndVelocity() const;
105 
107  double getEndAcceleration() const;
108 
113  bool setData(
114  const std::vector<double>& knotDurations,
115  const std::vector<double>& knotPositions,
116  double initialVelocity, double initialAcceleration,
117  double finalVelocity, double finalAcceleration);
118 
123  bool setData(
124  const std::vector<double>& knotDurations,
125  const std::vector<double>& knotPositions,
126  double initialVelocity, double finalVelocity);
127 
132  virtual bool setData(
133  const std::vector<double>& knotDurations,
134  const std::vector<double>& knotPositions);
135 
136  static constexpr double undefinedValue = std::numeric_limits<double>::quiet_NaN();
137 
138  protected:
148  inline int getCoeffIndex(const int splineIdx, const int aIdx) const {
149  return splineIdx*(splineOrder_+1) + aIdx;
150  }
151 
152  inline int getSplineColumnIndex(const int splineIdx) const {
153  return getCoeffIndex(splineIdx, 0);
154  }
155 
157  const Eigen::VectorXd& initialConditions,
158  unsigned int& constraintIdx);
159 
160  void addFinalConditions(
161  const Eigen::VectorXd& finalConditions,
162  unsigned int& constraintIdx,
163  const double lastSplineDuration,
164  const unsigned int lastSplineId);
165 
167  const std::vector<double>& splineDurations,
168  const std::vector<double>& knotPositions,
169  unsigned int& constraintIdx,
170  const unsigned int num_junctions);
171 
173  const Eigen::VectorXd& coeffs,
174  const std::vector<double>& splineDurations,
175  const unsigned int num_splines);
176 
179 
181  double timeOffset_;
182 
185 
188 
191 
194 
197 };
198 
199 } /* namespace */
200 
201 #include <curves/PolynomialSplineContainer.tpp>
int getActiveSplineIndexAtTime(double t, double &timeOffset) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PolynomialSplineContainer()
void addInitialConditions(const Eigen::VectorXd &initialConditions, unsigned int &constraintIdx)
double getPositionAtTime(double t) const
Get position at time t[seconds];.
int activeSplineIdx_
Spline index currently active.
double getVelocity() const
Get the velocity evaluated at the internal time.
double containerTime_
Current time of spline conjunction.
double getContainerDuration() const
Get total trajectory duration.
double containerDuration_
Total duration of spline conjunction.
bool addSpline(SplineType_ &&spline)
Add a spline to the container.
double getAccelerationAtTime(double t) const
Get acceleration at time t[seconds];.
int getCoeffIndex(const int splineIdx, const int aIdx) const
bool advance(double dt)
Update the spline internal time by dt [seconds].
SplineList splines_
Conjunction of smoothly interconnected splines.
bool reserveSplines(const unsigned int numSplines)
Reserve memory for the spline container.
Eigen::MatrixXd equalityConstraintJacobian_
Equality matrix of quadratic program (A in Ax=b).
SplineType * getSpline(int splineIndex)
Get a pointer to the spline with a given index.
double getEndVelocity() const
Get velocity at the end of the spline.
int getActiveSplineIndex() const
Return spline index at current time validity.
bool resetTime()
Set container.
bool reset()
Clear spline container.
virtual ~PolynomialSplineContainer()=default
Eigen::VectorXd equalityConstraintTargetValues_
Equality target values of quatratic program (b in Ax=b).
void addFinalConditions(const Eigen::VectorXd &finalConditions, unsigned int &constraintIdx, const double lastSplineDuration, const unsigned int lastSplineId)
int getSplineColumnIndex(const int splineIdx) const
const SplineList & getSplines() const
Get a reference to the spline with a given index.
double getPosition() const
Get the position evaluated at the internal time.
bool extractSplineCoefficients(const Eigen::VectorXd &coeffs, const std::vector< double > &splineDurations, const unsigned int num_splines)
double getAcceleration() const
Get the acceleration evaluated at the internal time.
bool setData(const std::vector< double > &knotDurations, const std::vector< double > &knotPositions, double initialVelocity, double initialAcceleration, double finalVelocity, double finalAcceleration)
void addJunctionsConditions(const std::vector< double > &splineDurations, const std::vector< double > &knotPositions, unsigned int &constraintIdx, const unsigned int num_junctions)
double getVelocityAtTime(double t) const
Get velocity at time t[seconds];.
void setContainerTime(double t)
Jump to a specific point in time domain.
bool isEmpty() const
True if splines are empty.
double getContainerTime() const
Get currently active time point.
double getEndPosition() const
Get position at the end of the spline.
double getEndAcceleration() const
Get acceleration at the end of the spline.