diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index b2908d816fe224cd3f8b5af575c8d95b4ba27b14..749ad8f3195e5f544e6e25532257f1527a78cee8 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -8,7 +8,35 @@
 
 namespace armarx::nav::server
 {
-    Navigator::Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor) :
+
+    // Stack result
+    core::TrajectoryPtr StackResult::trajectory() const
+    {
+        if (localTrajectory)
+        {
+            return localTrajectory;
+        }
+
+        ARMARX_CHECK_NOT_NULL(globalTrajectory);
+        return globalTrajectory;
+    }
+
+    core::Twist StackResult::velocity() const
+    {
+        if (safeVelocity.has_value())
+        {
+            return safeVelocity.value();
+        }
+
+        ARMARX_CHECK(controlVelocity.has_value());
+        return *controlVelocity;
+    }
+
+    // Navigator
+
+    Navigator::Navigator(const server::NavigationStack& navigationStack,
+                         const core::Scene& scene,
+                         ExecutorInterface& executor) :
         stack(navigationStack), scene(scene), executor{executor}
     {
         setTag("Navigator");
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 783d1accb82b7283afb7f1168b3671400b652251..848daae375654212fbad448d03b9b9cbac662b10 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -43,27 +43,8 @@ namespace armarx::nav::server
         std::optional<core::Twist> controlVelocity;
         std::optional<core::Twist> safeVelocity;
 
-        core::TrajectoryPtr trajectory() const
-        {
-            if (localTrajectory)
-            {
-                return localTrajectory;
-            }
-
-            ARMARX_CHECK_NOT_NULL(globalTrajectory);
-            return globalTrajectory;
-        }
-
-        core::Twist velocity() const
-        {
-            if (safeVelocity.has_value())
-            {
-                return safeVelocity.value();
-            }
-
-            ARMARX_CHECK(controlVelocity.has_value());
-            return *controlVelocity;
-        }
+        core::TrajectoryPtr trajectory() const;
+        core::Twist velocity() const;
     };
 
     class Navigator : public armarx::Logging