Curves
A library for curves generation.
PolynomialSplineVectorSpaceCurve.hpp
Go to the documentation of this file.
1 /*
2  * PolynomialSplineVectorSpaceCurve.hpp
3  *
4  * Created on: Mar 6, 2015
5  * Author: P├ęter Fankhauser
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
9 #pragma once
10 
11 #include <string>
12 #include <vector>
13 #include <Eigen/Core>
14 #include <glog/logging.h>
15 
16 #include "curves/Curve.hpp"
20 
21 namespace curves {
22 
23 template<typename SplineType, int N>
25 {
26  public:
28  typedef typename Parent::ValueType ValueType;
30 
32  : VectorSpaceCurve<N>(),
33  minTime_(0)
34  {
35  containers_.resize(N);
36  }
37 
39  {
40  }
41 
42  virtual void print(const std::string& str = "") const
43  {
44  }
45 
46  virtual Time getMinTime() const
47  {
48  return minTime_;
49  }
50 
51  virtual Time getMaxTime() const
52  {
53  return containers_.at(0).getContainerDuration();
54  }
55 
56  virtual bool evaluate(ValueType& value, Time time) const
57  {
58  for (size_t i = 0; i < N; ++i) {
59  value(i) = containers_.at(i).getPositionAtTime(time);
60  }
61  return true;
62  }
63 
64  virtual bool evaluateDerivative(DerivativeType& value, Time time, unsigned derivativeOrder) const
65  {
66 
67  for (size_t i = 0; i < N; ++i) {
68  if (derivativeOrder == 1) {
69  value(i) = containers_.at(i).getVelocityAtTime(time);
70  }
71  else if (derivativeOrder == 2) {
72  value(i) = containers_.at(i).getAccelerationAtTime(time);
73  }
74  else {
75  return false;
76  }
77  }
78  return true;
79  }
80 
81  virtual void extend(const std::vector<Time>& times, const std::vector<ValueType>& values,
82  std::vector<Key>* outKeys)
83  {
84  throw std::runtime_error("PolynomialSplineVectorSpaceCurve::extend is not yet implemented!");
85  }
86 
87  virtual void fitCurve(const std::vector<Time>& times, const std::vector<ValueType>& values,
88  std::vector<Key>* outKeys = NULL)
89  {
90  minTime_ = times.front();
91  for (size_t i = 0; i < N; ++i) {
92  std::vector<double> scalarValues;
93  scalarValues.reserve(times.size());
94  for (size_t t = 0; t < times.size(); ++t) scalarValues.push_back(values.at(t)(i));
95  containers_.at(i).setData(times, scalarValues, 0.0, 0.0, 0.0, 0.0);
96  }
97  }
98 
99  virtual void fitCurve(const std::vector<Time>& times,
100  const std::vector<ValueType>& values,
101  const DerivativeType& initialVelocity,
102  const DerivativeType& initialAcceleration,
103  const DerivativeType& finalVelocity,
104  const DerivativeType& finalAcceleration)
105  {
106  minTime_ = times.front();
107  for (size_t i = 0; i < N; ++i) {
108  std::vector<double> scalarValues;
109  scalarValues.reserve(times.size());
110  for (size_t t = 0; t < times.size(); ++t) scalarValues.push_back(values.at(t)(i));
111  containers_.at(i).setData(times, scalarValues, initialVelocity(i), initialAcceleration(i),
112  finalVelocity(i), finalAcceleration(i));
113  }
114  }
115 
116  virtual void fitCurve(const std::vector<Time>& times, const std::vector<ValueType>& values,
117  const std::vector<DerivativeType>& firstDerivatives,
118  const std::vector<DerivativeType>& secondDerivatives,
119  std::vector<Key>* outKeys = NULL)
120  {
121  minTime_ = times.front();
122  for (size_t i = 0; i < N; ++i) {
123  std::vector<double> scalarValues, scalarFirstDerivates, scalarSecondDerivates;
124  scalarValues.reserve(times.size());
125  scalarFirstDerivates.reserve(times.size());
126  scalarSecondDerivates.reserve(times.size());
127  for (size_t t = 0; t < times.size(); ++t) {
128  scalarValues.push_back(values.at(t)(i));
129  scalarFirstDerivates.push_back(firstDerivatives.at(t)(i));
130  scalarSecondDerivates.push_back(secondDerivatives.at(t)(i));
131  }
132  // TODO Copy all derivates, right now only first and last are supported.
133  containers_.at(i).setData(times, scalarValues,
134  *(scalarFirstDerivates.begin()), *(scalarSecondDerivates.begin()),
135  *(scalarFirstDerivates.end() - 1), *(scalarSecondDerivates.end() - 1));
136  }
137  }
138 
139 
140  virtual void fitCurve(const std::vector<SplineOptions>& values,
141  std::vector<Key>* outKeys = NULL)
142  {
143  // TODO
144  throw std::runtime_error("PolynomialSplineVectorSpaceCurve::fitCurve is not yet implemented!");
145  }
146 
147  virtual void clear()
148  {
149  for (size_t i = 0; i < N; ++i) {
150  containers_.at(i).reset();
151  }
152  }
153 
154  virtual void transformCurve(const ValueType T)
155  {
156  CHECK(false) << "Not implemented";
157  }
158 
159  private:
160  std::vector<PolynomialSplineContainerQuintic> containers_;
162 };
163 
165 
166 } // namespace
167 
PolynomialSplineVectorSpaceCurve< PolynomialSplineQuintic, 3 > PolynomialSplineQuinticVector3Curve
virtual bool evaluateDerivative(DerivativeType &value, Time time, unsigned derivativeOrder) const
virtual void print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
virtual void fitCurve(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
virtual void fitCurve(const std::vector< Time > &times, const std::vector< ValueType > &values, const std::vector< DerivativeType > &firstDerivatives, const std::vector< DerivativeType > &secondDerivatives, std::vector< Key > *outKeys=NULL)
virtual Time getMinTime() const
The dimension of the underlying manifold.
std::vector< PolynomialSplineContainerQuintic > containers_
double Time
Definition: Curve.hpp:13
virtual void fitCurve(const std::vector< Time > &times, const std::vector< ValueType > &values, const DerivativeType &initialVelocity, const DerivativeType &initialAcceleration, const DerivativeType &finalVelocity, const DerivativeType &finalAcceleration)
virtual bool evaluate(ValueType &value, Time time) const
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys)
Parent::DerivativeType DerivativeType
virtual Time getMaxTime() const
The one past the last valid time for the curve.
virtual void clear()
Clear all the curve coefficients.
virtual void fitCurve(const std::vector< SplineOptions > &values, std::vector< Key > *outKeys=NULL)
Parent::ValueType ValueType