Curves A library for curves generation.
SE3Curve.hpp
Go to the documentation of this file.
1 /*
2  * ScalarCurveConfig.hpp
3  *
4  * Created on: Mar 5, 2015
5  * Author: Paul Furgale, Renaud Dube, Péter Fankhauser
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8
9 #pragma once
10
11 #include "curves/SE3Config.hpp"
12 #include "curves/Curve.hpp"
13 #include <Eigen/Core>
14
15 namespace curves {
16
17 // Curves over SE3 inherit the interface from Curve and CurveBase and define specific
18 // methods to support physical interpretations of temporal derivatives.
19 //
20 // For the purposes of these uses, the curve is defined between Frame a and Frame b
21 // such that evaluate() returns \f$\mathbf{T}_{a,b} \f$, the transformation that takes
22 // points from Frame b to Frame a.
23 //
24 class SE3Curve : public Curve<SE3Config> {
25  public:
26  SE3Curve();
27  virtual ~SE3Curve();
28
32
35
38
41
44
48  virtual Vector6d evaluateTwistA(Time time) = 0;
49
53  virtual Vector6d evaluateTwistB(Time time) = 0;
54
56  virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time) = 0;
57
59  virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time) = 0;
60
62  virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time) = 0;
63
65  virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time) = 0;
66
70  virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time) = 0;
71
75  virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time) = 0;
76
77  // Following functions added from SlerpSE3 curve .. todo clean
78
80  virtual void setMinSamplingPeriod(Time time) = 0;
81
84  virtual void setSamplingRatio(const int ratio) = 0;
85
86  virtual void clear() = 0;
87
89  virtual void transformCurve(const ValueType T) = 0;
90
91  virtual void saveCurveTimesAndValues(const std::string& filename) const = 0;
92
93  virtual void saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const = 0;
94
95  virtual void saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const = 0;
96
97  virtual void getCurveTimes(std::vector<Time>* outTimes) const = 0;
98
99  // Fake functions to comply with the current interfaces of trajectories_optimizer
100  // todo : tidy up
101
103  virtual int correctionSize() const = 0;
104
107  virtual void foldInCorrections() = 0;
108
110  virtual void setCorrectionTimes(const std::vector<Time>& times) = 0;
111
113  virtual void removeCorrectionCoefficientAtTime(Time time) = 0;
114
116  virtual void setCorrectionCoefficientAtTime(Time time, ValueType value) = 0;
117
119  virtual void resetCorrectionCurve(const std::vector<Time>& times) = 0;
120
123  virtual void setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values) = 0;
124
126  virtual void setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values) = 0;
127
130  virtual void modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values) = 0;
131
132  virtual void getBaseCurveTimes(std::vector<Time>* outTimes) const = 0;
133
134  virtual void getBaseCurveTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const = 0;
135
136  virtual bool isEmpty() const = 0;
137
138  virtual int size() const = 0;
139
140  virtual int baseSize() const = 0;
141
142  virtual void saveCorrectionCurveTimesAndValues(const std::string& filename) const = 0;
143
145  //virtual size_t dim() const;
147  private:
148
149 };
150
151 } // namespace
Curve< SE3Config > Parent
Definition: SE3Curve.hpp:29
virtual void resetCorrectionCurve(const std::vector< Time > &times)=0
Reset the correction curve to identity values with knots at desired times.
virtual void removeCorrectionCoefficientAtTime(Time time)=0
Remove a correction coefficient at the specified time.
CurveConfig::DerivativeType DerivativeType
The curve&#39;s derivative type.
Definition: Curve.hpp:25
virtual void transformCurve(const ValueType T)=0
Perform a rigid transformation on the left side of the curve.
virtual bool isEmpty() const =0
virtual Eigen::Vector3d evaluateLinearVelocityB(Time time)=0
Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.
virtual void getBaseCurveTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const =0
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: SE3Config.hpp:16
virtual void saveCurveAtTimes(const std::string &filename, std::vector< Time > times) const =0
virtual void setCorrectionCoefficientAtTime(Time time, ValueType value)=0
Set the correction coefficient value at the specified time.
virtual void saveCurveTimesAndValues(const std::string &filename) const =0
virtual void foldInCorrections()=0
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
virtual int size() const =0
virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time)=0
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
virtual void setBaseCurvePart(const std::vector< Time > &times, const std::vector< ValueType > &values)=0
Add / replace the given coefficients without resetting the curve.
virtual void clear()=0
Clear all the curve coefficients.
virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time)=0
Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.
virtual void getBaseCurveTimes(std::vector< Time > *outTimes) const =0
double Time
Definition: Curve.hpp:13
virtual void setSamplingRatio(const int ratio)=0
Set the sampling ratio. eg. 4 will add a coefficient every 4 extend.
virtual Eigen::Vector3d evaluateAngularVelocityA(Time time)=0
Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a...
virtual void getCurveTimes(std::vector< Time > *outTimes) const =0
virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time)=0
evaluate the velocity/angular derivative of Frame b as seen from Frame a, expressed in Frame a...
virtual void modifyBaseCoefficientsValuesInBatch(const std::vector< Time > &times, const std::vector< ValueType > &values)=0
Modifies values of the base coefficient in batch, starting at times[0] and assuming that a coefficien...
virtual int baseSize() const =0
virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time)=0
Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a...
virtual void setBaseCurve(const std::vector< Time > &times, const std::vector< ValueType > &values)=0
Set the base curve to given values with knots at desired times Resets the curve beforehand.
virtual void setMinSamplingPeriod(Time time)=0
set the minimum sampling period
virtual void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const =0
virtual Vector6d evaluateTwistB(Time time)=0
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
virtual void setCorrectionTimes(const std::vector< Time > &times)=0
Add coefficients to the correction curve at given times.
virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time)=0
Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.
virtual Eigen::Vector3d evaluateLinearVelocityA(Time time)=0
Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.
virtual ~SE3Curve()
CurveConfig::ValueType ValueType
The value type of the curve.
Definition: Curve.hpp:22
Parent::DerivativeType DerivativeType
Definition: SE3Curve.hpp:31
virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time)=0
Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b...
virtual int correctionSize() const =0
Returns the number of coefficients in the correction curve.
virtual Vector6d evaluateTwistA(Time time)=0
evaluate the velocity/angular velocity of Frame b as seen from Frame a, expressed in Frame a...
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
Parent::ValueType ValueType
Definition: SE3Curve.hpp:30
virtual void saveCorrectionCurveTimesAndValues(const std::string &filename) const =0
virtual Eigen::Vector3d evaluateAngularVelocityB(Time time)=0
Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b...