diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index e60cc68e3c956db76d17c11ca6cba56bf806c46a..6935c3bdbe29f97d35f45ff778654e0f909f91b7 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -71,7 +71,7 @@ namespace armarx::nav::components
         updateContext();
 
         server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
-        server::Navigator navigator{stack, ctx};
+        server::Navigator navigator{stack, ctx, executor};
 
         SimplePeriodicTask<> task(
             [](){navigator.updateContext()}
diff --git a/source/Navigation/components/Navigator/Navigator.h b/source/Navigation/components/Navigator/Navigator.h
index 5d4d64ae60958df3116e7c4564f1abced2547c9a..2ef0bb185dbb7aeb7f4b94972092d2148f9f7fc5 100644
--- a/source/Navigation/components/Navigator/Navigator.h
+++ b/source/Navigation/components/Navigator/Navigator.h
@@ -103,7 +103,7 @@ namespace armarx::nav::components
     private:
         // TODO update context periodically
 
-        core::Context ctx;
+        core::Scene ctx;
 
         PlatformUnitListenerPrx platformUnit;
     };
diff --git a/source/Navigation/libraries/CMakeLists.txt b/source/Navigation/libraries/CMakeLists.txt
index 80281ff0d41208162324e3104c7e9902c0ef4632..308fb0d579ac797b75e8011cad781828776dac24 100644
--- a/source/Navigation/libraries/CMakeLists.txt
+++ b/source/Navigation/libraries/CMakeLists.txt
@@ -8,4 +8,4 @@ add_subdirectory(algorithms)
 add_subdirectory(factories)
 add_subdirectory(safety_control)
 add_subdirectory(server)
-add_subdirectory(util)
\ No newline at end of file
+add_subdirectory(util)
diff --git a/source/Navigation/libraries/client/NavigationStackConfig.h b/source/Navigation/libraries/client/NavigationStackConfig.h
index 26954c22a08d6b657af3c84668ffd4fb3b3895d9..5cc49488317e0784b853972256e0d38d8e9362e6 100644
--- a/source/Navigation/libraries/client/NavigationStackConfig.h
+++ b/source/Navigation/libraries/client/NavigationStackConfig.h
@@ -28,6 +28,8 @@
 #include <RobotAPI/interface/aron/Aron.h>
 #include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
 
+#include "Navigation/libraries/core/types.h"
+#include "Navigation/libraries/factories/NavigationStackFactory.h"
 #include "Navigation/libraries/global_planning/GlobalPlanner.h"
 #include "Navigation/libraries/local_planning/LocalPlanner.h"
 #include "Navigation/libraries/safety_control/SafetyController.h"
