diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 64cd1ac6dcffd96674bcc732183fa9ca3de8b73c..a8f71316e4e8ffc6748ab2f7eef6498f589aee2a 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -131,9 +131,11 @@ namespace armarx
 
     std::string Trajectory::output(const Ice::Current&) const
     {
+
         const ordered_view& ordv = dataMap.get<TagOrdered>();
         typename ordered_view::const_iterator itMap = ordv.begin();
         std::stringstream s;
+        s << "Dimensions names: " << dimensionNames;
         for (; itMap != ordv.end(); itMap++)
         {
             s << *itMap << std::endl;
@@ -1619,21 +1621,30 @@ namespace armarx
         }
 
         destination.clear();
+
         Ice::DoubleSeq timestamps = source.getTimestamps();
 
         for (size_t dim = 0; dim < source.dim(); dim++)
         {
-            destination.addDimension(source.getDimensionData(dim), timestamps);
+            destination.addDimension(source.getDimensionData(dim), timestamps
+                                    );
         }
+        CopyMetaData(source, destination);
+    }
+
+    void Trajectory::CopyMetaData(const Trajectory& source, Trajectory& destination)
+    {
         destination.setDimensionNames(source.getDimensionNames());
 
         destination.setLimitless(source.getLimitless());
     }
 
+
     void Trajectory::clear()
     {
         dataMap.erase(dataMap.begin(), dataMap.end());
         dimensionNames.clear();
+        limitless.clear();
     }
 
 
@@ -1646,12 +1657,12 @@ namespace armarx
         }
 
         Ice::DoubleSeq result;
-        int size = int((endTime - startTime) / stepSize) + 1;
+        size_t size = int((endTime - startTime) / stepSize) + 1;
         stepSize = (endTime - startTime) / (size - 1);
         result.reserve(size);
 
         double currentTimestamp = startTime;
-        int i = 0;
+        size_t i = 0;
 
         while (i < size)
         {
@@ -1659,7 +1670,7 @@ namespace armarx
             currentTimestamp += stepSize;
             i++;
         }
-
+        ARMARX_CHECK_EQUAL_W_HINT(result.size(), size, VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize));
         return result;
     }
 
@@ -1890,12 +1901,18 @@ namespace armarx
     {
         const ordered_view& ordv = dataMap.get<TagOrdered>();
         typename ordered_view::const_iterator itMap = ordv.begin();
-
+        Trajectory shiftedTraj;
+        CopyMetaData(*this, shiftedTraj);
+        auto d = dim();
         for (; itMap != ordv.end(); itMap++)
         {
-            // since all values are shifted this operation is OK
-            *const_cast<double*>(&(itMap->timestamp)) += shift;
+            for (size_t i = 0; i < d; ++i)
+            {
+                shiftedTraj.setEntries(itMap->timestamp + shift, i, *itMap->getData().at(i));
+            }
         }
+        //        dataMap.swap(shiftedTraj.dataMap);
+        *this = shiftedTraj;
     }
 
     void Trajectory::shiftValue(const Ice::DoubleSeq& shift)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index f7a88f977d472ca15e996a5664c2c8f27e2d61c9..03ddd3972f399c02b13c00b140ef2c423633796a 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -394,6 +394,8 @@ namespace armarx
 
 
         static void CopyData(const Trajectory& source, Trajectory& destination);
+        static void CopyMetaData(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);