|
|
The Trajectory class represents n-dimensional sampled trajectories. More...
#include <RobotAPI/libraries/core/Trajectory.h>
Inheritance diagram for Trajectory:Classes | |
| struct | TagOrdered |
| struct | TagTimestamp |
| struct | TrajData |
Public Types | |
| using | ordered_view |
| using | timestamp_view |
| typedef boost::multi_index::multi_index_container< TrajData, boost::multi_index::indexed_by< boost::multi_index::hashed_unique< boost::multi_index::tag< TagTimestamp >, boost::multi_index::member< TrajData, double, &TrajData::timestamp > >, boost::multi_index::ordered_unique< boost::multi_index::tag< TagOrdered >, boost::multi_index::member< TrajData, double, &TrajData::timestamp > > > > | TrajDataContainer |
| using | TrajMap = std::map<double, std::vector<DoubleSeqPtr>> |
Public Member Functions | |
| void | addDerivationsToDimension (size_t dimension, const double t, const Ice::DoubleSeq &derivs) |
| size_t | addDimension (const Ice::DoubleSeq &values, const Ice::DoubleSeq ×tamps=Ice::DoubleSeq(), const std::string name="") |
| void | addPositionsToDimension (size_t dimension, const Ice::DoubleSeq &values, const Ice::DoubleSeq ×tamps) |
| ordered_view::const_iterator | begin () const |
| Iterators that iterates in incremental order of the timestamps through the trajectory. | |
| TrajectoryPtr | calculateTimeOptimalTrajectory (const Eigen::VectorXd &maxVelocities, const Eigen::VectorXd &maxAccelerations, double maxDeviation, IceUtil::Time const ×tep) |
| TrajectoryPtr | calculateTimeOptimalTrajectory (double maxVelocity, double maxAcceleration, double maxDeviation, IceUtil::Time const ×tep) |
| void | clear (bool keepMetaData=false) |
| VariantDataClassPtr | clone (const Ice::Current &c=Ice::emptyCurrent) const override |
| TrajectoryPtr | cloneMetaData () const |
| TrajDataContainer & | data () |
| bool | dataExists (double t, size_t dimension=0, size_t derivation=0) const |
| void | deserialize (const ObjectSerializerBasePtr &, const Ice::Current &=Ice::emptyCurrent) override |
| void | differentiateDiscretly (size_t derivation) |
| void | differentiateDiscretlyForDim (size_t trajDimension, size_t derivation) |
| size_t | dim () const |
| dim returns the trajectory dimension count for this trajectory (e.g. | |
| ordered_view::const_iterator | end () const |
| void | gaussianFilter (double filterRadius) |
| gaussianFilter smoothes the trajectory | |
| std::vector< Ice::DoubleSeq > | getAllStates (double t, int maxDeriv=1) |
| double | getAmplitude (size_t dimension, size_t derivation, double startTime, double endTime) const |
| Ice::DoubleSeq | getDerivations (double t, size_t dimension, size_t derivations) const |
| Ice::DoubleSeq | getDimensionData (size_t dimension, size_t derivation=0) const |
| getDimensionData gets all entries for one dimensions with order of increasing timestamps | |
| Eigen::VectorXd | getDimensionDataAsEigen (size_t dimension, size_t derivation) const |
| Eigen::VectorXd | getDimensionDataAsEigen (size_t dimension, size_t derivation, double startTime, double endTime) const |
| std::string | getDimensionName (size_t dim) const |
| const Ice::StringSeq & | getDimensionNames () const |
| double | getDiscretDifferentiationForDimAtT (double t, size_t trajDimension, size_t derivation) const |
| Ice::DoubleSeq | getDiscreteDifferentiationForDim (size_t trajDimension, size_t derivation) const |
| double | getLength (size_t derivation, double startTime, double endTime) const |
| double | getLength (size_t derivation=0) const |
| Returns the sum of a all subsequent distances of the entries in the trajectories over all dimensions. | |
| double | getLength (size_t dimension, size_t derivation) const |
| double | getLength (size_t dimension, size_t derivation, double startTime, double endTime) const |
| LimitlessStateSeq | getLimitless () const |
| double | getMax (size_t dimension, size_t derivation, double startTime, double endTime) const |
| Ice::DoubleSeq | getMaxima (size_t dimension, size_t derivation, double startTime, double endTime) const |
| Ice::DoubleSeq | getMaximaTimestamps (size_t dimension, size_t derivation, double startTime, double endTime) const |
| double | getMin (size_t dimension, size_t derivation, double startTime, double endTime) const |
| Ice::DoubleSeq | getMinima (size_t dimension, size_t derivation, double startTime, double endTime) const |
| Calculate all minima. | |
| Ice::DoubleSeq | getMinimaTimestamps (size_t dimension, size_t derivation, double startTime, double endTime) const |
| TrajectoryPtr | getPart (double startTime, double endTime, size_t numberOfDerivations=0) const |
| double | getSquaredLength (size_t dimension, size_t derivation) const |
| double | getSquaredLength (size_t dimension, size_t derivation, double startTime, double endTime) const |
| double | getState (double t, size_t dim=0, size_t derivation=0) |
| double | getState (double t, size_t dim=0, size_t derivation=0) const |
| std::vector< DoubleSeqPtr > & | getStates (double t) |
| std::vector< DoubleSeqPtr > | getStates (double t) const |
| Ice::DoubleSeq | getStates (double t, size_t derivation) const |
| std::map< double, Ice::DoubleSeq > | getStatesAround (double t, size_t derivation, size_t extend) const |
| template<typename T> | |
| std::map< std::string, T > | getStatesMap (double t, size_t derivation=0) const |
| double | getTimeLength () const |
| Difference between biggest and smallest timestamp. | |
| Ice::DoubleSeq | getTimestamps () const |
| Ice::FloatSeq | getTimestampsFloat () const |
| Ice::Int | getType (const Ice::Current &c=Ice::emptyCurrent) const override |
| double | getWithFunc (const double &(*foo)(const double &, const double &), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const |
| Ice::ObjectPtr | ice_clone () const override |
| void | ice_postUnmarshal () override |
| void | ice_preMarshal () override |
| void | negateDim (size_t trajDimension) |
| negateDim changes the sign of all values of the given dimension. | |
| TrajectoryPtr | normalize (const double startTime=0.0, const double endTime=1.0) |
| Trajectory & | operator+= (const Trajectory traj) |
| Trajectory & | operator= (const Trajectory &source) |
| std::vector< DoubleSeqPtr > & | operator[] (double timestamp) |
| std::string | output (const Ice::Current &c=Ice::emptyCurrent) const override |
| ordered_view::const_reverse_iterator | rbegin () const |
| void | reconstructFromDerivativeForDim (double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState) |
| void | removeDerivation (size_t derivation) |
| void | removeDerivation (size_t dimension, size_t derivation) |
| void | removeDimension (size_t dimension) |
| ordered_view::const_reverse_iterator | rend () const |
| void | scaleValue (const Ice::DoubleSeq &factor) |
| void | serialize (const ObjectSerializerBasePtr &obj, const Ice::Current &=Ice::emptyCurrent) const override |
| void | setDimensionNames (const Ice::StringSeq dimNames) |
| void | setEntries (const double t, const size_t dimIndex, const Ice::DoubleSeq &y) |
| void | setLimitless (const LimitlessStateSeq &limitlessStates) |
| void | setPositionEntry (const double t, const size_t dimIndex, const double &y) |
| void | shiftTime (double shift) |
| void | shiftValue (const Ice::DoubleSeq &shift) |
| size_t | size () const |
| Returns number of data entries (i.e. | |
| Eigen::MatrixXd | toEigen (size_t derivation, double startTime, double endTime) const |
| Eigen::MatrixXd | toEigen (size_t derivation=0) const |
| Trajectory () | |
| Trajectory (const std::map< double, Ice::DoubleSeq > &data) | |
| template<typename T> | |
| Trajectory (const std::vector< std::vector< std::vector< T > > > &data, const Ice::DoubleSeq timestamps=Ice::DoubleSeq(), const Ice::StringSeq &dimensionNames={}) | |
| template<typename T> | |
| Trajectory (const std::vector< std::vector< T > > &data, const Ice::DoubleSeq timestamps=Ice::DoubleSeq(), const Ice::StringSeq &dimensionNames={}) | |
| Constructor to add n-dimensions with m-values. | |
| template<typename T> | |
| Trajectory (const std::vector< T > &data, const Ice::DoubleSeq timestamps=Ice::DoubleSeq(), const std::string &dimensionName="", typename std::enable_if_t< std::is_arithmetic_v< T > > *t=0) | |
| Trajectory (const Trajectory &source) | |
| bool | validate (const Ice::Current &c=Ice::emptyCurrent) override |
Static Public Member Functions | |
| static void | CopyData (const Trajectory &source, Trajectory &destination) |
| static void | CopyMetaData (const Trajectory &source, Trajectory &destination) |
| static Ice::DoubleSeq | DifferentiateDiscretly (const Ice::DoubleSeq ×tamps, const Ice::DoubleSeq &values, int derivationCount=1) |
| template<typename T> | |
| static Ice::DoubleSeq | GenerateTimestamps (const std::vector< T > &values) |
| static Ice::DoubleSeq | GenerateTimestamps (double startTime=0.0, double endTime=1.0, double stepSize=0.001) |
| static Ice::DoubleSeq | NormalizeTimestamps (const Ice::DoubleSeq ×tamps, const double startTime=0.0, const double endTime=1.0) |
| static Trajectory | NormalizeTimestamps (const Trajectory &traj, const double startTime=0.0, const double endTime=1.0) |
Protected Member Functions | |
| void | __addDimension () |
| std::vector< DoubleSeqPtr > | __calcBaseDataAtTimestamp (const double &t) const |
| void | __fillAllEmptyFields () |
| timestamp_view::iterator | __fillBaseDataAtTimestamp (const double &t) |
| double | __gaussianFilter (double filterRadius, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim) |
| virtual double | __interpolate (double t, ordered_view::const_iterator itPrev, ordered_view::const_iterator itNext, size_t dimension, size_t derivation) const |
| double | __interpolate (double t, size_t dimension, size_t derivation) const |
| double | __interpolate (typename ordered_view::const_iterator itMap, size_t dimension, size_t derivation) const |
| ~Trajectory () override | |
Protected Attributes | |
| TrajDataContainer | dataMap |
The Trajectory class represents n-dimensional sampled trajectories.
The class interpolates all missing values with linear interpolation and calculates derivations. This means trajectories with different timestamps can easily be used. Velocities or accelerations can easily be queried with the derivation parameters of many functions. Access complexity for a query with a timestamp is like from an unordered_map.
Basic usage:
The trajectory offers normal iterator capabilities:
With the iterators it can be used like a std container:
| using ordered_view |
| using timestamp_view |
| typedef boost::multi_index::multi_index_container< TrajData, boost::multi_index::indexed_by< boost::multi_index::hashed_unique< boost::multi_index::tag<TagTimestamp>, boost::multi_index::member<TrajData, double, &TrajData::timestamp> >, boost::multi_index::ordered_unique< boost::multi_index::tag<TagOrdered>, boost::multi_index::member< TrajData, double, &TrajData::timestamp> > > > TrajDataContainer |
| using TrajMap = std::map<double, std::vector<DoubleSeqPtr>> |
|
overrideprotecteddefault |
|
inline |
| Trajectory | ( | const Trajectory & | source | ) |
|
inline |
|
inline |
Constructor to add n-dimensions with m-values.
| data | data vector (size n) with positions (size m). Dimension 1 is the joint dimension and Dimension 2 is the position of that joint. |
| timestamps | timestamps of the trajectory. If not empty, need to be of same size as the inner values of data. If empty, timestamps from 0..1 are generated with a stepsize aligned with the size of the inner vector of data. |
|
inline |
| Trajectory | ( | const std::map< double, Ice::DoubleSeq > & | data | ) |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protectedvirtual |
|
protected |
|
protected |
| void addDerivationsToDimension | ( | size_t | dimension, |
| const double | t, | ||
| const Ice::DoubleSeq & | derivs ) |
| size_t addDimension | ( | const Ice::DoubleSeq & | values, |
| const Ice::DoubleSeq & | timestamps = Ice::DoubleSeq(), | ||
| const std::string | name = "" ) |
| void addPositionsToDimension | ( | size_t | dimension, |
| const Ice::DoubleSeq & | values, | ||
| const Ice::DoubleSeq & | timestamps ) |
| Trajectory::ordered_view::const_iterator begin | ( | ) | const |
| TrajectoryPtr calculateTimeOptimalTrajectory | ( | const Eigen::VectorXd & | maxVelocities, |
| const Eigen::VectorXd & | maxAccelerations, | ||
| double | maxDeviation, | ||
| IceUtil::Time const & | timestep ) |
| TrajectoryPtr calculateTimeOptimalTrajectory | ( | double | maxVelocity, |
| double | maxAcceleration, | ||
| double | maxDeviation, | ||
| IceUtil::Time const & | timestep ) |
| void clear | ( | bool | keepMetaData = false | ) |
|
override |
| TrajectoryPtr cloneMetaData | ( | ) | const |
|
static |
|
static |
| Trajectory::TrajDataContainer & data | ( | ) |
| bool dataExists | ( | double | t, |
| size_t | dimension = 0, | ||
| size_t | derivation = 0 ) const |
|
override |
|
static |
| void differentiateDiscretly | ( | size_t | derivation | ) |
| void differentiateDiscretlyForDim | ( | size_t | trajDimension, |
| size_t | derivation ) |
| size_t dim | ( | ) | const |
dim returns the trajectory dimension count for this trajectory (e.g.
taskspace w/o orientation would be 3)
| Trajectory::ordered_view::const_iterator end | ( | ) | const |
| void gaussianFilter | ( | double | filterRadius | ) |
gaussianFilter smoothes the trajectory
| filterRadius,in | easy words: time range to left and right from center that influences the resulting center value |
|
inlinestatic |
|
static |
| std::vector< Ice::DoubleSeq > getAllStates | ( | double | t, |
| int | maxDeriv = 1 ) |
| double getAmplitude | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| Ice::DoubleSeq getDerivations | ( | double | t, |
| size_t | dimension, | ||
| size_t | derivations ) const |
| Ice::DoubleSeq getDimensionData | ( | size_t | dimension, |
| size_t | derivation = 0 ) const |
getDimensionData gets all entries for one dimensions with order of increasing timestamps
| dimension | |
| derivation |
| Eigen::VectorXd getDimensionDataAsEigen | ( | size_t | dimension, |
| size_t | derivation ) const |
| Eigen::VectorXd getDimensionDataAsEigen | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| std::string getDimensionName | ( | size_t | dim | ) | const |
| const Ice::StringSeq & getDimensionNames | ( | ) | const |
| double getDiscretDifferentiationForDimAtT | ( | double | t, |
| size_t | trajDimension, | ||
| size_t | derivation ) const |
| Ice::DoubleSeq getDiscreteDifferentiationForDim | ( | size_t | trajDimension, |
| size_t | derivation ) const |
| double getLength | ( | size_t | derivation, |
| double | startTime, | ||
| double | endTime ) const |
| double getLength | ( | size_t | derivation = 0 | ) | const |
Returns the sum of a all subsequent distances of the entries in the trajectories over all dimensions.
| derivation | Derivation for which the length is calculated |
| double getLength | ( | size_t | dimension, |
| size_t | derivation ) const |
| double getLength | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| LimitlessStateSeq getLimitless | ( | ) | const |
| double getMax | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| Ice::DoubleSeq getMaxima | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| Ice::DoubleSeq getMaximaTimestamps | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| double getMin | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| Ice::DoubleSeq getMinima | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
Calculate all minima.
| dimension | |
| derivation | |
| startTime | |
| endTime |
| Ice::DoubleSeq getMinimaTimestamps | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| TrajectoryPtr getPart | ( | double | startTime, |
| double | endTime, | ||
| size_t | numberOfDerivations = 0 ) const |
| double getSquaredLength | ( | size_t | dimension, |
| size_t | derivation ) const |
| double getSquaredLength | ( | size_t | dimension, |
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
| double getState | ( | double | t, |
| size_t | dim = 0, | ||
| size_t | derivation = 0 ) |
| double getState | ( | double | t, |
| size_t | dim = 0, | ||
| size_t | derivation = 0 ) const |
| std::vector< DoubleSeqPtr > & getStates | ( | double | t | ) |
| std::vector< DoubleSeqPtr > getStates | ( | double | t | ) | const |
| Ice::DoubleSeq getStates | ( | double | t, |
| size_t | derivation ) const |
| std::map< double, Ice::DoubleSeq > getStatesAround | ( | double | t, |
| size_t | derivation, | ||
| size_t | extend ) const |
|
inline |
| double getTimeLength | ( | ) | const |
Difference between biggest and smallest timestamp.
If size() <=1 -> return 0.
| Ice::DoubleSeq getTimestamps | ( | ) | const |
| Ice::FloatSeq getTimestampsFloat | ( | ) | const |
|
override |
| double getWithFunc | ( | const double &(* | foo )(const double &, const double &), |
| double | initValue, | ||
| size_t | dimension, | ||
| size_t | derivation, | ||
| double | startTime, | ||
| double | endTime ) const |
|
override |
|
override |
|
override |
| void negateDim | ( | size_t | trajDimension | ) |
negateDim changes the sign of all values of the given dimension.
| TrajectoryPtr normalize | ( | const double | startTime = 0.0, |
| const double | endTime = 1.0 ) |
|
static |
|
static |
| Trajectory & operator+= | ( | const Trajectory | traj | ) |
| Trajectory & operator= | ( | const Trajectory & | source | ) |
| std::vector< DoubleSeqPtr > & operator[] | ( | double | timestamp | ) |
|
override |
| Trajectory::ordered_view::const_reverse_iterator rbegin | ( | ) | const |
| void reconstructFromDerivativeForDim | ( | double | valueAtFirstTimestamp, |
| size_t | trajDimension, | ||
| size_t | sourceDimOfSystemState, | ||
| size_t | targetDimOfSystemState ) |
| void removeDerivation | ( | size_t | derivation | ) |
| void removeDerivation | ( | size_t | dimension, |
| size_t | derivation ) |
| void removeDimension | ( | size_t | dimension | ) |
| Trajectory::ordered_view::const_reverse_iterator rend | ( | ) | const |
| void scaleValue | ( | const Ice::DoubleSeq & | factor | ) |
|
override |
|
inline |
| void setEntries | ( | const double | t, |
| const size_t | dimIndex, | ||
| const Ice::DoubleSeq & | y ) |
| void setLimitless | ( | const LimitlessStateSeq & | limitlessStates | ) |
| void setPositionEntry | ( | const double | t, |
| const size_t | dimIndex, | ||
| const double & | y ) |
| void shiftTime | ( | double | shift | ) |
| void shiftValue | ( | const Ice::DoubleSeq & | shift | ) |
| size_t size | ( | ) | const |
Returns number of data entries (i.e.
positions) in the trajectory.
| Eigen::MatrixXd toEigen | ( | size_t | derivation, |
| double | startTime, | ||
| double | endTime ) const |
| Eigen::MatrixXd toEigen | ( | size_t | derivation = 0 | ) | const |
|
override |
|
protected |