@@ -36,12 +38,33 @@
 
 namespace armarx::nav::client
 {
+    struct GeneralConfig
+    {
+        struct VelocityLimit
+        {
+            float linear;
+            float angular;
+        };
+
+        // if the desired velocity is below this threshold, the platform will not be moved
+        VelocityLimit minVel{0.F, 0.F};
+
+        //! max ...
+        VelocityLimit maxVel;
+
+        core::NavigationFrames navigationFrame = core::NavigationFrames::Absolute;
+
+        virtual aron::datanavigator::DictNavigatorPtr toAron() const;
+
+    };
 
     class NavigationStackConfig
     {
     public:
         NavigationStackConfig() = default;
 
+        NavigationStackConfig& configureGeneral(const GeneralConfig& cfg);
+
         NavigationStackConfig& configureGlobalPlanner(const glob_plan::GlobalPlannerParams& params);
 
         NavigationStackConfig& configureLocalPlanner(const loc_plan::LocalPlannerParams& params);
diff --git a/source/Navigation/libraries/core/Trajectory.h b/source/Navigation/libraries/core/Trajectory.h
index 8d0271c5f87e001f58622473888417b484934459..e300123fd33be30bac26555a6745823e4c8c5f6e 100644
--- a/source/Navigation/libraries/core/Trajectory.h
+++ b/source/Navigation/libraries/core/Trajectory.h
@@ -26,12 +26,23 @@
 namespace armarx::nav::core
 {
 
+    struct Point
+    {
+        Waypoint waypoint;
+        Twist twist;
+    };
 
-
-    class Trajectory : public std::vector<Waypoint>
+    class Trajectory
     {
     public:
-        Waypoint getProjection(const Pose& pose) const;
+        Trajectory(const std::vector<Point>& points) : points(points)
+        {
+        }
+
+        Point getProjection(const Pose& pose) const;
+
+    private:
+        std::vector<Point> points;
     };
 
     using TrajectoryPtr = std::shared_ptr<Trajectory>;
diff --git a/source/Navigation/libraries/core/types.h b/source/Navigation/libraries/core/types.h
index e027ac93f5d41237a1b82a57221c340ef5d2e55d..c4b992db5bb29d8f53ca3bad3e8ba75b50f9d34c 100644
--- a/source/Navigation/libraries/core/types.h
+++ b/source/Navigation/libraries/core/types.h
@@ -46,20 +46,24 @@ namespace armarx::nav::core
         {NavigationFrames::Absolute, "Absolute"}};
 
     using Pose = Eigen::Affine3f;
-    using Twist = Eigen::Vector6f;
+    
+    struct Twist
+    {
+        Eigen::Vector3f linear;
+        Eigen::Vector3f angular;
+    };
 
     struct Waypoint
     {
         Pose pose;
-        std::optional<Twist> twist = std::nullopt;
     };
 
 
 
-    struct Context
+    struct Scene
     {
-        core::StaticScenePtr staticScene;
-        core::DynamicScenePtr dynamicScene;
+        std::optional<core::StaticScene> staticScene = std::nullopt;
+        std::optional<core::DynamicScene> dynamicScene = std::nullopt;
         // TopologicMapPtr topologicMap;
         VirtualRobot::RobotPtr robot;
     };
diff --git a/source/Navigation/libraries/factories/GlobalPlannerFactory.cpp b/source/Navigation/libraries/factories/GlobalPlannerFactory.cpp
index f682e6036f5ee8693fcefee5922d4ce6586f114b..76c931f0a24b2ae91d04cd8f4e59dadba44e3dea 100644
--- a/source/Navigation/libraries/factories/GlobalPlannerFactory.cpp
+++ b/source/Navigation/libraries/factories/GlobalPlannerFactory.cpp
@@ -13,7 +13,7 @@ namespace armarx::nav::fac
 {
     glob_plan::GlobalPlannerPtr
     GlobalPlannerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
-                                 const core::Context& ctx)
+                                 const core::Scene& ctx)
     {
         namespace layer = glob_plan;
 
diff --git a/source/Navigation/libraries/factories/GlobalPlannerFactory.h b/source/Navigation/libraries/factories/GlobalPlannerFactory.h
index b8598100fc00d5d46a80b6d73653e5585cfc3696..0c81cf10f6e919310bea139ebec7a8db0419dbb5 100644
--- a/source/Navigation/libraries/factories/GlobalPlannerFactory.h
+++ b/source/Navigation/libraries/factories/GlobalPlannerFactory.h
@@ -29,7 +29,7 @@ namespace armarx::nav::fac
     {
     public:
         static glob_plan::GlobalPlannerPtr
-        create(const aron::datanavigator::DictNavigatorPtr& params, const core::Context& ctx);
+        create(const aron::datanavigator::DictNavigatorPtr& params, const core::Scene& ctx);
     };
 
 } // namespace armarx::nav::fac
\ No newline at end of file
diff --git a/source/Navigation/libraries/factories/LocalPlannerFactory.cpp b/source/Navigation/libraries/factories/LocalPlannerFactory.cpp
index 51c3cdb14889fc6a21c55d5e1740c994724d11d3..c5f4e44af3e72a1f5e74c45ae01be3e5db3897ba 100644
--- a/source/Navigation/libraries/factories/LocalPlannerFactory.cpp
+++ b/source/Navigation/libraries/factories/LocalPlannerFactory.cpp
@@ -11,7 +11,7 @@ namespace armarx::nav::fac
 {
     loc_plan::LocalPlannerPtr
     LocalPlannerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
-                                const core::Context& ctx)
+                                const core::Scene& ctx)
     {
         namespace layer = loc_plan;
 
diff --git a/source/Navigation/libraries/factories/LocalPlannerFactory.h b/source/Navigation/libraries/factories/LocalPlannerFactory.h
index 017e147d324d78d5b80c71e427f38820c5cad7ee..abc8475df141123b125631d3ddcb611c1b559344 100644
--- a/source/Navigation/libraries/factories/LocalPlannerFactory.h
+++ b/source/Navigation/libraries/factories/LocalPlannerFactory.h
@@ -31,7 +31,7 @@ namespace armarx::nav::fac
     {
     public:
         static loc_plan::LocalPlannerPtr create(const aron::datanavigator::DictNavigatorPtr& params,
-                                                const core::Context& ctx);
+                                                const core::Scene& ctx);
 
     protected:
     private:
diff --git a/source/Navigation/libraries/factories/NavigationStackFactory.cpp b/source/Navigation/libraries/factories/NavigationStackFactory.cpp
index 997e793e2c86eb1462424e59c6ab5a2ddc5a8281..2b1d8cebb7d3c3507fa0c1d07fe9b4d06757cf2a 100644
--- a/source/Navigation/libraries/factories/NavigationStackFactory.cpp
+++ b/source/Navigation/libraries/factories/NavigationStackFactory.cpp
@@ -11,7 +11,7 @@ namespace armarx::nav::fac
 {
     server::NavigationStack
     NavigationStackFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
-                                   const core::Context& ctx)
+                                   const core::Scene& ctx)
     {
         using aron::datanavigator::DictNavigator;
 
@@ -42,7 +42,7 @@ namespace armarx::nav::fac
     }
 
     server::NavigationStack NavigationStackFactory::create(const aron::data::AronDictPtr& params,
-            const core::Context& ctx)
+            const core::Scene& ctx)
     {
         const auto dict = aron::datanavigator::DictNavigator::FromAronDictPtr(params);
         return create(dict, ctx);
diff --git a/source/Navigation/libraries/factories/NavigationStackFactory.h b/source/Navigation/libraries/factories/NavigationStackFactory.h
index ea10b6d2b99b8965c3765478a2cc5555cab21ffe..d6528f4b515ae20bb7658028c3d960f24eb19a25 100644
--- a/source/Navigation/libraries/factories/NavigationStackFactory.h
+++ b/source/Navigation/libraries/factories/NavigationStackFactory.h
@@ -29,10 +29,10 @@ namespace armarx::nav::fac
     {
     public:
         static server::NavigationStack create(const aron::datanavigator::DictNavigatorPtr& params,
-                                              const core::Context& ctx);
+                                              const core::Scene& ctx);
 
         static server::NavigationStack create(const aron::data::AronDictPtr& params,
-                                              const core::Context& ctx);
+                                              const core::Scene& ctx);
 
     protected:
     private:
