diff --git a/scenarios/NavigationSimulation/config/LongtermMemory.cfg b/scenarios/NavigationSimulation/config/LongtermMemory.cfg
index e323230d5da82a370611eec3fad6358f913ce064..0eae3f1ab86c16681c70d88228002d456dcffa33 100644
--- a/scenarios/NavigationSimulation/config/LongtermMemory.cfg
+++ b/scenarios/NavigationSimulation/config/LongtermMemory.cfg
@@ -100,6 +100,14 @@
 MemoryX.LongtermMemory.ClassCollections = memdb.Longterm_Objects
 
 
+# MemoryX.LongtermMemory.CommonStorageName:  Name of common storage.
+#  Attributes:
+#  - Default:            CommonStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# MemoryX.LongtermMemory.CommonStorageName = CommonStorage
+
+
 # MemoryX.LongtermMemory.DatabaseName:  Mongo database to store LTM data
 #  Attributes:
 #  - Case sensitivity:   yes
@@ -165,6 +173,14 @@ MemoryX.LongtermMemory.DatabaseName = CeBITdb
 # MemoryX.LongtermMemory.PredictionDataCollection = ltm_predictiondata
 
 
+# MemoryX.LongtermMemory.PriorKnowledgeName:  Name of prior knowledge.
+#  Attributes:
+#  - Default:            PriorKnowledge
+#  - Case sensitivity:   yes
+#  - Required:           no
+# MemoryX.LongtermMemory.PriorKnowledgeName = PriorKnowledge
+
+
 # MemoryX.LongtermMemory.ProfilerDataCollection:  Mongo collection for storing Profiler data
 #  Attributes:
 #  - Default:            ltm_profilerdata
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index 8b9a042fee70b0ecf6544fbf22c6aef9efc54c30..cb044450c293979632d961f4b43d664997f7f727 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -92,6 +92,14 @@
 # ArmarX.LoggingGroup = ""
 
 
+# ArmarX.ObjectMemory.ArVizStorageName:  Name of the ArViz storage
+#  Attributes:
+#  - Default:            ArVizStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.ArVizStorageName = ArVizStorage
+
+
 # ArmarX.ObjectMemory.ArVizTopicName:  Name of the ArViz topic
 #  Attributes:
 #  - Default:            ArVizTopic
@@ -364,7 +372,7 @@ ArmarX.ObjectMemory.mem.cls.Floor.Show = 0
 #  - Default:            ""
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003.json
+ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge
 
 
 # ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName:  Name of the Instance core segment.
@@ -417,6 +425,32 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003.json
 # ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
 
 
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position:  Enable showing pose gaussians (orientation part).
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced:  Displace center orientation (co)variance circle arrows along their rotation axis.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale:  Scaling of pose gaussians (orientation part).
+#  Attributes:
+#  - Default:            100
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale = 100
+
+
 # ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame:  If true, show global poses. If false, show poses in robot frame.
 #  Attributes:
 #  - Default:            true
@@ -461,13 +495,21 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003.json
 # ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
 
 
-# ArmarX.ObjectMemory.mem.ltm.00_enabled:  
+# ArmarX.ObjectMemory.mem.ltm.enabled:  
 #  Attributes:
-#  - Default:            true
+#  - Default:            false
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.00_enabled = true
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.storagepath:  The path to the memory storage.
+#  Attributes:
+#  - Default:            /tmp/MemoryExport/Test
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.ltm.storagepath = /tmp/MemoryExport/Test
 
 
 # ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/NavigationSimulation/config/PriorKnowledge.cfg b/scenarios/NavigationSimulation/config/PriorKnowledge.cfg
index c44a2b83dc96d79b137906e8dd978541b343c8c2..d2a8fe85f077987cf9406710c2f49917fac04917 100644
--- a/scenarios/NavigationSimulation/config/PriorKnowledge.cfg
+++ b/scenarios/NavigationSimulation/config/PriorKnowledge.cfg
@@ -99,6 +99,14 @@
 MemoryX.PriorKnowledge.ClassCollections = CeBITdb.Prior_CeBIT
 
 
+# MemoryX.PriorKnowledge.CommonStorageName:  Name of common storage.
+#  Attributes:
+#  - Default:            CommonStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# MemoryX.PriorKnowledge.CommonStorageName = CommonStorage
+
+
 # MemoryX.PriorKnowledge.EnableProfiling:  enable profiler which is used for logging performance events
 #  Attributes:
 #  - Default:            false
diff --git a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
index 38b5f9fe20d0fe52ec54e4a9b0130cc47e643603..898aeab27aa2a861ed5606095559e22fbbe28f64 100644
--- a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
+++ b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
@@ -109,6 +109,14 @@
 # ArmarX.RemoteHandlesDeletionTimeout = 3000
 
 
+# ArmarX.RobotStateMemory.ArVizStorageName:  Name of the ArViz storage
+#  Attributes:
+#  - Default:            ArVizStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RobotStateMemory.ArVizStorageName = ArVizStorage
+
+
 # ArmarX.RobotStateMemory.ArVizTopicName:  Name of the ArViz topic
 #  Attributes:
 #  - Default:            ArVizTopic
@@ -151,10 +159,10 @@
 ArmarX.RobotStateMemory.ObjectName = RobotState
 
 
-# ArmarX.RobotStateMemory.RobotUnit.MemoryBatchSize:  The size of the entity snapshot to send to the memory. Minimum is 1.
+# ArmarX.RobotStateMemory.RobotUnit.MemoryBatchSize:  No Description
 #  Attributes:
-#  - Default:            50
-#  - Case sensitivity:   yes
+#  - Default:            1
+#  - Case sensitivity:   no
 #  - Required:           no
 ArmarX.RobotStateMemory.RobotUnit.MemoryBatchSize = 1
 
@@ -177,11 +185,21 @@ ArmarX.RobotStateMemory.RobotUnit.MemoryBatchSize = 1
 
 # ArmarX.RobotStateMemory.RobotUnitName:  Name of the RobotUnit
 #  Attributes:
+#  - Default:            ""
 #  - Case sensitivity:   yes
-#  - Required:           yes
+#  - Required:           no
 ArmarX.RobotStateMemory.RobotUnitName = Armar6Unit
 
 
+# ArmarX.RobotStateMemory.WaitForRobotUnit:  Add the robot unit as dependency to the component. This memory requires a running RobotUnit, therefore we should add it as explicit dependency.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.WaitForRobotUnit = false
+
+
 # ArmarX.RobotStateMemory.mem.MemoryName:  Name of this memory server.
 #  Attributes:
 #  - Default:            RobotState
@@ -222,13 +240,21 @@ ArmarX.RobotStateMemory.RobotUnitName = Armar6Unit
 # ArmarX.RobotStateMemory.mem.loc.MaxHistorySize = 1024
 
 
-# ArmarX.RobotStateMemory.mem.ltm.00_enabled:  
+# ArmarX.RobotStateMemory.mem.ltm.enabled:  
 #  Attributes:
-#  - Default:            true
+#  - Default:            false
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RobotStateMemory.mem.ltm.00_enabled = true
+# ArmarX.RobotStateMemory.mem.ltm.enabled = false
+
+
+# ArmarX.RobotStateMemory.mem.ltm.storagepath:  The path to the memory storage.
+#  Attributes:
+#  - Default:            /tmp/MemoryExport/RobotState
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RobotStateMemory.mem.ltm.storagepath = /tmp/MemoryExport/RobotState
 
 
 # ArmarX.RobotStateMemory.mem.prop.CoreSegmentName:  Name of the 'Proprioception' core segment.
diff --git a/scenarios/NavigationSimulation/config/SimulatorApp.cfg b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
index 628a4f5e39ef3f303aec73460334c9aac7df858d..c4c6185489f459cd19c7ec64adbc737b06953650 100644
--- a/scenarios/NavigationSimulation/config/SimulatorApp.cfg
+++ b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
@@ -289,7 +289,7 @@ ArmarX.Simulator.InitialRobotConfig = ArmR5_Elb1:1.57, ArmL5_Elb1:1.57
 #  - Default:            0
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.Simulator.InitialRobotPose.x = 0
+ArmarX.Simulator.InitialRobotPose.x = 1500
 
 
 # ArmarX.Simulator.InitialRobotPose.x_0:  x component of initial robot position (mm)
diff --git a/scenarios/NavigationSimulation/config/WorkingMemory.cfg b/scenarios/NavigationSimulation/config/WorkingMemory.cfg
index f33e4b8bc9dee1a831a65ab858d3c47cb4d09019..a6115c9f6b1e95a8d4a73aae1097813fdc128e58 100644
--- a/scenarios/NavigationSimulation/config/WorkingMemory.cfg
+++ b/scenarios/NavigationSimulation/config/WorkingMemory.cfg
@@ -272,6 +272,14 @@ MemoryX.ObjectLocalizationMemoryUpdater.WorkingMemoryName = "WorkingMemory"
 # MemoryX.Verbosity = Info
 
 
+# MemoryX.WorkingMemory.CommonStorageName:  Name of common storage.
+#  Attributes:
+#  - Default:            CommonStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# MemoryX.WorkingMemory.CommonStorageName = CommonStorage
+
+
 # MemoryX.WorkingMemory.EnableProfiling:  enable profiler which is used for logging performance events
 #  Attributes:
 #  - Default:            false
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index eae3a98a41a64ec9a70ba6c5456c37501b6552b3..b4fbaff1e4c9802d25e86e685da73bb43800ea18 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
@@ -92,6 +92,14 @@
 # ArmarX.LoggingGroup = ""
 
 
