diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 1bd39e90ff1fa82748c5057ca0842cc84a44361a..044aadda547e4b93c3b1672a85af91a208fec47f 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -17,6 +17,7 @@ set(SLICE_FILES
core/FramedPoseBase.ice
core/RobotState.ice
core/RobotStateObserverInterface.ice
+ core/Trajectory.ice
selflocalisation/SelfLocalisationProcess.ice
diff --git a/source/RobotAPI/interface/core/Trajectory.ice b/source/RobotAPI/interface/core/Trajectory.ice
new file mode 100644
index 0000000000000000000000000000000000000000..daeb1ce6fca713dab8cc238f90a495aa9380d92d
--- /dev/null
+++ b/source/RobotAPI/interface/core/Trajectory.ice
@@ -0,0 +1,55 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::RobotAPI
+* @author Mirko Waechter
+* @copyright 2016 Humanoids Group, HIS, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef _ARMARX_ROBOTAPI_TRAJECTORY_SLICE_
+#define _ARMARX_ROBOTAPI_TRAJECTORY_SLICE_
+
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+
+module armarx
+{
+ sequence<DoubleSeqSeq> DoubleSeqSeqSeq;
+
+ /**
+ * [TrajectoryBase] defines a n-dimension trajectory with n deriviations.
+ *
+ */
+ ["cpp:virtual"]
+ class TrajectoryBase extends VariantDataClass
+ {
+ ["protected"]
+ Ice::StringSeq dimensionNames;
+ /**
+ * first vector dimension is the frame of the trajectory
+ * second vector dimension is dimension of trajectory (e.g. which joint).
+ * third vector dimension is the derivation of the trajectory (position, velocity, acceleration)
+ * data might be incomplete (not all dimension 3 vector might have values)
+ */
+ ["protected"] DoubleSeqSeqSeq dataVec;
+ ["protected"] Ice::DoubleSeq timestamps;
+ };
+
+
+};
+
+#endif
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 05a4af6d9f1f88c93349ee33c25da9464368053a..9b50618c86521727b69bb49fa1ea9231cfe42208 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -22,6 +22,7 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
set(LIB_FILES
PIDController.cpp
+ Trajectory.cpp
Pose.cpp
FramedPose.cpp
LinkedPose.cpp
@@ -39,6 +40,8 @@ set(LIB_FILES
set(LIB_HEADERS
EigenStl.h
PIDController.h
+ Trajectory.h
+ VectorHelpers.h
Pose.h
FramedPose.h
LinkedPose.h
@@ -63,5 +66,6 @@ set(LIB_HEADERS
math/MatrixHelpers.h
)
+add_subdirectory(test)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
index 81ff0980712a4ffe79d12c7b2a4d0a91cdcd7a17..6298a152bf44c0f0b3f9b1f2c2a027e16379ab79 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
@@ -23,6 +23,8 @@
#ifndef _ARMARX_ROBOTAPI_OBJECT_FACTORIES_H
#define _ARMARX_ROBOTAPI_OBJECT_FACTORIES_H
+#include "Trajectory.h"
+
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/interface/core/RobotState.h>
@@ -183,7 +185,7 @@ namespace armarx
add<armarx::MatrixPercentileFilterBase, armarx::filters::MatrixPercentileFilter>(map);
add<armarx::MatrixPercentilesFilterBase, armarx::filters::MatrixPercentilesFilter>(map);
add<armarx::MatrixCumulativeFrequencyFilterBase, armarx::filters::MatrixCumulativeFrequencyFilter>(map);
-
+ add<armarx::TrajectoryBase, armarx::Trajectory>(map);
return map;
}
static const FactoryCollectionBaseCleanUp RobotAPIObjectFactoriesVar;
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..544072b37ef1ea55601c2a6e89c9444a47a73987
--- /dev/null
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -0,0 +1,1745 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "Trajectory.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include "VectorHelpers.h"
+
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+
+namespace armarx
+{
+
+ void Trajectory::ice_preMarshal()
+ {
+ dataVec.clear();
+ timestamps.clear();
+ dataVec.reserve(dim());
+ for (auto it = begin(); it != end(); it++)
+ {
+ std::vector< DoubleSeqPtr >& data = it->data;
+ DoubleSeqSeq new2DVec;
+ new2DVec.reserve(data.size());
+ for (DoubleSeqPtr & subVec : data)
+ {
+ new2DVec.emplace_back(*subVec);
+ }
+ dataVec.emplace_back(new2DVec);
+ }
+ timestamps = getTimestamps();
+ }
+
+ void Trajectory::ice_postUnmarshal()
+ {
+ int i = 0;
+ for (DoubleSeqSeq & _2DVec : dataVec)
+ {
+
+ double t = timestamps.at(i);
+ int d = 0;
+ for (auto & vec : _2DVec)
+ {
+ setEntries(t, d++, vec);
+ }
+
+ i++;
+ }
+ }
+
+ void Trajectory::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ {
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+ Ice::StringSeq columnContent;
+ Eigen::MatrixXd m = toEigen();
+ auto cols = m.cols();
+ auto rows = m.rows();
+ for (int col = 0; col < cols; col++)
+ {
+ std::stringstream ss;
+
+ for (int row = 0; row < rows; row++)
+ {
+ ss << m(row, col) << (row < rows - 1 ? "," : "");
+ }
+
+ columnContent.push_back(ss.str());
+ }
+ obj->setDoubleArray("timestamps", getTimestamps());
+ obj->setStringArray("dimensionData", columnContent);
+ obj->setStringArray("dimensionNames", dimensionNames);
+ }
+
+ void Trajectory::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ {
+ clear();
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+ Ice::StringSeq rowContent;
+ obj->getStringArray("dimensionData", rowContent);
+
+ Ice::DoubleSeq timestamps;
+ obj->getDoubleArray("timestamps", timestamps);
+ for (size_t row = 0; row < rowContent.size(); row++)
+ {
+ Ice::StringSeq values;
+ boost::split(values, rowContent[row], boost::is_any_of(","));
+ Ice::DoubleSeq newRow;
+ newRow.reserve(values.size());
+ for (std::string v : values)
+ {
+ newRow.push_back(atof(v.c_str()));
+ }
+ addDimension(timestamps, newRow);
+ }
+ obj->getStringArray("dimensionNames", dimensionNames);
+ }
+
+ VariantDataClassPtr Trajectory::clone(const Ice::Current&) const
+ {
+ return new Trajectory(*this);
+ }
+
+ std::string Trajectory::output(const Ice::Current&) const
+ {
+ return "Trajectory with " + std::to_string(dim()) + " dimensions";
+ }
+
+ Ice::Int Trajectory::getType(const Ice::Current&) const
+ {
+ return VariantType::Trajectory;
+ }
+
+ bool Trajectory::validate(const Ice::Current&)
+ {
+ return true;
+ }
+
+ Ice::ObjectPtr Trajectory::ice_clone() const
+ {
+ return clone();
+ }
+
+ Trajectory::Trajectory(const Trajectory& source) :
+ IceUtil::Shared(source),
+ TrajectoryBase(source)
+ {
+ CopyData(source, *this);
+ }
+
+
+
+ Trajectory::Trajectory(const Ice::DoubleSeq& x, const Ice::DoubleSeq& y, const std::string& dimensionName)
+ {
+ addDimension(x, y, dimensionName);
+ }
+
+
+ Trajectory::Trajectory(const std::map<double, Ice::DoubleSeq >& data)
+ {
+ if (data.size() == 0)
+ {
+ return;
+ }
+
+ typename std::map<double, Ice::DoubleSeq >::const_iterator it = data.begin();
+
+ for (; it != data.end(); it++)
+ {
+ TrajData dataEntry(*this);
+ dataEntry.timestamp = it->first;
+ const Ice::DoubleSeq& dataVec = it->second;
+
+ for (size_t i = 0; i < dataVec.size(); i++)
+ {
+ checkValue(dataVec[i]);
+ dataEntry.data.push_back(DoubleSeqPtr(new Ice::DoubleSeq(1, dataVec[i])));
+ }
+
+ dataMap.insert(dataEntry);
+ }
+ }
+
+ //
+ // Trajectory::Trajectory(const TrajMap &map)
+ // {
+ // copyMap(map, dataMap);
+ // }
+
+ Trajectory& Trajectory::operator=(const Trajectory& source)
+ {
+ CopyData(source, *this);
+ return *this;
+ }
+
+
+ double Trajectory::getState(double t, size_t dim, size_t deriv)
+ {
+ if (dim >= this->dim())
+ {
+ throw LocalException() << "dimension is to big: " << dim << " max: " << this->dim();
+ }
+
+ typename timestamp_view::iterator it = dataMap.find(t);
+
+ if (it == dataMap.end() || !it->data.at(dim))
+ {
+ __fillBaseDataAtTimestamp(t);// interpolates and retrieves
+ it = dataMap.find(t);
+ }
+
+ if (!it->data.at(dim))
+ // it->data.at(dim).reset(new Ice::DoubleSeq());
+ {
+ throw LocalException() << "std::vector ptr is NULL!?";
+ }
+
+ std::vector<DoubleSeqPtr>& vec = it->data;
+
+ if (deriv != 0 && vec.at(dim)->size() <= deriv)
+ {
+ //resize and calculate derivations
+ size_t curDeriv = it->data.at(dim)->size();
+ it->data.at(dim)->resize(deriv + 1);
+
+ while (curDeriv <= deriv)
+ {
+ double derivValue = getDiscretDifferentiationForDimAtT(t, dim, curDeriv);
+ checkValue(curDeriv);
+ it->data.at(dim)->at(curDeriv) = derivValue;
+ curDeriv++;
+ }
+
+ }
+
+ // std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
+ double result = it->data.at(dim)->at(deriv);
+ // checkValue(result);
+ return result;
+ }
+
+ double Trajectory::getState(double t, size_t dim, size_t deriv) const
+ {
+
+ if (dim >= this->dim())
+ {
+ throw LocalException() << "dimension is to big: " << dim << " max: " << this->dim();
+ }
+
+ typename timestamp_view::iterator it = dataMap.find(t);
+
+ if (!dataExists(t, dim, deriv))
+ {
+ if (deriv == 0)
+ {
+ return __calcBaseDataAtTimestamp(t).at(dim)->at(deriv); // interpolates and retrieves
+ }
+ else
+ {
+ return getDiscretDifferentiationForDimAtT(t, dim, deriv);
+ }
+ }
+ else
+ {
+ // std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
+ double result = it->data.at(dim)->at(deriv);
+
+ checkValue(result);
+
+
+ return result;
+ }
+ }
+
+ Ice::DoubleSeq Trajectory::getDerivations(double t, size_t dimension, size_t derivations) const
+ {
+ if (dimension >= dim())
+ {
+ throw LocalException() << "Dimension out of bounds: requested: " << dimension << " available: " << dim();
+ }
+
+ Ice::DoubleSeq result;
+
+ for (size_t i = 0; i <= derivations; i++)
+ {
+ result.push_back(getState(t, dimension, i));
+ }
+
+ return result;
+ }
+
+ std::string Trajectory::getDimensionName(size_t dim) const
+ {
+ return dimensionNames.at(dim);
+ }
+
+ Ice::StringSeq Trajectory::getDimensionNames() const
+ {
+ return dimensionNames;
+ }
+
+ Trajectory::TrajDataContainer& Trajectory::data()
+ {
+ return dataMap;
+ }
+
+ Ice::DoubleSeq
+ Trajectory::getDimensionData(size_t dimension, size_t deriv) const
+ {
+ if (dimension >= dim())
+ {
+ throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
+ }
+
+ Ice::DoubleSeq result;
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itMap = ordv.begin();
+
+ for (; itMap != ordv.end(); itMap++)
+ {
+ DoubleSeqPtr dataPtr = itMap->data[dimension];
+ result.push_back(itMap->getDeriv(dimension, deriv));
+ // if(dataPtr && dataPtr->size() > deriv)
+ // result.push_back(itMap->getDeriv(dimension, deriv));
+ // else result.push_back(getState(itMap->timestamp, dimension, deriv));
+ }
+
+ return result;
+
+ }
+
+ Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation) const
+ {
+ return getDimensionDataAsEigen(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ }
+
+ Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const
+ {
+ if (dimension >= dim())
+ {
+ throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
+ }
+
+ Ice::DoubleSeq result;
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+ // typename ordered_view::const_iterator itMap = ordv.begin();
+
+ size_t i = 0;
+ for (; itO != itOEnd && i < size(); itO++, i++)
+ {
+ DoubleSeqPtr dataPtr = itO->data[dimension];
+ result.push_back(itO->getDeriv(dimension, derivation));
+ // if(dataPtr && dataPtr->size() > deriv)
+ // result.push_back(itMap->getDeriv(dimension, deriv));
+ // else result.push_back(getState(itMap->timestamp, dimension, deriv));
+ }
+ Eigen::VectorXd resultVec(result.size());
+ for (size_t i = 0; i < result.size(); i++)
+ {
+ resultVec(i) = result[i];
+ }
+ return resultVec;
+ }
+
+ Eigen::MatrixXd Trajectory::toEigen(size_t derivation, double startTime, double endTime) const
+ {
+ Eigen::MatrixXd result(size(), dim());
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+ // typename ordered_view::const_iterator itMap = ordv.begin();
+
+ size_t i = 0;
+ for (; itO != itOEnd; itO++, i++)
+ {
+ // DoubleSeqPtr dataPtr = itO->data[dimension];
+ for (size_t d = 0; d < itO->data.size(); d++)
+ {
+ result(i, d) = (itO->getDeriv(d, derivation));
+ }
+ }
+
+ return result;
+ }
+
+ Eigen::MatrixXd Trajectory::toEigen(size_t derivation) const
+ {
+ if (size() == 0 || dim() == 0)
+ {
+ return Eigen::MatrixXd();
+ }
+ return toEigen(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ }
+
+ Trajectory Trajectory::getPart(double startTime, double endTime, size_t numberOfDerivations) const
+ {
+ Trajectory result;
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+ // typename ordered_view::const_iterator itMap = ordv.begin();
+
+ size_t i = 0;
+ for (; itO != itOEnd; itO++, i++)
+ {
+ // DoubleSeqPtr dataPtr = itO->data[dimension];
+ for (size_t d = 0; d < itO->data.size(); d++)
+ {
+ Ice::DoubleSeq derivs;
+ for (size_t i = 0; i < numberOfDerivations + 1; i++)
+ {
+ derivs.push_back(itO->getDeriv(d, i));
+ }
+
+ result.addDerivationsToDimension(d, itO->getTimestamp(), derivs);
+ }
+ // result.addPositionsToDimension(d, itO->getTimestamp(), itO->getDeriv(d, 0));
+ }
+
+ return result;
+ }
+
+ size_t Trajectory::dim() const
+ {
+ if (dataMap.size() == 0)
+ {
+ return 0;
+ }
+ else
+ {
+ return dataMap.begin()->data.size();
+ }
+ }
+
+ size_t Trajectory::size() const
+ {
+ return dataMap.size();
+ }
+
+
+ // Ice::DoubleSeq
+ // Trajectory::getDimensionData(size_t dimension, size_t deriv) const
+ // {
+ // if(dimension >= dim()){
+ // throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
+ // }
+
+ // Ice::DoubleSeq result;
+ // Ice::DoubleSeq tempResult = getDimensionData(dimension);
+ // typename Ice::DoubleSeq::iterator it = tempResult.begin();
+ // for(; it != tempResult.end(); it++)
+ // result.push_back(it->at(deriv));
+
+ // return result;
+ // }
+
+
+ double Trajectory::getTimeLength() const
+ {
+ const ordered_view& ordView = dataMap.get<TagOrdered>();
+
+ if (ordView.begin() != ordView.end())
+ {
+ return ordView.rbegin()->timestamp - ordView.begin()->timestamp;
+ }
+ else
+ {
+ return 0;
+ }
+ }
+
+ double Trajectory::getSpaceLength(size_t dimension, size_t stateDim) const
+ {
+ return getSquaredSpaceLength(dimension, stateDim, begin()->getTimestamp(), rbegin()->getTimestamp());
+ }
+
+ double Trajectory::getSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ double length = 0.0;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ // timestamp_view::iterator it = dataMap.find(startTime);
+ // Ice::DoubleSeq data = getDimensionData(0);
+ // timestamp_view::iterator itEnd= dataMap.find(endTime);
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+ if (itO == ordv.end())
+ {
+ return 0.0;
+ }
+ double prevValue = itO->getDeriv(dimension, stateDim);
+
+ for (;
+ itO != itOEnd;
+ itO++
+ )
+ {
+ length += fabs(prevValue - itO->getDeriv(dimension, stateDim));
+ prevValue = itO->getDeriv(dimension, stateDim);
+ }
+
+ return length;
+ }
+
+ double Trajectory::getSquaredSpaceLength(size_t dimension, size_t stateDim) const
+ {
+ return getSquaredSpaceLength(dimension, stateDim, begin()->getTimestamp(), rbegin()->getTimestamp());
+ }
+
+ double Trajectory::getSquaredSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ double length = 0.0;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ // timestamp_view::iterator it = dataMap.find(startTime);
+ // Ice::DoubleSeq data = getDimensionData(0);
+ // timestamp_view::iterator itEnd= dataMap.find(endTime);
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+ if (itO == ordv.end())
+ {
+ return 0.0;
+ }
+ // double prevValue = itO->data[dimension]->at(stateDim);
+ double prevValue = itO->getDeriv(dimension, stateDim);
+
+ for (;
+ itO != itOEnd;
+ itO++
+ )
+ {
+ length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, stateDim), 2.0));
+ prevValue = itO->getDeriv(dimension, stateDim);
+ }
+
+ return length;
+ }
+
+ double Trajectory::getMax(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, stateDim, startTime, endTime);
+ }
+
+ double Trajectory::getMin(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, stateDim, startTime, endTime);
+ }
+
+ double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ double bestValue = initValue;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+
+ if (itO == ordv.end())
+ {
+ return bestValue;
+ }
+ for (;
+ itO != itOEnd;
+ itO++
+ )
+ {
+ bestValue = foo(bestValue, itO->getDeriv(dimension, stateDim));
+ }
+
+ return bestValue;
+ }
+
+ double Trajectory::getTrajectorySpace(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ return getMax(dimension, stateDim, startTime, endTime) - getMin(dimension, stateDim, startTime, endTime);
+ }
+
+ Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ Ice::DoubleSeq result;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+
+ if (itO == ordv.end())
+ {
+ return result;
+ }
+ double preValue = itO->getDeriv(dimension, stateDim);
+ for (;
+ itO != itOEnd;
+
+ )
+ {
+ // double t = itO->getTimestamp();
+ double cur = itO->getDeriv(dimension, stateDim);
+ itO++;
+ if (itO == ordv.end())
+ {
+ break;
+ }
+ double next = itO->getDeriv(dimension, stateDim);
+ if (cur <= preValue && cur <= next)
+ {
+ result.push_back(cur);
+ }
+ preValue = cur;
+ }
+
+ return result;
+ }
+
+ Ice::DoubleSeq Trajectory::getMinimaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ Ice::DoubleSeq result;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+
+ if (itO == ordv.end())
+ {
+ return result;
+ }
+ double preValue = itO->getDeriv(dimension, stateDim);
+ for (;
+ itO != itOEnd;
+
+ )
+ {
+ // double t = itO->getTimestamp();
+ double cur = itO->getDeriv(dimension, stateDim);
+ itO++;
+ if (itO == ordv.end())
+ {
+ break;
+ }
+ double next = itO->getDeriv(dimension, stateDim);
+ if (cur <= preValue && cur <= next)
+ {
+ result.push_back(itO->getTimestamp());
+ }
+ preValue = cur;
+ }
+
+ return result;
+ }
+
+ Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ Ice::DoubleSeq result;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+
+ if (itO == ordv.end())
+ {
+ return result;
+ }
+ double preValue = itO->getDeriv(dimension, stateDim);
+ for (;
+ itO != itOEnd;
+
+ )
+ {
+ double cur = itO->getDeriv(dimension, stateDim);
+ itO++;
+ if (itO == ordv.end())
+ {
+ break;
+ }
+ double next = itO->getDeriv(dimension, stateDim);
+ if (cur >= preValue && cur >= next)
+ {
+ result.push_back(cur);
+ }
+ preValue = cur;
+ }
+
+ return result;
+ }
+
+ Ice::DoubleSeq Trajectory::getMaximaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ {
+ Ice::DoubleSeq result;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+
+ if (itO == ordv.end())
+ {
+ return result;
+ }
+ double preValue = itO->getDeriv(dimension, stateDim);
+ for (;
+ itO != itOEnd;
+
+ )
+ {
+ double cur = itO->getDeriv(dimension, stateDim);
+ itO++;
+ if (itO == ordv.end())
+ {
+ break;
+ }
+ double next = itO->getDeriv(dimension, stateDim);
+
+ if (cur >= preValue && cur >= next)
+ {
+ result.push_back(itO->getTimestamp());
+ }
+ preValue = cur;
+ }
+
+ return result;
+ }
+
+
+ std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
+ {
+ // typename timestamp_view::const_iterator it = dataMap.find(t);
+ // if(it != dataMap.end())
+ // return it->data;
+ std::vector<DoubleSeqPtr> result;
+
+ for (size_t dimension = 0; dimension < dim(); dimension++)
+ {
+ double newValue = __interpolate(t, dimension, 0);
+ checkValue(newValue);
+ result.push_back(DoubleSeqPtr(new Ice::DoubleSeq(1, newValue)));
+
+ }
+
+ return result;
+
+ }
+
+ bool Trajectory::dataExists(double t, size_t dimension, size_t deriv) const
+ {
+ typename timestamp_view::iterator it = dataMap.find(t);
+
+ if (it == dataMap.end() || !it->data.at(dimension) || it->data.at(dimension)->size() <= deriv)
+ {
+ return false;
+ }
+ else
+ {
+ return true;
+ }
+ }
+
+
+ Trajectory::timestamp_view::iterator Trajectory::__fillBaseDataAtTimestamp(const double& t)
+ {
+ typename timestamp_view::const_iterator it = dataMap.find(t);
+
+ if (it != dataMap.end())
+ {
+ bool foundEmpty = false;
+
+ for (size_t i = 0; i < it->data.size(); i++)
+ {
+ if (!it->data.at(i))
+ {
+ foundEmpty = true;
+ }
+ }
+
+ if (!foundEmpty)
+ {
+ return it;
+ }
+ }
+
+ TrajData entry(*this);
+ entry.timestamp = t;
+ entry.data.resize(dim());
+ dataMap.insert(entry);
+ it = dataMap.find(t);
+
+ assert(it != dataMap.end());
+ // const ordered_view& ordv = dataMap.get<TagOrdered>();
+ // typename ordered_view::iterator itOrdered = ordv.iterator_to(*it);
+ it->data = __calcBaseDataAtTimestamp(t);
+
+ return it;
+ }
+
+ std::vector<DoubleSeqPtr>& Trajectory::getStates(double t)
+ {
+ typename timestamp_view::const_iterator it = dataMap.find(t);
+
+ if (it == dataMap.end())
+ {
+ // interpolate and insert entry
+ it = __fillBaseDataAtTimestamp(t);
+ }
+
+
+ return it->data;
+ }
+
+ std::vector<DoubleSeqPtr> Trajectory::getStates(double t) const
+ {
+ typename timestamp_view::const_iterator it = dataMap.find(t);
+
+ if (it == dataMap.end())
+ {
+ // interpolate and return data
+ return __calcBaseDataAtTimestamp(t);
+ }
+
+
+ return it->data;
+ }
+
+
+
+ Ice::DoubleSeq Trajectory::getStates(double t, size_t derivation) const
+ {
+ Ice::DoubleSeq result;
+ size_t dimensions = dim();
+
+ for (size_t i = 0; i < dimensions; i++)
+ {
+ result.push_back(getState(t, i, derivation));
+ }
+
+ return result;
+ }
+
+ std::map<double, Ice::DoubleSeq> Trajectory::getStatesAround(double t, size_t derivation, size_t extend) const
+ {
+
+ std::map<double, Ice::DoubleSeq> res;
+ typename timestamp_view::iterator itMap = dataMap.find(t);
+
+ if (itMap != dataMap.end())
+ {
+ for (size_t i = 0; i < dim(); i++)
+ {
+ res.insert(std::pair<double, Ice::DoubleSeq>(t, {itMap->data[i]->at(derivation)}));
+ }
+
+ return res;
+ }
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+
+ typename ordered_view::iterator itNext = ordv.upper_bound(t);
+
+ typename ordered_view::iterator itPrev = itNext;
+
+ itPrev--;
+
+ if (itPrev == ordv.end())
+ {
+
+ throw LocalException("Cannot find value at timestamp ") << t;
+ }
+
+ for (size_t i = 0; i < extend; i++)
+ {
+ Ice::DoubleSeq preData = getStates(itPrev->timestamp, derivation);
+ Ice::DoubleSeq nexData = getStates(itNext->timestamp, derivation);
+
+ res.insert(std::pair<double, Ice::DoubleSeq>(itPrev->timestamp, preData));
+ res.insert(std::pair<double, Ice::DoubleSeq>(itNext->timestamp, nexData));
+
+ if (itPrev == ordv.begin() || itNext == ordv.end())
+ {
+ std::cout << "Warning: the timestamp is out of the range. " <<
+ "The current result will be returned" << std::endl;
+ break;
+ }
+ itPrev--;
+ itNext++;
+ }
+
+ return res;
+
+ }
+
+
+
+
+
+
+
+
+
+
+ Trajectory& Trajectory::operator +=(const Trajectory traj)
+ {
+ int dims = traj.dim();
+ Ice::DoubleSeq timestamps = traj.getTimestamps();
+
+ for (int d = 0; d < dims; ++d)
+ {
+ addPositionsToDimension(d, timestamps, traj.getDimensionData(d));
+ }
+
+ return *this;
+ }
+
+
+ void Trajectory::__addDimension()
+ {
+ size_t newDim = dim() + 1;
+
+ typename timestamp_view::iterator itMap = dataMap.begin();
+
+ for (; itMap != dataMap.end(); itMap++)
+ {
+ itMap->data.resize(newDim);
+ }
+ }
+
+ void Trajectory::setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y)
+ {
+ typename timestamp_view::iterator itMap = dataMap.find(t);
+
+ if (itMap == dataMap.end() || dim() == 0)
+ {
+ // double dura = getTimeLength();
+ TrajData newData(*this);
+ newData.timestamp = t;
+ newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
+ newData.data[dimIndex] = DoubleSeqPtr(new Ice::DoubleSeq(y));
+ dataMap.insert(newData);
+ }
+ else
+ {
+ assert(dim() > 0);
+
+ while (dim() <= dimIndex)
+ {
+ __addDimension();
+ }
+
+ itMap->data.resize(dim());
+ itMap->data.at(dimIndex) = DoubleSeqPtr(new Ice::DoubleSeq(y));
+ }
+
+ }
+
+
+ void Trajectory::setPositionEntry(const double t, const size_t dimIndex, const double& y)
+ {
+ typename timestamp_view::iterator itMap = dataMap.find(t);
+
+ if (itMap == dataMap.end() || dim() == 0)
+ {
+ TrajData newData(*this);
+ newData.timestamp = t;
+ newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
+ newData.data[dimIndex] = DoubleSeqPtr(new Ice::DoubleSeq(1, y));
+ dataMap.insert(newData);
+ }
+ else
+ {
+ assert(dim() > 0);
+ itMap->data.resize(dim());
+ itMap->data.at(dimIndex) = DoubleSeqPtr(new Ice::DoubleSeq(1, y));
+ }
+
+ }
+
+
+ void Trajectory::__fillAllEmptyFields()
+ {
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itMap = ordv.begin();
+
+ for (; itMap != ordv.end(); itMap++)
+ {
+ for (size_t dimension = 0; dimension < dim(); dimension++)
+ {
+ if (!itMap->data[dimension])
+ {
+ itMap->data[dimension] = DoubleSeqPtr(new Ice::DoubleSeq(__interpolate(itMap, dimension, 0)));
+ }
+ }
+ }
+ }
+
+
+
+
+ Ice::DoubleSeq Trajectory::getTimestamps() const
+ {
+ Ice::DoubleSeq result;
+ result.reserve(size());
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itMap = ordv.begin();
+
+ for (; itMap != ordv.end(); itMap++)
+ {
+ result.push_back(itMap->timestamp);
+ }
+
+ return result;
+ }
+
+
+
+ Ice::DoubleSeq
+ Trajectory::getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const
+ {
+ if (trajDimension >= dim())
+ {
+ throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
+ }
+
+ // if(sourceDimOfSystemState >= double().size()){
+ // throw LocalException("sourceDimOfSystemState is out of range: ") << sourceDimOfSystemState << " actual dimensions: " << double().size();
+ // }
+ // if(targetDimOfSystemState >= double().size()){
+ // throw LocalException("sourceDimOfSystemState is out of range: ") << targetDimOfSystemState << " actual dimensions: " << double().size();
+ // }
+
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itPrev = ordv.begin();
+
+ typename ordered_view::const_iterator itCurrent = itPrev;
+ // itCurrent++;
+ typename ordered_view::const_iterator itNext = itCurrent;
+ itNext++;
+
+ Ice::DoubleSeq result;
+
+ for (; itCurrent != ordv.end();)
+ {
+ if (itNext == ordv.end()) // last step-> differentiate between current and prev
+ {
+ itNext = itCurrent;
+ }
+
+
+ // get current data
+ DoubleSeqPtr prevStatePtr = itPrev->data[trajDimension];
+ DoubleSeqPtr nextStatePtr = itNext->data[trajDimension];
+ // double nextValue = nextStatePtr->getValues()[sourceDimOfSystemState];
+ // double prevValue = prevStatePtr->getValues()[sourceDimOfSystemState];
+ // double nextTime = itNext->timestamp;
+ // double prevTime = itPrev->timestamp;
+ // differentiateDiscretly now
+ result.push_back((nextStatePtr->at(derivation) - prevStatePtr->at(derivation)) /
+ (itNext->timestamp - itPrev->timestamp));
+ checkValue(*result.rbegin());
+
+
+ itCurrent++, itPrev++, itNext++;
+
+ if (itPrev == itCurrent)
+ {
+ // first step-> reestablish that current is > prev
+ itPrev--;
+ }
+
+ }
+
+ return result;
+ }
+
+
+ Ice::DoubleSeq Trajectory::DifferentiateDiscretly(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, int derivationCount)
+ {
+ if (derivationCount < 0)
+ {
+ throw LocalException("Negative derivation value is not allowed!");
+ }
+
+ if (derivationCount == 0)
+ {
+ return values;
+ }
+
+ Ice::DoubleSeq result;
+ int size = std::min(timestamps.size(), values.size());
+
+ if (size < 2)
+ {
+ return values;
+ }
+
+ result.resize(size);
+
+ // boundaries
+ result[0] = (values.at(1) - values.at(0)) / (timestamps.at(1) - timestamps.at(0)) ;
+ result[size - 1] = (values.at(size - 1) - values.at(size - 2)) / (timestamps.at(size - 1) - timestamps.at(size - 2)) ;
+
+ //#pragma omp parallel for
+ for (int i = 1; i < size - 1; ++i)
+ {
+ result[i] = (values.at(i + 1) - values.at(i - 1)) / (timestamps.at(i + 1) - timestamps.at(i - 1)) ;
+ checkValue(result[i]);
+ }
+
+ if (derivationCount > 1) // recursivly differentiate
+ {
+ result = DifferentiateDiscretly(timestamps, result, derivationCount - 1);
+ }
+
+ return result;
+
+ }
+
+
+ double Trajectory::getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t deriv) const
+ {
+ if (deriv == 0)
+ {
+ getState(t, trajDimension, deriv);
+ }
+
+ typename timestamp_view::iterator it = dataMap.find(t);
+ // if(it == dataMap.end())
+ // {
+ // getStates(t); // interpolate and insert entry
+ // it = dataMap.find(t);
+ // }
+
+ const ordered_view& ordV = dataMap.get<TagOrdered>();
+ typename ordered_view::iterator itCurrent = ordV.end();
+
+ if (it != dataMap.end())
+ {
+ itCurrent = ordV.iterator_to(*it);
+ }
+
+ typename ordered_view::iterator itNext = ordV.upper_bound(t); // first element after t
+
+ if (it != dataMap.end() && itNext == ordV.end())
+ {
+ itNext = itCurrent; // current item is last, set next to current
+ }
+ else if (itNext == ordV.end() && it == dataMap.end())
+ {
+ throw LocalException() << "Cannot interpolate for t " << t << " no data in trajectory";
+ }
+
+ typename ordered_view::iterator itPrev = itNext;
+ itPrev--; // now equal to current (if current exists) or before current
+
+ if (itCurrent != ordV.end()) // check if current item exists
+ {
+ itPrev--; //element in front of t
+ }
+
+ if (itPrev == ordV.end())
+ {
+ //element in front of t does not exist
+ if (itCurrent != ordV.end())
+ {
+ itPrev = itCurrent; // set prev element to current
+ }
+ else
+ {
+ throw LocalException() << "Cannot interpolate for t " << t;
+ }
+ }
+
+ if (itNext == ordV.end())
+ {
+ //element after t does not exist
+ if (itCurrent != ordV.end())
+ {
+ itNext = itCurrent; // set next element to current
+ }
+ else
+ {
+ throw LocalException() << "Cannot interpolate for t " << t;
+ }
+ }
+
+ if (itNext == itPrev)
+ {
+ throw LocalException() << "Interpolation failed: the next data and the previous are missing";
+ }
+
+ // double diff = itNext->data[trajDimension]->at(deriv-1) - itPrev->data[trajDimension]->at(deriv-1);
+ double tNext;
+
+ if (dataExists(itNext->timestamp, trajDimension, deriv - 1) || deriv > 1)
+ {
+ tNext = itNext->timestamp;
+ }
+ else
+ {
+ tNext = t;
+ }
+
+ double next = getState(tNext, trajDimension, deriv - 1);
+
+ double tBefore;
+
+ if (dataExists(itPrev->timestamp, trajDimension, deriv - 1) || deriv > 1)
+ {
+ tBefore = itPrev->timestamp;
+ }
+ else
+ {
+ tBefore = t;
+ }
+
+ double before = getState(tBefore, trajDimension, deriv - 1);
+
+ if (fabs(tNext - tBefore) < 1e-10)
+ {
+ throw LocalException() << "Interpolation failed: the next data and the previous are missing";
+ }
+
+ double duration = tNext - tBefore;
+ double delta = next - before;
+ delta = delta / duration;
+ checkValue(delta);
+ return delta;
+ }
+
+ void Trajectory::differentiateDiscretly(size_t derivation)
+ {
+
+ for (size_t d = 0; d < dim(); d++)
+ {
+ differentiateDiscretlyForDim(d, derivation);
+ }
+ }
+
+ void Trajectory::differentiateDiscretlyForDim(size_t trajDimension, size_t derivation)
+ {
+ removeDerivation(trajDimension, derivation);
+ typename ordered_view::iterator itOrd = begin();
+
+ for (; itOrd != end(); itOrd++)
+ {
+ getState(itOrd->timestamp, trajDimension, derivation);
+ }
+ }
+
+
+
+
+ void Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
+ {
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::iterator it = ordv.begin();
+
+ if (it == ordv.end())
+ {
+ return;
+ }
+
+ it->data[trajDimension]->at(targetDimOfSystemState) = valueAtFirstTimestamp;
+ double previousValue = valueAtFirstTimestamp;
+ double previousTimestamp = it->timestamp;
+
+ // size_t stateDim = double().size();
+ for (it++; it != ordv.end(); it++)
+ {
+ double slope;
+ if (it->data[trajDimension]->size() > sourceDimOfSystemState)
+ {
+ slope = it->data[trajDimension]->at(sourceDimOfSystemState);
+ }
+ else
+ {
+ slope = 0;
+ }
+
+ double diff = it->timestamp - previousTimestamp;
+ it->data[trajDimension]->at(targetDimOfSystemState) = previousValue + diff * slope;
+ previousTimestamp = it->timestamp;
+ previousValue = it->data[trajDimension]->at(targetDimOfSystemState);
+ }
+
+ }
+
+
+ void Trajectory::negateDim(size_t trajDimension)
+ {
+ if (trajDimension >= dim())
+ {
+ throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
+ }
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+
+ Trajectory filteredTraj;
+
+ for (typename ordered_view::iterator it = ordv.begin(); it != ordv.end(); it++)
+ {
+ (*it->data.at(trajDimension)) *= -1;
+ }
+ }
+
+
+
+
+
+ double
+ Trajectory::__interpolate(double t, size_t dimension, size_t deriv) const
+ {
+ typename timestamp_view::const_iterator it = dataMap.find(t);
+
+ if (it != dataMap.end() && it->data.size() > dimension && it->data.at(dimension) && it->data.at(dimension)->size() > deriv)
+ {
+ return it->data.at(dimension)->at(deriv);
+ }
+
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+
+ typename ordered_view::iterator itNext = ordv.upper_bound(t);
+
+ typename ordered_view::iterator itPrev = itNext;
+
+ itPrev--;
+
+ double result = __interpolate(t, itPrev, itNext, dimension, deriv);
+
+ checkValue(result);
+
+ return result;
+ }
+
+ double
+ Trajectory::__interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t deriv) const
+ {
+ typename ordered_view::iterator itPrev = itMap;
+ itPrev--;
+ typename ordered_view::iterator itNext = itMap;
+ itNext++;
+ return __interpolate(itMap->timestamp, itPrev, itNext, dimension, deriv);
+ }
+
+ double
+ Trajectory::__interpolate(double t, typename ordered_view::const_iterator itPrev, typename ordered_view::const_iterator itNext, size_t dimension, size_t deriv) const
+ {
+ // typename ordered_view::const_iterator itCur = itPrev;
+ // itCur++;
+
+ const ordered_view& ordView = dataMap.get<TagOrdered>();
+ double previous;
+ double next;
+
+ // find previous SystemState that exists for that dimension
+ while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL /*|| itPrev->data[dimension]->size() <= deriv*/))
+ {
+ itPrev--;
+ }
+
+ if (itPrev != ordView.end())
+ {
+ previous = getState(itPrev->timestamp, dimension, deriv);
+ }
+
+ // find next SystemState that exists for that dimension
+ while (itNext != ordView.end() && (itNext->data.at(dimension) == NULL /*|| itNext->data[dimension]->size() <= deriv*/))
+ {
+ itNext++;
+ }
+
+ if (itNext != ordView.end())
+ {
+ next = getState(itNext->timestamp, dimension, deriv);
+ }
+
+
+
+ if (itNext == ordView.end() && itPrev == ordView.end())
+ {
+ throw LocalException() << "Cannot find next or prev values in dim " << dimension << " at timestamp " << t;
+ }
+
+ if (itNext == ordView.end())
+ {
+ return getState(itPrev->timestamp, dimension, deriv) + getState(itPrev->timestamp, dimension, deriv + 1) * (t - itPrev->timestamp);
+ }
+ else if (itPrev == ordView.end())
+ {
+ return getState(itNext->timestamp, dimension, deriv) - getState(itNext->timestamp, dimension, deriv + 1) * (itNext->timestamp - t);
+ }
+ else
+ {
+ // linear interpolation
+
+ double distance = fabs(itNext->timestamp - itPrev->timestamp);
+ double weightPrev = fabs(t - itNext->timestamp) / distance;
+ double weightNext = fabs(itPrev->timestamp - t) / distance;
+
+ return weightNext * next + weightPrev * previous;
+
+ }
+ }
+ void Trajectory::gaussianFilter(double filterRadius)
+ {
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ Trajectory filteredTraj;
+ Ice::DoubleSeq timestamps = getTimestamps();
+
+ for (size_t d = 0; d < dim(); d++)
+ {
+ Ice::DoubleSeq entries;
+
+ for (typename ordered_view::iterator it = ordv.begin(); it != ordv.end(); it++)
+ {
+ entries.push_back(__gaussianFilter(filterRadius, it, d, 0));
+ }
+
+ filteredTraj.addDimension(timestamps, entries);
+ }
+
+ CopyData(filteredTraj, *this);
+ }
+
+
+
+ // gauss macro
+#define g(x) exp(-double((x)*(x))/(2*sigma*sigma)) / ( sigma * sqrt(2*3.14159265))
+#define round(x) int(0.5+x)
+
+
+ double
+ Trajectory::__gaussianFilter(double filterRadiusInTime, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim)
+ {
+ const ordered_view& ordView = dataMap.get<TagOrdered>();
+ // typename ordered_view::iterator start = centerIt;
+ // while(start != ordView.end() && start->timestamp > centerIt->timestamp - filterRadiusInTime)
+ // start--;
+ // if(start == ordView.end())
+ // start = ordView.begin();
+
+ // typename ordered_view::iterator end;
+ // while(end != ordView.end() && end->timestamp < centerIt->timestamp + filterRadiusInTime)
+ // end++;
+
+ const double sigma = filterRadiusInTime / 2.5;
+ //-log(0.05) / pow(itemsToCheck+1+centerIndex,2) * ( sqrt(1.0/h) * sqrt(2*3.14159265));
+
+ double weightedSum = 0;
+ double sumOfWeight = 0;
+ ordered_view::iterator start = centerIt;
+
+ const double sqrt2PI = sqrt(2 * M_PI);
+
+ for (int sign = -1; sign < 2; sign += 2)
+ {
+ for (ordered_view::iterator it = start;
+ it != ordView.end()
+ && fabs(it->timestamp - centerIt->timestamp) <= fabs(filterRadiusInTime * 2);
+ )
+ {
+ double value;
+ value = it->getDeriv(trajNum, dim);//data[trajNum]->at(dim);
+ double diff = (it->timestamp - centerIt->timestamp);
+ double squared = diff * diff;
+ const double gaussValue = exp(-squared / (2 * sigma * sigma)) / (sigma * sqrt2PI);
+ sumOfWeight += gaussValue;
+ weightedSum += gaussValue * value;
+
+ if (sign > 0)
+ {
+ it++;
+ }
+ else
+ {
+ it--;
+ }
+
+ checkValue(weightedSum);
+ }
+
+ start++;// skip center value in second loop iteration
+ }
+
+ double result;
+ result = weightedSum / sumOfWeight;
+ checkValue(result);
+ // centerIt->data[trajNum]->at(dim) = result;
+ return result;
+
+ }
+
+
+ void Trajectory::CopyData(const Trajectory& source, Trajectory& destination)
+ {
+ if (&source == &destination)
+ {
+ return;
+ }
+
+ destination.clear();
+ Ice::DoubleSeq timestamps = source.getTimestamps();
+
+ for (size_t dim = 0; dim < source.dim(); dim++)
+ {
+ destination.addDimension(timestamps, source.getDimensionData(dim));
+ }
+
+ }
+
+ void Trajectory::clear()
+ {
+ dataMap.erase(dataMap.begin(), dataMap.end());
+ dimensionNames.clear();
+ }
+
+
+
+ Ice::DoubleSeq Trajectory::GenerateTimestamps(double startTime, double endTime, double stepSize)
+ {
+ if (startTime >= endTime)
+ {
+ throw LocalException("startTime must be smaller than endTime.");
+ }
+
+ Ice::DoubleSeq result;
+ int size = int((endTime - startTime) / stepSize) + 1;
+ stepSize = (endTime - startTime) / (size - 1);
+ result.reserve(size);
+
+ double currentTimestamp = startTime;
+ int i = 0;
+
+ while (i < size)
+ {
+ result.push_back(currentTimestamp);
+ currentTimestamp += stepSize;
+ i++;
+ }
+
+ // if(*result.rbegin() != endTime)
+ // result.push_back(endTime);
+ return result;
+ }
+
+
+
+
+ Ice::DoubleSeq Trajectory::normalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime, const double endTime)
+ {
+ Ice::DoubleSeq normTimestamps;
+ normTimestamps.resize(timestamps.size());
+ const double minValue = *timestamps.begin();
+ const double maxValue = *timestamps.rbegin();
+ const double duration = maxValue - minValue;
+
+ for (size_t i = 0; i < timestamps.size(); i++)
+ {
+ normTimestamps[i] = startTime + (timestamps.at(i) - minValue) / duration * (endTime - startTime);
+ }
+
+ return normTimestamps;
+ }
+
+
+ Trajectory Trajectory::normalizeTimestamps(const Trajectory& traj, const double startTime, const double endTime)
+ {
+
+ if (traj.size() <= 1 || (traj.begin()->timestamp == startTime && traj.rbegin()->timestamp == endTime))
+ {
+ return traj; // already normalized
+ }
+
+
+ Ice::DoubleSeq timestamps = traj.getTimestamps();
+
+
+ Ice::DoubleSeq normTimestamps = normalizeTimestamps(timestamps, startTime, endTime);
+ Trajectory normExampleTraj;
+
+ for (size_t dim = 0; dim < traj.dim(); dim++)
+ {
+ Ice::DoubleSeq dimensionData = traj.getDimensionData(dim);
+ normExampleTraj.addDimension(normTimestamps, dimensionData);
+ }
+
+ return normExampleTraj;
+
+
+ }
+
+
+
+
+
+
+
+ size_t Trajectory::addDimension(const Ice::DoubleSeq& x, const Ice::DoubleSeq& y, const std::string name)
+ {
+
+ size_t newDim = dim() + 1;
+
+ __addDimension();
+
+ addPositionsToDimension(newDim - 1, x, y);
+ dimensionNames.push_back(name);
+ return newDim - 1;
+ }
+
+ void Trajectory::removeDimension(size_t dimension)
+ {
+ typename timestamp_view::iterator itMap = dataMap.begin();
+
+ for (; itMap != dataMap.end(); itMap++)
+ {
+ std::vector< DoubleSeqPtr >& data = itMap->data;
+
+ if (dimension < data.size())
+ {
+ data.erase(data.begin() + dimension);
+ }
+ }
+ if (dimension < dimensionNames.size())
+ {
+ dimensionNames.erase(dimensionNames.begin() + dimension);
+ }
+ }
+
+ void Trajectory::removeDerivation(size_t deriv)
+ {
+ typename timestamp_view::iterator itMap = dataMap.begin();
+
+ for (; itMap != dataMap.end(); itMap++)
+ {
+ std::vector< DoubleSeqPtr >& data = itMap->data;
+
+ for (size_t i = 0; i < data.size(); i++)
+ {
+ DoubleSeqPtr& vec = data.at(i);
+
+ if (deriv + 1 < vec->size())
+ {
+ vec->resize(deriv);
+ }
+ }
+ }
+ }
+
+ void Trajectory::removeDerivation(size_t dimension, size_t deriv)
+ {
+ typename timestamp_view::iterator itMap = dataMap.begin();
+
+ for (; itMap != dataMap.end(); itMap++)
+ {
+ std::vector< DoubleSeqPtr >& data = itMap->data;
+
+ if (data.size() > dimension && deriv + 1 < data.at(dimension)->size())
+ {
+ data.at(dimension)->resize(deriv);
+ }
+ }
+ }
+
+ Trajectory::ordered_view::const_iterator Trajectory::begin() const
+ {
+ return dataMap.get<TagOrdered>().begin();
+ }
+
+ Trajectory::ordered_view::const_iterator Trajectory::end() const
+ {
+ return dataMap.get<TagOrdered>().end();
+ }
+
+ Trajectory::ordered_view::const_reverse_iterator Trajectory::rbegin() const
+ {
+ return dataMap.get<TagOrdered>().rbegin();
+ }
+
+ Trajectory::ordered_view::const_reverse_iterator Trajectory::rend() const
+ {
+ return dataMap.get<TagOrdered>().rend();
+ }
+
+ std::vector<DoubleSeqPtr>& Trajectory::operator[](double timestamp)
+ {
+ return getStates(timestamp);
+ }
+
+
+
+ void Trajectory::addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& x, const Ice::DoubleSeq& y)
+ {
+ if (dimension >= dim() && dim() > 0)
+ {
+ addDimension(x, y);
+ }
+ else
+ {
+ BOOST_ASSERT(x.size() == y.size());
+
+ for (size_t i = 0; i < x.size(); ++i)
+ {
+ checkValue(x[i]);
+ checkValue(y[i]);
+ setPositionEntry(x[i], dimension, y[i]);
+ }
+ }
+ }
+
+
+ void Trajectory::addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs)
+ {
+ setEntries(t, dimension, derivs);
+ }
+
+ Trajectory::TrajData::TrajData(Trajectory& traj) :
+ trajectory(traj)
+ {
+
+ }
+
+ void Trajectory::shiftTime(double shift)
+ {
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itMap = ordv.begin();
+
+ for (; itMap != ordv.end(); itMap++)
+ {
+ //itMap->timestamp -= shift;
+ }
+ }
+
+ void Trajectory::shiftValue(const Ice::DoubleSeq& shift)
+ {
+ if (shift.size() > dim())
+ {
+ throw LocalException("dimension is out of range: ") << shift.size() << " actual dimensions: " << dim();
+ }
+
+
+ for (size_t dimension = 0; dimension < dim(); dimension++)
+ {
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itMap = ordv.begin();
+
+ for (; itMap != ordv.end(); itMap++)
+ {
+ itMap->data[dimension]->at(0) += shift[dimension];
+ }
+ }
+
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
new file mode 100644
index 0000000000000000000000000000000000000000..3573e5dd8c6006fcbf9ef834fd318df62c49e1fa
--- /dev/null
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -0,0 +1,327 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#ifndef ARMARX_ROBOTAPI_TRAJECTORY_H
+#define ARMARX_ROBOTAPI_TRAJECTORY_H
+
+
+#include <boost/tokenizer.hpp>
+#include <boost/unordered_map.hpp>
+#include <boost/multi_index_container.hpp>
+#include <boost/multi_index/hashed_index.hpp>
+#include <boost/multi_index/random_access_index.hpp>
+#include <boost/multi_index/ordered_index.hpp>
+#include <boost/multi_index/member.hpp>
+#include <boost/typeof/typeof.hpp>
+#include <boost/shared_ptr.hpp>
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+
+#include <Eigen/Core>
+
+
+#include <RobotAPI/interface/core/Trajectory.h>
+namespace armarx
+{
+
+ namespace VariantType
+ {
+ // Variant types
+ const VariantTypeId Trajectory = Variant::addTypeName("::armarx::TrajectoryBase");
+ }
+
+ typedef boost::shared_ptr<Ice::DoubleSeq> DoubleSeqPtr;
+
+ class Trajectory;
+ typedef IceInternal::Handle<Trajectory> TrajectoryPtr;
+
+ /**
+ * @class The Trajectory class represents n-dimensional sampled trajectories.
+ * @ingroup VariantsGrp
+ *
+ * The class interpolates all missing values with linear interpolation and calculates derivations.
+ * This means dimensions with different timestamps can easily be used.
+ * Access complexity for a query with a timestamp is like from an unordered_map
+ *
+ * Basic usage:
+ * \verbatim
+ * Ice::DoubleSeq myPositionValues // fill with your values;
+ * Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1); // if you dont have timestamps
+ * Trajectory trajectory;
+ * trajectory.addDimension(timestamps, myPositionValues, "Shoulder 1 L");
+ *
+ * double timestamp = 0.1;
+ * double dimension = 0;
+ * double derivation = 0;
+ * double pos = trajectory.getState(timestamp, dimension, derivation);
+ * // or
+ * Ice::DoubleSeq positions = getDimensionData(dimension, derivation);
+ * \endverbatim
+ */
+ class Trajectory : public TrajectoryBase
+ {
+ public:
+ // Object interface
+ void ice_preMarshal();
+ void ice_postUnmarshal();
+
+ // Serializable interface
+ void serialize(const ObjectSerializerBasePtr& obj, const Ice::Current& = Ice::Current()) const;
+ void deserialize(const ObjectSerializerBasePtr&, const Ice::Current& = Ice::Current());
+
+ // VariantDataClass interface
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::Current()) const;
+ std::string output(const Ice::Current& c = Ice::Current()) const;
+ Ice::Int getType(const Ice::Current& c = Ice::Current()) const;
+ bool validate(const Ice::Current& c = Ice::Current());
+
+ Ice::ObjectPtr ice_clone() const;
+
+
+ typedef std::map<double, std::vector< DoubleSeqPtr > > TrajMap;
+
+
+ struct TrajData
+ {
+ TrajData(Trajectory& traj);
+ DoubleSeqPtr operator[](size_t dim)
+ {
+ return data.at(dim);
+ }
+ inline double getTimestamp() const
+ {
+ return timestamp;
+ }
+ inline double getPosition(int dim) const
+ {
+ return getDeriv(dim, 0);
+ }
+ inline double getDeriv(size_t dim, size_t derivation) const
+ {
+ return trajectory.getState(timestamp, dim, derivation);
+ }
+ inline const std::vector< DoubleSeqPtr >& getData() const
+ {
+ return data;
+ }
+
+ double timestamp;
+ mutable std::vector< DoubleSeqPtr > data; // mutable so that it can be changed
+ Trajectory& trajectory;
+ friend class Trajectory;
+
+ };
+
+ // structs for indices
+ struct TagTimestamp {};
+ struct TagOrdered {};
+
+
+ // Trajectory data container
+ 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> > // this index represents timestamp incremental order
+ >
+ > TrajDataContainer;
+ typedef typename boost::multi_index::index<TrajDataContainer, TagTimestamp>::type timestamp_view;
+ typedef typename boost::multi_index::index<TrajDataContainer, TagOrdered>::type ordered_view;
+
+
+
+ Trajectory() {}
+ Trajectory(const Trajectory& source);
+ Trajectory(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, const std::string& dimensionName = "");
+
+ /**
+ * Constructor to add n-dimensions with m-values.
+ * @param data data vector with positions. Dimension 1 is the joint dimension and Dimension is the position of that joint.
+ * @param 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.
+ *
+ */
+ template <typename T>
+ Trajectory(const std::vector<std::vector<T>>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const Ice::StringSeq& dimensionNames = "")
+ {
+ if (data.size() == 0)
+ {
+ return;
+ }
+ Ice::DoubleSeq generatedTimestamps;
+ ARMARX_CHECK_EXPRESSION(timestamps.size() == 0 || data.at(0).size() == timestamps.size());
+ ARMARX_CHECK_EXPRESSION(data.size() == dimensionNames.size());
+ if (timestamps.size() == 0)
+ {
+ generatedTimestamps = GenerateTimestamps(0, 1, 1.0 / (data.at(0).size() - 1));
+ }
+ const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : generatedTimestamps;
+ size_t i = 0;
+ for (const auto & subvec : data)
+ {
+ Ice::DoubleSeq dvec(subvec.begin(), subvec.end());
+ addDimension(tempTimestamps, dvec, dimensionNames.at(i++));
+ }
+ }
+
+ Trajectory(const std::map<double, Ice::DoubleSeq>& data);
+
+
+ Trajectory& operator=(const Trajectory& source);
+
+
+ size_t addDimension(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, const std::string name = "");
+ void addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& x, const Ice::DoubleSeq& y);
+ void addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs);
+ Trajectory& operator+=(const Trajectory traj);
+ void removeDimension(size_t dimension);
+ void removeDerivation(size_t deriv);
+ void removeDerivation(size_t dimension, size_t deriv);
+
+ // iterators
+ typename ordered_view::const_iterator begin() const;
+ typename ordered_view::const_iterator end() const;
+ typename ordered_view::const_reverse_iterator rbegin() const;
+ typename ordered_view::const_reverse_iterator rend() const;
+
+ ///// getters
+ std::vector<DoubleSeqPtr>& operator[](double timestamp);
+ Ice::DoubleSeq getTimestamps() const;
+ std::vector<DoubleSeqPtr>& getStates(double t);
+ std::vector<DoubleSeqPtr> getStates(double t) const;
+ Ice::DoubleSeq getStates(double t, size_t derivation) const;
+ template<typename T>
+ std::map<std::string, T> getStatesMap(double t, size_t derivation = 0) const
+ {
+ std::map<std::string, T> result;
+ size_t dimensions = dim();
+
+ for (size_t dim = 0; dim < dimensions; dim++)
+ {
+ result[getDimensionName(dim)] = getState(t, dim, derivation);
+ }
+
+ return result;
+ }
+
+ double getState(double t, size_t dim = 0, size_t deriv = 0);
+ double getState(double t, size_t dim = 0, size_t deriv = 0) const;
+ Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const;
+ std::string getDimensionName(size_t dim) const;
+ Ice::StringSeq getDimensionNames() const;
+
+ TrajDataContainer& data();
+
+ /**
+ * @brief getDimensionData gets all entries for one dimensions with order of increasing timestamps
+ * @param dimension
+ * @return
+ */
+ // Ice::DoubleSeq getDimensionData(size_t dimension = 0) const;
+ Ice::DoubleSeq getDimensionData(size_t dimension, size_t deriv = 0) const;
+ Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation) const;
+ Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Eigen::MatrixXd toEigen(size_t derivation, double startTime, double endTime) const;
+ Eigen::MatrixXd toEigen(size_t derivation = 0) const;
+ Trajectory getPart(double startTime, double endTime, size_t numberOfDerivations = 0) const;
+ /**
+ * @brief dim returns the trajectory dimension count for this trajectory (e.g. taskspace w/o orientation would be 3)
+ * @return
+ */
+ size_t dim() const;
+ size_t size() const;
+ double getTimeLength() const;
+ double getSpaceLength(size_t dimension, size_t stateDim) const;
+ double getSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ double getSquaredSpaceLength(size_t dimension, size_t stateDim) const;
+ double getSquaredSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ double getMax(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ double getMin(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ double getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ double getTrajectorySpace(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ Ice::DoubleSeq getMinima(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ Ice::DoubleSeq getMaxima(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ Ice::DoubleSeq getMinimaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ Ice::DoubleSeq getMaximaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+
+ // calculations
+
+ Ice::DoubleSeq getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const;
+ static Ice::DoubleSeq DifferentiateDiscretly(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, int derivationCount = 1);
+ double getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t deriv) const;
+
+ void differentiateDiscretlyForDim(size_t trajDimension, size_t derivation);
+ void differentiateDiscretly(size_t derivation);
+ // void differentiateDiscretly( size_t sourceDimOfSystemState, size_t targetDimOfSystemState);
+
+ void reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState);
+
+ /**
+ * @brief negateDim changes the sign of all values of the given dimension.
+ */
+ void negateDim(size_t trajDimension);
+ /**
+ * @brief gaussianFilter smoothes the trajectory
+ * @param filterRadius, in easy words: time range to left and right from center that
+ * influences the resulting center value
+ */
+ void gaussianFilter(double filterRadius);
+
+ static Trajectory normalizeTimestamps(const Trajectory& traj, const double startTime = 0.0, const double endTime = 1.0);
+ static Ice::DoubleSeq normalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime = 0.0, const double endTime = 1.0);
+ static Ice::DoubleSeq GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
+ void shiftTime(double shift);
+ void shiftValue(const Ice::DoubleSeq& shift);
+
+
+ static void CopyData(const Trajectory& source, Trajectory& destination);
+ void clear();
+ void setPositionEntry(const double t, const size_t dimIndex, const double& y);
+ void setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y);
+ bool dataExists(double t, size_t dimension = 0, size_t deriv = 0) const;
+
+
+ std::map<double, Ice::DoubleSeq> getStatesAround(double t, size_t derivation, size_t extend) const;
+
+ protected:
+ void __addDimension();
+ void __fillAllEmptyFields();
+ double __interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t deriv) const;
+ double __gaussianFilter(double filterRadius , typename ordered_view::iterator centerIt , size_t trajNum, size_t dim);
+ timestamp_view::iterator __fillBaseDataAtTimestamp(const double& t);
+ std::vector<DoubleSeqPtr> __calcBaseDataAtTimestamp(const double& t) const;
+
+
+ TrajDataContainer dataMap;
+
+ virtual double __interpolate(double t, ordered_view::const_iterator itPrev, ordered_view::const_iterator itNext, size_t dimension, size_t deriv) const;
+ double __interpolate(double t, size_t dimension, size_t deriv) const;
+
+
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/core/VectorHelpers.h b/source/RobotAPI/libraries/core/VectorHelpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..04fbfd27e9473ca1f6affeca99b638b4541440f0
--- /dev/null
+++ b/source/RobotAPI/libraries/core/VectorHelpers.h
@@ -0,0 +1,322 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#ifndef VECTORHELPERS_H
+#define VECTORHELPERS_H
+
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+#include <eigen3/Eigen/Geometry>
+#include <cmath>
+#include <map>
+#include <vector>
+#include <limits>
+
+namespace armarx
+{
+ template<class T>
+ inline void checkValue(const T& value)
+ {
+ if (std::numeric_limits<T>::infinity() == value)
+ {
+ throw LocalException("result value is inf");
+ }
+
+ if (value != value)
+ {
+ throw LocalException("result value is nan");
+ }
+ }
+
+ template<class T>
+ inline void checkValues(const std::vector<T >& values)
+ {
+ for (size_t i = 0; i < values.size(); ++i)
+ {
+ checkValue(values[i]);
+ }
+ }
+
+
+
+ template <class T> std::vector<T> operator -(const std::vector<T>& v1, const std::vector<T>& v2)
+ {
+ std::vector<T> res(std::min(v1.size(), v2.size()), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = v1[j] - v2[j];
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> std::vector<T> operator +(const std::vector<T>& v1, const std::vector<T>& v2)
+ {
+ std::vector<T> res(std::min(v1.size(), v2.size()), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = v1[j] + v2[j];
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> std::vector<T>& operator +=(std::vector<T>& v1, const std::vector<T>& v2)
+ {
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = v1.begin(); i != v1.end(); ++i)
+ {
+ *i += v2[j];
+ ++j;
+ }
+
+ return v1;
+ }
+
+
+ template <class T> std::vector<T>& operator -=(std::vector<T>& v1, const std::vector<T>& v2)
+ {
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = v1.begin(); i != v1.end(); ++i)
+ {
+ *i -= v2[j];
+ ++j;
+ }
+
+ return v1;
+ }
+
+ template <class T> std::vector<T>& operator *=(std::vector<T>& v1, double c)
+ {
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = v1.begin(); i != v1.end(); ++i)
+ {
+ *i *= c;
+ ++j;
+ }
+
+ return v1;
+ }
+
+
+
+ template <class T> std::vector<T>& operator /=(std::vector<T>& v1, double c)
+ {
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = v1.begin(); i != v1.end(); ++i)
+ {
+ *i /= c;
+ ++j;
+ }
+
+ return v1;
+ }
+
+ template <class T> std::vector<T> operator *(double c, const std::vector<T>& v)
+ {
+ std::vector<T> res(v.size(), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = c * v[j];
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> double operator *(const std::vector<T>& v, const std::vector<T>& v2)
+ {
+ double res = 0;
+
+ if (v.size() != v2.size())
+ {
+ throw LocalException("vectors do not match in size: ") << v.size() << " vs. " << v2.size();
+ }
+
+ int j = 0;
+
+ for (typename std::vector<T>::const_iterator i = v.begin(); i != v.end(); ++i)
+ {
+ res += *i * v2[j];
+ ++j;
+ }
+
+ return res;
+ }
+
+
+ template <class T> std::vector<T> operator +(const std::vector<T>& v, double s)
+ {
+ std::vector<T> res(v.size(), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = v[j] + s;
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> std::vector<T> operator -(const std::vector<T>& v, double s)
+ {
+ std::vector<T> res(v.size(), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = v[j] - s;
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> double vecLength(const std::vector<T>& v)
+ {
+ double result = 0.0;
+
+ for (typename std::vector<T>::const_iterator it = v.begin(); it != v.end(); it++)
+ {
+ result += *it** it;
+ }
+
+ return sqrt(result);
+ }
+
+ template <class T> double vecSum(const std::vector<T>& v)
+ {
+ double result = 0.0;
+
+ for (typename std::vector<T>::const_iterator it = v.begin(); it != v.end(); it++)
+ {
+ result += *it;
+ }
+
+ return result;
+ }
+
+ template <class T> std::vector<T> normalizedVec(const std::vector<T>& v)
+ {
+ double length = vecLength(v);
+ std::vector<T> result = v;
+ result /= length;
+ return result;
+ }
+
+ template <class T> std::vector<T> operator /(const std::vector<T>& v, double c)
+ {
+ std::vector<T> res(v.size(), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = v[j] / c;
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> std::vector<T> abs(const std::vector<T>& v)
+ {
+ std::vector<T> res(v.size(), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = fabs(v[j]);
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> std::vector<T> max(const std::vector<T>& v1, const std::vector<T>& v2)
+ {
+ std::vector<T> res(std::min(v1.size(), v2.size()), T());
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = std::max(v1[j], v2[j]);
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> T max(const std::vector<T>& v1)
+ {
+ T maxValue = std::numeric_limits<T>::min();
+
+ for (typename std::vector<T>::const_iterator i = v1.begin(); i != v1.end(); ++i)
+ {
+ maxValue = std::max(maxValue, *i);
+
+ }
+
+ return maxValue;
+ }
+
+ template <class T> std::vector<T> min(const std::vector<T>& v1, const std::vector<T>& v2)
+ {
+ std::vector<T> res(std::min(v1.size(), v2.size()));
+ int j = 0;
+
+ for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
+ {
+ *i = std::min(v1[j], v2[j]);
+ ++j;
+ }
+
+ return res;
+ }
+
+ template <class T> T min(const std::vector<T>& v1)
+ {
+ T minValue = std::numeric_limits<T>::max();
+
+ for (typename std::vector<T>::const_iterator i = v1.begin(); i != v1.end(); ++i)
+ {
+ minValue = std::min(minValue, *i);
+
+ }
+
+ return minValue;
+ }
+}
+#endif //
diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..35d29e49c514f5f7e957e937b8d33eeb034cab0a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(LIBS ${LIBS} RobotAPICore )
+
+armarx_add_test(TrajectoryTest TrajectoryTest.cpp "${LIBS}")
+
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65289894b8f0065fb63d782e6852f651985f2703
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -0,0 +1,222 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::RobotAPI::Test
+* @author Nils Adermann (naderman at naderman dot de)
+* @date 2010
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#define BOOST_TEST_MODULE RobotAPI::Trajectory::Test
+#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+#include "../Trajectory.h"
+using namespace armarx;
+
+
+//BOOST_AUTO_TEST_CASE(testTrajectoryLoading)
+//{
+// // Load the trajectory
+// Trajectory traj;
+// // traj.readFromCSVFile("pouring_x_short.csv");
+// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// BOOST_CHECK(traj.begin()->getPosition(0) == 445.35);
+// traj = Trajectory::normalizeTimestamps(traj, 0, 1);
+// Ice::DoubleSeq timestamps = traj.getTimestamps();
+// traj.differentiateDiscretly(2);
+// Ice::DoubleSeq d0 = traj.getDimensionData(0, 0);
+// Ice::DoubleSeq d1 = traj.getDimensionData(0, 1);
+// BOOST_CHECK(traj.begin()->getDeriv(0, 1) == d1.at(0));
+// std::cout << "end" << std::endl;
+//}
+
+//#define SIGN_FACTOR(x) ((x>=0)?1:-1)
+
+//BOOST_AUTO_TEST_CASE(testExtrapolation)
+//{
+// // Load the trajectory
+// Trajectory traj;
+// // traj.readFromCSVFile("pouring_x_short.csv");
+// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+
+// // traj = Trajectory::normalizeTimestamps(traj,0,1);
+// Ice::DoubleSeq timestamps = traj.getTimestamps();
+// traj.differentiateDiscretly(2);
+// double first = traj.begin()->getPosition(0);
+// double derivFirst = traj.begin()->getDeriv(0, 1);
+// double before = traj.getState(timestamps.at(0) - 5, 0, 0);
+// BOOST_CHECK(before < SIGN_FACTOR(derivFirst) * first);
+// std::cout << "end" << std::endl;
+//}
+
+//BOOST_AUTO_TEST_CASE(testInplaceInterpolation)
+//{
+// // Load the trajectory
+// Trajectory traj;
+// // traj.readFromCSVFile("pouring_x_short.csv");
+// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// Ice::DoubleSeq timestamps = traj.getTimestamps();
+// double middle = (timestamps.at(1) + timestamps.at(0)) * 0.5;
+// double before = traj.getState(timestamps.at(0));
+// double after = traj.getState(timestamps.at(1));
+// double interpolation = traj.getState(middle);
+// BOOST_CHECK(before < interpolation && after > interpolation);
+// std::cout << "end" << std::endl;
+//}
+
+//BOOST_AUTO_TEST_CASE(testDerivation)
+//{
+// // Load the trajectory
+// Trajectory traj;
+// // traj.readFromCSVFile("pouring_x_short.csv");
+// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// traj.differentiateDiscretly(1);
+// traj.differentiateDiscretly(2);
+// Trajectory traj2;
+// traj2.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// traj2.differentiateDiscretly(2);
+
+// BOOST_CHECK_EQUAL(traj.begin()->getDeriv(0, 2), traj2.begin()->getDeriv(0, 2));
+// BOOST_CHECK_EQUAL(traj.rbegin()->getDeriv(0, 2), traj2.rbegin()->getDeriv(0, 2));
+// BOOST_CHECK_EQUAL(traj.rbegin()->getDeriv(0, 1), traj2.rbegin()->getDeriv(0, 1));
+// double middleTimestamp = traj.getTimestamps().at(0) + traj.getTimeLength() / 2;
+// BOOST_CHECK_EQUAL(traj.getState(middleTimestamp, 0, 2), traj2.getState(middleTimestamp, 0, 2));
+// BOOST_CHECK_EQUAL(traj.getState(middleTimestamp, 0, 1), traj2.getState(middleTimestamp, 0, 1));
+// std::cout << "end" << std::endl;
+//}
+
+
+
+//BOOST_AUTO_TEST_CASE(testInplaceDerivation)
+//{
+// // Load the trajectory
+// Trajectory traj;
+// // traj.readFromCSVFile("pouring_x_short.csv");
+// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// double d2 = traj.begin()->getDeriv(0, 2);
+// BOOST_CHECK(d2 != 0);
+// std::cout << "end" << std::endl;
+//}
+
+BOOST_AUTO_TEST_CASE(testInplaceDerivation2)
+{
+ // Load the trajectory
+ Ice::DoubleSeq timestamps { 0, 1, 2 };
+ Ice::DoubleSeq positions { 1, 2, 5 };
+
+ Trajectory traj(timestamps, positions);
+ Trajectory traj2(timestamps, positions);
+ traj.differentiateDiscretly(1);
+ BOOST_CHECK_EQUAL(traj.getState(-1, 0, 1), traj2.getState(-1, 0, 1));
+}
+
+BOOST_AUTO_TEST_CASE(testInplaceDerivation3)
+{
+ // Load the trajectory
+ Ice::DoubleSeq timestamps { 0, 1, 2 };
+ Ice::DoubleSeq positions { 1, 2, 5 };
+
+ Trajectory traj(timestamps, positions);
+ Trajectory traj2(timestamps, positions);
+ traj.differentiateDiscretly(2);
+ BOOST_CHECK_EQUAL(traj.getState(-1, 0, 2), traj2.getState(-1, 0, 2));
+}
+
+BOOST_AUTO_TEST_CASE(testDerivationRemoval)
+{
+ // Load the trajectory
+ Ice::DoubleSeq timestamps { 0, 1, 2 };
+ Ice::DoubleSeq positions { 1, 2, 5 };
+
+ Trajectory traj(timestamps, positions);
+ // Trajectory traj2(timestamps, positions);
+ traj.differentiateDiscretly(2);
+ BOOST_CHECK_EQUAL(traj.begin()->getData().at(0)->size(), 3);
+ traj.removeDerivation(1);
+ BOOST_CHECK_EQUAL(traj.begin()->getData().at(0)->size(), 1);
+}
+
+
+
+BOOST_AUTO_TEST_CASE(TrajectoryMarshalTest)
+{
+ // Load the trajectory
+ Ice::DoubleSeq timestamps { 0, 1, 2 };
+ Ice::DoubleSeq positions { 1, 2, 5 };
+
+ Trajectory traj(timestamps, positions);
+ traj.ice_preMarshal();
+ TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj.clone());
+ traj2->clear();
+ traj2->ice_postUnmarshal();
+ BOOST_CHECK_EQUAL(traj.getState(0), traj2->getState(0));
+}
+
+BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
+{
+ // Load the trajectory
+ Ice::DoubleSeq timestamps;
+ int count = 1000;
+ for (int i = 0; i < count; ++i) {
+ timestamps.push_back(rand());
+ }
+ Trajectory traj;
+ for (int var = 0; var < 8; ++var) {
+ Ice::DoubleSeq p;
+ p.reserve(count);
+ for (int i = 0; i < count; ++i) {
+ p.push_back(rand());
+ }
+ traj.addDimension(timestamps, p);
+ }
+ ARMARX_INFO_S << "Starting";
+ traj.ice_preMarshal();
+ ARMARX_INFO_S << "Marshalled";
+ TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj.clone());
+ ARMARX_INFO_S << "Cloned";
+
+ traj2->clear();
+ ARMARX_INFO_S << "Cleared";
+ traj2->ice_postUnmarshal();
+ ARMARX_INFO_S << "Unmarshalled";
+
+ BOOST_CHECK_EQUAL(traj.getState(0), traj2->getState(0));
+}
+
+
+BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
+{
+ // Load the trajectory
+ FloatSeqSeq positions {{ 1, 2, 5 }, { 10, 2332, 53425 }};
+ JSONObjectPtr json = new JSONObject();
+ Trajectory traj(positions, {}, {"joint1", "joint2"});
+ traj.serialize(json);
+ ARMARX_INFO_S << json->asString(true);
+ Trajectory traj2;
+ traj2.deserialize(json);
+ BOOST_CHECK_CLOSE(traj.getState(0), traj2.getState(0), 0.01);
+ BOOST_CHECK_CLOSE(traj.getState(0.4,1), traj2.getState(0.4,1), 0.01);
+ BOOST_CHECK_EQUAL(traj.getDimensionName(0), traj2.getDimensionName(0));
+}
+
+