diff --git a/source/Navigation/libraries/factories/SafetyControllerFactory.cpp b/source/Navigation/libraries/factories/SafetyControllerFactory.cpp
index 917d1dd913377d3a03ff492e521727411fd65ea1..38da6fb4eb55b3a7455ce6a560eda34705d250d0 100644
--- a/source/Navigation/libraries/factories/SafetyControllerFactory.cpp
+++ b/source/Navigation/libraries/factories/SafetyControllerFactory.cpp
@@ -13,7 +13,7 @@ namespace armarx::nav::fac
 {
     safe_ctrl::SafetyControllerPtr
     SafetyControllerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
-                                    const core::Context& ctx)
+                                    const core::Scene& ctx)
     {
         namespace layer = safe_ctrl;
 
diff --git a/source/Navigation/libraries/factories/SafetyControllerFactory.h b/source/Navigation/libraries/factories/SafetyControllerFactory.h
index ad0e745a83520c7adef4194eef30316e00296d11..9c48ea97791e4c7f74442d8f7f6ffdd75efc1460 100644
--- a/source/Navigation/libraries/factories/SafetyControllerFactory.h
+++ b/source/Navigation/libraries/factories/SafetyControllerFactory.h
@@ -32,7 +32,7 @@ namespace armarx::nav::fac
     {
     public:
         static safe_ctrl::SafetyControllerPtr
-        create(const aron::datanavigator::DictNavigatorPtr& params, const core::Context& ctx);
+        create(const aron::datanavigator::DictNavigatorPtr& params, const core::Scene& ctx);
 
     protected:
     private:
diff --git a/source/Navigation/libraries/factories/TrajectoryControllerFactory.cpp b/source/Navigation/libraries/factories/TrajectoryControllerFactory.cpp
index 3e11bffaa46ee85981c4b82ec50ab93647c6367e..9daf57247df9f565ba601638e35a0e06936daa9e 100644
--- a/source/Navigation/libraries/factories/TrajectoryControllerFactory.cpp
+++ b/source/Navigation/libraries/factories/TrajectoryControllerFactory.cpp
@@ -12,7 +12,7 @@ namespace armarx::nav::fac
 {
     traj_ctrl::TrajectoryControllerPtr
     TrajectoryControllerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
-                                        const core::Context& ctx)
+                                        const core::Scene& ctx)
     {
         namespace layer = traj_ctrl;
 
diff --git a/source/Navigation/libraries/factories/TrajectoryControllerFactory.h b/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
index aa677514903c7e18d2622d7dbeef41cf353c91d9..ba584c2cc7f25c8b7b229190fcecd7f7409b01ec 100644
--- a/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
+++ b/source/Navigation/libraries/factories/TrajectoryControllerFactory.h
@@ -33,7 +33,7 @@ namespace armarx::nav::fac
     {
     public:
         static traj_ctrl::TrajectoryControllerPtr
-        create(const aron::datanavigator::DictNavigatorPtr& params, const core::Context& ctx);
+        create(const aron::datanavigator::DictNavigatorPtr& params, const core::Scene& ctx);
 
     protected:
     private:
diff --git a/source/Navigation/libraries/global_planning/AStar.cpp b/source/Navigation/libraries/global_planning/AStar.cpp
index 5dcffcde447c819bf4e0c2b49a5b9d4aefa178f4..3209a24fc4fba4ff97607a7939de2c53e6df5b9a 100644
--- a/source/Navigation/libraries/global_planning/AStar.cpp
+++ b/source/Navigation/libraries/global_planning/AStar.cpp
@@ -22,7 +22,7 @@ namespace armarx::nav::glob_plan
 
     // AStar
 
-    AStar::AStar(const AStarParams& params, const core::Context& ctx) :
+    AStar::AStar(const AStarParams& params, const core::Scene& ctx) :
         GlobalPlanner(ctx), params(params)
     {
     }
diff --git a/source/Navigation/libraries/global_planning/AStar.h b/source/Navigation/libraries/global_planning/AStar.h
index f6cf2958cfec3880b627a6e06de9bdedf2eb7c46..ec12f2f5b3d06375b7947884d1fcd007e3f3842a 100644
--- a/source/Navigation/libraries/global_planning/AStar.h
+++ b/source/Navigation/libraries/global_planning/AStar.h
@@ -39,7 +39,7 @@ namespace armarx::nav::glob_plan
     class AStar : public GlobalPlanner
     {
     public:
-        AStar(const AStarParams& params, const core::Context& ctx);
+        AStar(const AStarParams& params, const core::Scene& ctx);
 
         core::TrajectoryPtr plan(const core::Pose& goal) override;
 
diff --git a/source/Navigation/libraries/global_planning/GlobalPlanner.cpp b/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
index c5ec014ca2f2ffb5356216805622b9385e6479f0..34481f7cf036fe954b40199decd80b201c741779 100644
--- a/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
+++ b/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
@@ -4,7 +4,7 @@
 
 namespace armarx::nav::glob_plan
 {
-    GlobalPlanner::GlobalPlanner(const core::Context& context) : context(context)
+    GlobalPlanner::GlobalPlanner(const core::Scene& context) : context(context)
     {
     }
 
diff --git a/source/Navigation/libraries/global_planning/GlobalPlanner.h b/source/Navigation/libraries/global_planning/GlobalPlanner.h
index 9efb21cdac041dafdb5e34554f56dd4564bb10a6..fb5ecceb6bd7919db0839a173f39e20f5b857b74 100644
--- a/source/Navigation/libraries/global_planning/GlobalPlanner.h
+++ b/source/Navigation/libraries/global_planning/GlobalPlanner.h
@@ -43,13 +43,13 @@ namespace armarx::nav::glob_plan
     class GlobalPlanner
     {
     public:
-        GlobalPlanner(const core::Context& context);
+        GlobalPlanner(const core::Scene& context);
         virtual ~GlobalPlanner() = default;
 
         virtual core::TrajectoryPtr plan(const core::Pose& goal) = 0;
 
     protected:
-        const core::Context context;
+        const core::Scene context;
 
     private:
     };
diff --git a/source/Navigation/libraries/global_planning/Point2Point.cpp b/source/Navigation/libraries/global_planning/Point2Point.cpp
index 3a38ec96aee187473a616b28529c0b233f635569..37cd1d66866cb65f743b0d63e9336776ea68f057 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.cpp
+++ b/source/Navigation/libraries/global_planning/Point2Point.cpp
@@ -30,7 +30,7 @@ namespace armarx::nav::glob_plan
 
     // Point2Point
 
-    Point2Point::Point2Point(const Point2Point::Params& params, const core::Context& ctx) :
+    Point2Point::Point2Point(const Point2Point::Params& params, const core::Scene& ctx) :
         GlobalPlanner(ctx), params(params)
     {
     }
diff --git a/source/Navigation/libraries/global_planning/Point2Point.h b/source/Navigation/libraries/global_planning/Point2Point.h
index 3204fb0ed96cdc468ac9170bd6c9cbba3bdb97d0..45068d1ba675083772e20e5428373ac938e8b207 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.h
+++ b/source/Navigation/libraries/global_planning/Point2Point.h
@@ -41,7 +41,7 @@ namespace armarx::nav::glob_plan
     public:
         using Params = Point2PointParams;
 
-        Point2Point(const Params& params, const core::Context& ctx);
+        Point2Point(const Params& params, const core::Scene& ctx);
 
         core::TrajectoryPtr plan(const core::Pose& goal) override;
 
diff --git a/source/Navigation/libraries/local_planning/LocalPlanner.cpp b/source/Navigation/libraries/local_planning/LocalPlanner.cpp
index 135550094876d92d52e87b30bee74c919a115596..94ee6d97df33a8b406ed0f98e926c0d20c09d61c 100644
--- a/source/Navigation/libraries/local_planning/LocalPlanner.cpp
+++ b/source/Navigation/libraries/local_planning/LocalPlanner.cpp
@@ -2,7 +2,7 @@
 
 namespace armarx::nav::loc_plan
 {
-    LocalPlanner::LocalPlanner(const core::Context& context) : context(context)
+    LocalPlanner::LocalPlanner(const core::Scene& context) : context(context)
     {
 
     }
diff --git a/source/Navigation/libraries/local_planning/LocalPlanner.h b/source/Navigation/libraries/local_planning/LocalPlanner.h
index d9b8d9aa0b2772a61d0bdb2011a91f784bca5e02..a2846c4d7a1416a781a21db8819c92ed1d2b136d 100644
--- a/source/Navigation/libraries/local_planning/LocalPlanner.h
+++ b/source/Navigation/libraries/local_planning/LocalPlanner.h
@@ -45,14 +45,14 @@ namespace armarx::nav::loc_plan
     class LocalPlanner
     {
     public:
-        LocalPlanner(const core::Context& context);
+        LocalPlanner(const core::Scene& context);
         virtual ~LocalPlanner() = default;
 
         virtual core::TrajectoryPtr plan(const core::TrajectoryPtr& goal) = 0;
 
     protected:
     private:
-        const core::Context context;
+        const core::Scene context;
     };
 
     using LocalPlannerPtr = std::shared_ptr<LocalPlanner>;
diff --git a/source/Navigation/libraries/local_planning/TimedElasticBands.cpp b/source/Navigation/libraries/local_planning/TimedElasticBands.cpp
index 02474dd6e09d69d6a4cb5f94eff129338041ad03..36d9723edf3870bab4f8c93c19fba7a534f371c3 100644
--- a/source/Navigation/libraries/local_planning/TimedElasticBands.cpp
+++ b/source/Navigation/libraries/local_planning/TimedElasticBands.cpp
@@ -26,7 +26,7 @@ namespace armarx::nav::loc_plan
 
     // TimedElasticBands
 
-    TimedElasticBands::TimedElasticBands(const Params& params, const core::Context& ctx) :
+    TimedElasticBands::TimedElasticBands(const Params& params, const core::Scene& ctx) :
         LocalPlanner(ctx), params(params)
     {
     }
diff --git a/source/Navigation/libraries/local_planning/TimedElasticBands.h b/source/Navigation/libraries/local_planning/TimedElasticBands.h
index 3d96a4aa53c82f944cd80a93dbfbf113687f5d14..adc625a6962eed031f4c99dfcab27c4ea2eabe3d 100644
--- a/source/Navigation/libraries/local_planning/TimedElasticBands.h
+++ b/source/Navigation/libraries/local_planning/TimedElasticBands.h
@@ -43,7 +43,7 @@ namespace armarx::nav::loc_plan
     public:
         using Params = TimedElasticBandsParams;
 
-        TimedElasticBands(const Params& params, const core::Context& ctx);
+        TimedElasticBands(const Params& params, const core::Scene& ctx);
         ~TimedElasticBands() override = default;
 
         core::TrajectoryPtr plan(const core::TrajectoryPtr& goal) override;
diff --git a/source/Navigation/libraries/safety_control/LaserBasedProximity.cpp b/source/Navigation/libraries/safety_control/LaserBasedProximity.cpp
index e19b795a9973bb810d10643bf75109332da7cb36..2134e9d689c57d3dbc2973f597e2fd0724ba2b2d 100644
--- a/source/Navigation/libraries/safety_control/LaserBasedProximity.cpp
+++ b/source/Navigation/libraries/safety_control/LaserBasedProximity.cpp
@@ -21,7 +21,7 @@ namespace armarx::nav::safe_ctrl
         return LaserBasedProximityParams(); // TODO implement
     }
 
-    LaserBasedProximity::LaserBasedProximity(const Params& params, const core::Context& ctx) :
+    LaserBasedProximity::LaserBasedProximity(const Params& params, const core::Scene& ctx) :
         SafetyController(ctx), params(params)
     {
     }
diff --git a/source/Navigation/libraries/safety_control/LaserBasedProximity.h b/source/Navigation/libraries/safety_control/LaserBasedProximity.h
index a6c3b35ebb1d2e1863aff1316e2ebff8d68cc3fd..45363b1260d4b1922c87fe95b670ebf317a3cf39 100644
--- a/source/Navigation/libraries/safety_control/LaserBasedProximity.h
+++ b/source/Navigation/libraries/safety_control/LaserBasedProximity.h
@@ -40,7 +40,7 @@ namespace armarx::nav::safe_ctrl
     public:
         using Params = LaserBasedProximityParams;
 
-        LaserBasedProximity(const Params& params, const core::Context& ctx);
+        LaserBasedProximity(const Params& params, const core::Scene& ctx);
         ~LaserBasedProximity() override = default;
 
         core::Twist control(const core::Twist& twist) override;
diff --git a/source/Navigation/libraries/safety_control/SafetyController.cpp b/source/Navigation/libraries/safety_control/SafetyController.cpp
index 5d7f17b448debdaeb74e3c8e89c63cf6419a2b16..954a7a5cdd500f638f927a62869e4ca44fcf82a5 100644
--- a/source/Navigation/libraries/safety_control/SafetyController.cpp
+++ b/source/Navigation/libraries/safety_control/SafetyController.cpp
@@ -2,7 +2,7 @@
 
 namespace armarx::nav::safe_ctrl
 {
-    SafetyController::SafetyController(const core::Context& context) : context(context)
+    SafetyController::SafetyController(const core::Scene& context) : context(context)
     {
 
     }
diff --git a/source/Navigation/libraries/safety_control/SafetyController.h b/source/Navigation/libraries/safety_control/SafetyController.h
index 40135b0b540921fcca5416efb0bcc4c8e29ae0e0..917cfa6f75d60836e44ee1b0f100db32000b0f95 100644
--- a/source/Navigation/libraries/safety_control/SafetyController.h
+++ b/source/Navigation/libraries/safety_control/SafetyController.h
@@ -45,14 +45,14 @@ namespace armarx::nav::safe_ctrl
     class SafetyController
     {
     public:
-        SafetyController(const core::Context& context);
+        SafetyController(const core::Scene& context);
         virtual ~SafetyController() = default;
 
         virtual core::Twist control(const core::Twist& twist) = 0;
 
     protected:
     private:
-        const core::Context context;
+        const core::Scene context;
     };
 
     using SafetyControllerPtr = std::shared_ptr<SafetyController>;
diff --git a/source/Navigation/libraries/server/NavigationStack.h b/source/Navigation/libraries/server/NavigationStack.h
index 7c5912f8d9897db1c67991baa4741b0fb9638a26..1c2d82e98836f04d06984e96ebfbdfb872cb03e7 100644
--- a/source/Navigation/libraries/server/NavigationStack.h
+++ b/source/Navigation/libraries/server/NavigationStack.h
@@ -32,8 +32,8 @@ namespace armarx::nav::server
     struct NavigationStack
     {
         glob_plan::GlobalPlannerPtr globalPlanner;
-        loc_plan::LocalPlannerPtr localPlanner;
+        loc_plan::LocalPlannerPtr localPlanner = nullptr;
         traj_ctrl::TrajectoryControllerPtr trajectoryControl;
-        safe_ctrl::SafetyControllerPtr safetyControl;
+        safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
     };
 } // namespace armarx::nav::server
\ No newline at end of file
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index e09b193f66f248b33b5f98e6937817cb3ae7d683..14ad6b73edd2e4040c85e2883cbd7b2d31a85885 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -8,8 +8,8 @@
 
 namespace armarx::nav::server
 {
-    Navigator::Navigator(const server::NavigationStack& navigationStack, const core::Context& ctx) :
-        stack(navigationStack), ctx(ctx)
+    Navigator::Navigator(const server::NavigationStack& navigationStack, const core::Scene& ctx) :
+        stack(navigationStack), scene(ctx)
     {
         setTag("Navigator");
     }
@@ -23,7 +23,7 @@ namespace armarx::nav::server
                 globalPose = goal;
                 break;
             case core::NavigationFrames::Relative:
-                globalPose = core::Pose(ctx.robot->getGlobalPose()) * goal;
+                globalPose = core::Pose(scene.robot->getGlobalPose()) * goal;
                 break;
         }
 
@@ -59,7 +59,7 @@ namespace armarx::nav::server
                 res.safeVelocity = stack.safetyControl->control(res.controlVelocity);
             }
 
-            // TODO callback
+            executor.move(res.velocity());
         }
     }
 } // namespace armarx::nav::server
\ No newline at end of file
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index ed46a6ff155f420efd9f893506e8d9460f6711fa..38e12c3e8045b6963f3ad7b1fbf30ff97bf7c46d 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -35,8 +35,8 @@ namespace armarx::nav::server
     {
         core::TrajectoryPtr globalTrajectory;
         core::TrajectoryPtr localTrajectory;
-        core::Twist controlVelocity;
-        core::Twist safeVelocity;
+        std::optional<core::Twist> controlVelocity;
+        std::optional<core::Twist> safeVelocity;
 
         core::TrajectoryPtr trajectory() const
         {
@@ -48,12 +48,23 @@ namespace armarx::nav::server
             ARMARX_CHECK_NOT_NULL(globalTrajectory);
             return globalTrajectory;
         }
+
+        core::Twist velocity() const
+        {
+            if (safeVelocity.has_value())
+            {
+                return safeVelocity.value();
+            }
+
+            ARMARX_CHECK_NOT_NULL(controlVelocity.has_value());
+            return *controlVelocity;
+        }
     };
 
     class Navigator : public armarx::Logging
     {
     public:
-        Navigator(const server::NavigationStack& navigationStack, const core::Context& ctx);
+        Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene);
 
         void moveTo(const core::Pose& goal, const core::NavigationFrames& navigationFrame);
         void moveToAbsolute(const core::Pose& goal);
@@ -61,6 +72,8 @@ namespace armarx::nav::server
     protected:
     private:
         server::NavigationStack stack;
-        const core::Context ctx;
+        const core::Scene& scene;
+
+        ExecutorInterface& executor; 
     };
 } // namespace armarx::nav::server