+# ArmarX.ObjectMemory.ArVizStorageName:  Name of the ArViz storage
+#  Attributes:
+#  - Default:            ArVizStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.ArVizStorageName = ArVizStorage
+
+
 # ArmarX.ObjectMemory.ArVizTopicName:  Name of the ArViz topic
 #  Attributes:
 #  - Default:            ArVizTopic
@@ -417,6 +425,32 @@
 # ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
 
 
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position:  Enable showing pose gaussians (orientation part).
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced:  Displace center orientation (co)variance circle arrows along their rotation axis.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale:  Scaling of pose gaussians (orientation part).
+#  Attributes:
+#  - Default:            100
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale = 100
+
+
 # ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame:  If true, show global poses. If false, show poses in robot frame.
 #  Attributes:
 #  - Default:            true
@@ -461,13 +495,21 @@
 # ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
 
 
-# ArmarX.ObjectMemory.mem.ltm.00_enabled:  
+# ArmarX.ObjectMemory.mem.ltm.enabled:  
 #  Attributes:
-#  - Default:            true
+#  - Default:            false
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.00_enabled = true
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.storagepath:  The path to the memory storage.
+#  Attributes:
+#  - Default:            /tmp/MemoryExport/Test
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.ltm.storagepath = /tmp/MemoryExport/Test
 
 
 # ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 1e9d06004511f601b82a565208f8292aee182c70..37803dc2c31b0c11bc041b6f716ff2459479fdbf 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -194,13 +194,21 @@
 # ArmarX.VisionMemory.mem.MemoryName = Vision
 
 
-# ArmarX.VisionMemory.mem.ltm.00_enabled:  
+# ArmarX.VisionMemory.mem.ltm.enabled:  
 #  Attributes:
-#  - Default:            true
+#  - Default:            false
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.00_enabled = true
+# ArmarX.VisionMemory.mem.ltm.enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.storagepath:  The path to the memory storage.
+#  Attributes:
+#  - Default:            /tmp/MemoryExport/Test
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.mem.ltm.storagepath = /tmp/MemoryExport/Test
 
 
 # ArmarX.VisionMemory.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index c51d2b49152134c46ed72c5a3801107f50051a9c..eb9eee5dc5e9131260ca6c03f5b52b00f10c0ebc 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -142,13 +142,21 @@
 # ArmarX.NavigationMemory.mem.MemoryName = Navigation
 
 
-# ArmarX.NavigationMemory.mem.ltm.00_enabled:  
+# ArmarX.NavigationMemory.mem.ltm.enabled:  
 #  Attributes:
-#  - Default:            true
+#  - Default:            false
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.mem.ltm.00_enabled = true
+# ArmarX.NavigationMemory.mem.ltm.enabled = false
+
+
+# ArmarX.NavigationMemory.mem.ltm.storagepath:  The path to the memory storage.
+#  Attributes:
+#  - Default:            /tmp/MemoryExport/Test
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.NavigationMemory.mem.ltm.storagepath = /tmp/MemoryExport/Test
 
 
 # ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 4873c6bbc55e212ef90bafa9f8d95dfaba76c5ae..57675d7aaaac68fc70916d79ec3b07e8d759256f 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -92,6 +92,14 @@
 # ArmarX.LoggingGroup = ""
 
 
+# ArmarX.Navigator.ArVizStorageName:  Name of the ArViz storage
+#  Attributes:
+#  - Default:            ArVizStorage
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.Navigator.ArVizStorageName = ArVizStorage
+
+
 # ArmarX.Navigator.ArVizTopicName:  Name of the ArViz topic
 #  Attributes:
 #  - Default:            ArVizTopic
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 6266c680f8fb74e54b51735bb07310fb09b1a94c..368ca6495f189e7263bc654a5e66816a72ae1bfa 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -57,7 +57,7 @@ armarx::navigation::client::ComponentPlugin::configureNavigator(
 {
     ARMARX_TRACE;
 
-    ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+    // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
 
     eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
     {
@@ -88,9 +88,7 @@ armarx::navigation::client::ComponentPlugin::configureNavigator(
 {
     ARMARX_TRACE;
 
-    ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
-
-    eventHandler = std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
+    // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
 
     iceNavigator.createConfig(stackConfig, configId);
 
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index 38ab565fb95c1bea71ee78e4838dbb6f050771ed..043a1e3c2917f03b8d7a06191c8596666f2e2bef 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -38,7 +38,8 @@ namespace armarx::navigation::client
                                 const std::string& configId);
 
         void configureNavigator(const client::NavigationStackConfig& stackConfig,
-                                const std::string& configId, armarx::armem::client::MemoryNameSystem& memoryNameSystem);
+                                const std::string& configId,
+                                armarx::armem::client::MemoryNameSystem& memoryNameSystem);
 
     private:
         static constexpr const char* PROPERTY_NAME = "nav.NavigatorName";
@@ -62,13 +63,12 @@ namespace armarx::navigation::client
         void configureNavigator(const client::NavigationStackConfig& stackConfig);
 
         Navigator& getNavigator();
-        
+
         // Non-API
         ~ComponentPluginUser() override;
 
     private:
         ComponentPlugin* plugin = nullptr;
-
     };
 
 } // namespace armarx::navigation::client
diff --git a/source/armarx/navigation/client/NavigationStackConfig.cpp b/source/armarx/navigation/client/NavigationStackConfig.cpp
index 85a5a5dfc57a6f3cfd76475db31716f0d2dfcb4b..6acdc7f0e8aaa925ab770f14cbe421fc1bc10358 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.cpp
+++ b/source/armarx/navigation/client/NavigationStackConfig.cpp
@@ -60,12 +60,12 @@ namespace armarx::navigation::client
     }
 
     NavigationStackConfig&
-    NavigationStackConfig::globalPlanner(const glob_plan::GlobalPlannerParams& params)
+    NavigationStackConfig::globalPlanner(const global_planning::GlobalPlannerParams& params)
     {
         aron::data::DictPtr element(new aron::data::Dict);
         element->addElement(core::NAME_KEY,
                             std::make_shared<aron::data::String>(
-                                glob_plan::AlgorithmNames.to_name(params.algorithm())));
+                                global_planning::AlgorithmNames.to_name(params.algorithm())));
         element->addElement(core::PARAMS_KEY, params.toAron());
 
         dict.addElement(core::StackLayerNames.to_name(core::StackLayer::GlobalPlanner), element);
diff --git a/source/armarx/navigation/client/NavigationStackConfig.h b/source/armarx/navigation/client/NavigationStackConfig.h
index c20a36bb367890c773b5aaeb215a45149f298f46..2e3d690105ae0c898915b63c8e04c1d7cdbf3f63 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.h
+++ b/source/armarx/navigation/client/NavigationStackConfig.h
@@ -59,7 +59,7 @@ namespace armarx::navigation::client
 
         NavigationStackConfig& general(const GeneralConfig& cfg);
 
-        NavigationStackConfig& globalPlanner(const glob_plan::GlobalPlannerParams& params);
+        NavigationStackConfig& globalPlanner(const global_planning::GlobalPlannerParams& params);
 
         NavigationStackConfig& localPlanner(const loc_plan::LocalPlannerParams& params);
 
diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 9780c8c009164d4a5ea1753ac9089a48f632ecab..6054d0987dfb215589839a797d9c980d11344c4a 100644
--- a/source/armarx/navigation/client/Navigator.cpp
+++ b/source/armarx/navigation/client/Navigator.cpp
@@ -1,8 +1,12 @@
 #include "Navigator.h"
 
 #include <algorithm>
+#include <chrono>
+#include <future>
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/util/CPPUtility/trace.h>
 
 #include <armarx/navigation/client/PathBuilder.h>
@@ -114,14 +118,55 @@ namespace armarx::navigation::client
 
 
     StopEvent
-    Navigator::waitForStop()
+    Navigator::waitForStop(const std::int64_t timeoutMs)
     {
         ARMARX_TRACE;
-        std::unique_lock l{stoppedInfo.m};
-        stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
+
+        std::future<void> future = std::async(
+            std::launch::async,
+            [&]()
+            {
+                std::unique_lock l{stoppedInfo.m};
+                stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
+            });
+
+
+        if (timeoutMs > 0)
+        {
+            ARMARX_INFO << "future.wait()";
+            auto status = future.wait_for(std::chrono::milliseconds(timeoutMs));
+            ARMARX_INFO << "done";
+
+            switch (status)
+            {
+                case std::future_status::ready:
+                    ARMARX_INFO << "waitForStop: terminated on goal reached";
+                    break;
+                case std::future_status::timeout:
+                    ARMARX_INFO << "waitForStop: terminated due to timeout";
+                    ARMARX_INFO << "Stopping robot due to timeout";
+                    stop();
+
+                    throw LocalException("Navigator::waitForStop: timeout");
+                    break;
+                case std::future_status::deferred:
+                    ARMARX_INFO << "waitForStop: deferred";
+                    break;
+            }
+        }
+        else
+        {
+            ARMARX_INFO << "future.wait()";
+            future.wait();
+            ARMARX_INFO << "done";
+        }
+
+        // only due to timeout, stoppedInfo.event should be nullopt
+        ARMARX_CHECK(stoppedInfo.event.has_value());
 
         StopEvent e = stoppedInfo.event.value();
         stoppedInfo.event.reset();
+
         return e;
     }
 
diff --git a/source/armarx/navigation/client/Navigator.h b/source/armarx/navigation/client/Navigator.h
index 77547742eed62f88b36494417fe735ab6b63b5b8..836ebffba750c9ee8a8ae2e7a10f14933b05bdbc 100644
--- a/source/armarx/navigation/client/Navigator.h
+++ b/source/armarx/navigation/client/Navigator.h
@@ -143,7 +143,7 @@ namespace armarx::navigation::client
 
         void onWaypointReached(const std::function<void(int)>& callback);
 
