Curves
A library for curves generation.
CubicHermiteE3Curve.hpp
Go to the documentation of this file.
1 /*
2  * CubicHermiteE3Curve.hpp
3  *
4  * Created on: Aug, 2016
5  * Author: Christian Gehring
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
9 #pragma once
10 
11 #include <kindr/Core>
12 
14 #include "curves/SE3Curve.hpp"
15 
16 struct HermiteE3Knot {
20 
21  public:
22  HermiteE3Knot(): HermiteE3Knot(Position::Zero(), Velocity::Zero()) {}
23  HermiteE3Knot(const Position& position,
24  const Velocity& velocity) : position_(position), velocity_(velocity){}
25  virtual ~HermiteE3Knot(){}
26 
27  Position getPosition() const {
28  return position_;
29  }
30 
31  Velocity getVelocity() const {
32  return velocity_;
33  }
34 
35  void setPosition(const Position& position) {
36  position_ = position;
37  }
38 
39  void setVelocity(const Velocity& velocity) {
40  velocity_ = velocity;
41  }
42 
43  private:
44  Position position_;
45  Velocity velocity_;
46 };
47 
48 namespace curves {
49 
52 //
55 // Let t_W_A, t_W_B denote the control point values (=translations) and W_v_W_A, W_v_W_B
56 // the control derivatives (=velocities in R³) at the interval's boundaries.
57 // Then (b == beta):
58 // p0 = t_W_A, p1 = t_W_B, p2 = W_v_W_A, p3 = W_v_W_B
59 // b0 = 2t³-3t²+1, b_1 = -2t³+3t², b_2 = t³-2t²+t, b_3 = t³-t²
60 // Spline equation:
61 // p(t) = p0 * b0 + p1 * b1 + p2 * b2 + p3 + b3
62 
64  public:
70  public:
72  virtual ~CubicHermiteE3Curve();
73 
75  virtual void print(const std::string& str = "") const;
76 
77  virtual bool writeEvalToFile(const std::string& filename, int nSamples) const;
78 
80  virtual Time getMinTime() const;
81 
83  virtual Time getMaxTime() const;
84 
85  bool isEmpty() const;
86 
87  // return number of coefficients curve is composed of
88  int size() const;
89 
91  DerivativeType calculateSlope(const Time& timeA,
92  const Time& timeB,
93  const ValueType& coeffA,
94  const ValueType& coeffB) const;
95 
99  virtual void extend(const std::vector<Time>& times,
100  const std::vector<ValueType>& values,
101  std::vector<Key>* outKeys = NULL);
102 
107  virtual void fitCurve(const std::vector<Time>& times,
108  const std::vector<ValueType>& values,
109  std::vector<Key>* outKeys = NULL);
110 
111  virtual void fitCurveWithDerivatives(const std::vector<Time>& times,
112  const std::vector<ValueType>& values,
113  const DerivativeType& initialDerivative = DerivativeType(),
114  const DerivativeType& finalDerivative = DerivativeType(),
115  std::vector<Key>* outKeys = NULL);
116 
117 
118  virtual void fitPeriodicCurve(const std::vector<Time>& times,
119  const std::vector<ValueType>& values,
120  std::vector<Key>* outKeys = NULL);
121 
123  virtual bool evaluate(ValueType& value, Time time) const;
124 
126  virtual bool evaluateDerivative(DerivativeType& derivative, Time time,
127  unsigned int derivativeOrder) const;
128 
129  bool evaluateLinearAcceleration(Acceleration& linearAcceleration, Time time) const;
130 
131  // clear the curve
132  virtual void clear();
133 
134  private:
136 };
137 
138 } // namespace curves
SE3Curve::DerivativeType DerivativeType
Eigen::Vector3d Velocity
void setPosition(const Position &position)
HermiteE3Knot(const Position &position, const Velocity &velocity)
double Time
Definition: Curve.hpp:13
HermiteE3Knot::Acceleration Acceleration
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
HermiteE3Knot::Velocity DerivativeType
Eigen::Vector3d Acceleration
TimeToKeyCoefficientMap::const_iterator CoefficientIter
Velocity getVelocity() const
Position getPosition() const
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
Eigen::Vector3d Position
LocalSupport2CoefficientManager< Coefficient > manager_
void setVelocity(const Velocity &velocity)
HermiteE3Knot::Position ValueType