\ No newline at end of file
diff --git a/source/Navigation/libraries/server/test/serverTest.cpp b/source/Navigation/libraries/server/test/serverTest.cpp
index 42eb2fb3413b6fc0e739c7b56a18e29f21c7c5f1..eb368c31d5619f91ce732060f9dcc7c463562bf8 100644
--- a/source/Navigation/libraries/server/test/serverTest.cpp
+++ b/source/Navigation/libraries/server/test/serverTest.cpp
@@ -20,6 +20,8 @@
  *             GNU General Public License
  */
 
+#include <chrono>
+
 #include "Navigation/libraries/client/NavigationStackConfig.h"
 #include "Navigation/libraries/core/types.h"
 #include "Navigation/libraries/factories/NavigationStackFactory.h"
@@ -35,32 +37,75 @@
 
 #include <iostream>
 
-#include <Navigation/Test.h>
-
 #include <VirtualRobot/Robot.h>
 
+#include <Navigation/Test.h>
+
 using namespace armarx::nav;
 
+class ExecutorInterface
+{
+public:
+    virtual ~ExecutorInterface() = default;
+    virtual void move(const core::Twist& twist) = 0;
+};
+
+class DummyExecutor : virtual public ExecutorInterface
+{
+public:
+    struct Params
+    {
+        
+    };
+
+    DummyExecutor(const VirtualRobot::RobotPtr& robot, const Params& params) : robot(robot)
+    {
+    }
+
+    void move(const core::Twist& twist) override 
+    {
+        this->twist = twist;
+        const auto now = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now());
+
+        const float dt = now - lastUpdated;
+
+        lastUpdated = now;
+
+        const Eigen::Affine3f diff = Eigen::Affine3f(Eigen::Translation3f(twist.linear * dt)) * Eigen::Affine3f(Eigen::AngleAxisf(dt, twist.angular)));
+
+        robot->setGlobalPose(robot->getGlobalPose() * diff);
+    }
+
+
+
+private:
+    VirtualRobot::RobotPtr robot;
+
+    core::Twist twist;
+
+    std::chrono::microseconds lastUpdated;
+
+    
+};
+
 BOOST_AUTO_TEST_CASE(testNavigator)
 {
 
     const core::Pose goal = core::Pose::Identity();
 
     // create Navigator
-    core::Context ctx;
-    ctx.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
-    ctx.robot->setGlobalPose(Eigen::Matrix4f::Identity(), false);
-
-
-    server::NavigationStack stack
-    {
-        .globalPlanner = std::make_shared<glob_plan::Point2Point>(glob_plan::Point2PointParams(), ctx),
-        .localPlanner = nullptr,
+    core::Scene scene;
+    scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
+    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity(), false);
+
+    // TODO create static shared ctor
+    server::NavigationStack stack{
+        .globalPlanner =
+            std::make_shared<glob_plan::Point2Point>(glob_plan::Point2PointParams(), scene),
         .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
-            traj_ctrl::TrajectoryFollowingControllerParams(), ctx),
-        .safetyControl = nullptr};
+            traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
 
