Curves
A library for curves generation.
CubicHermiteSE3Curve.hpp
Go to the documentation of this file.
1 /*
2  * CubicHermiteSE3Curve.hpp
3  *
4  * Created on: Feb 10, 2015
5  * Author: Abel Gawel, Renaud Dube, Péter Fankhauser, Christian Gehring
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
9 #pragma once
10 
11 #include <kindr/Core>
12 
16 #include "curves/SE3Curve.hpp"
17 
18 // wrapper class for Hermite-style coefficients (made of QuatTransformation and Vector6)
19 namespace kindr {
20 
21 template <typename Scalar>
23  typedef kindr::HomTransformQuatD Transform;
24  typedef kindr::TwistGlobalD Twist;
25 
26  public:
28  HermiteTransformation(const Transform& transform, const Twist& derivatives);
29  virtual ~HermiteTransformation();
30 
31  Transform getTransformation() const {
32  return transformation_;
33  }
34 
37  }
38 
39  void setTransformation(const Transform& transformation) {
40  transformation_ = transformation;
41  }
42 
43  void setTransformationDerivative(const Twist& transformationDerivative) {
44  transformationDerivative_ = transformationDerivative;
45  }
46 
47  private:
48  Transform transformation_;
50 };
51 
52 template <typename Scalar>
54 
55 template <typename Scalar>
57  const Twist& derivatives) :
58  transformation_(transform),
59  transformationDerivative_(derivatives) {};
60 
61 template <typename Scalar>
63 
64 }
65 
66 namespace curves {
67 
73 
76 //
79 // Let t_W_A, t_W_B denote the control point values (=translations) and W_v_W_A, W_v_W_B
80 // the control derivatives (=velocities in R³) at the interval's boundaries.
81 // Then (b == beta):
82 // p0 = t_W_A, p1 = t_W_B, p2 = W_v_W_A, p3 = W_v_W_B
83 // b0 = 2t³-3t²+1, b_1 = -2t³+3t², b_2 = t³-2t²+t, b_3 = t³-t²
84 // Spline equation:
85 // p(t) = p0 * b0 + p1 * b1 + p2 * b2 + p3 + b3
86 //
89 // Let quat_W_A, quat_W_B denote the control point values (=unit quaternions) and va, vb
90 // the control derivatives (=angular speeds in R³) at the interval's boundaries.
91 // Then (w == omega, b == beta):
92 // w_1 = va / 3
93 // w_2 = log[ exp(w_1)^{-1} * quat_W_A^{-1} * quat_W_B * exp(w_3)^{-1} ]
94 // w_3 = vb / 3
95 // b_1 = t³-3t²+3t, b_2 = -2t³+3t², b_3 = t³
96 // Spline equation:
97 // q(t) = p_1 * exp(w_1*b_1) * exp(w_2*b_2) * exp(w_3*b_3)
98 
100 
101  friend class SamplingPolicy;
102  public:
104 
106  virtual ~CubicHermiteSE3Curve();
107 
109  virtual void print(const std::string& str = "") const;
110 
111  virtual bool writeEvalToFile(const std::string& filename, int nSamples) const;
112 
114  virtual Time getMinTime() const;
115 
117  virtual Time getMaxTime() const;
118 
119  bool isEmpty() const;
120 
121  // return number of coefficients curve is composed of
122  int size() const;
123 
125  DerivativeType calculateSlope(const Time& timeA,
126  const Time& timeB,
127  const ValueType& coeffA,
128  const ValueType& coeffB) const;
129 
133  virtual void extend(const std::vector<Time>& times,
134  const std::vector<ValueType>& values,
135  std::vector<Key>* outKeys = NULL);
136 
141  virtual void fitCurve(const std::vector<Time>& times,
142  const std::vector<ValueType>& values,
143  std::vector<Key>* outKeys = NULL);
144 
145  virtual void fitCurveWithDerivatives(const std::vector<Time>& times,
146  const std::vector<ValueType>& values,
147  const DerivativeType& initialDerivative = DerivativeType(),
148  const DerivativeType& finalDerivative = DerivativeType(),
149  std::vector<Key>* outKeys = NULL);
150 
151  virtual void fitPeriodicCurve(const std::vector<Time>& times,
152  const std::vector<ValueType>& values,
153  std::vector<Key>* outKeys = NULL);
154 
155 
157  virtual bool evaluate(ValueType& value, Time time) const;
158 
160  virtual bool evaluateDerivative(DerivativeType& derivative, Time time, unsigned int derivativeOrder) const;
161 
162  virtual void setTimeRange(Time minTime, Time maxTime);
163 
164  bool evaluateLinearAcceleration(kindr::Acceleration3D& linearAcceleration, Time time);
165 
167  virtual Eigen::Vector3d evaluateAngularVelocityA(Time time);
168 
170  virtual Eigen::Vector3d evaluateAngularVelocityB(Time time);
171 
173  virtual Eigen::Vector3d evaluateLinearVelocityA(Time time);
174 
176  virtual Eigen::Vector3d evaluateLinearVelocityB(Time time);
177 
181  virtual Vector6d evaluateTwistA(Time time);
182 
186  virtual Vector6d evaluateTwistB(Time time);
187 
189  virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time);
190 
192  virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time);
193 
195  virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time);
196 
198  virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time);
199 
203  virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time);
204 
208  virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time);
209 
210  // set the minimum sampling period
211  void setMinSamplingPeriod(Time time);
212 
215  void setSamplingRatio(const int ratio);
216 
217  // clear the curve
218  virtual void clear();
219 
221  void transformCurve(const ValueType T);
222 
223  void saveCurveTimesAndValues(const std::string& filename) const;
224 
225  void saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const;
226 
227  void saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const {};
228 
229  void getCurveTimes(std::vector<Time>* outTimes) const;
230 
232  int correctionSize() const {return 0;};
233 
236  void foldInCorrections() {};
237 
239  void setCorrectionTimes(const std::vector<Time>& times) {};
240 
243 
245  void setCorrectionCoefficientAtTime(Time time, ValueType value) {};
246 
248  void resetCorrectionCurve(const std::vector<Time>& times) {};
249 
252  void setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
253 
255  void setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
256 
259  void modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
260 
261  void getBaseCurveTimes(std::vector<Time>* outTimes) const {};
262 
263  void getBaseCurveTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const {};
264 
265  // return number of coefficients curve is composed of
266  int baseSize() const {return size();};
267 
268  void saveCorrectionCurveTimesAndValues(const std::string& filename) const {};
269  private:
272 };
273 
274 typedef kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3;
275 typedef SE3::Rotation SO3;
276 typedef kindr::AngleAxisPD AngleAxis;
277 typedef kindr::RotationQuaternionPD RotationQuaternion;
278 typedef Eigen::Matrix<double, 6, 1> Vector6;
279 typedef kindr::TwistGlobalD Twist;
280 
281 SE3 transformationPower(SE3 T, double alpha);
282 
283 SE3 composeTransformations(SE3 A, SE3 B);
284 
285 SE3 inverseTransformation(SE3 T);
286 
287 SE3 invertAndComposeImplementation(SE3 A, SE3 B);
288 
289 // implements the special (extend) policies for Cubic Hermite curves
290 template <>
291 inline Key SamplingPolicy::defaultExtend<CubicHermiteSE3Curve, ValueType>(const Time& time,
292  const ValueType& value,
293  CubicHermiteSE3Curve* curve) {
294 
295  DerivativeType derivative;
296  //cases:
297 
298  // manager is empty
299  if (curve->manager_.size() == 0) {
300  derivative.setZero();
301  // 1 value in manager (2 in total)
302  } else if (curve->manager_.size() == 1) {
303  // get latest coefficient from manager
304  CoefficientIter last = curve->manager_.coefficientBegin();
305  // calculate slope
306  // note: unit of derivative is m/s for first 3 and rad/s for last 3 entries
307  derivative = curve->calculateSlope(last->first,
308  time,
309  last->second.coefficient.getTransformation(),
310  value);
311  // update previous coefficient
312  Coefficient updated(last->second.coefficient.getTransformation(), derivative);
313  curve->manager_.updateCoefficientByKey(last->second.key, updated);
314  // more than 1 values in manager
315  } else if (curve->manager_.size() > 1) {
316  // get latest 2 coefficients from manager
317  CoefficientIter rVal0, rVal1;
318  CoefficientIter last = --curve->manager_.coefficientEnd();
319  curve->manager_.getCoefficientsAt(last->first,
320  &rVal0,
321  &rVal1);
322 
323  // update derivative of previous coefficient
324  DerivativeType derivative0;
325  derivative0 = curve->calculateSlope(rVal0->first, time,
326  rVal0->second.coefficient.getTransformation(), value);
327  Coefficient updated(rVal1->second.coefficient.getTransformation(), derivative0);
328  curve->manager_.updateCoefficientByKey(rVal1->second.key, updated);
329 
330  // calculate slope
331  derivative = curve->calculateSlope(rVal1->first, time,
332  rVal1->second.coefficient.getTransformation(), value);
333  }
334  measurementsSinceLastExtend_ = 0;
335  lastExtend_ = time;
336  return curve->manager_.insertCoefficient(time, Coefficient(value, derivative));
337 }
338 
339 template <>
340 inline Key SamplingPolicy::interpolationExtend<CubicHermiteSE3Curve, ValueType>(const Time& time,
341  const ValueType& value,
342  CubicHermiteSE3Curve* curve) {
343  DerivativeType derivative;
344  if (measurementsSinceLastExtend_ == 0) {
345  // extend curve with new interpolation coefficient if necessary
346  CoefficientIter rValInterp = --curve->manager_.coefficientEnd();
347  CoefficientIter last = --curve->manager_.coefficientEnd();
348  derivative = last->second.coefficient.getTransformationDerivative();
349  } else {
350  // assumes the interpolation coefficient is already set (at end of curve)
351  // assumes same velocities as last Coefficient
352  CoefficientIter rVal0, rValInterp;
353  CoefficientIter last = --curve->manager_.coefficientEnd();
354  curve->manager_.getCoefficientsAt(last->first,
355  &rVal0,
356  &rValInterp);
357 
358  derivative = curve->calculateSlope(rVal0->first, time,
359  rVal0->second.coefficient.getTransformation(), value);
360 
361  // update the interpolated coefficient with given values and velocities from last coefficeint
362  curve->manager_.removeCoefficientAtTime(rValInterp->first);
363  }
364 
365  ++measurementsSinceLastExtend_;
366  return curve->manager_.insertCoefficient(time, Coefficient(value, derivative));
367 }
368 
369 template<>
370 inline void SamplingPolicy::extend<CubicHermiteSE3Curve, ValueType>(const std::vector<Time>& times,
371  const std::vector<ValueType>& values,
372  CubicHermiteSE3Curve* curve,
373  std::vector<Key>* outKeys) {
374  for (int i = 0; i < times.size(); ++i) {
375  // ensure time strictly increases
376  CHECK((times[i] > curve->manager_.getMaxTime()) || curve->manager_.size() == 0) << "curve can only be extended into the future. Requested = "
377  << times[i] << " < curve max time = " << curve->manager_.getMaxTime();
378  if (curve->manager_.size() == 0) {
379  defaultExtend(times[i], values[i], curve);
380  } else if((measurementsSinceLastExtend_ >= minimumMeasurements_ &&
381  lastExtend_ + minSamplingPeriod_ < times[i])) {
382  // delete interpolated coefficient
383  CoefficientIter last = --curve->manager_.coefficientEnd();
384  curve->manager_.removeCoefficientAtTime(last->first);
385  // todo write outkeys
386  defaultExtend(times[i], values[i], curve);
387  } else {
388  interpolationExtend(times[i], values[i], curve);
389  }
390  }
391 }
392 
393 } // namespace curves
394 
395 namespace kindr {
396 
400 
401 }
void setCorrectionTimes(const std::vector< Time > &times)
Add coefficients to the correction curve at given times.
void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: SE3Config.hpp:16
void setCorrectionCoefficientAtTime(Time time, ValueType value)
Set the correction coefficient value at the specified time.
void saveCorrectionCurveTimesAndValues(const std::string &filename) const
SE3 inverseTransformation(SE3 T)
std::map< Time, KeyCoefficient > TimeToKeyCoefficientMap
kindr::RotationQuaternionPD RotationQuaternion
kindr::HomTransformQuatD Transform
void foldInCorrections()
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
void setTransformationDerivative(const Twist &transformationDerivative)
void resetCorrectionCurve(const std::vector< Time > &times)
Reset the correction curve to identity values with knots at desired times.
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
LocalSupport2CoefficientManager< Coefficient >::TimeToKeyCoefficientMap TimeToKeyCoefficientMap
void setBaseCurvePart(const std::vector< Time > &times, const std::vector< ValueType > &values)
Add / replace the given coefficients without resetting the curve.
kindr::AngleAxisPD AngleAxis
SE3 transformationPower(SE3 T, double alpha)
SE3 invertAndComposeImplementation(SE3 A, SE3 B)
double Time
Definition: Curve.hpp:13
void removeCorrectionCoefficientAtTime(Time time)
Remove a correction coefficient at the specified time.
Eigen::Matrix< double, 6, 1 > Vector6
curves::SE3Curve::DerivativeType DerivativeType
void setTransformation(const Transform &transformation)
SE3::Rotation SO3
void modifyBaseCoefficientsValuesInBatch(const std::vector< Time > &times, const std::vector< ValueType > &values)
Modifies values of the base coefficient in batch, starting at times[0] and assuming that a coefficien...
void setBaseCurve(const std::vector< Time > &times, const std::vector< ValueType > &values)
Set the base curve to given values with knots at desired times Resets the curve beforehand.
kindr::HermiteTransformation< double > Coefficient
SE3 composeTransformations(SE3 A, SE3 B)
void getBaseCurveTimes(std::vector< Time > *outTimes) const
Parent::DerivativeType DerivativeType
Definition: SE3Curve.hpp:31
TimeToKeyCoefficientMap::const_iterator CoefficientIter
void getBaseCurveTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const
size_t Key
Definition: Curve.hpp:14
kindr::HermiteTransformation< double > Coefficient
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
Parent::ValueType ValueType
Definition: SE3Curve.hpp:30
int correctionSize() const
Returns the number of coefficients in the correction curve.
kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3
curves::SE3Curve::ValueType ValueType