diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index c3f8a56c719298d92f49e512c776a738ad33ca8e..50058ae5f98b821c4a97100034839128f39ad587 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -1487,7 +1487,7 @@ namespace armarx
 
         for (size_t dim = 0; dim < source.dim(); dim++)
         {
-            destination.addDimension(source.getDimensionData(dim), timestamps);
+            destination.addDimension(source.getDimensionData(dim), timestamps, source.getDimensionName(dim));
         }
 
     }
@@ -1583,20 +1583,20 @@ namespace armarx
 
         const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(values);
 
-        size_t newDim = dim() + 1;
+        size_t newDimIndex = dim();
 
         __addDimension();
 
-        addPositionsToDimension(newDim - 1, values, tempTimestamps);
-        if (dimensionNames.size() > newDim - 1)
+        addPositionsToDimension(newDimIndex, values, tempTimestamps);
+        if (newDimIndex < dimensionNames.size())
         {
-            dimensionNames.at(newDim - 1) = name;
+            dimensionNames.at(newDimIndex) = name;
         }
         else
         {
             dimensionNames.push_back(name);
         }
-        return newDim - 1;
+        return newDimIndex;
     }
 
     void Trajectory::removeDimension(size_t dimension)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 835d04d428ed8e979b1dcac9929340076445e195..2f54847e8c2cb9a7c5ea7d0d28ecf0cd6c919dea 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -111,6 +111,50 @@ namespace armarx
 
             double getTimestamp() const;
             double getPosition(size_t dim) const;
+
+            template<class T = double>
+            std::vector<T> getPositionsAs() const
+            {
+                if (!trajectory)
+                {
+                    throw LocalException("Ptr to trajectory is NULL");
+                }
+                auto numDim = trajectory->dim();
+                std::vector<T> result;
+                result.reserve(numDim);
+                for (std::size_t i = 0; i < numDim; ++i)
+                {
+                    result.emplace_back(getPosition(i));
+                }
+                return result;
+            }
+            std::vector<double> getPositions() const
+            {
+                return getPositionsAs<double>();
+            }
+
+            template<class T>
+            void writePositionsToNameValueMap(std::map<std::string, T>& map) const
+            {
+                if (!trajectory)
+                {
+                    throw LocalException("Ptr to trajectory is NULL");
+                }
+                auto numDim = trajectory->dim();
+                for (std::size_t i = 0; i < numDim; ++i)
+                {
+                    map[trajectory->getDimensionName(i)] = getPosition(i);
+                }
+            }
+            template<class T>
+            std::map<std::string, T> getPositionsAsNameValueMap() const
+            {
+                std::map<std::string, T> result;
+                writePositionsToNameValueMap(result);
+                return result;
+            }
+
+
             double getDeriv(size_t dim, size_t derivation) const;
             const std::vector< DoubleSeqPtr >& getData() const;
 
@@ -169,7 +213,7 @@ namespace armarx
 
         /**
          * Constructor to add n-dimensions with m-values.
-         * @param data data vector (size n) with positions (size m). Dimension 1 is the joint dimension and Dimension is the position of that joint.
+         * @param data data vector (size n) with positions (size m). Dimension 1 is the joint dimension and Dimension 2 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.
          *
@@ -186,7 +230,7 @@ namespace armarx
             for (const auto & subvec : data)
             {
                 Ice::DoubleSeq dvec(subvec.begin(), subvec.end());
-                addDimension(dvec, tempTimestamps, dimensionNames.size() > i - 1 ? dimensionNames.at(i++) : "");
+                addDimension(dvec, tempTimestamps, i < dimensionNames.size() ? dimensionNames.at(i++) : "");
             }
         }