-    server::Navigator navigator(stack, ctx);
+    server::Navigator navigator(stack, scene, executor);
     navigator.moveTo(goal, core::NavigationFrames::Absolute);
 
     BOOST_CHECK_EQUAL(true, true);
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryController.cpp
index 62c3c65498faf224069860ab146040fbabe6088e..c475d071a60b46ce539fa0edf8aea51a4f56f9aa 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryController.cpp
@@ -2,7 +2,7 @@
 
 namespace armarx::nav::traj_ctrl
 {
-    TrajectoryController::TrajectoryController(const core::Context& context) : context(context)
+    TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
     {
     }
 
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryController.h b/source/Navigation/libraries/trajectory_control/TrajectoryController.h
index 62951c81bcfe99d23ac228699721ff2dfef2fd9f..a7c981d713925c4aa2218f4c02cd32b3dcebc79c 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryController.h
@@ -29,8 +29,8 @@
 
 #include "Navigation/libraries/core/DynamicScene.h"
 #include "Navigation/libraries/core/StaticScene.h"
-#include "Navigation/libraries/core/types.h"
 #include "Navigation/libraries/core/Trajectory.h"
+#include "Navigation/libraries/core/types.h"
 #include "core.h"
 
 namespace armarx::nav::traj_ctrl
@@ -38,6 +38,12 @@ namespace armarx::nav::traj_ctrl
 
     struct TrajectoryControllerParams
     {
+        struct
+        {
+            float linearVelocity{1.0};
+            float angularVelocity{0.F};
+        } limits;
+
         virtual Algorithms algorithm() const = 0;
         virtual aron::datanavigator::DictNavigatorPtr toAron() const = 0;
     };
@@ -45,13 +51,14 @@ namespace armarx::nav::traj_ctrl
     class TrajectoryController
     {
     public:
-        TrajectoryController(const core::Context& context);
+        TrajectoryController(const core::Scene& context);
         virtual ~TrajectoryController() = default;
 
         virtual core::Twist control(const core::TrajectoryPtr& goal) = 0;
 
     protected:
-        const core::Context context;
+        const core::Scene context;
+
     private:
     };
     using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index 810d3bf6c615aefaf9e25ff438bc188a52837e89..c15d37aa9049f05d9ab9fb2a4c80e03d2fe83c89 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -35,7 +35,7 @@ namespace armarx::nav::traj_ctrl
     }
 
     TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