-        StopEvent waitForStop();
+        StopEvent waitForStop(std::int64_t timeoutMs = -1);
 
     protected:
     private:
diff --git a/source/armarx/navigation/client/services/EventSubscriptionInterface.h b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
index b4e24d97ab4e2c925ba30c64b4cbdf7f26bc7c18..83cb40f2ccfdc1f2f3a4ab4df73d014c77e9efbc 100644
--- a/source/armarx/navigation/client/services/EventSubscriptionInterface.h
+++ b/source/armarx/navigation/client/services/EventSubscriptionInterface.h
@@ -15,7 +15,7 @@ namespace armarx::navigation::client
 {
 
     using GlobalTrajectoryUpdatedCallback =
-        std::function<void(const glob_plan::GlobalPlannerResult&)>;
+        std::function<void(const global_planning::GlobalPlannerResult&)>;
     using LocalTrajectoryUpdatedCallback = std::function<void(const loc_plan::LocalPlannerResult&)>;
     using TrajectoryControllerUpdatedCallback =
         std::function<void(const traj_ctrl::TrajectoryControllerResult&)>;
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index e287d426c8a30f8da960c3bd5d5bd6fe5db705ae..033ea36fac8deca65420af0b8de792ca88b77ffd 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -27,12 +27,37 @@ namespace armarx::navigation::client
         ARMARX_INFO << "MemorySubscriber: will handle all events newer than " << lastMemoryPoll
                     << ".";
 
+        // subscription api
+        armem::MemoryID subscriptionID;
+        subscriptionID.coreSegmentName = "Events";
+        subscriptionID.providerSegmentName = callerId;
+
+        // TODO add: memoryNameSystem.subscribe(subscriptionID, this, &MemorySubscriber::onEntityUpdate);
+
+        // polling api
         ARMARX_TRACE;
         task = new armarx::PeriodicTask<MemorySubscriber>(
             this, &MemorySubscriber::runPollMemoryEvents, 20);
         task->start();
     }
 
+    void
+    MemorySubscriber::onEntityUpdate(const armem::MemoryID& subscriptionID,
+                                     const std::vector<armem::MemoryID>& snapshotIDs)
+    {
+        ARMARX_INFO << "Received " << snapshotIDs.size() << " events from memory";
+        const armem::client::QueryResult qResult = memoryReader.queryMemoryIDs(snapshotIDs);
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            ARMARX_WARNING << deactivateSpam(0.1F) << "Memory lookup failed.";
+            return;
+        }
+
+        handleEvents(qResult.memory);
+    }
+
+
     void
     MemorySubscriber::handleEvent(const armem::wm::EntityInstance& memoryEntity)
     {
@@ -133,14 +158,14 @@ namespace armarx::navigation::client
         .snapshots().timeRange(lastMemoryPoll, now);
         // clang-format on
 
-        ARMARX_DEBUG << "Polling memory events in interval "
-                     << "[" << lastMemoryPoll << ", " << now << "]";
+        // ARMARX_DEBUG << "Polling memory events in interval "
+        //              << "[" << lastMemoryPoll << ", " << now << "]";
 
         lastMemoryPoll = now;
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
-        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+        // ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
         if (not qResult.success) /* c++20 [[unlikely]] */
         {
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.h b/source/armarx/navigation/client/services/MemorySubscriber.h
index 5a9fcbc6b251ca21438c8043b3a42e187b1b82a4..059305ddf91b90020a878e85dd1de24088c99d99 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.h
+++ b/source/armarx/navigation/client/services/MemorySubscriber.h
@@ -38,6 +38,10 @@ namespace armarx::navigation::client
         void runPollMemoryEvents();
         void handleEvents(const armem::wm::Memory& memory);
 
+
+        void onEntityUpdate(const armem::MemoryID& subscriptionID,
+                            const std::vector<armem::MemoryID>& snapshotIDs);
+                            
     private:
         const std::string callerId;
         armem::client::MemoryNameSystem& memoryNameSystem;
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.cpp b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
index cecf6e36268771f9ba33f1c3a64142e0cc2c7147..c570e6ac23298d651367fad07c85b60eeded0fca 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.cpp
+++ b/source/armarx/navigation/client/services/SimpleEventHandler.cpp
@@ -121,7 +121,7 @@ armarx::navigation::client::SimpleEventHandler::internalError(const core::Intern
 namespace armarx::navigation::client
 {
     void
-    SimpleEventHandler::globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& event)
+    SimpleEventHandler::globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& event)
     {
         for (const auto& callback : subscriptions.globalTrajectoryUpdatedCallbacks)
         {
diff --git a/source/armarx/navigation/client/services/SimpleEventHandler.h b/source/armarx/navigation/client/services/SimpleEventHandler.h
index 73fd4a53bb4a6ea1672f2bfa941368725f36e9c7..00fe7037fafa861a6a764dec2a20128db961111c 100644
--- a/source/armarx/navigation/client/services/SimpleEventHandler.h
+++ b/source/armarx/navigation/client/services/SimpleEventHandler.h
@@ -28,7 +28,7 @@ namespace armarx::navigation::client
 
         // EventPublishingInterface
     public:
-        void globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& event) override;
+        void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& event) override;
         void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& event) override;
         void
         trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& event) override;
diff --git a/source/armarx/navigation/client/test/clientTest.cpp b/source/armarx/navigation/client/test/clientTest.cpp
index 65b0d4188939e0b03e1ecc264280e355c30f2859..afea15205b2e1ed0abb5eec10fcbaaded2a12545 100644
--- a/source/armarx/navigation/client/test/clientTest.cpp
+++ b/source/armarx/navigation/client/test/clientTest.cpp
@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE(testExample)
 
     // create navigation stack config
     client::NavigationStackConfig cfg;
-    cfg.configureGlobalPlanner(glob_plan::AStarParams());
+    cfg.configureGlobalPlanner(global_planning::AStarParams());
 
     const auto stackConfig = cfg.toAron();
     // here, we would send data over Ice ...
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 512837bf861099c875aaf4b5b10ec81fb9ccbd00..5534d3b904661c267c660681fcd2a38e27b7f376 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -50,6 +50,7 @@
 
 #include "RobotAPI/components/ArViz/Client/Elements.h"
 #include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
 #include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/armem_vision/OccupancyGridHelper.h"
 #include "RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h"
@@ -163,7 +164,6 @@ namespace armarx::navigation::components
         navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
         navRemoteGui->enable();
 
-
         initialized = true;
 
         ARMARX_INFO << "Initialized. Will now respond to navigation requests.";
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
index e3d407bf7ee69c56860e7f1238d4dd69a6fe2ea1..a121450f04b3c513aaccc6d6c8844296533370fe 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
@@ -241,19 +241,19 @@ namespace armarx::navigation::components
             // server::NavigationStack stack
             // {
             // .globalPlanner =
-            // std::make_shared<glob_plan::Point2Point>(glob_plan::Point2PointParams(), scene),
+            // std::make_shared<global_planning::Point2Point>(global_planning::Point2PointParams(), scene),
             // .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
             //     traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
 
             // server::NavigationStack stack
             // {
             //     .globalPlanner =
-            //     std::make_shared<glob_plan::AStar>(glob_plan::AStarParams(), scene),
+            //     std::make_shared<global_planning::AStar>(global_planning::AStarParams(), scene),
             //     .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
             //         traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
 
             client::NavigationStackConfig cfg;
-            cfg.globalPlanner(glob_plan::AStarParams());
+            cfg.globalPlanner(global_planning::AStarParams());
             cfg.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams());
 
             const auto stackConfig = cfg.toAron();
