Curves
A library for curves generation.
SlerpSE3Curve.hpp
Go to the documentation of this file.
1 /*
2  * SlerpSE3Curve.hpp
3  *
4  * Created on: Oct 10, 2014
5  * Author: Renaud Dube, Abel Gawel, P├ęter Fankhauser
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
9 #pragma once
10 
11 #include "SE3Curve.hpp"
13 #include "kindr/Core"
14 #include "SE3CompositionCurve.hpp"
15 #include "SamplingPolicy.hpp"
16 #include "CubicHermiteSE3Curve.hpp"
17 
18 namespace curves {
19 
23 class SlerpSE3Curve : public SE3Curve {
26  friend class SamplingPolicy;
27  public:
30  typedef ValueType Coefficient;
33 
34  SlerpSE3Curve();
35  virtual ~SlerpSE3Curve();
36 
38  virtual void print(const std::string& str = "") const;
39 
41  virtual Time getMinTime() const;
42 
44  virtual Time getMaxTime() const;
45 
46  bool isEmpty() const;
47 
48  // return number of coefficients curve is composed of
49  int size() const;
50 
54  virtual void extend(const std::vector<Time>& times,
55  const std::vector<ValueType>& values,
56  std::vector<Key>* outKeys = NULL);
57 
62  virtual void fitCurve(const std::vector<Time>& times,
63  const std::vector<ValueType>& values,
64  std::vector<Key>* outKeys = NULL);
65 
68  void setCurve(const std::vector<Time>& times,
69  const std::vector<ValueType>& values);
70 
72  virtual ValueType evaluate(Time time) const;
73 
81  virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const;
82 
83  virtual void setTimeRange(Time minTime, Time maxTime);
84 
87 
90 
93 
96 
100  virtual Vector6d evaluateTwistA(Time time);
101 
105  virtual Vector6d evaluateTwistB(Time time);
106 
108  virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time);
109 
111  virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time);
112 
114  virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time);
115 
117  virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time);
118 
122  virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time);
123 
127  virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time);
128 
129  // set minimum sampling period
130  void setMinSamplingPeriod(Time time);
131 
134  void setSamplingRatio(const int ratio);
135 
136  virtual void clear();
137 
139  void transformCurve(const ValueType T);
140 
141  void saveCurveTimesAndValues(const std::string& filename) const;
142 
143  void saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const;
144 
145  void saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const {};
146 
147  void getCurveTimes(std::vector<Time>* outTimes) const;
148 
149  // Fake functions to comply with the current interfaces of trajectories_optimizer
150  // todo : tidy up
151 
153  int correctionSize() const {return 0;};
154 
157  void foldInCorrections() {};
158 
160  void setCorrectionTimes(const std::vector<Time>& times) {};
161 
164 
166  void setCorrectionCoefficientAtTime(Time time, ValueType value) {};
167 
169  void resetCorrectionCurve(const std::vector<Time>& times) {};
170 
173  void setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
174 
176  void setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
177 
180  void modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
181 
182  void getBaseCurveTimes(std::vector<Time>* outTimes) const {};
183 
184  void getBaseCurveTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const {};
185 
186  // return number of coefficients curve is composed of
187  int baseSize() const {return size();};
188 
189  void saveCorrectionCurveTimesAndValues(const std::string& filename) const {};
190 
191  private:
194 };
195 
196 typedef kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3;
197 typedef SE3::Rotation SO3;
198 typedef kindr::AngleAxisPD AngleAxis;
199 
200 SE3 transformationPower(SE3 T, double alpha);
201 
202 SE3 composeTransformations(SE3 A, SE3 B);
203 
204 SE3 inverseTransformation(SE3 T);
205 
206 // extend policy for slerp curves
207 template<>
208 inline void SamplingPolicy::extend<SlerpSE3Curve, SE3>(const std::vector<Time>& times,
209  const std::vector<SE3>& values,
210  SlerpSE3Curve* curve,
211  std::vector<Key>* outKeys) {
212  //todo: deal with minSamplingPeriod_ when extending with multiple times
213  if (times.size() != 1) {
214  curve->manager_.insertCoefficients(times, values, outKeys);
215  } else {
216  //If the curve is empty or of size 1, simply add the new coefficient
217  if (curve->isEmpty() || curve->size() == 1) {
218  curve->manager_.insertCoefficients(times, values, outKeys);
219  } else {
220  if (minimumMeasurements_ == 1) {
221  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
222  } else {
223  ++measurementsSinceLastExtend_;
224 
225  if (measurementsSinceLastExtend_ == 1) {
226  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
227  } else {
228  SlerpSE3Curve::TimeToKeyCoefficientMap::iterator itPrev = (--curve->manager_.coefficientEnd());
229  curve->manager_.modifyCoefficient(itPrev, times[0], values[0]);
230  }
231  if (measurementsSinceLastExtend_ == minimumMeasurements_) {
232  measurementsSinceLastExtend_ = 0;
233  }
234  }
235  }
236  }
237 }
238 
239 } // namespace curves
240 
virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const
virtual Time getMinTime() const
The first valid time for the curve.
virtual void print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
virtual Vector6d evaluateTwistA(Time time)
evaluate the velocity/angular velocity of Frame b as seen from Frame a, expressed in Frame a...
virtual Eigen::Vector3d evaluateLinearVelocityA(Time time)
Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.
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...
SE3Curve::ValueType ValueType
virtual void clear()
Clear all the curve coefficients.
virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.
virtual Vector6d evaluateTwistB(Time time)
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
virtual Eigen::Vector3d evaluateAngularVelocityB(Time time)
Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b...
void setMinSamplingPeriod(Time time)
set the minimum sampling period
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: SE3Config.hpp:16
virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time)
evaluate the velocity/angular derivative of Frame b as seen from Frame a, expressed in Frame a...
void saveCurveTimesAndValues(const std::string &filename) const
virtual Eigen::Vector3d evaluateAngularVelocityA(Time time)
Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a...
void saveCurveAtTimes(const std::string &filename, std::vector< Time > times) const
SE3 inverseTransformation(SE3 T)
std::map< Time, KeyCoefficient > TimeToKeyCoefficientMap
void transformCurve(const ValueType T)
Perform a rigid transformation on the left side of the curve.
SamplingPolicy slerpPolicy_
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.
void setSamplingRatio(const int ratio)
Set the sampling ratio. eg. 4 will add a coefficient every 4 extend.
void setBaseCurvePart(const std::vector< Time > &times, const std::vector< ValueType > &values)
Add / replace the given coefficients without resetting the curve.
virtual ValueType evaluate(Time time) const
Evaluate the ambient space of the curve.
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
void getCurveTimes(std::vector< Time > *outTimes) const
virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b...
void resetCorrectionCurve(const std::vector< Time > &times)
Reset the correction curve to identity values with knots at desired times.
void getBaseCurveTimes(std::vector< Time > *outTimes) const
kindr::AngleAxisPD AngleAxis
void saveCorrectionCurveTimesAndValues(const std::string &filename) const
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.
void getBaseCurveTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const
SE3 transformationPower(SE3 T, double alpha)
void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const
double Time
Definition: Curve.hpp:13
virtual Eigen::Vector3d evaluateLinearVelocityB(Time time)
Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.
bool isEmpty() const
LocalSupport2CoefficientManager< Coefficient > manager_
virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time)
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
LocalSupport2CoefficientManager< Coefficient >::TimeToKeyCoefficientMap TimeToKeyCoefficientMap
void removeCorrectionCoefficientAtTime(Time time)
Remove a correction coefficient at the specified time.
virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a...
SE3::Rotation SO3
SE3Curve::DerivativeType DerivativeType
void setCorrectionTimes(const std::vector< Time > &times)
Add coefficients to the correction curve at given times.
void foldInCorrections()
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
void setCurve(const std::vector< Time > &times, const std::vector< ValueType > &values)
Set some coefficients of the curve The existing curve will NOT be cleared.
SE3 composeTransformations(SE3 A, SE3 B)
virtual Time getMaxTime() const
The one past the last valid time for the curve.
Parent::DerivativeType DerivativeType
Definition: SE3Curve.hpp:31
TimeToKeyCoefficientMap::const_iterator CoefficientIter
virtual void setTimeRange(Time minTime, Time maxTime)
int correctionSize() const
Returns the number of coefficients in the correction curve.
void setCorrectionCoefficientAtTime(Time time, ValueType value)
Set the correction coefficient value at the specified time.
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
Parent::ValueType ValueType
Definition: SE3Curve.hpp:30
virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.
kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3