-            const core::Context& context) :
+            const core::Scene& context) :
         TrajectoryController(context), params(params)
     {
     }
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
index 6e1d7532ef49bae1c00e997e90b37a026068474e..0bc4fcdfd8d7d7b9f3e4fc6a741bfd2168b72d47 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
@@ -35,11 +35,7 @@ namespace armarx::nav::traj_ctrl
             float Kd{0.F};
         } pid;
 
-        struct
-        {
-            float linearVelocity{1.0};
-            float angularVelocity{0.F};
-        } limits;
+       
 
         Algorithms algorithm() const override;
         aron::datanavigator::DictNavigatorPtr toAron() const override;
@@ -52,7 +48,7 @@ namespace armarx::nav::traj_ctrl
     public:
         using Params = TrajectoryFollowingControllerParams;
 
-        TrajectoryFollowingController(const Params& params, const core::Context& context);
+        TrajectoryFollowingController(const Params& params, const core::Scene& context);
         ~TrajectoryFollowingController() override = default;
 
         core::Twist control(const core::TrajectoryPtr& goal) override;
diff --git a/source/Navigation/libraries/trajectory_control/WaypointController.cpp b/source/Navigation/libraries/trajectory_control/WaypointController.cpp
index e78a36ebbe0fe3b41fcb97ea4b975842b22bbb9c..bb201876884a4c5bf2741414f5ca3ea354d0876e 100644
--- a/source/Navigation/libraries/trajectory_control/WaypointController.cpp
+++ b/source/Navigation/libraries/trajectory_control/WaypointController.cpp
@@ -25,7 +25,7 @@ namespace armarx::nav::traj_ctrl
 
     // WaypointController
 
-    WaypointController::WaypointController(const Params& params, const core::Context& context) :
+    WaypointController::WaypointController(const Params& params, const core::Scene& context) :
         TrajectoryController(context), params(params)
     {
     }
diff --git a/source/Navigation/libraries/trajectory_control/WaypointController.h b/source/Navigation/libraries/trajectory_control/WaypointController.h
index 15e6e4c0ea5c0188f69aa4148904df9449b0f4d1..7ac0503c3adf511451e518e9b60da768f9da779c 100644
--- a/source/Navigation/libraries/trajectory_control/WaypointController.h
+++ b/source/Navigation/libraries/trajectory_control/WaypointController.h
@@ -39,7 +39,7 @@ namespace armarx::nav::traj_ctrl
     public:
         using Params = WaypointControllerParams;
 
-        WaypointController(const Params& params, const core::Context& context);
+        WaypointController(const Params& params, const core::Scene& context);
         ~WaypointController() override = default;
 
         core::Twist control(const core::TrajectoryPtr& goal) override ;