diff --git a/source/armarx/navigation/components/example_client/Component.cpp b/source/armarx/navigation/components/example_client/Component.cpp
index 5b702a537dfc0b489e0ef23bf363a923e1b0bceb..51f8a6d617e8531a177618714a3323f96e36d117 100644
--- a/source/armarx/navigation/components/example_client/Component.cpp
+++ b/source/armarx/navigation/components/example_client/Component.cpp
@@ -99,7 +99,7 @@ namespace armarx::navigation::components::example_client
         configureNavigator(
             client::NavigationStackConfig()
                 .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
-                .globalPlanner(glob_plan::AStarParams())
+                .globalPlanner(global_planning::AStarParams())
                 .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
 
         // Example of registering a lambda as callback.
@@ -202,7 +202,7 @@ namespace armarx::navigation::components::example_client
         configureNavigator(
             client::NavigationStackConfig()
                 .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
-                .globalPlanner(glob_plan::AStarParams())
+                .globalPlanner(global_planning::AStarParams())
                 .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
 
         // Example of registering a lambda as callback.
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index f33715ddb614160a62ba9d7205da6a42fa7756c5..cfacb438deb2af1104db457f157b742f0ea6f831 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -29,6 +29,7 @@
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include "armarx/navigation/core/basic_types.h"
 #include "range/v3/algorithm/none_of.hpp"
 #include <armarx/navigation/conversions/eigen.h>
 #include <armarx/navigation/core/types.h>
@@ -427,17 +428,34 @@ namespace armarx::navigation::core
     {
         ARMARX_CHECK_GREATER_EQUAL(pts.size(), 2);
 
-        core::Positions resampledPathForward = resamplePath(pts, eps);
+        const core::Positions resampledPathForward = resamplePath(pts, eps);
+        const core::Positions resampledPathBackward =
+            resamplePath(pts | ranges::views::reverse, eps);
 
-        ARMARX_CHECK_NOT_EMPTY(resampledPathForward) << "Resampled path must not be empty.";
+        if (resampledPathForward.empty() or resampledPathBackward.empty())
+        {
+            ARMARX_DEBUG << "The resampled path contains no points. This means that it is likely "
+                            "very short.";
+
+            const core::Trajectory testTrajectory = core::Trajectory::FromPath(
+                pts.front().waypoint.pose, resampledPathForward, pts.back().waypoint.pose, 0.F);
+
+            ARMARX_CHECK_LESS_EQUAL(testTrajectory.length(), eps)
+                << "The resampled trajectory is only allowed to contains no points if it is "
+                   "shorter than eps";
+
+            return Trajectory::FromPath(
+                pts.front().waypoint.pose,
+                {}, // no intermediate points
+                pts.back().waypoint.pose,
+                500.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+        }
 
-        core::Positions resampledPathBackward = resamplePath(pts | ranges::views::reverse, eps);
 
-        // TODO(fabian.reister): debug
-        ARMARX_INFO << "Resampled path forward contains " << resampledPathForward.size()
-                    << " waypoints";
-        ARMARX_INFO << "Resampled path backwards contains " << resampledPathBackward.size()
-                    << " waypoints";
+        ARMARX_DEBUG << "Resampled path forward contains " << resampledPathForward.size()
+                     << " waypoints";
+        ARMARX_DEBUG << "Resampled path backwards contains " << resampledPathBackward.size()
+                     << " waypoints";
 
         core::Position pivot = resampledPathBackward.front();
 
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index 9162ffbfac24171d2a1f5bd8ab20243ceab45a51..b1c727f35a5be6206c757caef8363bc19109173f 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -60,8 +60,7 @@ namespace armarx::navigation::core
         LastWaypoint
     };
 
-    class Trajectory;
-    using TrajectoryPtr = std::shared_ptr<Trajectory>;
+    using TrajectoryPtr = std::shared_ptr<class Trajectory>;
 
     class Trajectory
     {
diff --git a/source/armarx/navigation/factories/GlobalPlannerFactory.cpp b/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
index 2e9944a1ffac8b2521b24339f1014ff22d5c9d2f..757b56e84dc861a907d8b27fd1baf38e1dbc81ed 100644
--- a/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
+++ b/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
@@ -12,11 +12,11 @@
 
 namespace armarx::navigation::fac
 {
-    glob_plan::GlobalPlannerPtr
+    global_planning::GlobalPlannerPtr
     GlobalPlannerFactory::create(const aron::data::DictPtr& params,
                                  const core::Scene& ctx)
     {
-        namespace layer = glob_plan;
+        namespace layer = global_planning;
 
         if (not params)
         {
@@ -34,16 +34,16 @@ namespace armarx::navigation::fac
             aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
         ARMARX_CHECK_NOT_NULL(algoParams);
 
-        glob_plan::GlobalPlannerPtr globalPlanner;
+        global_planning::GlobalPlannerPtr globalPlanner;
         switch (algo)
         {
-            case glob_plan::Algorithms::AStar:
-                globalPlanner = std::make_shared<glob_plan::AStar>(
-                    glob_plan::AStarParams::FromAron(algoParams), ctx);
+            case global_planning::Algorithms::AStar:
+                globalPlanner = std::make_shared<global_planning::AStar>(
+                    global_planning::AStarParams::FromAron(algoParams), ctx);
                 break;
-            case glob_plan::Algorithms::Point2Point:
-                globalPlanner = std::make_shared<glob_plan::Point2Point>(
-                    glob_plan::Point2PointParams::FromAron(algoParams), ctx);
+            case global_planning::Algorithms::Point2Point:
+                globalPlanner = std::make_shared<global_planning::Point2Point>(
+                    global_planning::Point2PointParams::FromAron(algoParams), ctx);
                 break;
         }
 
diff --git a/source/armarx/navigation/factories/GlobalPlannerFactory.h b/source/armarx/navigation/factories/GlobalPlannerFactory.h
index 4b366f82904c499e117fd59755776ecf87779962..4ef9a82393c6966cad3c4029bebcaf9f1c97092a 100644
--- a/source/armarx/navigation/factories/GlobalPlannerFactory.h
+++ b/source/armarx/navigation/factories/GlobalPlannerFactory.h
@@ -30,13 +30,13 @@
 namespace armarx::navigation::fac
 {
     /**
-    * @brief Factory to create a glob_plan::GlobalPlanner
+    * @brief Factory to create a global_planning::GlobalPlanner
     *
     */
     class GlobalPlannerFactory
     {
     public:
-        static glob_plan::GlobalPlannerPtr
+        static global_planning::GlobalPlannerPtr
         create(const aron::data::DictPtr& params, const core::Scene& ctx);
     };
 
diff --git a/source/armarx/navigation/factories/fwd.h b/source/armarx/navigation/factories/fwd.h
index 8d3d16706fdca371b0d8d57e7e21b68ca23f8b5b..7975180e3e3d6000b1dc1d87ef4e19f23f6bc496 100644
--- a/source/armarx/navigation/factories/fwd.h
+++ b/source/armarx/navigation/factories/fwd.h
@@ -26,11 +26,11 @@
 namespace armarx::navigation
 {
 
-    namespace glob_plan
+    namespace global_planning
     {
         class GlobalPlanner;
         using GlobalPlannerPtr = std::shared_ptr<GlobalPlanner>;
-    } // namespace glob_plan
+    } // namespace global_planning
 
     namespace loc_plan
     {
diff --git a/source/armarx/navigation/factories/test/factoriesTest.cpp b/source/armarx/navigation/factories/test/factoriesTest.cpp
index f2a6cbaf726183acd0f8c5fb739ccf4dd2ca15a5..adb15278328d732d3b2bccda4c35885db313f027 100644
--- a/source/armarx/navigation/factories/test/factoriesTest.cpp
+++ b/source/armarx/navigation/factories/test/factoriesTest.cpp
@@ -36,7 +36,7 @@
 BOOST_AUTO_TEST_CASE(testGlobalPlannerCreation)
 {
     // armarx::navigation::client::NavigationStackConfig cfg;
-    // cfg.globalPlanner(armarx::navigation::glob_plan::AStarParams())
+    // cfg.globalPlanner(armarx::navigation::global_planning::AStarParams())
     // .trajectoryController(armarx::navigation::traj_ctrl::TrajectoryFollowingController::Params());
 
     // const auto dto = cfg.toAron();
@@ -45,7 +45,7 @@ BOOST_AUTO_TEST_CASE(testGlobalPlannerCreation)
 
     // const auto navStack = armarx::navigation::fac::NavigationStackFactory::create(dto, scene);
 
-    // BOOST_CHECK(dynamic_cast<armarx::navigation::glob_plan::AStar*>(navStack.globalPlanner.get()) != nullptr);
+    // BOOST_CHECK(dynamic_cast<armarx::navigation::global_planning::AStar*>(navStack.globalPlanner.get()) != nullptr);
     // BOOST_CHECK(dynamic_cast<armarx::navigation::traj_ctrl::TrajectoryFollowingController*>(navStack.trajectoryControl.get()) != nullptr);
 }
 
@@ -53,7 +53,7 @@ BOOST_AUTO_TEST_CASE(testGlobalPlannerCreation)
 BOOST_AUTO_TEST_CASE(testStackCreation)
 {
     armarx::navigation::client::NavigationStackConfig cfg;
-    cfg.globalPlanner(armarx::navigation::glob_plan::AStarParams())
+    cfg.globalPlanner(armarx::navigation::global_planning::AStarParams())
         .trajectoryController(
             armarx::navigation::traj_ctrl::TrajectoryFollowingController::Params());
 
@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE(testStackCreation)
     BOOST_CHECK(navStack.globalPlanner.get() != nullptr);
     BOOST_CHECK(navStack.trajectoryControl.get() != nullptr);
 
-    BOOST_CHECK(dynamic_cast<armarx::navigation::glob_plan::AStar*>(navStack.globalPlanner.get()) !=
+    BOOST_CHECK(dynamic_cast<armarx::navigation::global_planning::AStar*>(navStack.globalPlanner.get()) !=
                 nullptr);
     BOOST_CHECK(dynamic_cast<armarx::navigation::traj_ctrl::TrajectoryFollowingController*>(
                     navStack.trajectoryControl.get()) != nullptr);
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 01ed0d548d6414751bbc56ebd2e1a46f6ece47a6..be670e2486fd1679c9f857ddde92e681e4acdebe 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -29,7 +29,7 @@
 #include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
 
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
 
     // AStarParams
@@ -287,4 +287,4 @@ namespace armarx::navigation::glob_plan
         ARMARX_TRACE;
         return GlobalPlannerResult{.trajectory = result.trajectory.value()};
     }
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/AStar.h b/source/armarx/navigation/global_planning/AStar.h
index 326284d8abad2ed94e89c1d64e218a16a5b1f56a..bb583dd571106cf686cd3821ceca6be93168a661 100644
--- a/source/armarx/navigation/global_planning/AStar.h
+++ b/source/armarx/navigation/global_planning/AStar.h
@@ -25,7 +25,7 @@
 #include "GlobalPlanner.h"
 #include <armarx/navigation/core/types.h>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
 
     /**
@@ -66,4 +66,4 @@ namespace armarx::navigation::glob_plan
         AStarParams params;
     };
 
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/CMakeLists.txt b/source/armarx/navigation/global_planning/CMakeLists.txt
index 4de838c4800f93ee893ef7ec22398800902063a6..7100ea9068fc250c98076cceb441e9d2ff259a57 100644
--- a/source/armarx/navigation/global_planning/CMakeLists.txt
+++ b/source/armarx/navigation/global_planning/CMakeLists.txt
@@ -29,6 +29,8 @@ armarx_add_library(global_planning
         ./Point2Point.h
         ./aron_conversions.h
         ./optimization/OrientationOptimizer.h
+        ./optimization/math.h
+        ./optimization/cost_functions.h
 )
 
 
diff --git a/source/armarx/navigation/global_planning/GlobalPlanner.cpp b/source/armarx/navigation/global_planning/GlobalPlanner.cpp
index 92d458986ef0e25ff88613f8d6e59f03a782f260..cddbcea968ee571531f8365e293183693f8330c6 100644
--- a/source/armarx/navigation/global_planning/GlobalPlanner.cpp
+++ b/source/armarx/navigation/global_planning/GlobalPlanner.cpp
@@ -3,11 +3,11 @@
 #include <armarx/navigation/core/constants.h>
 #include <armarx/navigation/core/types.h>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
 
     GlobalPlanner::GlobalPlanner(const core::Scene& context) : scene(context)
     {
     }
 
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/GlobalPlanner.h b/source/armarx/navigation/global_planning/GlobalPlanner.h
index 8a703bd3079c844ee9dcad99d9ba0603f8836089..8298361991b4235638517a83225183ac39d62f6b 100644
--- a/source/armarx/navigation/global_planning/GlobalPlanner.h
+++ b/source/armarx/navigation/global_planning/GlobalPlanner.h
@@ -32,7 +32,7 @@
 #include <armarx/navigation/core/types.h>
 #include <armarx/navigation/global_planning/core.h>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
 
     struct GlobalPlannerResult
@@ -78,4 +78,4 @@ namespace armarx::navigation::glob_plan
     };
 
     using GlobalPlannerPtr = std::shared_ptr<GlobalPlanner>;
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/Point2Point.cpp b/source/armarx/navigation/global_planning/Point2Point.cpp
index 53a66810ede1b473dec6b9d98cd564dbf57d5d5f..e6714c9abc07b011b14d9834414153fdccb39c28 100644
--- a/source/armarx/navigation/global_planning/Point2Point.cpp
+++ b/source/armarx/navigation/global_planning/Point2Point.cpp
@@ -14,7 +14,7 @@
 #include <armarx/navigation/global_planning/aron_conversions.h>
 #include <armarx/navigation/global_planning/core.h>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
 
     // Point2PointParams
@@ -92,4 +92,4 @@ namespace armarx::navigation::glob_plan
         return GlobalPlannerResult{.trajectory = trajectory};
     }
 
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/Point2Point.h b/source/armarx/navigation/global_planning/Point2Point.h
index 0477d96fe50e5644f0fa40f9f2edfccbf2cca4dd..33bfbd0398b671bed95c40e545ecd6a1b7aecf3e 100644
--- a/source/armarx/navigation/global_planning/Point2Point.h
+++ b/source/armarx/navigation/global_planning/Point2Point.h
@@ -25,7 +25,7 @@
 #include <armarx/navigation/core/types.h>
 #include <armarx/navigation/global_planning/GlobalPlanner.h>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
 
     /**
@@ -65,4 +65,4 @@ namespace armarx::navigation::glob_plan
 
     private:
     };
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/aron/AStarParams.xml b/source/armarx/navigation/global_planning/aron/AStarParams.xml
index 0cfb8d3acbc021644f6d6057626e7c44b7b9cb82..1e35c5c50bb94441ffa421b86f15320a5f59fe7d 100644
--- a/source/armarx/navigation/global_planning/aron/AStarParams.xml
+++ b/source/armarx/navigation/global_planning/aron/AStarParams.xml
@@ -9,8 +9,8 @@
 
     <GenerateTypes>
 
-        <Object name='armarx::navigation::glob_plan::arondto::AStarParams'>
-        <!-- <Object name='armarx::navigation::glob_plan::arondto::AStarParams' extends="armarx::navigation::glob_plan::arondto::GlobalPlannerParams"> -->
+        <Object name='armarx::navigation::global_planning::arondto::AStarParams'>
+        <!-- <Object name='armarx::navigation::global_planning::arondto::AStarParams' extends="armarx::navigation::global_planning::arondto::GlobalPlannerParams"> -->
              <ObjectChild key='linearVelocity'>
                 <float />
             </ObjectChild>
diff --git a/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml b/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml
index 9b6d4dd1fd54cf0f3e7d61fdc3c3e5b0fbc3e010..8139d1129215fa1d566b51a2b9f9537d7bc18574 100644
--- a/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml
+++ b/source/armarx/navigation/global_planning/aron/GlobalPlannerParams.xml
@@ -7,7 +7,7 @@
 
     <GenerateTypes>
 
-        <Object name='armarx::navigation::glob_plan::arondto::GlobalPlannerParams'>
+        <Object name='armarx::navigation::global_planning::arondto::GlobalPlannerParams'>
 
         </Object>
 
diff --git a/source/armarx/navigation/global_planning/aron/Point2PointParams.xml b/source/armarx/navigation/global_planning/aron/Point2PointParams.xml
index d4dd36f74256115d8053e8715a21bbe2ba833664..a1e6b05bacf27de3cb53c08418c6030f124d3ce0 100644
--- a/source/armarx/navigation/global_planning/aron/Point2PointParams.xml
+++ b/source/armarx/navigation/global_planning/aron/Point2PointParams.xml
@@ -8,8 +8,8 @@
     </AronIncludes>
 
     <GenerateTypes>
-        <!-- <Object name='armarx::navigation::glob_plan::arondto::Point2PointParams' extends="armarx::navigation::glob_plan::arondto::GlobalPlannerParams"> -->
-        <Object name='armarx::navigation::glob_plan::arondto::Point2PointParams'>
+        <!-- <Object name='armarx::navigation::global_planning::arondto::Point2PointParams' extends="armarx::navigation::global_planning::arondto::GlobalPlannerParams"> -->
+        <Object name='armarx::navigation::global_planning::arondto::Point2PointParams'>
             <ObjectChild key='includeStartPose'>
                 <bool />
             </ObjectChild>
diff --git a/source/armarx/navigation/global_planning/aron_conversions.cpp b/source/armarx/navigation/global_planning/aron_conversions.cpp
index 2ea3f799da41b505948b1418ce516d964fe67d34..84ec219a337197fdfc1c656975ffcfe14da14047 100644
--- a/source/armarx/navigation/global_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/global_planning/aron_conversions.cpp
@@ -11,7 +11,7 @@
 #include <armarx/navigation/global_planning/aron/GlobalPlannerParams.aron.generated.h>
 #include <armarx/navigation/global_planning/aron/Point2PointParams.aron.generated.h>
 
-namespace armarx::navigation::glob_plan::aron_conv
+namespace armarx::navigation::global_planning::aron_conv
 {
     void
     toAron(arondto::GlobalPlannerParams& dto, const GlobalPlannerParams& bo)
@@ -58,4 +58,4 @@ namespace armarx::navigation::glob_plan::aron_conv
         aron::fromAron(dto.linearVelocity, bo.linearVelocity);
         aron::fromAron(dto.resampleDistance, bo.resampleDistance);
     }
-} // namespace armarx::navigation::glob_plan::aron_conv
+} // namespace armarx::navigation::global_planning::aron_conv
diff --git a/source/armarx/navigation/global_planning/aron_conversions.h b/source/armarx/navigation/global_planning/aron_conversions.h
index 0c84a574d1bc927931877f928b896a7f18bc1c12..a27a6b0246452e9725187587a9bc7c7c82a14975 100644
--- a/source/armarx/navigation/global_planning/aron_conversions.h
+++ b/source/armarx/navigation/global_planning/aron_conversions.h
@@ -1,6 +1,6 @@
 #pragma once
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
     struct GlobalPlannerParams;
     struct Point2PointParams;
@@ -14,9 +14,9 @@ namespace armarx::navigation::glob_plan
 
     } // namespace arondto
 
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
 
-namespace armarx::navigation::glob_plan::aron_conv
+namespace armarx::navigation::global_planning::aron_conv
 {
     void toAron(arondto::GlobalPlannerParams& dto, const GlobalPlannerParams& bo);
     void fromAron(const arondto::GlobalPlannerParams& dto, GlobalPlannerParams& bo);
@@ -27,4 +27,4 @@ namespace armarx::navigation::glob_plan::aron_conv
     void toAron(arondto::AStarParams& dto, const AStarParams& bo);
     void fromAron(const arondto::AStarParams& dto, AStarParams& bo);
 
-} // namespace armarx::navigation::glob_plan::aron_conv
+} // namespace armarx::navigation::global_planning::aron_conv
diff --git a/source/armarx/navigation/global_planning/core.h b/source/armarx/navigation/global_planning/core.h
index a5d5aa31131614418dddd91967a6b0dbd68e891f..792e7b6f9f36afdc5a438e59375fd33bdca9c877 100644
--- a/source/armarx/navigation/global_planning/core.h
+++ b/source/armarx/navigation/global_planning/core.h
@@ -24,7 +24,7 @@
 
 #include <SimoxUtility/meta/enum/EnumNames.hpp>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning
 {
     /**
     * @defgroup Library-GlobalPlanner GlobalPlanner
@@ -45,4 +45,4 @@ namespace armarx::navigation::glob_plan
         {Algorithms::AStar, "AStar"},
         {Algorithms::Point2Point, "Point2Point"}};
 
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 554553dc0a0a3a0d2f39051414bf6073bcb02a7f..eb98cee4bb3139c5dcf846e7dd19fbde5a07af11 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -1,5 +1,6 @@
 #include "OrientationOptimizer.h"
 
+#include <algorithm>
 #include <cmath>
 
 #include <range/v3/range/conversion.hpp>
@@ -11,18 +12,21 @@
 #include <ceres/ceres.h>
 #include <ceres/cost_function.h>
 #include <ceres/jet.h>
+#include <ceres/rotation.h>
+#include <ceres/sized_cost_function.h>
 
 #include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
 #include <SimoxUtility/math/periodic/periodic_diff.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
 #include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/global_planning/optimization/cost_functions.h>
 
-
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning::optimization
 {
     OrientationOptimizer::OrientationOptimizer(const core::Trajectory& trajectory,
                                                const Params& params) :
@@ -30,117 +34,6 @@ namespace armarx::navigation::glob_plan
     {
     }
 
-    //   problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1);
-
-
-    inline auto
-    periodicDiff(auto a, auto b)
-    {
-        return a - b;
-
-        // angle to corresponding vector
-        const auto a1 = ceres::cos(a);
-        const auto a2 = ceres::sin(a);
-
-        // angle to corresponding vector
-        const auto b1 = ceres::cos(b);
-        const auto b2 = ceres::sin(b);
-
-        // cosine similarity
-        const auto cosSim = a1 * b1 + a2 * b2;
-
-        const auto angleDiff = ceres::acos(cosSim);
-
-        return angleDiff;
-    }
-
-    struct SmoothOrientationCostFunctor
-    {
-
-        SmoothOrientationCostFunctor(const double weight) : weight(weight)
-        {
-        }
-
-        template <typename T>
-        bool
-        operator()(const T* const yawPrev,
-                   const T* const yaw,
-                   const T* const yawNext,
-                   T* residual) const
-        {
-            residual[0] = weight * periodicDiff(yawPrev[0], yaw[0]);
-            residual[1] = weight * periodicDiff(yawNext[0], yaw[0]);
-            return true;
-        }
-
-    private:
-        const double weight;
-    };
-
-    struct SmoothOrientationFixedPreCostFunctor
-    {
-
-        SmoothOrientationFixedPreCostFunctor(const double yawPre, const double weight) :
-            yawPre(yawPre), weight(weight)
-        {
-        }
-
-        template <typename T>
-        bool
-        operator()(const T* const yaw, const T* const yawNext, T* residual) const
-        {
-            residual[0] = weight * periodicDiff(yawPre, yaw[0]);
-            residual[1] = weight * periodicDiff(yawNext[0], yaw[0]);
-            return true;
-        }
-
-    private:
-        const double yawPre;
-        const double weight;
-    };
-
-    struct SmoothOrientationFixedNextCostFunctor
-    {
-
-        SmoothOrientationFixedNextCostFunctor(const double yawNext, const double weight) :
-            yawNext(yawNext), weight(weight)
-        {
-        }
-
-        template <typename T>
-        bool
-        operator()(const T* const yawPre, const T* const yaw, T* residual) const
-        {
-            residual[0] = weight * periodicDiff(yawPre[0], yaw[0]);
-            residual[1] = weight * periodicDiff(yawNext, yaw[0]);
-            return true;
-        }
-
-    private:
-        const double yawNext;
-        const double weight;
-    };
-
-    struct OrientationPriorCostFunctor
-    {
-
-        OrientationPriorCostFunctor(const double prior, const double weight) :
-            prior(prior), weight(weight)
-        {
-        }
-
-        template <typename T>
-        bool
-        operator()(const T* const x, T* residual) const
-        {
-            residual[0] = weight * periodicDiff(prior, x[0]);
-            return true;
-        }
-
-    private:
-        const double prior;
-        const double weight;
-    };
 
     OrientationOptimizer::OptimizationResult
     OrientationOptimizer::optimize()
@@ -178,8 +71,7 @@ namespace armarx::navigation::glob_plan
             for (size_t i = 1; i < (orientations.size() - 1); i++)
             {
                 ceres::CostFunction* priorCostFunction =
-                    new ceres::AutoDiffCostFunction<OrientationPriorCostFunctor, 1, 1>(
-                        new OrientationPriorCostFunctor(orientations.at(i), params.priorWeight));
+                    new OrientationPriorCostFunctor(orientations.at(i), params.priorWeight);
 
                 problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
             }
@@ -193,8 +85,7 @@ namespace armarx::navigation::glob_plan
             for (size_t i = 2; i < (orientations.size() - 2); i++)
             {
                 ceres::CostFunction* smoothCostFunction =
-                    new ceres::AutoDiffCostFunction<SmoothOrientationCostFunctor, 2, 1, 1, 1>(
-                        new SmoothOrientationCostFunctor(params.smoothnessWeight));
+                    new SmoothOrientationCostFunctor(params.smoothnessWeight);
 
                 problem.AddResidualBlock(smoothCostFunction,
                                          nullptr,
@@ -210,10 +101,8 @@ namespace armarx::navigation::glob_plan
         {
             ARMARX_VERBOSE << "Enabled SmoothOrientationFixedPreCost";
 
-            ceres::CostFunction* smoothCostFunction =
-                new ceres::AutoDiffCostFunction<SmoothOrientationFixedPreCostFunctor, 2, 1, 1>(
-                    new SmoothOrientationFixedPreCostFunctor(orientations.front(),
-                                                             params.smoothnessWeightStartGoal));
+            ceres::CostFunction* smoothCostFunction = new SmoothOrientationFixedPreCostFunctor(
+                orientations.front(), params.smoothnessWeightStartGoal);
 
             problem.AddResidualBlock(
                 smoothCostFunction, nullptr, &orientations.at(1), &orientations.at(2));
@@ -224,10 +113,8 @@ namespace armarx::navigation::glob_plan
         {
             ARMARX_VERBOSE << "Enabled SmoothOrientationFixedNextCost";
 
-            ceres::CostFunction* smoothCostFunction =
-                new ceres::AutoDiffCostFunction<SmoothOrientationFixedNextCostFunctor, 2, 1, 1>(
-                    new SmoothOrientationFixedNextCostFunctor(orientations.back(),
-                                                              params.smoothnessWeightStartGoal));
+            ceres::CostFunction* smoothCostFunction = new SmoothOrientationFixedNextCostFunctor(
+                orientations.back(), params.smoothnessWeightStartGoal);
 
             problem.AddResidualBlock(smoothCostFunction,
                                      nullptr,
@@ -249,10 +136,15 @@ namespace armarx::navigation::glob_plan
         std::cout << summary.FullReport() << "\n";
         // std::cout << summary.BriefReport() << "\n";
 
+        const auto clampInPlace = [](auto& val)
+        { val = simox::math::periodic_clamp(val, -M_PI, M_PI); };
+
+        std::for_each(orientations.begin(), orientations.end(), clampInPlace);
+
         ARMARX_INFO << "orientations after: " << orientations;
         ARMARX_INFO << "Optimization: " << summary.iterations.size() << " iterations";
 
-        if(not summary.IsSolutionUsable())
+        if (not summary.IsSolutionUsable())
         {
             ARMARX_ERROR << "Orientation optimization failed!";
             // TODO write to file
@@ -276,4 +168,4 @@ namespace armarx::navigation::glob_plan
     }
 
 
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning::optimization
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
index a8a0ce6bec3e43bc80ddc4b44edb960006cf751e..f0b405adcad4f2f56581586acba559af3d497a55 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.h
@@ -23,7 +23,7 @@
 
 #include <armarx/navigation/core/Trajectory.h>
 
-namespace armarx::navigation::glob_plan
+namespace armarx::navigation::global_planning::optimization
 {
 
     struct OrientationOptimizerParams
@@ -60,4 +60,10 @@ namespace armarx::navigation::glob_plan
 
         const Params params;
     };
-} // namespace armarx::navigation::glob_plan
+} // namespace armarx::navigation::global_planning::optimization
+
+// FIXME remove
+namespace armarx::navigation::global_planning
+{
+    using namespace armarx::navigation::global_planning::optimization;
+}
diff --git a/source/armarx/navigation/global_planning/optimization/cost_functions.h b/source/armarx/navigation/global_planning/optimization/cost_functions.h
new file mode 100644
index 0000000000000000000000000000000000000000..ec6046f3b73311317f136e7099d0e4c955f6fc63
--- /dev/null
+++ b/source/armarx/navigation/global_planning/optimization/cost_functions.h
@@ -0,0 +1,260 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ceres/ceres.h>
+
+#include <armarx/navigation/global_planning/optimization/math.h>
+
+namespace armarx::navigation::global_planning::optimization
+{
+
+    struct SmoothOrientationFixedPreCostFunctor : ceres::CostFunction
+    {
+
+        SmoothOrientationFixedPreCostFunctor(const double yawPre, const double weight) :
+            yawPre(yawPre), weight(weight)
+        {
+            set_num_residuals(2);
+
+            mutable_parameter_block_sizes()->push_back(1);
+            mutable_parameter_block_sizes()->push_back(1);
+        }
+
+
+        bool
+        Evaluate(double const* const* parameters,
+                 double* residuals,
+                 double** jacobians) const override
+        {
+            const double yaw = parameters[0][0];
+            const double yawNext = parameters[0][1];
+
+            residuals[0] = weight * periodicDiff(yawPre, yaw);
+            residuals[1] = weight * periodicDiff(yaw, yawNext);
+
+            const auto periodicDiffJ0 = periodicDiffJacobian(yawPre, yaw);
+            const auto periodicDiffJ1 = periodicDiffJacobian(yaw, yawNext);
+
+            if (jacobians == nullptr)
+            {
+                return true;
+            }
+            double* jacobian0 = jacobians[0];
+            if (jacobian0 == nullptr)
+            {
+                return true;
+            }
+
+            double* jacobian1 = jacobians[1];
+            if (jacobian1 == nullptr)
+            {
+                return true;
+            }
+
+
+            // d r_i / d p_0
+            jacobian0[0] = weight * periodicDiffJ0.at(1);
+            jacobian0[1] = weight * periodicDiffJ1.at(0);
+            // jacobian0[2] = weight * periodicDiffJ1.at(0);
+
+            // d r_i / d p_1
+            jacobian1[0] = 0.;
+            jacobian1[1] = weight * periodicDiffJ1.at(1);
+
+            return true;
+        }
+
+    private:
+        const double yawPre;
+        const double weight;
+    };
+
+    struct SmoothOrientationFixedNextCostFunctor : ceres::CostFunction
+    {
+
+        SmoothOrientationFixedNextCostFunctor(const double yawNext, const double weight) :
+            yawNext(yawNext), weight(weight)
+        {
+            set_num_residuals(2);
+
+            mutable_parameter_block_sizes()->push_back(1);
+            mutable_parameter_block_sizes()->push_back(1);
+        }
+
+
+        bool
+        Evaluate(double const* const* parameters,
+                 double* residuals,
+                 double** jacobians) const override
+        {
+            const double yawPre = parameters[0][0];
+            const double yaw = parameters[0][1];
+
+            residuals[0] = weight * periodicDiff(yawPre, yaw);
+            residuals[1] = weight * periodicDiff(yaw, yawNext);
+
+            const auto periodicDiffJ0 = periodicDiffJacobian(yawPre, yaw);
+            const auto periodicDiffJ1 = periodicDiffJacobian(yaw, yawNext);
+
+            if (jacobians == nullptr)
+            {
+                return true;
+            }
+            double* jacobian0 = jacobians[0];
+            if (jacobian0 == nullptr)
+            {
+                return true;
+            }
+
+            double* jacobian1 = jacobians[1];
+            if (jacobian1 == nullptr)
+            {
+                return true;
+            }
+
+            // d r_i / d p_0
+            jacobian0[0] = weight * periodicDiffJ0.at(0);
+            jacobian0[1] = 0;
+
+            // d r_i / d p_1
+            jacobian1[0] = weight * periodicDiffJ0.at(1);
+            jacobian1[1] = weight * periodicDiffJ1.at(0);
+
+            return true;
+        }
+
+    private:
+        const double yawNext;
+        const double weight;
+    };
+
+    struct OrientationPriorCostFunctor : ceres::SizedCostFunction<1, 1>
+    {
+
+        OrientationPriorCostFunctor(const double prior, const double weight) :
+            prior(prior), weight(weight)
+        {
+        }
+
+        bool
+        Evaluate(double const* const* parameters,
+                 double* residuals,
+                 double** jacobians) const override
+        {
+            const double yaw = parameters[0][0];
+
+            residuals[0] = weight * periodicDiff(prior, yaw);
+
+            const auto periodicDiffJ = periodicDiffJacobian(prior, yaw);
+
+            if (jacobians == nullptr)
+            {
+                return true;
+            }
+            double* jacobian = jacobians[0];
+            if (jacobian == nullptr)
+            {
+                return true;
+            }
+
+
+            jacobian[0] = weight * periodicDiffJ.at(1);
+            return true;
+        }
+
+    private:
+        const double prior;
+        const double weight;
+    };
+
+
+    struct SmoothOrientationCostFunctor : ceres::CostFunction
+    {
+
+        SmoothOrientationCostFunctor(const double weight) : weight(weight)
+        {
+            set_num_residuals(2);
+
+            mutable_parameter_block_sizes()->push_back(1);
+            mutable_parameter_block_sizes()->push_back(1);
+            mutable_parameter_block_sizes()->push_back(1);
+        }
+
+        bool
+        Evaluate(double const* const* parameters,
+                 double* residuals,
+                 double** jacobians) const override
+        {
+            const double yawPrev = parameters[0][0];
+            const double yaw = parameters[0][1];
+            const double yawNext = parameters[0][2];
+
+            residuals[0] = weight * periodicDiff(yawPrev, yaw);
+            residuals[1] = weight * periodicDiff(yaw, yawNext);
+
+            const auto periodicDiffJ0 = periodicDiffJacobian(yawPrev, yaw);
+            const auto periodicDiffJ1 = periodicDiffJacobian(yaw, yawNext);
+
+            if (jacobians == nullptr)
+            {
+                return true;
+            }
+
+            double* jacobian0 = jacobians[0];
+            if (jacobian0 == nullptr)
+            {
+                return true;
+            }
+
+            double* jacobian1 = jacobians[1];
+            if (jacobian1 == nullptr)
+            {
+                return true;
+            }
+
+            double* jacobian2 = jacobians[2];
+            if (jacobian2 == nullptr)
+            {
+                return true;
+            }
+
+            // d r_i / d p_0
+            jacobian0[0] = weight * periodicDiffJ0.at(0);
+            jacobian0[1] = 0.;
+
+            // d r_i / d p_2
+            jacobian1[0] = weight * periodicDiffJ0.at(1);
+            jacobian1[1] = weight * periodicDiffJ1.at(0);
+
+            // d r_i / d p_1
+            jacobian2[0] = 0.;
+            jacobian2[1] = weight * periodicDiffJ1.at(1);
+
+            return true;
+        }
+
+    private:
+        const double weight;
+    };
+
+} // namespace armarx::navigation::global_planning::optimization
diff --git a/source/armarx/navigation/global_planning/optimization/math.h b/source/armarx/navigation/global_planning/optimization/math.h
new file mode 100644
index 0000000000000000000000000000000000000000..2007edb3906c9ab58c03c8caaa8cf0bc4fc5b2e5
--- /dev/null
+++ b/source/armarx/navigation/global_planning/optimization/math.h
@@ -0,0 +1,84 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <array>
+
+#include <ceres/ceres.h>
+
+namespace armarx::navigation::global_planning::optimization
+{
+
+    auto
+    periodicDiff(const auto a, const auto b)
+    {
+        // angle to corresponding vector
+        const auto a1 = ceres::cos(a);
+        const auto a2 = ceres::sin(a);
+
+        // angle to corresponding vector
+        const auto b1 = ceres::cos(b);
+        const auto b2 = ceres::sin(b);
+
+        // cosine similarity
+        const auto cosSim = a1 * b1 + a2 * b2;
+
+        const auto angleDiff = ceres::acos(cosSim);
+
+        return angleDiff;
+    }
+
+    std::array<double, 2> inline periodicDiffJacobian(const auto a, const auto b)
+    {
+        const auto derivative = [&a, &b]() -> double
+        {
+            if (a == 0. and b == 0.)
+            {
+                return 0.;
+            }
+
+
+            const double denomSquared =
+                1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b), 2);
+
+            // prevent division by 0
+            if(denomSquared < 0.001)
+            {
+                return 0.;
+            }
+
+            return (ceres::sin(a) * ceres::cos(b) - ceres::cos(a) * ceres::sin(b)) /
+                   (ceres::sqrt(
+                       1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b),
+                                      2)));
+        }();
+
+
+        std::array<double, 2> J;
+
+        // the partial derivative for a and b is similar
+        J.at(0) = derivative;
+        J.at(1) = -derivative;
+
+        return J;
+    }
+} // namespace armarx::navigation::global_planning::optimization
diff --git a/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
index 15cb8418333c1fecda89c52201737913ab897c66..5d6c131a4e9d2eb172380c317a534b7963f4c735 100644
--- a/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
+++ b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
@@ -42,7 +42,7 @@
 BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalPrior)
 {
     namespace nav = armarx::navigation;
-    namespace gp = armarx::navigation::glob_plan;
+    namespace gp = armarx::navigation::global_planning;
 
     nav::core::Path path;
     path.emplace_back(nav::core::Pose::Identity());
@@ -68,7 +68,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalPrior)
 BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalSmoothness)
 {
     namespace nav = armarx::navigation;
-    namespace gp = armarx::navigation::glob_plan;
+    namespace gp = armarx::navigation::global_planning;
 
     nav::core::Path path;
     path.emplace_back(nav::core::Pose::Identity());
@@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalSmoothness)
 BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalSmoothnessStartGoal)
 {
     namespace nav = armarx::navigation;
-    namespace gp = armarx::navigation::glob_plan;
+    namespace gp = armarx::navigation::global_planning;
 
     nav::core::Path path;
     path.emplace_back(nav::core::Pose::Identity());
@@ -120,7 +120,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoalMinimalSmoothnessStar
 BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoal)
 {
     namespace nav = armarx::navigation;
-    namespace gp = armarx::navigation::glob_plan;
+    namespace gp = armarx::navigation::global_planning;
 
     nav::core::Path path;
     path.emplace_back(nav::core::Pose::Identity());
@@ -163,7 +163,7 @@ BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoal)
 BOOST_AUTO_TEST_CASE(testOrientationOptimizationCircular)
 {
     namespace nav = armarx::navigation;
-    namespace gp = armarx::navigation::glob_plan;
+    namespace gp = armarx::navigation::global_planning;
 
     nav::core::Path path;
     path.emplace_back(nav::core::Pose::Identity());
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.cpp b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
index fd777c0b45465041c752265775d0bc1e204ae41d..95048a88103b77b72e1e2ecd14ae7e3cf2df4c2a 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.cpp
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
@@ -34,7 +34,7 @@ namespace armarx::navigation::mem::client::stack_result
     }
 
     bool
-    Writer::store(const armarx::navigation::glob_plan::GlobalPlannerResult& result,
+    Writer::store(const armarx::navigation::global_planning::GlobalPlannerResult& result,
                   const std::string& clientID)
     {
         armem::Commit commit;
@@ -56,7 +56,7 @@ namespace armarx::navigation::mem::client::stack_result
     }
 
     bool
-    Writer::storePrepare(const armarx::navigation::glob_plan::GlobalPlannerResult& result,
+    Writer::storePrepare(const armarx::navigation::global_planning::GlobalPlannerResult& result,
                          const std::string& clientID,
                          armem::Commit& commit)
     {
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.h b/source/armarx/navigation/memory/client/stack_result/Writer.h
index 1e13f1b08e562341046b003e3b14313f30d2e97f..5d7dac465e68cfb11f68c60af70c8bc4a365f5f1 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.h
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.h
@@ -38,14 +38,14 @@ namespace armarx::navigation::mem::client::stack_result
 
         bool store(const server::StackResult& result, const std::string& clientID);
 
-        bool store(const armarx::navigation::glob_plan::GlobalPlannerResult& result,
+        bool store(const armarx::navigation::global_planning::GlobalPlannerResult& result,
                    const std::string& clientID);
         bool store(const armarx::navigation::traj_ctrl::TrajectoryControllerResult& result,
                    const std::string& clientID);
 
 
     private:
-        bool storePrepare(const armarx::navigation::glob_plan::GlobalPlannerResult& result,
+        bool storePrepare(const armarx::navigation::global_planning::GlobalPlannerResult& result,
                           const std::string& clientID,
                           armem::Commit& commit);
 
diff --git a/source/armarx/navigation/server/NavigationStack.h b/source/armarx/navigation/server/NavigationStack.h
index 8841a61f6b26dd61c1fc11ba46cceed42890de77..9c047123f4a851cb85448f5ba3424ac3a4e625be 100644
--- a/source/armarx/navigation/server/NavigationStack.h
+++ b/source/armarx/navigation/server/NavigationStack.h
@@ -32,7 +32,7 @@ namespace armarx::navigation::server
 
     struct NavigationStack
     {
-        glob_plan::GlobalPlannerPtr globalPlanner;
+        global_planning::GlobalPlannerPtr globalPlanner;
         loc_plan::LocalPlannerPtr localPlanner = nullptr;
         traj_ctrl::TrajectoryControllerPtr trajectoryControl;
         safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 6bac92f26e0b915b30d14bc2dd2004aaaede75a4..0473f2ec5257055b9a45a07e6c6b0f3fa0f50f6f 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -574,7 +574,7 @@ namespace armarx::navigation::server
         // this is our `global plan`
         ARMARX_TRACE;
 
-        globalPlan = glob_plan::GlobalPlannerResult{.trajectory = globalPlanTrajectory};
+        globalPlan = global_planning::GlobalPlannerResult{.trajectory = globalPlanTrajectory};
 
         // the following is similar to moveToAbsolute
         // TODO(fabian.reister): remove code duplication
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index 26e4e81d39eee8a9f8369420173bc96f1883cd25..b02a8b50e21ca7fe8ef42020dcf24121b9b72a95 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -158,7 +158,7 @@ namespace armarx::navigation::server
         std::optional<GoalReachedMonitor> goalReachedMonitor;
 
 
-        std::optional<glob_plan::GlobalPlannerResult> globalPlan;
+        std::optional<global_planning::GlobalPlannerResult> globalPlan;
         std::optional<loc_plan::LocalPlannerResult> localPlan;
         std::optional<traj_ctrl::TrajectoryControllerResult> trajectoryCtrlResult;
         std::optional<safe_ctrl::SafetyControllerResult> safetyCtrlResult;
diff --git a/source/armarx/navigation/server/StackResult.h b/source/armarx/navigation/server/StackResult.h
index 007511e42da0a053fd421c9062831764c48d591c..c7f84cd3577c0c1ea3e7cae2cee2026a7a2323f7 100644
--- a/source/armarx/navigation/server/StackResult.h
+++ b/source/armarx/navigation/server/StackResult.h
@@ -38,7 +38,7 @@ namespace armarx::navigation::server
         using LocalPlannerResult = core::TrajectoryPtr;
         using SafetyControllerResult = std::optional<core::Twist>;
 
-        glob_plan::GlobalPlannerResult globalPlan;
+        global_planning::GlobalPlannerResult globalPlan;
         LocalPlannerResult localTrajectory;
         traj_ctrl::TrajectoryControllerResult controlVelocity;
         SafetyControllerResult safeVelocity;
diff --git a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
index 8aea91ad19cb6551106b8f6e45fec84155530c91..416d8cc61155b02144932de10002d803f7c87788 100644
--- a/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
+++ b/source/armarx/navigation/server/event_publishing/EventPublishingInterface.h
@@ -18,7 +18,7 @@ namespace armarx::navigation::server
 
     public:
         // TODO(fabian.reister): fwd
-        virtual void globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& res) = 0;
+        virtual void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) = 0;
         virtual void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) = 0;
         virtual void
         trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) = 0;
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
index e700334e99f8fff5afb568c8249e08b0060e4a2c..0f3cc13354c0cb23dc7b694132911422dfb473e3 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -46,7 +46,7 @@ namespace armarx::navigation::server
     }
 
     void
-    MemoryPublisher::globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& res)
+    MemoryPublisher::globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res)
     {
         resultWriter->store(res, clientId);
     }
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
index a0b031eeecbe291f0dc36fdad404395a5774ee32..1b130a83f09e0440e0ce3aca24cfb4e3bd54a5b5 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
@@ -16,7 +16,7 @@ namespace armarx::navigation::server
                         armarx::navigation::mem::client::events::Writer* eventsWriter,
                         const std::string& clientId);
 
-        void globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& res) override;
+        void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) override;
         void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) override;
         void trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) override;
 
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index ef944d6ee79a94ae4716cd68b7d2a75eeb4b9d4f..d14fd2e93e94eaf41d055e4b4aef3c76054262f7 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -33,7 +33,7 @@ namespace armarx::navigation::server
     // TODO maybe run with predefined frequency instead of
 
     void
-    ArvizIntrospector::onGlobalPlannerResult(const glob_plan::GlobalPlannerResult& result)
+    ArvizIntrospector::onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result)
     {
         std::lock_guard g{mtx};
 
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index 1bffdc3d63eea38e9b9f5a934cadc47bcb3aa6df..1cd9cf1265893097e3c5ba97b33c6ec0dcd74288 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -52,7 +52,7 @@ namespace armarx::navigation::server
         ArvizIntrospector(armarx::viz::Client arviz, const VirtualRobot::RobotPtr& robot);
         ~ArvizIntrospector() override = default;
 
-        void onGlobalPlannerResult(const glob_plan::GlobalPlannerResult& result) override;
+        void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
         void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
         void
         onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) override;
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 7f8ea471515126ee8777d304fa7d72821af2a478..92176601e325c9d2912c2f88e061afaea83abc83 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -42,7 +42,7 @@ namespace armarx::navigation::server
         virtual void success() = 0;
         virtual void failure() = 0;
 
-        virtual void onGlobalPlannerResult(const glob_plan::GlobalPlannerResult& result) = 0;
+        virtual void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) = 0;
         virtual void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) = 0;
         virtual void
         onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) = 0;
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
index 51b53ab276c6c47ab2effd14fa2d2ffe34fe1d27..bb0e4d0b01e7c30a084dfd57383cdbe8670b7749 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -11,7 +11,7 @@ namespace armarx::navigation::server
     }
 
     void
-    MemoryIntrospector::onGlobalPlannerResult(const glob_plan::GlobalPlannerResult& result)
+    MemoryIntrospector::onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result)
     {
         globPlanWriter.store(result, clientID);
     }
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index d2d2242d2979e04a9ca0f356efddff992cdf7037..c153b5218a529f8db719ad001a638e7c45f8cb0a 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -38,7 +38,7 @@ namespace armarx::navigation::server
         ~MemoryIntrospector() override = default;
 
 
-        void onGlobalPlannerResult(const glob_plan::GlobalPlannerResult& result) override;
+        void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
         void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
         void
         onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) override;
diff --git a/source/armarx/navigation/server/test/serverTest.cpp b/source/armarx/navigation/server/test/serverTest.cpp
index 5ed94415baae522a3f56c6a97546ed16e2b74785..7bcfcc6db4599200ef5501eede53208890aa0924 100644
--- a/source/armarx/navigation/server/test/serverTest.cpp
+++ b/source/armarx/navigation/server/test/serverTest.cpp
@@ -57,7 +57,7 @@ BOOST_AUTO_TEST_CASE(testNavigator)
     // TODO create static shared ctor
     server::NavigationStack stack{
         .globalPlanner =
-            std::make_shared<glob_plan::Point2Point>(glob_plan::Point2PointParams(), scene),
+            std::make_shared<global_planning::Point2Point>(global_planning::Point2PointParams(), scene),
         .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
             traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
 
@@ -80,7 +80,7 @@ BOOST_AUTO_TEST_CASE(testNavigatorWithFactory)
 
     // create navigation stack config
     client::NavigationStackConfig cfg;
-    cfg.configureGlobalPlanner(glob_plan::AStarParams());
+    cfg.configureGlobalPlanner(global_planning::AStarParams());
     cfg.configureTrajectoryController(traj_ctrl::TrajectoryFollowingControllerParams());
 
     const auto stackConfig = cfg.toAron();
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
index cef91163a5575444f3e05c0caaf6ede48075cd00..4d707e173afbf3f02d0ef7958fd7bb084caf5e90 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.cpp
@@ -65,7 +65,7 @@ namespace armarx::navigation::statecharts::navigation_group
         // parameterize the navigation stack
         client::NavigationStackConfig cfg;
         cfg.general(client::GeneralConfig{});
-        cfg.globalPlanner(armarx::navigation::glob_plan::AStarParams{});
+        cfg.globalPlanner(armarx::navigation::global_planning::AStarParams{});
         cfg.trajectoryController(
             armarx::navigation::traj_ctrl::TrajectoryFollowingControllerParams{});