diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index ffd7288aae77878a2b49853aaa14fb5c808d7e51..64cd1ac6dcffd96674bcc732183fa9ca3de8b73c 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -276,7 +276,7 @@ namespace armarx
         {
             if (derivation == 0)
             {
-                return __calcBaseDataAtTimestamp(t).at(dim)->at(derivation);    // interpolates and retrieves
+                return __interpolate(t, dim, derivation);
             }
             else
             {
@@ -336,7 +336,7 @@ namespace armarx
         return dimensionNames.at(dim);
     }
 
-    Ice::StringSeq Trajectory::getDimensionNames() const
+    const Ice::StringSeq& Trajectory::getDimensionNames() const
     {
         return dimensionNames;
     }
@@ -821,7 +821,6 @@ namespace armarx
             double newValue = __interpolate(t, dimension, 0);
             checkValue(newValue);
             result.push_back(DoubleSeqPtr(new Ice::DoubleSeq(1, newValue)));
-
         }
 
         return result;
@@ -1210,7 +1209,7 @@ namespace armarx
     {
         if (derivation == 0)
         {
-            getState(t, trajDimension, derivation);
+            return getState(t, trajDimension, derivation);
         }
 
         typename timestamp_view::iterator it = dataMap.find(t);
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 15cef1251f47b3f1c7d782b4dc54c1aea4e96217..f7a88f977d472ca15e996a5664c2c8f27e2d61c9 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -162,7 +162,7 @@ namespace armarx
                 stream << rhs.timestamp << ": ";
                 for (size_t d = 0; d < rhs.data.size(); ++d)
                 {
-                    stream << (rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
+                    stream << (rhs.data[d] && rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
                     if (d <= rhs.data.size() - 1)
                     {
                         stream << ", ";
@@ -296,7 +296,7 @@ namespace armarx
         std::vector<Ice::DoubleSeq > getAllStates(double t, int maxDeriv = 1);
         Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations)  const;
         std::string getDimensionName(size_t dim) const;
-        Ice::StringSeq getDimensionNames() const;
+        const Ice::StringSeq& getDimensionNames() const;
 
         TrajDataContainer& data();