Curves
A library for curves generation.
PolynomialSplineScalarCurve.hpp
Go to the documentation of this file.
1 /*
2  * PolynomialSplineScalarCurve.hpp
3  *
4  * Created on: Mar 5, 2015
5  * Author: P├ęter Fankhauser, Dario Bellicoso
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
9 #pragma once
10 
11 
12 // stl
13 #include <string>
14 #include <vector>
15 
16 // glog
17 #include <glog/logging.h>
18 
19 // curves
20 #include "curves/Curve.hpp"
24 
25 namespace curves {
26 
27 template<typename SplineContainerType_>
28 class PolynomialSplineScalarCurve : public Curve<ScalarCurveConfig>
29 {
30  public:
32  typedef typename Parent::ValueType ValueType;
34 
35  using SplineContainerType = SplineContainerType_;
36 
38  : Parent(),
39  container_(),
40  minTime_(0.0)
41  {
42  }
43 
45 
46  }
47 
48  virtual void print(const std::string& str = "") const
49  {
50  const double minTime = getMinTime();
51  const double maxTime = getMaxTime();
52  double timeAtEval = minTime;
53  int nPoints = 15;
54  double timeDiff = (maxTime-minTime)/(nPoints-1);
55 
56  for (int i=0;i<nPoints;i++) {
57  double firstDerivative;
58  double secondDerivative;
59  double value;
60  evaluate(value, timeAtEval);
61  evaluateDerivative(firstDerivative, timeAtEval, 1);
62  evaluateDerivative(secondDerivative, timeAtEval, 2);
63  printf("t: %lf, x: %lf dx: %lf dxx: %lf\n",
64  timeAtEval,
65  value,
66  firstDerivative,
67  secondDerivative);
68  timeAtEval += timeDiff;
69  }
70  }
71 
72  virtual Time getMinTime() const
73  {
74  return minTime_;
75  }
76 
77  virtual Time getMaxTime() const
78  {
79  return container_.getContainerDuration() + minTime_;
80  }
81 
82  virtual bool evaluate(ValueType& value, Time time) const
83  {
84  time -= minTime_;
85  value = container_.getPositionAtTime(time);
86  return true;
87  }
88 
89  virtual bool evaluateDerivative(DerivativeType& value, Time time, unsigned derivativeOrder) const
90  {
91  time -= minTime_;
92  switch (derivativeOrder) {
93  case(1): {
94  value = container_.getVelocityAtTime(time);
95  } break;
96 
97  case(2): {
98  value = container_.getAccelerationAtTime(time);
99  } break;
100 
101  default:
102  return false;
103  }
104 
105  return true;
106  }
107 
108  virtual void extend(const std::vector<Time>& times, const std::vector<ValueType>& values,
109  std::vector<Key>* outKeys)
110  {
111  throw std::runtime_error("extend is not yet implemented!");
112  }
113 
114  virtual void fitCurve(const std::vector<Time>& times, const std::vector<ValueType>& values,
115  std::vector<Key>* outKeys = NULL)
116  {
117  container_.setData(times, values, 0.0, 0.0, 0.0, 0.0);
118  minTime_ = times.front();
119  }
120 
121  virtual void fitCurve(const std::vector<Time>& times, const std::vector<ValueType>& values,
122  double initialVelocity, double initialAcceleration,
123  double finalVelocity, double finalAcceleration,
124  std::vector<Key>* outKeys = NULL)
125  {
126  container_.setData(times, values, initialVelocity, initialAcceleration, finalVelocity,
127  finalAcceleration);
128  minTime_ = times.front();
129  }
130 
131  virtual void fitCurve(const std::vector<SplineOptions>& optionList,
132  std::vector<Key>* outKeys = NULL)
133  {
134  container_.reserveSplines(optionList.size());
135  for (const auto& options : optionList) {
136  container_.addSpline(PolynomialSplineQuintic(options));
137  }
138  minTime_ = 0.0;
139  }
140 
141  virtual void clear()
142  {
143  container_.reset();
144  minTime_ = 0.0;
145  }
146 
147  virtual void transformCurve(const ValueType T)
148  {
149  CHECK(false) << "Not implemented";
150  }
151 
152  protected:
155 };
156 
158 
159 } /* namespace curves */
160 
virtual void fitCurve(const std::vector< Time > &times, const std::vector< ValueType > &values, double initialVelocity, double initialAcceleration, double finalVelocity, double finalAcceleration, std::vector< Key > *outKeys=NULL)
ScalarCurveConfig::DerivativeType DerivativeType
The curve&#39;s derivative type.
Definition: Curve.hpp:25
virtual bool evaluateDerivative(DerivativeType &value, Time time, unsigned derivativeOrder) const
PolynomialSpline< 5 > PolynomialSplineQuintic
virtual Time getMinTime() const
The dimension of the underlying manifold.
virtual void print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys)
virtual void clear()
Clear all the curve coefficients.
double Time
Definition: Curve.hpp:13
virtual void transformCurve(const ValueType T)
Perform a rigid transformation on the left side of the curve.
virtual void fitCurve(const std::vector< SplineOptions > &optionList, std::vector< Key > *outKeys=NULL)
ScalarCurveConfig::ValueType ValueType
The value type of the curve.
Definition: Curve.hpp:22
virtual Time getMaxTime() const
The one past the last valid time for the curve.
virtual void fitCurve(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
Fit a new curve to these data points.
virtual bool evaluate(ValueType &value, Time time) const
Evaluate the ambient space of the curve.