diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 8b869fe31552807e5b47222e396eefa9d564092f..218aed8cfaedf754b40a52f68acd33da0b7ae69c 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -17,6 +17,13 @@
         <Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory"  include="RobotAPI/libraries/core/Trajectory.h"/>
     </Lib>
     <Lib name="RobotAPIInterfaces">
+
+        <Class typeName="NJointCartesianWaypointControllerConfig" include="RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h"  />
+        <Class typeName="FramedPoseBase" include="RobotAPI/interface/core/FramedPoseBase.h" />
+        <Class typeName="Vector3Base" include="RobotAPI/interface/core/PoseBase.h" />
+        <Class typeName="QuaternionBase" include="RobotAPI/interface/core/PoseBase.h" />
+        <Class typeName="armarx::viz::data::Element" include="RobotAPI/interface/ArViz/Elements.h" />
+        <Class typeName="armarx::viz::data::LayerUpdate" include="RobotAPI/interface/ArViz/Component.h" />
         <Proxy include="RobotAPI/interface/units/KinematicUnitInterface.h"
             humanName="Kinematic Unit"
             typeName="KinematicUnitInterfacePrx"
@@ -155,22 +162,6 @@
             getterName="getRobotUnit"
             propertyName="RobotUnitName"
             propertyIsOptional="false" />
-        <Proxy include="ArmarXCore/interface/observers/ObserverInterface.h"
-            humanName="Robot Unit Observer"
-            typeName="ObserverInterfacePrx"
-            memberName="robotUnitObserver"
-            getterName="getRobotUnitObserver"
-            propertyName="RobotUnitObserverName"
-            propertyIsOptional="true"
-            propertyDefaultValue="RobotUnitObserver" />
-        <Proxy include="ArmarXCore/interface/observers/ObserverInterface.h"
-            humanName="Speech Observer"
-            typeName="ObserverInterfacePrx"
-            memberName="speechObserver"
-            getterName="getSpeechObserver"
-            propertyName="SpeechObserverName"
-            propertyIsOptional="true"
-            propertyDefaultValue="SpeechObserver" />
         <Proxy include="RobotAPI/interface/units/CyberGloveObserverInterface.h"
             humanName="CyberGlove Observer"
             typeName="CyberGloveObserverInterfacePrx"
@@ -195,6 +186,16 @@
                 propertyName="NJointTrajectoryControllerName"
                 propertyIsOptional="true"
                 propertyDefaultValue="NJointTrajectoryController" />
+        <Proxy include="RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h"
+                humanName="NJoint Cartesian Waypoint Controller"
+                typeName="NJointCartesianControllerInterfacePrx"
+                memberName="NJointCartesianWaypointController"
+                getterName="getNJointCartesianWaypointController"
+                propertyName="NJointCartesianWaypointControllerName"
+                propertyIsOptional="true"
+                propertyDefaultValue="NJointCartesianWaypointController" />
+ 
+
         <Proxy include="RobotAPI/interface/core/RobotState.h"
             humanName="Robot State Component"
             typeName="RobotStateComponentInterfacePrx"
@@ -264,6 +265,17 @@
             propertyIsOptional="true"
             propertyDefaultValue="ArVizTopic">
         </Topic>
+        <Proxy include="RobotAPI/interface/ArViz/Component.h"
+            humanName="ArViz Storage"
+            typeName="armarx::viz::StorageInterfacePrx"
+            memberName="storageProxy"
+            getterName="getStorageProxy"
+            propertyName="StorageName"
+            propertyIsOptional="true"
+            propertyDefaultValue="ArViz">
+        </Proxy>
+
+
         <Topic include="RobotAPI/interface/speech/SpeechInterface.h"
             humanName="Text to Speech Topic"
             typeName="TextListenerInterfacePrx"
@@ -298,7 +310,7 @@
             propertyDefaultValue="DynamicObstacleManager" />
         <Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
             humanName="Grasp Candidate Observer"
-            typeName="grasping::GraspCandidateObserverInterfacePrx"
+            typeName="armarx::grasping::GraspCandidateObserverInterfacePrx"
             memberName="graspCandidateObserver"
             getterName="getGraspCandidateObserver"
             propertyName="GraspCandidateObserverName"
diff --git a/scenarios/ArMemCore/config/ArVizStorage.cfg b/scenarios/ArMemCore/config/ArVizStorage.cfg
index 5e8b630736a25dcd37ccfe9547d599b49a8dd6c6..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf 100644
--- a/scenarios/ArMemCore/config/ArVizStorage.cfg
+++ b/scenarios/ArMemCore/config/ArVizStorage.cfg
@@ -27,7 +27,7 @@
 # ArmarX.ArVizStorage.EnableProfiling = false
 
 
-# ArmarX.ArVizStorage.HistoryPath:  Destination path where the history are serialized to
+# ArmarX.ArVizStorage.HistoryPath:  Destination path where the history is serialized to
 #  Attributes:
 #  - Default:            RobotAPI/ArVizStorage
 #  - Case sensitivity:   yes
@@ -68,7 +68,7 @@
 # ArmarX.ArVizStorage.TopicName = ArVizTopic
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemCore/config/DebugObserver.cfg b/scenarios/ArMemCore/config/DebugObserver.cfg
index 459d0d1a5bcd9ea89ef01e161eb903fc8a4bf1cb..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d 100644
--- a/scenarios/ArMemCore/config/DebugObserver.cfg
+++ b/scenarios/ArMemCore/config/DebugObserver.cfg
@@ -18,7 +18,7 @@
 # ArmarX.ApplicationName = ""
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemCore/config/MemoryNameSystem.cfg b/scenarios/ArMemCore/config/MemoryNameSystem.cfg
index dfe9b389392a186f5baef2403145e31d49646f58..b8bc70a66ca7f32a628886ad1bf13e373f9750d3 100644
--- a/scenarios/ArMemCore/config/MemoryNameSystem.cfg
+++ b/scenarios/ArMemCore/config/MemoryNameSystem.cfg
@@ -18,7 +18,7 @@
 # ArmarX.ApplicationName = ""
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg
index 4683563a9c97e6bb11805b8d903dbe2f705a3fd2..4b6abea40d72afd7d313ee47a9b191f3b26de30d 100644
--- a/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg
@@ -18,7 +18,7 @@
 # ArmarX.ApplicationName = ""
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemExample/config/DebugObserver.cfg b/scenarios/ArMemExample/config/DebugObserver.cfg
index 459d0d1a5bcd9ea89ef01e161eb903fc8a4bf1cb..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d 100644
--- a/scenarios/ArMemExample/config/DebugObserver.cfg
+++ b/scenarios/ArMemExample/config/DebugObserver.cfg
@@ -18,7 +18,7 @@
 # ArmarX.ApplicationName = ""
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index aac8c2e88be85372ac0296f79579f8d2c32665e4..e2e7bb719d4629ed88b2127cf7c97159bdb79341 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -26,7 +26,7 @@
 ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
@@ -143,6 +143,23 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
 # ArmarX.ExampleMemory.mem.MemoryName = Example
 
 
+# ArmarX.ExampleMemory.mem.ltm..buffer.storeFreq:  Frequency to store the buffer to the LTM in Hz.
+#  Attributes:
+#  - Default:            10
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ExampleMemory.mem.ltm..buffer.storeFreq = 10
+
+
+# ArmarX.ExampleMemory.mem.ltm.depthImageExtractor.Enabled:  
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
 # ArmarX.ExampleMemory.mem.ltm.enabled:  
 #  Attributes:
 #  - Default:            false
@@ -152,12 +169,98 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
 # ArmarX.ExampleMemory.mem.ltm.enabled = false
 
 
-# ArmarX.ExampleMemory.mem.ltm.storagepath:  The path to the memory storage.
+# ArmarX.ExampleMemory.mem.ltm.exrConverter.Enabled:  
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.ExampleMemory.mem.ltm.imageExtractor.Enabled:  
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.Enabled:  
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.WaitingTime:  Waiting time in MS after each LTM update.
+#  Attributes:
+#  - Default:            -1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ExampleMemory.mem.ltm.pngConverter.Enabled:  
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.ExampleMemory.mem.ltm.sizeToCompressDataInMegaBytes:  The size in MB to compress away the current export. Exports are numbered (lower number means newer).
 #  Attributes:
-#  - Default:            /tmp/MemoryExport/Example
+#  - Default:            1024
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.ExampleMemory.mem.ltm.storagepath = /tmp/MemoryExport/Example
+# ArmarX.ExampleMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.Enabled:  
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.MaxWaitingTime:  Max Waiting time in MS after each Entity update.
+#  Attributes:
+#  - Default:            -1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.Enabled:  
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.WaitingTime:  Waiting time in MS after each Entity update.
+#  Attributes:
+#  - Default:            -1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ExampleMemory.mem.ltm.storagepath:  The path to the memory storage (the memory will be stored in a seperate subfolder).
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ExampleMemory.mem.ltm.storagepath = Default value not mapped.
 
 
 # ArmarX.ExampleMemory.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
@@ -178,6 +281,15 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
 # ArmarX.ExampleMemory.mns.MemoryNameSystemName = MemoryNameSystem
 
 
+# ArmarX.ExampleMemory.p.enableRemoteGui:  If true, the memory cotent is shown in the remote gui.Can be very slow for high-frequency updates!
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.p.enableRemoteGui = false
+
+
 # ArmarX.ExampleMemory.tpc.pub.DebugObserver:  Name of the `DebugObserver` topic to publish data to.
 #  Attributes:
 #  - Default:            DebugObserver
diff --git a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
index df84ec74b9f589842815ba742960cbc360da01e4..9c012266f8691c27cfa40be3ad793c081877178e 100644
--- a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
@@ -26,7 +26,7 @@
 ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemExample/config/MemoryNameSystem.cfg b/scenarios/ArMemExample/config/MemoryNameSystem.cfg
index dfe9b389392a186f5baef2403145e31d49646f58..b8bc70a66ca7f32a628886ad1bf13e373f9750d3 100644
--- a/scenarios/ArMemExample/config/MemoryNameSystem.cfg
+++ b/scenarios/ArMemExample/config/MemoryNameSystem.cfg
@@ -18,7 +18,7 @@
 # ArmarX.ApplicationName = ""
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/scenarios/ArMemExample/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemExample/config/RemoteGuiProviderApp.cfg
index 4683563a9c97e6bb11805b8d903dbe2f705a3fd2..4b6abea40d72afd7d313ee47a9b191f3b26de30d 100644
--- a/scenarios/ArMemExample/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/ArMemExample/config/RemoteGuiProviderApp.cfg
@@ -18,7 +18,7 @@
 # ArmarX.ApplicationName = ""
 
 
-# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
 #  Attributes:
 #  - Default:            mongo/.cache
 #  - Case sensitivity:   yes
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index d5c7d69917c8cbba5fafee284b18229f351ea9de..8e172a97d95ed3ce515743a877c8c5bb3ed168d1 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -87,7 +87,7 @@ namespace armarx::articulated_object
         articulatedObjectReader->connect();
 
         ARMARX_IMPORTANT << "Running example.";
-        start = armem::Time::now();
+        start = armem::Time::Now();
 
         task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
             this, &ArticulatedObjectLocalizerExample::run,
@@ -108,7 +108,7 @@ namespace armarx::articulated_object
     {
         const std::string dishwasherName = "Kitchen/mobile-dishwasher";
 
-        const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());
+        const auto descriptions = articulatedObjectReader->queryDescriptions(armem::Time::Now());
 
         ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
 
@@ -155,20 +155,22 @@ namespace armarx::articulated_object
 
         ARMARX_DEBUG << "Reporting articulated objects";
 
-        const IceUtil::Time now = TimeUtil::GetTime();
-        const float t           = float((now - start).toSecondsDouble());
+        const armem::Time now = armem::Time::Now();
+        const float t = float((now - start).toSecondsDouble());
 
         // move joints at certain frequency
         const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
 
-        const std::map<std::string, float> jointValues{{"dishwasher_door_joint", M_PIf32 / 2 * k},
-            {"drawer_joint", 350 * k}};
+        const std::map<std::string, float> jointValues
+        {
+            {"dishwasher_door_joint", M_PIf32 / 2 * k},
+            {"drawer_joint", 350 * k}
+        };
 
         dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
         dishwasher->setJointValues(jointValues);
 
-
-        articulatedObjectWriter->storeArticulatedObject(dishwasher, IceUtil::Time::now());
+        articulatedObjectWriter->storeArticulatedObject(dishwasher, now);
     }
 
 } // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index 22ff30654400ffb349b013b3d58e69b007f8ed18..5227bee38e728999d4a0f7fed6809913144573b6 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -43,6 +43,7 @@ namespace armarx
         ARMARX_INFO << "oninit GamepadControlUnit end";
 
 
+
     }
 
 
@@ -67,12 +68,42 @@ namespace armarx
 
     armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
     {
-        return armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
+        auto defs = armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
                 getConfigIdentifier()));
+	defs->topic(robotHealthTopic);
+	defs->optional(enableHeartBeat, "EnableHeartBeat", "Flag to enable send a heart beat to the robot healh topic");
+	return defs;
     }
 
     void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
     {
+
+
+       // struct GamepadData {
+       //     float leftStickX;
+       //     float leftStickY;
+       //     float rightStickX;
+       //     float rightStickY;
+       //     float dPadX;
+       //     float dPadY;
+       //     float leftTrigger;
+       //     float rightTrigger;
+       //
+       //     bool leftButton;
+       //     bool rightButton;
+       //     bool backButton;
+       //     bool startButton;
+       //     bool xButton;
+       //     bool yButton;
+       //     bool aButton;
+       //     bool bButton;
+       //     bool theMiddleButton;
+       //     bool leftStickButton;
+       //     bool rightStickButton;
+       //
+       // };
+
+
         if (data.leftTrigger > 0)
         {
             emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
@@ -91,6 +122,66 @@ namespace armarx
         {
             platformUnitPrx->move(0, 0, 0);
         }
+
+
+	if (data.leftButton)
+	{
+
+		if(leftHandTime <= 0.0)
+		{
+			leftHandTime = IceUtil::Time::now().toMicroSeconds();
+		}
+		else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000* 1000)
+		{
+
+			HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
+			if (handUnit)
+			{
+				std::string shapeName = (leftHandOpen) ? "Close" : "Open";
+				handUnit->setShape(shapeName);
+				leftHandOpen = !leftHandOpen;
+				leftHandTime = 0.0;
+			}
+		}
+	}
+	else
+	{
+		leftHandTime = 0.0;
+	}
+
+	if(data.rightButton)
+	{
+	
+		if(rightHandTime <= 0.0)
+		{
+			rightHandTime = IceUtil::Time::now().toMicroSeconds();
+		}
+		else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000* 1000)
+		{
+			HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
+			if (handUnit)
+			{
+				std::string shapeName = (rightHandOpen) ? "Close" : "Open";
+				handUnit->setShape(shapeName);
+				rightHandOpen = !rightHandOpen;
+				rightHandTime = 0.0;
+			}
+		}
+	}
+	else
+	{
+		rightHandTime = 0.0;
+	}
+
+
+	if(enableHeartBeat)
+	{
+		RobotHealthHeartbeatArgs args;
+        	args.maximumCycleTimeErrorMS = 1000;
+		robotHealthTopic->heartbeat(getDefaultName(), args);
+
+	}
+                
         //ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
     }
 }
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
index abdf77a77d42aa73a48bce6c93a15416bb8c64bc..9b0bacd5092012472d3f975e1c6d40357ba6594c 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
@@ -28,6 +28,10 @@
 
 #include <RobotAPI/interface/units/GamepadUnit.h>
 
+#include <RobotAPI/interface/units/HandUnitInterface.h>
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+
 namespace armarx
 {
     /**
@@ -103,11 +107,22 @@ namespace armarx
 
     private:
         PlatformUnitInterfacePrx platformUnitPrx;
+
+
+
+	bool enableHeartBeat = false;
+        RobotHealthInterfacePrx robotHealthTopic;
         float scaleX;
         float scaleY;
         float scaleRotation;
         EmergencyStopMasterInterfacePrx emergencyStop;
 
+	bool leftHandOpen = true;
+	bool rightHandOpen = true;
+
+	long leftHandTime = 0;
+	long rightHandTime = 0;
+
     public:
         void reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data,
                                 const TimestampBasePtr& timestamp, const Ice::Current& c) override;
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index adab12c7d13d5a4e9e032b0e964f9fea6e324c73..bbf6438cb32566dae0436d22770f8f89dd7d43c9 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -195,7 +195,7 @@ namespace armarx
                 }
 
                 pose.confidence = 0.75 + 0.25 * std::sin(t - i);
-                pose.timestamp = TimeUtil::GetTime();
+                pose.timestamp = DateTime::Now();
 
                 i++;
             }
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 19d3e19df844731d355a05a95986216d68ead88a..4d4d2399bb3ee5059d9788fb60f61cf23f78c2b0 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -356,10 +356,10 @@ namespace armarx
 
                 insertPose(time, globalRobotPose.transform);
                 _synchronized->setGlobalPose(globalRobotPose.transform);
-                _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
 
                 if (_sharedRobotServant)
                 {
+                    _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
                     _sharedRobotServant->setTimestamp(time);
                 }
             }
@@ -703,6 +703,7 @@ namespace armarx
         const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
 
         IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+        // ARMARX_IMPORTANT << VAROUT(currentPose.timestampInMicroSeconds);
 
         TransformStamped stamped;
         stamped.header.frame = armarx::GlobalFrame;
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index cdc049a27ad64057d179096185a2a5968f34d0d0..b8ab1db644a6228974453e648271c5b7975283fe 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -20,14 +20,14 @@
  *             GNU General Public License
  */
 
-// Header
 #include "LegacyRobotStateMemoryAdapter.h"
 
-// ArmarX
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
 
+
 namespace armarx::armem
 {
 
@@ -87,7 +87,7 @@ namespace armarx::armem
 
         // is this corect??
         prop.platform.acceleration = Eigen::Vector3f();
-        prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
+        prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x,  // this should be globasl AFAIK
                                                          update.platformPose.y,
                                                          update.platformPose.rotationAroundZ);
         prop.platform.velocity = Eigen::Vector3f(std::get<0>(update.platformVelocity),
@@ -96,7 +96,7 @@ namespace armarx::armem
 
         armem::EntityUpdate entityUpdate;
         entityUpdate.entityID = propEntityID;
-        entityUpdate.timeCreated = IceUtil::Time::microSeconds(_timestampUpdateFirstModifiedInUs); // we take the oldest timestamp
+        entityUpdate.timeCreated = Time(Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));  // we take the oldest timestamp
 
         entityUpdate.instancesData =
         {
@@ -128,7 +128,7 @@ namespace armarx::armem
 
             armem::EntityUpdate locUpdate;
             locUpdate.entityID = locEntityID;
-            locUpdate.timeCreated = IceUtil::Time::microSeconds(_timestampUpdateFirstModifiedInUs);
+            locUpdate.timeCreated = Time(Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
             locUpdate.instancesData =
             {
                transform.toAron()
@@ -247,7 +247,7 @@ namespace armarx::armem
         ARMARX_IMPORTANT << "Commiting Armar3 to descriptions";
         armem::arondto::RobotDescription desc;
         desc.name = "Armar3";
-        desc.timestamp = armem::Time::now();
+        toAron(desc.timestamp, armem::Time::Now());
         desc.xml.package = "RobotAPI";
         desc.xml.path = "RobotAPI/robots/Armar3/ArmarIII.xml";
 
@@ -261,7 +261,7 @@ namespace armarx::armem
         entityUpdate.entityID.entityName = "Armar3";
 
         entityUpdate.instancesData = { desc.toAron() };
-        entityUpdate.timeCreated = armem::Time::now();
+        entityUpdate.timeCreated = armem::Time::Now();
         auto res = memoryWriter.commit(c);
         if (!res.allSuccess())
         {
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index db3fb7ee4c04b359d97ca606e54fe2961750043c..05bb57827346342762bf482f906d6a6e10b83d5a 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -34,7 +34,6 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 
-
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -146,7 +145,7 @@ namespace armarx
     void ExampleMemoryClient::run()
     {
         ARMARX_IMPORTANT << "Running example.";
-        run_started = IceUtil::Time::now();
+        runStarted = armem::Time::Now();
 
         armem::MemoryID snapshotID = commitSingleSnapshot(exampleEntityID);
         if (true)
@@ -167,6 +166,10 @@ namespace armarx
             queryExampleData();
         }
         if (true)
+        {
+            commitExamplesWithIDs();
+        }
+        if (true)
         {
             commitExamplesWithLinks();
         }
@@ -210,9 +213,9 @@ namespace armarx
         // Prepare the update with some empty instances.
         armem::EntityUpdate update;
         update.entityID = entityID;
-        update.timeCreated = armem::Time::now();
+        update.timeCreated = armem::Time::Now();
 
-        double diff = (update.timeCreated - run_started).toMilliSecondsDouble() / 1000;
+        double diff = (update.timeCreated - runStarted).toMilliSecondsDouble() / 1000;
 
         auto dict1 = std::make_shared<aron::data::Dict>();
         auto dict2 = std::make_shared<aron::data::Dict>();
@@ -257,7 +260,7 @@ namespace armarx
         {
             armem::EntityUpdate& update = commit.add();
             update.entityID = entityID;
-            update.timeCreated = armem::Time::now() + armem::Time::seconds(i);
+            update.timeCreated = armem::Time::Now() + armem::Duration::Seconds(i);
             for (int j = 0; j < i; ++j)
             {
                 update.instancesData.push_back(std::make_shared<aron::data::Dict>());
@@ -364,7 +367,15 @@ namespace armarx
         }
         exampleDataProviderID = armem::MemoryID(addSegmentResult.segmentID);
 
-        const armem::Time time = armem::Time::now();
+        addSegmentResult = memoryWriter.addSegment("LinkedData", getName());
+        if (!addSegmentResult.success)
+        {
+            ARMARX_ERROR << addSegmentResult.errorMessage;
+            return;
+        }
+        linkedDataProviderID = armem::MemoryID(addSegmentResult.segmentID);
+
+        const armem::Time time = armem::Time::Now();
         armem::Commit commit;
 
         //commit to default
@@ -374,7 +385,8 @@ namespace armarx
             update.timeCreated = time;
 
             armem::example::ExampleData data;
-            toAron(data.memoryLink, armem::MemoryID());
+            toAron(data.memoryID, armem::MemoryID());
+            toAron(data.memoryLink.memoryID, armem::MemoryID());
             ARMARX_CHECK_NOT_NULL(data.toAron());
             update.instancesData = { data.toAron() };
         }
@@ -388,7 +400,7 @@ namespace armarx
 
             armem::example::ExampleData data;
             data.the_bool = true;
-            data.the_double = std::sin(time.toSecondsDouble());
+            data.the_double = std::sin(time.toDurationSinceEpoch().toSecondsDouble());
             data.the_float = 21.5;
             data.the_int = 42;
             data.the_long = 424242;
@@ -418,7 +430,8 @@ namespace armarx
             data.the_3x1_vector = { 24, 42, 2442 };
             data.the_4x4_matrix = 42 * Eigen::Matrix4f::Identity();
 
-            toAron(data.memoryLink, armem::MemoryID()); // ////1/1
+            toAron(data.memoryID, armem::MemoryID()); // ////1/1
+            toAron(data.memoryLink.memoryID, armem::MemoryID());
 
             simox::ColorMap cmap = simox::color::cmaps::plasma();
             {
@@ -466,10 +479,27 @@ namespace armarx
             update.instancesData = { data.toAron() };
         }
 
+        // commit to linked data
+        {
+            armem::EntityUpdate& update = commit.add();
+            update.entityID = linkedDataProviderID.withEntityName("yet_more_data");
+            update.timeCreated = time;
+
+            armem::example::LinkedData data;
+            data.yet_another_int = 42;
+            data.yet_another_string = "Hi! I'm from another core segment!";
+            data.yet_another_object.element_int = 8349;
+            data.yet_another_object.element_float = -1e3;
+            data.yet_another_object.element_string = "I'm a nested object in some linked data.";
+            ARMARX_CHECK_NOT_NULL(data.toAron());
+            update.instancesData = { data.toAron() };
+        }
+
         armem::CommitResult commitResult = memoryWriter.commit(commit);
         if (commitResult.allSuccess())
         {
-            armem::fromIce(commitResult.results.at(1).snapshotID, theAnswerSnapshotID);
+            theAnswerSnapshotID = commitResult.results.at(1).snapshotID;
+            yetMoreDataSnapshotID = commitResult.results.at(2).snapshotID;
         }
         else
         {
@@ -501,50 +531,54 @@ namespace armarx
     }
 
 
-    void ExampleMemoryClient::commitExamplesWithLinks()
+    void ExampleMemoryClient::commitExamplesWithIDs()
     {
         ARMARX_IMPORTANT << "Committing multiple entity updates with links ...";
-        const armem::Time time = armem::Time::now();
+        const armem::Time time = armem::Time::Now();
 
         armem::Commit commit;
         {
             armem::EntityUpdate& update = commit.add();
-            update.entityID = exampleDataProviderID.withEntityName("link to the_answer");
+            update.entityID = exampleDataProviderID.withEntityName("id to the_answer");
             update.timeCreated = time;
 
             armem::example::ExampleData data;
-            armem::toAron(data.memoryLink, theAnswerSnapshotID);
+            armem::toAron(data.memoryID, theAnswerSnapshotID);
+            armem::toAron(data.memoryLink.memoryID, armem::MemoryID());
 
             update.instancesData = { data.toAron() };
         }
         {
             armem::EntityUpdate& update = commit.add();
-            update.entityID = exampleDataProviderID.withEntityName("link to self");
+            update.entityID = exampleDataProviderID.withEntityName("id to self");
             update.timeCreated = time;
 
             armem::example::ExampleData data;
-            armem::toAron(data.memoryLink, update.entityID.withTimestamp(time));
+            armem::toAron(data.memoryID, update.entityID.withTimestamp(time));
+            armem::toAron(data.memoryLink.memoryID, armem::MemoryID());
 
             update.instancesData = { data.toAron() };
         }
 
         {
             armem::EntityUpdate& update = commit.add();
-            update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot");
-            update.timeCreated = time - armem::Time::seconds(1);  // 1 sec in the past
+            update.entityID = exampleDataProviderID.withEntityName("id to previous snapshot");
+            update.timeCreated = time - armem::Duration::Seconds(1);  // 1 sec in the past
 
             armem::example::ExampleData data;
-            armem::toAron(data.memoryLink, armem::MemoryID());  // First entry - invalid link
+            armem::toAron(data.memoryID, armem::MemoryID());  // First entry - invalid link
+            armem::toAron(data.memoryLink.memoryID, armem::MemoryID());
 
             update.instancesData = { data.toAron() };
         }
         {
             armem::EntityUpdate& update = commit.add();
-            update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot");
+            update.entityID = exampleDataProviderID.withEntityName("id to previous snapshot");
             update.timeCreated = time;
 
             armem::example::ExampleData data;
-            armem::toAron(data.memoryLink, update.entityID.withTimestamp(time - armem::Time::seconds(1)));
+            armem::toAron(data.memoryID, update.entityID.withTimestamp(time - armem::Duration::Seconds(1)));
+            armem::toAron(data.memoryLink.memoryID, armem::MemoryID());
 
             update.instancesData = { data.toAron() };
         }
@@ -584,9 +618,64 @@ namespace armarx
         }
     }
 
+
+    void ExampleMemoryClient::commitExamplesWithLinks()
+    {
+        ARMARX_IMPORTANT << "Committing an entity update with a link...";
+
+        const armem::Time time = armem::Time::Now();
+
+        armem::Commit commit;
+        {
+            armem::EntityUpdate& update = commit.add();
+            update.entityID = exampleDataProviderID.withEntityName("link to yet_more_data");
+            update.timeCreated = time;
+
+            armem::example::ExampleData data;
+            armem::toAron(data.memoryID, armem::MemoryID());
+            armem::toAron(data.memoryLink.memoryID, yetMoreDataSnapshotID);
+
+            update.instancesData = { data.toAron() };
+        }
+
+        ARMARX_CHECK_EQUAL(commit.updates.size(), 1);
+        armem::CommitResult commitResult = memoryWriter.commit(commit);
+
+        if (!commitResult.allSuccess() || commitResult.results.size() != commit.updates.size())
+        {
+            ARMARX_WARNING << commitResult.allErrorMessages();
+        }
+
+
+        // Resolve memory IDs via memory name system (works for IDs from different servers).
+        ARMARX_IMPORTANT << "Resolving multiple memory IDs via Memory Name System:";
+        {
+            std::vector<armem::MemoryID> ids;
+            for (armem::EntityUpdateResult& result : commitResult.results)
+            {
+                ids.push_back(result.snapshotID);
+            }
+            ARMARX_CHECK_EQUAL(ids.size(), commit.updates.size());
+
+            std::map<armem::MemoryID, armem::wm::EntityInstance> instances =
+                    memoryNameSystem().resolveEntityInstances(ids);
+            ARMARX_CHECK_EQUAL(instances.size(), commit.updates.size());
+
+            std::stringstream ss;
+            for (const auto& [id, instance]: instances)
+            {
+                ss << "- Snapshot " << id << " "
+                   << "\n--> Instance" << instance.id()
+                   << " (# keys in data: " << instance.data()->childrenSize() << ")"
+                   << "\n";
+            }
+            ARMARX_INFO << ss.str();
+        }
+    }
+
     void ExampleMemoryClient::commitExampleImages()
     {
-        const armem::Time time = armem::Time::now();
+        const armem::Time time = armem::Time::Now();
 
         armem::Commit commit;
         {
@@ -608,7 +697,7 @@ namespace armarx
 
     void ExampleMemoryClient::commitExamplesWithUntypedData()
     {
-        const armem::Time time = armem::Time::now();
+        const armem::Time time = armem::Time::Now();
 
         armem::Commit commit;
         {
@@ -617,7 +706,8 @@ namespace armarx
             update.timeCreated = time;
 
             armem::example::ExampleData data;
-            toAron(data.memoryLink, armem::MemoryID()); // ////1/1
+            toAron(data.memoryID, armem::MemoryID()); // ////1/1
+            toAron(data.memoryLink.memoryID, armem::MemoryID());
 
             aron::data::DictPtr aron = data.toAron();
             aron->addElement("unexpectedString", std::make_shared<aron::data::String>("unexpected value"));
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 930abfea7618c4ece3f7298c97376a54b201bac6..dcbcad65cb019296032664ee1c12fc14aee1d56e 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -98,6 +98,7 @@ namespace armarx
         void commitExampleData();
         void queryExampleData();
 
+        void commitExamplesWithIDs();
         void commitExamplesWithLinks();
 
         void commitExampleImages();
@@ -122,9 +123,11 @@ namespace armarx
 
         armem::MemoryID exampleDataProviderID;
         armem::MemoryID theAnswerSnapshotID;
+        armem::MemoryID linkedDataProviderID;
+        armem::MemoryID yetMoreDataSnapshotID;
 
 
-        IceUtil::Time run_started;
+        armem::Time runStarted;
         unsigned int imageCounter = 0;
 
         armarx::RunningTask<ExampleMemoryClient>::pointer_type task;
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index b249d3f8f42e4a019810759ef502180e644bff05..4a040fe4cf8a63fdef943265fdc401d5148d521e 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -85,21 +85,24 @@ namespace armarx
             // initialize all necessary fields of a grasp candidate and use writer to commit it to memory
             grasping::GraspCandidate candidate = makeDummyGraspCandidate();
             candidate.groupNr = i; //non-necessary field, but used to commit different candidates
+
             writer.commitGraspCandidate(candidate, armem::Time::now(), "candidateProvider");
 
             // initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
             grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
             bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
+
             writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::now(), "bimanualProvider");
 
+
             //test for writing Seqs, candidates from the same object appear as instances of the same snapshot
             grasping::GraspCandidateSeq candidatesToWrite;
             candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
             candidate.side = "Left";
             candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
 
-            writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::now(), "candidateProvider");
 
+            writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::now(), "candidateProvider");
 
             // test reader and debug by logging the group number of the candidate
 
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 552f68ac7ea45c24a0041dbedaff43a06c3e2c31..639203e464ecd0e1b75b085c0de7a59b8b283435 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -74,13 +74,14 @@ namespace armarx::robot_state
 
     void VirtualRobotReaderExampleClient::run()
     {
+        const armem::Time now = armem::Time::Now();
 
         // initialize if needed
         if (virtualRobot == nullptr)
         {
             TIMING_START(getRobot);
 
-            virtualRobot = virtualRobotReader.getRobot(p.robotName, IceUtil::Time::now());
+            virtualRobot = virtualRobotReader.getRobot(p.robotName, now);
 
             if (virtualRobot == nullptr)
             {
@@ -93,13 +94,9 @@ namespace armarx::robot_state
 
         ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
 
-        const IceUtil::Time now = TimeUtil::GetTime();
-
         TIMING_START(synchronizeRobot);
         virtualRobotReader.synchronizeRobot(*virtualRobot, now);
         TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
-
-
     }
 
 }  // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 58c3758d1ed5e56127d702137a0746ddfcd454a3..7ddc79776f7e56d2c71596f4e3c0c5656f18875f 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -1,6 +1,7 @@
 add_subdirectory(ExampleMemory)
 add_subdirectory(GeneralPurposeMemory)
 add_subdirectory(ObjectMemory)
+add_subdirectory(ReasoningMemory)
 add_subdirectory(RobotStateMemory)
 add_subdirectory(SkillsMemory)
 add_subdirectory(GraspMemory)
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 98788a3a67c1803f64d97d65a101223f0685e810..dc7309cf18c368d85994cc06a2944b856d9832f4 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -51,6 +51,9 @@ namespace armarx
                        "If enabled, core segments are added when required by a new provider segment."
                        "This will usually be off for most memory servers.");
 
+        defs->optional(p.enableRemoteGui, "p.enableRemoteGui",
+                       "If true, the memory cotent is shown in the remote gui."
+                       "Can be very slow for high-frequency updates!");
         return defs;
     }
 
@@ -60,10 +63,12 @@ namespace armarx
         return "ExampleMemory";
     }
 
+
     void ExampleMemory::onInitComponent()
     {
         // Usually, the memory server will specify a number of core segments with a specific aron type.
         workingMemory().addCoreSegment("ExampleData", armem::example::ExampleData::ToAronType());
+        workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType());
 
         // For illustration purposes, we add more segments (without types).
         bool trim = true;
@@ -72,14 +77,18 @@ namespace armarx
 
         for (const std::string& name : p.core.defaultCoreSegments)
         {
-            workingMemory().addCoreSegment(name);
+            auto& c = workingMemory().addCoreSegment(name);
+            c.setMaxHistorySize(100);
         }
     }
 
     void ExampleMemory::onConnectComponent()
     {
-        createRemoteGuiTab();
-        RemoteGui_startRunningTask();
+        if (p.enableRemoteGui)
+        {
+            createRemoteGuiTab();
+            RemoteGui_startRunningTask();
+        }
     }
 
     void ExampleMemory::onDisconnectComponent()
@@ -180,7 +189,7 @@ namespace armarx
                         .withProviderSegmentName(memoryID.providerSegmentName)
                         .withEntityName(memoryID.entityName);
                     update.entityID = newID;
-                    update.timeCreated = armem::Time::now();
+                    update.timeCreated = armem::Time::Now();
                     update.instancesData = { instance->data() };
 
                     armem::Commit newCommit;
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index 66a500eeb43043ceb660d9ad9c720d2dc946fb66..62ea177f496296082cac90aecc6e9e6f35d450a8 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -96,6 +96,8 @@ namespace armarx
             };
 
             CoreSegments core;
+
+            bool enableRemoteGui = false;
         };
         Properties p;
 
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index be9a9788fae8045e98d211d6a2b7397d6da455eb..4efce411b680ce9a91afceff7da5523b8f67d6cc 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -14,6 +14,7 @@ An example data containing different native ARON types.
 
     <AronIncludes>
         <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryLink.xml>" autoinclude="true" />
     </AronIncludes>
 
     <GenerateTypes>
@@ -29,6 +30,21 @@ An example data containing different native ARON types.
             </ObjectChild>
         </Object>
 
+        <Object name='armarx::armem::example::LinkedData'>
+
+            <ObjectChild key='yet_another_int'>
+                <Int />
+            </ObjectChild>
+            <ObjectChild key='yet_another_string'>
+                <String />
+            </ObjectChild>
+
+            <ObjectChild key='yet_another_object'>
+                <armarx::armem::example::InnerClass />
+            </ObjectChild>
+
+        </Object>
+
         <Object name='armarx::armem::example::ExampleData'>
 
             <ObjectChild key='the_int'>
@@ -43,9 +59,9 @@ An example data containing different native ARON types.
             <ObjectChild key='the_double'>
                 <Double />
             </ObjectChild>
-                <ObjectChild key='the_string'>
-            <String />
-                </ObjectChild>
+            <ObjectChild key='the_string'>
+                <String />
+            </ObjectChild>
             <ObjectChild key='the_bool'>
                 <Bool />
             </ObjectChild>
@@ -106,11 +122,16 @@ An example data containing different native ARON types.
                 </Dict>
             </ObjectChild>
 
-            <ObjectChild key="memoryLink">
+            <ObjectChild key="memoryID">
                 <armarx::armem::arondto::MemoryID />
             </ObjectChild>
 
-            </Object>
+            <ObjectChild key="memoryLink">
+                <armarx::armem::arondto::MemoryLink template="armarx::armem::example::LinkedData"/>
+            </ObjectChild>
+
+        </Object>
+
         </GenerateTypes>
 
 </AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index 5526876a302cd9658036a9603b753e47fd721121..362a2d2f338fe4e007a34756d006338ebd4611dc 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -41,7 +41,7 @@ BOOST_AUTO_TEST_CASE(test_ExampleData_type)
 {
     armarx::aron::type::ObjectPtr type = ExampleData::ToAronType();
 
-    BOOST_CHECK_EQUAL(type->childrenSize(), 16);
+    BOOST_CHECK_EQUAL(type->childrenSize(), 20);
 
     armem::wm::Memory memory;
     armem::wm::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index fd0f71b6456f5088f8d1e5740e3c2da0d3fa6047..50fee9d896eb0df9b59562c631c0c3be1f9d9328 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -23,7 +23,7 @@ namespace armarx::armem::server::grasp
         defs->topic(debugObserver);
 
         defs->optional(memoryName, "memory.Name", "Name of this memory server.");
-
+        defs->optional(enableRemoteGui, "remoteGui.enable", "Enable/Disable Remote GUI");
         return defs;
     }
 
@@ -54,10 +54,11 @@ namespace armarx::armem::server::grasp
 
     void GraspMemory::onConnectComponent()
     {
-
-       createRemoteGuiTab();
-       RemoteGui_startRunningTask();
-
+        if (enableRemoteGui)
+        {
+            createRemoteGuiTab();
+            RemoteGui_startRunningTask();
+        }
     }
 
     void GraspMemory::onDisconnectComponent()
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 45b1774441555563a0be414dc6e021324373b169..01c2a6760c653f49576575951bbf63ecf4f488b4 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -104,5 +104,7 @@ namespace armarx::armem::server::grasp
         };
         GuiInfo gui;
 
+        bool enableRemoteGui{true};
+
     };
 }
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 1d9c4ac147ddc2407952ed6db08c8fd4cfcc279b..15de8583b000cd27b844e6770e076e41ee5bfa9a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -22,6 +22,8 @@
 
 #include "ObjectMemory.h"
 
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+
 
 namespace armarx::armem::server::obj
 {
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d31cfd8a9646d5df26104efb45cfe855d6a6214
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_component_set_name("ReasoningMemory")
+
+
+set(COMPONENT_LIBS
+    ArmarXCore ArmarXCoreInterfaces  # for DebugObserverInterface
+    ArmarXGuiComponentPlugins
+    RobotAPICore RobotAPIInterfaces armem_server
+    RobotAPIComponentPlugins  # for ArViz and other plugins
+    armem_reasoning
+    armem_reasoning_server
+)
+
+set(SOURCES
+    ReasoningMemory.cpp
+)
+set(HEADERS
+    ReasoningMemory.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(
+    COMPONENT_NAMESPACE ::armarx::armem::server
+)
+
+
+
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ea316348abfa01cf08700ea362a6faa57a3afb9
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
@@ -0,0 +1,77 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ExampleMemory
+ * @author     Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "ReasoningMemory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::server
+{
+    armarx::PropertyDefinitionsPtr ReasoningMemory::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        setMemoryName("Reasoning");
+        return defs;
+    }
+
+
+    ReasoningMemory::ReasoningMemory() :
+        anticipationSegment(iceAdapter())
+    {
+
+    }
+
+    std::string ReasoningMemory::getDefaultName() const
+    {
+        return "ReasoningMemory";
+    }
+
+    void ReasoningMemory::onInitComponent()
+    {
+	
+	{
+        wm::CoreSegment& cs = workingMemory().addCoreSegment("Anticipation", armarx::reasoning::arondto::Anticipation::ToAronType());
+		cs.setMaxHistorySize(20);
+	}
+
+
+    }
+
+    void ReasoningMemory::onConnectComponent()
+    {
+    }
+
+    void ReasoningMemory::onDisconnectComponent()
+    {
+    }
+
+    void ReasoningMemory::onExitComponent()
+    {
+    }
+}
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ad670842bb15e1159326a977d23497af714aaa8
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
@@ -0,0 +1,55 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ExampleMemory
+ * @author     Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h>
+
+namespace armarx::armem::server
+{
+    class ReasoningMemory :
+        virtual public armarx::Component, 
+        virtual public armem::server::ReadWritePluginUser
+    {
+    public:
+        ReasoningMemory();
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+    protected:
+
+        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+        void onInitComponent() override;
+        void onConnectComponent() override;
+        void onDisconnectComponent() override;
+        void onExitComponent() override;
+
+
+    private:
+        reasoning::AnticipationSegment anticipationSegment;
+    };
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index c1bf3bc93f73ece3f5f85cf7a43a7a2d1ad270bb..5da13f82b5f74770c16f7fcbfb9c18723dd6cb80 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -191,7 +191,7 @@ namespace armarx::armem::server::robot_state
 
     armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp_us, const std::string& robotName, const ::Ice::Current& /*unused*/)
     {
-        auto timestamp = armem::Time::microSeconds(timestamp_us);
+        armem::Time timestamp { armem::Duration::MicroSeconds(timestamp_us) };
         ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp << ".";
 
         auto poseMap = localizationSegment.getRobotGlobalPoses(timestamp);
@@ -201,7 +201,7 @@ namespace armarx::armem::server::robot_state
             << "Robot with name " << robotName << " does not exist at or before timestamp " << timestamp << ".\n"
             << "Available robots are: " << simox::alg::get_keys(poseMap);
 
-        return new Pose(poseMap[robotName].matrix());
+        return { new Pose(poseMap[robotName].matrix()) };
     }
 
 
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
index ee1ea4e51bb0156822513f8481cc13338cc79b62..afbb7ef2abd07d6e718c7480ba7d84455574041d 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -6,7 +6,7 @@ set(COMPONENT_LIBS
     ArmarXGuiComponentPlugins
     RobotAPICore 
     RobotAPIInterfaces 
-    RobotAPI::skills
+    RobotAPISkills
     aron
     aronjsonconverter
 )
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 1ddbdb5a9efa1c2c98b4baefdb1e805a9af90fb8..ab6944e97cbcd54f16a6a6319928474e67ec7910 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -19,7 +19,7 @@ namespace armarx::skills::provider
             armarx::skills::Example::HelloWorldAcceptedType::ToAronType()
         })
     {}
-    Skill::Status HelloWorldSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
+    Skill::Status HelloWorldSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
     {
         ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n" <<
                             "I received the following data: \n" <<
@@ -28,6 +28,42 @@ namespace armarx::skills::provider
         return Skill::Status::Succeeded;
     }
 
+    ChainingSkill::ChainingSkill() :
+        Skill(SkillDescription{
+            "ChainingSkill",
+            "This skill calls the HelloWorld skill three times.",
+            {},
+            3000,
+            nullptr
+        })
+    {}
+    Skill::Status ChainingSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
+    {
+        armarx::skills::Example::HelloWorldAcceptedType exec1;
+        armarx::skills::Example::HelloWorldAcceptedType exec2;
+        armarx::skills::Example::HelloWorldAcceptedType exec3;
+
+        exec1.some_text = "Hello from the ChainingSkill 1";
+        exec2.some_text = "Hello from the ChainingSkill 2";
+        exec3.some_text = "Hello from the ChainingSkill 3";
+
+        manager::dto::SkillExecutionInfo exec;
+        exec.providerName = "SkillProviderExample";
+        exec.skillName = "HelloWorld";
+        exec.waitUntilSkillFinished = false;
+
+        exec.params = exec1.toAron()->toAronDictDTO();
+        ownerManager->executeSkill(exec);
+
+        exec.params = exec2.toAron()->toAronDictDTO();
+        ownerManager->executeSkill(exec);
+
+        exec.params = exec3.toAron()->toAronDictDTO();
+        ownerManager->executeSkill(exec);
+
+        return Skill::Status::Succeeded;
+    }
+
     TimeoutSkill::TimeoutSkill() :
         Skill(SkillDescription{
             "Timeout",
@@ -37,7 +73,7 @@ namespace armarx::skills::provider
             nullptr
         })
     {}
-    Skill::Status TimeoutSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
+    Skill::Status TimeoutSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
     {
         int i = 0;
         while (!timeoutReached)
@@ -64,7 +100,7 @@ namespace armarx::skills::provider
             nullptr
         })
     {}
-    Skill::Status CallbackSkill::_execute(const aron::data::DictPtr& d, const CallbackT& callback)
+    Skill::Status CallbackSkill::execute(const aron::data::DictPtr& d, const CallbackT& callback)
     {
         ARMARX_IMPORTANT << "Logging three updates via the callback";
         auto up1 = std::make_shared<aron::data::Dict>();
@@ -101,7 +137,7 @@ namespace armarx::skills::provider
     void SkillProviderExample::onInitComponent()
     {
         // Add example skill
-        addSkill(std::make_shared<HelloWorldSkill>());
+        addSkill(std::make_unique<HelloWorldSkill>());
 
         // Add another lambda example skill
         {
@@ -114,10 +150,13 @@ namespace armarx::skills::provider
         }
 
         // Add another example skill
-        addSkill(std::make_shared<CallbackSkill>());
+        addSkill(std::make_unique<CallbackSkill>());
 
         // Add timeout skill
-        addSkill(std::make_shared<TimeoutSkill>());
+        addSkill(std::make_unique<TimeoutSkill>());
+
+        // chaining
+        addSkill(std::make_unique<ChainingSkill>());
     }
 
     void SkillProviderExample::onConnectComponent()
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 2cd96b5ebea1cd9ab860521f1cc48b103ee21fec..57853d6420b99f99df2a15cd9523a88c48cc031f 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -36,7 +36,14 @@ namespace armarx::skills::provider
     {
     public:
         HelloWorldSkill();
-        Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+        Status execute(const aron::data::DictPtr&, const CallbackT&) final;
+    };
+
+    class ChainingSkill : public Skill
+    {
+    public:
+        ChainingSkill();
+        Status execute(const aron::data::DictPtr&, const CallbackT&) final;
     };
 
 
@@ -44,7 +51,7 @@ namespace armarx::skills::provider
     {
     public:
         TimeoutSkill();
-        Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+        Status execute(const aron::data::DictPtr&, const CallbackT&) final;
     };
 
 
@@ -52,7 +59,7 @@ namespace armarx::skills::provider
     {
     public:
         CallbackSkill();
-        Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+        Status execute(const aron::data::DictPtr&, const CallbackT&) final;
     };
 
     /**
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index d4b5ea81cab4f670ba0a073b7ae120b82295de49..41ffefe98882f69004610a233dee16614f5b4f60 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -126,7 +126,7 @@ void GraspCandidateObserver::reportGraspCandidates(const std::string& providerNa
 {
     std::unique_lock lock(dataMutex);
     this->candidates[providerName] = candidates;
-    graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::now(), providerName);
+    graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::Now(), providerName);
     handleProviderUpdate(providerName, candidates.size());
 }
 
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index c33e816f5fc871710c8c45c44742da32a55a14c6..bcbf667a05a4bf246cc074c2f253ed2f08f56810 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -111,7 +111,9 @@ set(LIB_HEADERS
     JointControllers/JointController.h
 
     NJointControllers/NJointController.h
-    NJointControllers/NJointController.ipp
+    NJointControllers/NJointControllerBase.h
+    NJointControllers/NJointControllerRegistry.h
+    NJointControllers/NJointControllerWithTripleBuffer.h
     NJointControllers/NJointTrajectoryController.h
     NJointControllers/NJointKinematicUnitPassThroughController.h
     NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
index 2f2bd887a7dfe5776b66892198bc719725bec3c0..68512914578343ac958a7ed4377b427a04357603 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
@@ -23,7 +23,8 @@
 #pragma once
 
 #include "ControlTargetBase.h"
-#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
 
 namespace armarx
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index 011b0c4283ae180a49cdc8ce3a80c43829eb067a..cf19d2b9f4cf9583bbd5e75e82b71f7cca6e8d0d 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -22,20 +22,22 @@
 
 #pragma once
 
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
+#include <RobotAPI/components/units/RobotUnit//util/HeterogenousContinuousContainerMacros.h>
+#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
+
+#include <IceUtil/Time.h>
+#include <Ice/Handle.h>
+
 #include <memory>
 #include <string>
 #include <map>
 
-#include <ArmarXCore/observers/variant/TimedVariant.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-
-#include "../util.h"
-#include "../Constants.h"
-#include "../ControlModes.h"
-#include "../util/HeterogenousContinuousContainerMacros.h"
 
 namespace armarx
 {
+    typedef ::IceInternal::Handle<class VariantBase> VariantBasePtr;
+
     /**
     * @class ControlTargetBase
     * @ingroup Library-RobotUnit
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
index 04b537b8545c57b162ff67f99735d4c21fa095b4..cad312b44637034c4c254a8b300e00c42cbb09fb 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
@@ -23,7 +23,8 @@
 #pragma once
 
 #include "ControlTargetBase.h"
-#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
 
 namespace armarx
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
index b4e2a00f3ca0439e70b6a9606f0a3ef3527922ac..869ad5163e781a218795dcedf119f1b096be9ccb 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
@@ -22,17 +22,24 @@
 
 #pragma once
 
-#include "../ControlTargets/ControlTargetBase.h"
-#include "../util.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
+
+#include <Ice/ProxyHandle.h>
 
 #include <memory>
 #include <atomic>
 
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+namespace IceProxy::armarx
+{
+    class DebugDrawerInterface;
+    class DebugObserverInterface;
+}
 
 namespace armarx
 {
     class ControlDevice;
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
 
     /**
      * @brief The JointController class represents one joint in one control mode.
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 28bc1e4819946d6734110fa3831e33f4bc1009b0..3cbe59bdfd36963431399159f48886f6ed24942a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -4,6 +4,10 @@
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 #include <VirtualRobot/math/Helpers.h>
 
@@ -59,7 +63,7 @@ namespace armarx
     }
 
     NJointCartesianNaturalPositionController::NJointCartesianNaturalPositionController(
-        RobotUnitPtr,
+        RobotUnit*,
         const NJointCartesianNaturalPositionControllerConfigPtr& config,
         const VirtualRobot::RobotPtr&)
     {
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 2a59d6d7dd6481df81fe6299e7ab2d9d598f9e7c..41276169749fe1156623f14bf4a8589c500b2266 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -1,13 +1,13 @@
 #pragma once
 
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
 #include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
 
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
 namespace armarx
 {
 
@@ -24,7 +24,7 @@ namespace armarx
     public:
         using ConfigPtrT = NJointCartesianNaturalPositionControllerConfigPtr;
         NJointCartesianNaturalPositionController(
-            RobotUnitPtr robotUnit,
+            RobotUnit* robotUnit,
             const NJointCartesianNaturalPositionControllerConfigPtr& config,
             const VirtualRobot::RobotPtr&);
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index cb488380e67ad03f0245ea270061870528ceb086..11b3246034d8b9f7ae38f6d60244e0b1f9f5f9cb 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -22,9 +22,13 @@
  *             GNU General Public License
  */
 #include "NJointCartesianTorqueController.h"
-#include <VirtualRobot/RobotNodeSet.h>
 #include "../RobotUnit.h"
 
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+
 #define DEFAULT_TCP_STRING "default TCP"
 
 namespace armarx
@@ -37,7 +41,7 @@ namespace armarx
         return "NJointCartesianTorqueController";
     }
 
-    NJointCartesianTorqueController::NJointCartesianTorqueController(RobotUnitPtr, NJointCartesianTorqueControllerConfigPtr config, const VirtualRobot::RobotPtr& r)
+    NJointCartesianTorqueController::NJointCartesianTorqueController(RobotUnit*, NJointCartesianTorqueControllerConfigPtr config, const VirtualRobot::RobotPtr& r)
     {
         ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
         useSynchronizedRtRobot();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index 2615b64cd005a228a351200c9b1d4e7e43749a41..2bb10f999e6cf7c9a0a873ce1316e5148550732f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -24,9 +24,8 @@
 #pragma once
 
 
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include <VirtualRobot/Robot.h>
-#include "../RobotUnit.h"
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
 #include "../SensorValues/SensorValue1DoFActuator.h"
 #include <VirtualRobot/IK/DifferentialIK.h>
@@ -77,7 +76,7 @@ namespace armarx
     {
     public:
         using ConfigPtrT = NJointCartesianTorqueControllerConfigPtr;
-        NJointCartesianTorqueController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+        NJointCartesianTorqueController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
         // NJointControllerInterface interface
         std::string getClassName(const Ice::Current&) const override;
@@ -93,7 +92,7 @@ namespace armarx
 
         static NJointCartesianTorqueControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
         NJointCartesianTorqueController(
-            RobotUnitPtr prov,
+            RobotUnit* prov,
             NJointCartesianTorqueControllerConfigPtr config,
             const VirtualRobot::RobotPtr& r);
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index eede99223f70e52f7d1a8ab1ed4e9b101901caa1..2b55cc15c11be833e581c36eafbdf31cdffec55c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -21,9 +21,13 @@
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
+
 #include "NJointCartesianVelocityController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
 #include <VirtualRobot/RobotNodeSet.h>
-#include "../RobotUnit.h"
+#include <VirtualRobot/Robot.h>
 
 #define DEFAULT_TCP_STRING "default TCP"
 
@@ -37,7 +41,7 @@ namespace armarx
         return "NJointCartesianVelocityController";
     }
 
-    NJointCartesianVelocityController::NJointCartesianVelocityController(RobotUnitPtr robotUnit, NJointCartesianVelocityControllerConfigPtr config, const VirtualRobot::RobotPtr& r)
+    NJointCartesianVelocityController::NJointCartesianVelocityController(RobotUnit* robotUnit, const NJointCartesianVelocityControllerConfigPtr& config, const VirtualRobot::RobotPtr& r)
     {
         useSynchronizedRtRobot();
         ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index 7afd5493e7dcf8f0d11cb898e29c22d09293e490..aed56406e6f690a8f58013848b44b6deffd6f9c5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -24,34 +24,19 @@
 #pragma once
 
 
-#include "NJointController.h"
-#include <VirtualRobot/Robot.h>
-#include "../RobotUnit.h"
-#include "../ControlTargets/ControlTarget1DoFActuator.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
 
 namespace armarx
 {
-
     TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityController);
-    //TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerConfig);
-
-    /*class NJointCartesianVelocityControllerConfig : virtual public NJointControllerConfig
-    {
-    public:
-        NJointCartesianVelocityControllerConfig(std::string const& nodeSetName, const std::string& tcpName, VirtualRobot::IKSolver::CartesianSelection mode):
-            nodeSetName(nodeSetName),
-            tcpName(tcpName),
-            mode(mode)
-        {}
-
-        const std::string nodeSetName;
-        const std::string tcpName;
-        const VirtualRobot::IKSolver::CartesianSelection mode;
-    };*/
 
     TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerControlData);
     class NJointCartesianVelocityControllerControlData
@@ -90,7 +75,7 @@ namespace armarx
     {
     public:
         using ConfigPtrT = NJointCartesianVelocityControllerConfigPtr;
-        NJointCartesianVelocityController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+        NJointCartesianVelocityController(RobotUnit* prov, const NJointCartesianVelocityControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
         // NJointControllerInterface interface
         std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 24cafe472b7c53b0548c8221f03206b13534f597..59f4b58be6c94973b3be1bf8de30e67a6d8f25b6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -22,9 +22,13 @@
  *             GNU General Public License
  */
 #include "NJointCartesianVelocityControllerWithRamp.h"
-#include <VirtualRobot/RobotNodeSet.h>
+
 #include "../RobotUnit.h"
 
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
 #define DEFAULT_TCP_STRING "default TCP"
 
 namespace armarx
@@ -38,7 +42,7 @@ namespace armarx
     }
 
     NJointCartesianVelocityControllerWithRamp::NJointCartesianVelocityControllerWithRamp(
-        const RobotUnitPtr& robotUnit,
+        RobotUnit* robotUnit,
         const NJointCartesianVelocityControllerWithRampConfigPtr& config,
         const VirtualRobot::RobotPtr& r)
     {
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index 7b9994929a379228956f72a0f6de7f8efbdcf474..37a362c9b5215db5367938b136fcb85e9f665622 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -24,9 +24,8 @@
 #pragma once
 
 
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include <VirtualRobot/Robot.h>
-#include "../RobotUnit.h"
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
 #include "../SensorValues/SensorValue1DoFActuator.h"
 #include <VirtualRobot/IK/DifferentialIK.h>
@@ -98,7 +97,7 @@ namespace armarx
     {
     public:
         using ConfigPtrT = NJointCartesianVelocityControllerWithRampConfigPtr;
-        NJointCartesianVelocityControllerWithRamp(const RobotUnitPtr& robotUnit, const NJointCartesianVelocityControllerWithRampConfigPtr& config, const VirtualRobot::RobotPtr&);
+        NJointCartesianVelocityControllerWithRamp(RobotUnit* robotUnit, const NJointCartesianVelocityControllerWithRampConfigPtr& config, const VirtualRobot::RobotPtr&);
 
         // NJointControllerInterface interface
         std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 6ad411f67b743fecaf8ed8fa82a31f0d3e0d0b8c..0ae54afc683c28b67a68856f86cdc8a2a3eb9e91 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -4,6 +4,11 @@
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 #include "NJointCartesianWaypointController.h"
 
@@ -43,7 +48,7 @@ namespace armarx
     }
 
     NJointCartesianWaypointController::NJointCartesianWaypointController(
-        RobotUnitPtr,
+        RobotUnit*,
         const NJointCartesianWaypointControllerConfigPtr& config,
         const VirtualRobot::RobotPtr&)
     {
@@ -312,6 +317,19 @@ namespace armarx
         _tripBufWpCtrl.commitWrite();
         ARMARX_IMPORTANT << "set new waypoint\n" << wp;
     }
+    void NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
+    {
+        Eigen::Matrix4f wp(data.data());
+        std::lock_guard g{_tripBufWpCtrlMut};
+        auto& w = _tripBufWpCtrl.getWriteBuffer();
+        w.wpsUpdated = true;
+        w.wps.clear();
+        w.wps.emplace_back(wp);
+        _tripBufWpCtrl.commitWrite();
+        ARMARX_IMPORTANT << "set new waypoint\n" << wp;
+    }
+
+
     void NJointCartesianWaypointController::setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
             const std::vector<Eigen::Matrix4f>& wps,
             const Ice::Current&)
@@ -468,4 +486,9 @@ namespace armarx
             }
         }
     }
+
+    int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+    {
+        return _publishWpsCur;
+    }
 }
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 9ed0b775c33ca9bc3a32c239d6635fd9d021044e..affada4d1467a0c313a1d32b708469a5c703ca50 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -1,13 +1,14 @@
 #pragma once
 
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
 
 #include <RobotAPI/libraries/core/CartesianWaypointController.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
 
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
 namespace armarx
 {
 
@@ -24,7 +25,7 @@ namespace armarx
     public:
         using ConfigPtrT = NJointCartesianWaypointControllerConfigPtr;
         NJointCartesianWaypointController(
-            RobotUnitPtr robotUnit,
+            RobotUnit* robotUnit,
             const NJointCartesianWaypointControllerConfigPtr& config,
             const VirtualRobot::RobotPtr&);
 
@@ -51,9 +52,11 @@ namespace armarx
     public:
         bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
         bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
+        int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
 
         void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
         void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
+        void setWaypointAx(const Ice::FloatSeq& data, const Ice::Current& = Ice::emptyCurrent) override;
         void setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
         void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
         void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 4a603b481ecee7da16744165688a26ce346f99fb..a127273c7551352855d23e762cb70b5ff3b5bfba 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -20,7 +20,8 @@
  *             GNU General Public License
  */
 
-#include "NJointController.h"
+#include "NJointControllerBase.h"
+#include "NJointControllerRegistry.h"
 
 #include <ArmarXCore/core/util/algorithm.h>
 #include <ArmarXCore/core/application/Application.h>
@@ -29,6 +30,14 @@
 
 #include <atomic>
 
+namespace armarx
+{
+    RobotUnit* getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
+    {
+        return cmngr->_modulePtr<RobotUnit>();
+    }
+}
+
 namespace armarx::RobotUnitModule
 {
 
@@ -121,10 +130,10 @@ namespace armarx
         return ds;
     }
 
-    RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
-    {
-        return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
-    }
+//    RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
+//    {
+//        return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
+//    }
 
     void NJointControllerBase::activateController(const Ice::Current&)
     {
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 057e069963abd69c62f7da0a54c86c7628b34654..f00b35bdbcabc38ece77e4e3514ad62a640d80c7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -22,943 +22,6 @@
 
 #pragma once
 
-#include <map>
-#include <atomic>
-#include <mutex>
-#include <functional>
-#include <unordered_set>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/util/Registrar.h>
-#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXGui/interface/WidgetDescription.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
-#include "../ControlTargets/ControlTargetBase.h"
-
-//units
-#include "../Devices/ControlDevice.h"
-#include "../Devices/SensorDevice.h"
-
-#include "../util.h"
-#include "../util/JointAndNJointControllers.h"
-#include "../util/ControlThreadOutputBuffer.h"
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <optional>
-
-namespace armarx::RobotUnitModule
-{
-    class NJointControllerAttorneyForPublisher;
-    class NJointControllerAttorneyForControlThread;
-    class NJointControllerAttorneyForControllerManagement;
-}
-
-namespace armarx::detail
-{
-    template<class> class NJointControllerRegistryEntryHelper;
-}
-
-namespace armarx
-{
-    using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
-
-    TYPEDEF_PTRS_HANDLE(NJointControllerBase);
-    TYPEDEF_PTRS_HANDLE(SynchronousNJointController);
-    TYPEDEF_PTRS_HANDLE(AsynchronousNJointController);
-
-    TYPEDEF_PTRS_HANDLE(RobotUnit);
-
-    /**
-     * @defgroup Library-RobotUnit-NJointControllers NJointControllers
-     * @ingroup Library-RobotUnit
-     */
-
-    /**
-    * @ingroup Library-RobotUnit-NJointControllers
-    * @brief A high level controller writing its results into \ref ControlTargetBase "ControlTargets".
-    *
-    * This is the abstract base class for all NJointControllers.
-    * It implements basic management routines required by the RobotUnit and some basic ice calls.
-    * \ref NJointControllerBase "NJointControllers" are instantiated and managed by the \ref RobotUnit.
-    *
-    * A \ref NJointControllerBase calculates \ref ControlTargetBase "ControlTargets" for a set of
-    * \ref ControlDevice "ControlDevices" in a specific ControlMode.
-    * This ControlMode is defined for each \ref ControlDevice during construction.
-    *
-    * \section nj-state Requested and Active
-    * A \ref NJointControllerBase can is requested / not requested and active / inactive.
-    * All four combinations are possible.
-    *
-    * \subsection nj-state-req Requested / Not Requested
-    * If the user wants this \ref NJointControllerBase to be executed it is in a requested state (see \ref isControllerRequested).
-    * Otherwise the controler is not requested.
-    *
-    * Calling \ref activateController sets the state to requested.
-    * Calling \ref deactivateController sets the state to not requested.
-    * If the \ref NJointControllerBase causes an error or a different \ref NJointControllerBase using one or more of the same
-    * \ref ControlDevice's is requested, this \ref NJointControllerBase is deactivated.
-    *
-    *
-    * \subsection nj-state-act Active / Inactive
-    * Is the \ref NJointControllerBase executed by the \ref RobotUnitModules::ControlThread "ControlThread" it is active
-    * (see \ref isControllerActive).
-    *
-    * To be executed by the \ref RobotUnitModules::ControlThread "ControlThread", the \ref NJointControllerBase has to be
-    * requested at some point in the past.
-    *
-    * If a controller is active, it has to write a valid \ref ControlTargetBase "ControlTarget"
-    * for each of its \ref ControlDevice "ControlDevicea" (if it fails, it will be deactivated).
-    *
-    * \section Constructor
-    * In the Constructor, a \ref NJointControllerBase has to declare all \ref ControlDevice "ControlDevices" it uses.
-    *
-    * The constructor takes a pointer to a configuration structure of the type \ref NJointControllerBase::ConfigPtrT.
-    * If an implementation requires a special configuration structure (e.g.: SomeOtherCfg), it has to override this type by calling adding
-    * \code{.cpp}
-    * using ConfigPtrT = SomeOtherCfgPtr;
-    * \endcode
-    * to it's clas definition.
-    * The type SomeOtherCfg has to derive \ref NJointControllerConfigPtr.
-    *
-    * There is a way to generate a small gui widget for controller construction (see this \ref nj-ctor-gui "section").
-    *
-    * \subsection nj-ctor-req-ctrl Using ControlTargets
-    * A \ref NJointControllerBase can use \ref peekControlDevice to examine a \ref ControlDevice before using it
-    * (e.g.: checking the supported \ref ControlTargetBase "ControlTargets").
-    *
-    * If a \ref ControlDevice should be used by this \ref NJointControllerBase, it has to call \ref useControlDevice.
-    * This sets the ControlMode for the \ref ControlDevice and returns a pointer to the \ref ControlTargetBase "ControlTarget".
-    * This pointer has to be used to send commands in each iteration of \ref rtRun.
-    * The ControlMode can't be changed afterwards (A new \ref NJointControllerBase has to be created).
-    *
-    *
-    * \subsection nj-ctor-req-sens Using SensorValues
-    * A \ref NJointControllerBase can use \ref peekSensorDevice to examine a \ref SensorDevice before using it.
-    *
-    * If a \ref SensorDevice should be used by this \ref NJointControllerBase, it has to call \ref useSensorDevice.
-    * This returns a pointer to the \ref SensorValueBase "SensorValue".
-    *
-    * \subsection nj-ctor-rob A synchronized Virtualrobot
-    * If the controller needs a simox robot in \ref rtRun, it should call \ref useSynchronizedRtRobot.
-    * This will provide a simoxRobot via \ref rtGetRobot.
-    * This robot is synchronized with the real robots state before calling \ref rtRun
-    *
-    * \section nj-parts Rt and non Rt
-    * Each \ref NJointControllerBase has two parts:
-    * \li The RT controll loop
-    * \li The NonRT ice communication
-    *
-    * \subsection rtcontrollloop The Rt controll loop (\ref rtRun)
-    * \warning This part has to satisfy realtime conditions!
-    * All realtime functions of \ref NJointControllerBase have the 'rt' prefix.
-    *
-    * Here the \ref NJointControllerBase has access to the robot's current state
-    * and has to write results into \ref ControlTargetBase "ControlTargets".
-    *
-    * It must not:
-    * \li call any blocking operation
-    * \li allocate ram on the heap (since this is a blocking operation)
-    * \li resize any datastructure (e.g. vector::resize) (this sometimes allocates ram on the heap)
-    * \li insert new datafields into datastructures (e.g. vector::push_back) (this sometimes allocates ram on the heap)
-    * \li write to any stream (e.g. ARMARX_VERBOSE, std::cout, print, filestreams) (this sometimes blocks/allocates ram on the heap)
-    * \li make network calls (e.g. through ice) (this blocks/allocates ram on the heap) (using begin_... end_... version of ice calls IS NO SOLUTION)
-    * \li do any expensive calculations (e.g. calculate IK, run some solver, invert big matrices)
-    *
-    * \subsection nonrtpart The NonRT ice communication
-    * This part consits of any ice communication.
-    * Here the \ref NJointControllerBase can get new controll parameters or targets from other components.
-    *
-    * \section rtnrtcomm Communication between RT and NonRT
-    * All communication between RT and NonRT has to be lockfree.
-    * The \ref NJointControllerBase has to use constructs like atomics or
-    * \ref TripleBuffer "TripleBuffers" (See \ref armarx::NJointControllerWithTripleBuffer).
-    *
-    * \image html NJointControllerGeneralDataFlow.svg "General Dataflow in a NJointControllerBase"
-    *
-    * \image html NJointControllerAtomicDataFlow.svg "Dataflow in a NJointControllerBase using atomics for communication between RT and NonRT"
-    *
-    * \image html NJointControllerTripleBufferDataFlow.svg "Dataflow in a NJointControllerBase using a triple buffer for communication between RT and NonRT"
-    *
-    *
-    * \image html NJointControllerDataFlow_Graph.svg "Dataflow in a NJointControllerBase as a Graph of the two participating domains"
-    * The above image shows the two participating domains: one RT thread and multiple ICE threads.
-    * If data has to flow along an arrow, you need some construct for non blocking message passing.
-    *
-    * \warning If you use \ref TrippleBuffer "TrippleBuffers" or \ref WriteBufferedTrippleBuffer "WriteBufferedTrippleBuffers" you need a separate one for each arrow.
-    *
-    * \section expensivework How to do expensive calculations
-    * You have to execute expensive calculations in a different worker thread.
-    * This thread could calculate target values at its own speed (e.g. 100 Hz).
-    *
-    * While rtRun runs at a higher frequency (e.g. 1000 Hz) and:
-    * \li reads target values
-    * \li optionally passes the target values to a PID controller
-    * \li writes them to the targets
-    * \li sends the sensor values to the worker thread.
-    *
-    * If you do some additional calculation in rtRun, you maybe need to pass config parameters from NonRT to RT using a nonblocking method.
-    *
-    * \image html NJointControllerWorkerThreadDataFlow.svg "Dataflow in a NJointControllerBase using a worker thread"
-    *
-    * \image html NJointControllerWorkerThreadDataFlow_Graph.svg "Dataflow in a NJointControllerBase using a worker thread as a Graph of the three participating domains"
-    * The above image shows the three participating domains: one RT thread, one worker trhead and multiple ICE threads.
-    * If data has to flow along an arrow, you need some construct for non blocking message passing.
-    *
-    * \warning If you use \ref TrippleBuffer "TrippleBuffers" or \ref WriteBufferedTrippleBuffer "WriteBufferedTrippleBuffers" you need a separate one for each arrow.
-    *
-    * \section nj-ctor-gui Providing a gui for controller construction.
-    * By implementing these functions:
-    * \code{.cpp}
-     static WidgetDescription::WidgetPtr GenerateConfigDescription
-     (
-        const VirtualRobot::RobotPtr& robot,
-        const std::map<std::string, ConstControlDevicePtr>& controlDevices,
-        const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
-     ); //describes how the widget is supposed to look
-     static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap&); // turns the resulting variants into a config
-    * \endcode
-    *
-    * The \ref RobotUnitGui will provide a widget to configure and construct a \ref NJointControllerBase of this type.
-    *
-    * \section Examples
-    *
-    * More examples can be found in the Tutorials Package
-    *
-    * \subsection nj-example-1 A simple pass Position controller
-    * \note The code can be found in the Tutorial package
-    *
-    * \subsection nj-example-1-h Header
-    * Include headers
-    * \code{.cpp}
-        #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-        #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-        #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-    * \endcode
-    *
-    * Open the namespace
-    * \code{.cpp}
-        namespace armarx
-        {
-    * \endcode
-    *
-    * Typedef some pointers
-    * \code{.cpp}
-            TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughController);
-            TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughControllerConfig);
-    * \endcode
-    *
-    * The config has to inherit \ref NJointControllerConfig and consists only of the joint name
-    * \code{.cpp}
-            class NJointPositionPassThroughControllerConfig : virtual public NJointControllerConfig
-            {
-            public:
-                NJointPositionPassThroughControllerConfig(const std::string& name): deviceName {name} {}
-                std::string deviceName;
-            };
-    * \endcode
-    *
-    * The controller class has to inherit \ref NJointControllerBase
-    * \code{.cpp}
-            class NJointPositionPassThroughController: public NJointControllerBase
-            {
-            public:
-    * \endcode
-    *
-    * The \ref NJointControllerBase provides a config widget for the \ref RobotUnitGuiPlugin
-    * \code{.cpp}
-                static WidgetDescription::WidgetPtr GenerateConfigDescription
-                (
-                    const VirtualRobot::RobotPtr&,
-                    const std::map<std::string, ConstControlDevicePtr>& controlDevices,
-                    const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
-                );
-                static NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
-    * \endcode
-    *
-    * The ctor receives a pointer to the \ref RobotUnit, a pointer to the config and a pointer to a VirtualRobot model.
-    * \code{.cpp}
-                NJointPositionPassThroughController(
-                    const RobotUnitPtr& prov,
-                    NJointControllerConfigPtr config,
-                    const VirtualRobot::RobotPtr& r
-                );
-    * \endcode
-    *
-    * The function to provide the class name.
-    * \code{.cpp}
-                std::string getClassName(const Ice::Current&) const override;
-    * \endcode
-    *
-    * This controller provides widgets for function calls.
-    * \code{.cpp}
-                WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
-                void callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&) override;
-    * \endcode
-    *
-    * The run function executed to calculate new targets
-    * \code{.cpp}
-                void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-    * \endcode
-    *
-    * Hooks for this \ref NJointControllerBase to execute code during publishing.
-    * \code{.cpp}
-                void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-                void onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
-                void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
-    * \endcode
-    *
-    * This \ref NJointControllerBase uses one position \ref SensorValue1DoFActuatorPosition "SensorValue", calculates one
-    * position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and stores the current position target and position value in atomics.
-    * \code{.cpp}
-                const SensorValue1DoFActuatorPosition* sensor;
-                ControlTarget1DoFActuatorPosition* target;
-                std::atomic<float> targetPos {0};
-                std::atomic<float> currentPos {0};
-    * \endcode
-    *
-    * Close the class and the namespace
-    * \code{.cpp}
-            };
-        }
-    * \endcode
-    *
-    * \subsection nj-example-1-c Source file
-    *
-    * Include the required headers.
-    * \code{.cpp}
-        #include "NJointPositionPassThroughController.h"
-        #include <RobotAPI/libraries/core/Pose.h>
-    * \endcode
-    *
-    * Open the namespace
-    * \code{.cpp}
-        namespace armarx
-        {
-    * \endcode
-    *
-    * This generates a config widget used to display a config gui in \ref RobotUnitGui.
-    * It consist out of a \ref HBoxLayout containing a \ref Label and a \ref StringComboBox
-    * \code{.cpp}
-            WidgetDescription::WidgetPtr NJointPositionPassThroughController::GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
-            {
-                using namespace armarx::WidgetDescription;
-                HBoxLayoutPtr layout = new HBoxLayout;
-                LabelPtr label = new Label;
-                label->text = "control device name";
-                layout->children.emplace_back(label);
-                StringComboBoxPtr box = new StringComboBox;
-                box->defaultIndex = 0;
-    * \endcode
-    *
-    * The \ref StringComboBox only contains the names of \ref ControlDevice "ControlDevices" accepting
-    * a position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and providing a position
-    * \ref SensorValue1DoFActuatorPosition "SensorValue"
-    * \code{.cpp}
-                //filter control devices
-                for (const auto& name2dev : controlDevices)
-                {
-                    const ConstControlDevicePtr& dev = name2dev.second;
-                    const auto& name = name2dev.first;
-                    if (
-                        dev->hasJointController(ControlModes::Position1DoF) &&
-                        dev->getJointController(ControlModes::Position1DoF)->getControlTarget()->isA<ControlTarget1DoFActuatorPosition>() &&
-                        sensorDevices.count(name) &&
-                        sensorDevices.at(name)->getSensorValue()->isA<SensorValue1DoFActuatorPosition>()
-                    )
-                    {
-                        box->options.emplace_back(name);
-                    }
-                }
-                box->name = "name";
-                layout->children.emplace_back(box);
-                return layout;
-            }
-    * \endcode
-    *
-    * This turns a map of variants into the config required by this \ref NJointControllerBase.
-    * The \ref Variant for the key 'name' defines the \ref ControlDevice.
-    * \code{.cpp}
-            NJointControllerConfigPtr NJointPositionPassThroughController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
-            {
-                //you should check here for types null additional params etc
-                return new NJointPositionPassThroughControllerConfig {values.at("name")->getString()};
-            }
-    * \endcode
-    *
-    * The constructors implementation.
-    * \code{.cpp}
-            NJointPositionPassThroughController::NJointPositionPassThroughController(
-                const RobotUnitPtr& ru,
-                NJointControllerConfigPtr config,
-                const VirtualRobot::RobotPtr&
-            )
-            {
-                ARMARX_CHECK_EXPRESSION(ru);
-    * \endcode
-    *
-    * It checks whether the provided config has the correct type.
-    * \code{.cpp}
-                NJointPositionPassThroughControllerConfigPtr cfg = NJointPositionPassThroughControllerConfigPtr::dynamicCast(config);
-                ARMARX_CHECK_EXPRESSION(cfg) << "The provided config has the wrong type! The type is " << config->ice_id();
-    * \endcode
-    *
-    * Then it retrieves the \ref SensorValue1DoFActuatorPosition "SensorValue" and
-    * \ref ControlTarget1DoFActuatorPosition "ControlTarget", casts then to the required types and stores them to the member variables.
-    * \code{.cpp}
-                const SensorValueBase* sv = useSensorValue(cfg->deviceName);
-                ControlTargetBase* ct = useControlTarget(cfg->deviceName, ControlModes::Position1DoF);
-                ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
-                ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
-                sensor = sv->asA<SensorValue1DoFActuatorPosition>();
-                target = ct->asA<ControlTarget1DoFActuatorPosition>();
-            }
-    * \endcode
-    *
-    * The class name has to be unique, hence the c++ class name should be used.
-    * \code{.cpp}
-            std::string NJointPositionPassThroughController::getClassName(const Ice::Current&) const
-            {
-                return "NJointPositionPassThroughController";
-            }
-
-    * \endcode
-    *
-    * This \ref NJointControllerBase provides two widgets for function calls.
-    * \code{.cpp}
-            WidgetDescription::StringWidgetDictionary NJointPositionPassThroughController::getFunctionDescriptions(const Ice::Current&) const
-            {
-                using namespace armarx::WidgetDescription;
-    * \endcode
-    *
-    * The first widget has a \ref Label and a \ref FloatSpinBox.
-    * \code{.cpp}
-                HBoxLayoutPtr layoutSetPos = new HBoxLayout;
-                {
-                    LabelPtr label = new Label;
-                    label->text = "positiontarget";
-                    layoutSetPos->children.emplace_back(label);
-                    FloatSpinBoxPtr spin = new FloatSpinBox;
-                    spin->defaultValue = 0;
-                    spin->max = 1;
-                    spin->min = -1;
-    * \endcode
-    *
-    * The \ref FloatSpinBox creates a variant called 'spinmearound'
-    * \code{.cpp}
-                    spin->name = "spinmearound";
-                    layoutSetPos->children.emplace_back(spin);
-                }
-    * \endcode
-    *
-    * The second widget also has a \ref Label and a \ref FloatSpinBox.
-    * \code{.cpp}
-                VBoxLayoutPtr layoutSetHalfPos = new VBoxLayout;
-                {
-                    LabelPtr label = new Label;
-                    label->text = "positiontarget / 2";
-                    layoutSetHalfPos->children.emplace_back(label);
-                    FloatSpinBoxPtr spin = new FloatSpinBox;
-                    spin->defaultValue = 0;
-                    spin->max = 0.5;
-                    spin->min = -0.5;
-                    spin->name = "spinmehalfaround";
-                    layoutSetHalfPos->children.emplace_back(spin);
-                }
-    * \endcode
-    *
-    * This returns a map of boths widgets.
-    * The keys will be used to identify the called function.
-    * \code{.cpp}
-                return {{"SetPosition", layoutSetPos}, {"SetPositionHalf", layoutSetHalfPos}};
-            }
-    * \endcode
-    *
-    * This function is called from the \ref RobotUnitGuiPlugin when the prior defined functions are called.
-    * Both functions set the target position.
-    * \code{.cpp}
-            void NJointPositionPassThroughController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&)
-            {
-                if (name == "SetPosition")
-                {
-                    //you should check here for types null additional params etc
-                    targetPos = value.at("spinmearound")->getFloat();
-                }
-                else if (name == "SetPositionHalf")
-                {
-                    //you should check here for types null additional params etc
-                    targetPos = value.at("spinmehalfaround")->getFloat() * 2;
-                }
-                else
-                {
-                    ARMARX_ERROR << "CALLED UNKNOWN REMOTE FUNCTION: " << name;
-                }
-            }
-    * \endcode
-    *
-    * The \ref rtRun function sets the \ref ControlTarget1DoFActuatorPosition "ControlTarget" to the
-    * target value set via the atomic and stores the current
-    * \ref SensorValue1DoFActuatorPosition "SensorValue" to the other atomic.
-    * \code{.cpp}
-            void NJointPositionPassThroughController::rtRun(const IceUtil::Time& t, const IceUtil::Time&)
-            {
-                ARMARX_RT_LOGF_ERROR("A MESSAGE PARAMETER %f", t.toSecondsDouble()).deactivateSpam(1);
-                ARMARX_RT_LOGF_IMPORTANT("A MESSAGE WITHOUT PARAMETERS").deactivateSpam(1);
-                target->position = targetPos;
-                currentPos = sensor->position;
-            }
-    * \endcode
-    *
-    * The publish activation \ref onPublishActivation "hook" does nothing, but could be used to call
-    * \ref DebugDraver::setRobotVisu.
-    * \code{.cpp}
-            void NJointPositionPassThroughController::onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
-            {
-                //we could do some setup for the drawer here. e.g. add a robot
-            }
-
-    * \endcode
-    *
-    * The publish deactivation \ref onPublishDeactivation "hook" clears the \ref DebugDrawer layer
-    * \code{.cpp}
-            void NJointPositionPassThroughController::onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
-            {
-                drawer->removeLayer("Layer_" + getInstanceName());
-            }
-
-    * \endcode
-    *
-    * The publish \ref onPublish "hook" draws a sphere with radius of the position stored in the atomic times 2000 to the \ref DebugDrawer.
-    * \code{.cpp}
-            void NJointPositionPassThroughController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
-            {
-                drawer->setSphereVisu("Layer_" + getInstanceName(), "positionball", new Vector3, armarx::DrawColor {1, 1, 1, 1}, currentPos * 2000);
-            }
-    * \endcode
-    *
-    * This registers a factory for this \ref NJointControllerBase.
-    * This factory is used by the \ref RobotUnit to create the \ref NJointControllerBase.
-    * The passed name has to match the name returned by \ref getClassName.
-    * \code{.cpp}
-            NJointControllerRegistration<NJointPositionPassThroughController> registrationControllerNJointPositionPassThroughController("NJointPositionPassThroughController");
-    * \endcode
-    *
-    * This statically asserts a factory for the type NJointPositionPassThroughController is present.
-    * \code{.cpp}
-            ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(NJointPositionPassThroughController);
-    * \endcode
-    * Close the namespace
-    * \code{.cpp}
-        }
-    * \endcode
-    */
-    class NJointControllerBase:
-        virtual public NJointControllerInterface,
-        virtual public ManagedIceObject
-    {
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // /////////////////////////////////////// typedefs /////////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    public:
-        using ConfigPtrT = NJointControllerConfigPtr;
-        using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr(*)
-                (
-                    const VirtualRobot::RobotPtr&,
-                    const std::map<std::string, ConstControlDevicePtr>& controlDevices,
-                    const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
-                );
-        template<class ConfigPrtType>
-        using GenerateConfigFromVariantsFunctionSignature = ConfigPrtType(*)(const StringVariantBaseMap&);
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ///////////////////////////// constructor setup functions ////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    public:
-        /**
-         * @brief Get a const ptr to the given \ref SensorDevice
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         * @param deviceName The \ref SensorDevice's name
-         * @return A const ptr to the given \ref SensorDevice
-         */
-        ConstSensorDevicePtr   peekSensorDevice(const std::string& deviceName) const;
-        /**
-         * @brief Get a const ptr to the given \ref ControlDevice
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         * @param deviceName The \ref ControlDevice's name
-         * @return A const ptr to the given \ref ControlDevice
-         */
-        ConstControlDevicePtr peekControlDevice(const std::string& deviceName) const;
-
-        /**
-         * @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
-         * for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         * @param deviceName The \ref ControlDevice's name
-         * @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
-         * @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
-         */
-        ControlTargetBase* useControlTarget(const std::string& deviceName, const std::string& controlMode);
-        /**
-         * @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
-         * for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         * @param deviceName The \ref ControlDevice's name
-         * @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
-         * @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
-         */
-        template<class T>
-        T* useControlTarget(const std::string& deviceName, const std::string& controlMode);
-
-        /**
-         * @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         * @param deviceName The \ref SensorDevice's name
-         * @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
-         */
-        const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
-        /**
-         * @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         * @param deviceName The \ref SensorDevice's name
-         * @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
-         */
-        template<class T>
-        const T* useSensorValue(const std::string& deviceName) const;
-
-        /**
-         * @brief Requests a VirtualRobot for use in \ref rtRun
-         *          *
-         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
-         *
-         * The robot is updated before \ref rtRun is called and can be accessed via \rtGetRobot
-         * @param updateCollisionModel Whether the robot's collision model should be updated
-         * @see rtGetRobot
-         */
-        const VirtualRobot::RobotPtr& useSynchronizedRtRobot(bool updateCollisionModel = false);
-
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ////////////////////////////////// Component interface ///////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    protected:
-        /// @see Component::getDefaultName
-        std::string getDefaultName() const override;
-        /// @see Component::onInitComponent
-        void onInitComponent() final;
-        /// @see Component::onConnectComponent
-        void onConnectComponent() final;
-        /// @see Component::onDisconnectComponent
-        void onDisconnectComponent() final;
-        /// @see Component::onExitComponent
-        void onExitComponent() final;
-
-
-        virtual void onInitNJointController() {}
-        virtual void onConnectNJointController() {}
-        virtual void onDisconnectNJointController() {}
-        virtual void onExitNJointController() {}
-
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ////////////////////////////////// ThreadPool functionality///////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    protected:
-        ThreadPoolPtr getThreadPool() const;
-
-        /**
-         * @brief Executes a given task in a separate thread from the Application ThreadPool.
-         * @param taskName Descriptive name of this task to identify it on errors.
-         * @param task std::function object (or lambda) that is to be executed.
-         * @note This task will be joined in onExitComponent of the NJointControllerBase. So make sure it terminates, when the
-         * controller is deactivated or removed!
-         *
-         * @code{.cpp}
-         *  runTask("PlotterTask", [&]
-                {
-                    CycleUtil c(30);
-                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
-                    while (getState() == eManagedIceObjectStarted)
-                    {
-                        if (isControllerActive())
-                        {
-                            StringVariantBaseMap errors;
-                            for (size_t i = 0; i < sensors.size(); i++)
-                            {
-                                errors[cfg->jointNames.at(i)] = new Variant(currentError(i));
-                            }
-                            dbgObs->setDebugChannel("TrajectoryController", errors);
-                        }
-                        c.waitForCycleDuration();
-                    }
-                });
-         * @endcode
-         */
-        template < typename Task >
-        void runTask(const std::string& taskName, Task&& task)
-        {
-            std::unique_lock lock(threadHandlesMutex);
-            ARMARX_CHECK_EXPRESSION(!taskName.empty());
-            ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
-            ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
-            ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
-            auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
-            ARMARX_CHECK_EXPRESSION(handlePtr->isValid()) << "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
-            threadHandles[taskName] = handlePtr;
-        }
-        std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
-        std::mutex threadHandlesMutex;
-
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ///////////////////////////////////// ice interface //////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    public:
-        bool isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override;
-        bool isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override;
-        bool isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override;
-        bool hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override;
-
-        std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override  = 0;
-        std::string getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override;
-
-        NJointControllerDescription getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
-        NJointControllerStatus getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
-        NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
-        RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = Ice::emptyCurrent) const final override;
-
-        void activateController(const Ice::Current& = Ice::emptyCurrent) final override;
-        void deactivateController(const Ice::Current& = Ice::emptyCurrent) final override;
-        void deleteController(const Ice::Current& = Ice::emptyCurrent) final override;
-        void deactivateAndDeleteController(const Ice::Current& = Ice::emptyCurrent) final override;
-
-        WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
-        void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = Ice::emptyCurrent) override;
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ///////////////////////////////////// rt interface ///////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    public: ///TODO make protected and use attorneys
-
-        /**
-         * @brief Returns the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
-         * @return The virtual robot used by this \ref NJointControllerBase in the \ref rtRun
-         * @see useSynchronizedRtRobot
-         * @see rtGetRobotNodes
-         */
-        const VirtualRobot::RobotPtr& rtGetRobot()
-        {
-            return rtRobot;
-        }
-        /**
-         * @brief Returns the nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
-         * @return The nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
-         * @see useSynchronizedRtRobot
-         * @see rtGetRobot
-         */
-        const std::vector<VirtualRobot::RobotNodePtr>& rtGetRobotNodes()
-        {
-            return rtRobotNodes;
-        }
-        /**
-         * @brief Returns whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
-         * for the given \ref ControlDevice
-         * @param deviceIndex The \ref ControlDevice's index
-         * @return Whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
-         * for the given \ref ControlDevice
-         */
-        bool rtUsesControlDevice(std::size_t deviceIndex) const;
-        /**
-         * @brief Returns the indices of all \ref ControlDevice's this \ref NJointControllerBase
-         * calculates a \ref ControlTargetBase "ControlTarget" for.
-         * @return The indices of all \ref ControlDevice's this \ref NJointControllerBase
-         * calculates a \ref ControlTargetBase "ControlTarget" for.
-         */
-        const std::vector<std::size_t>& rtGetControlDeviceUsedIndices() const;
-
-        /**
-         * @brief Sets the error state to true. This will deactivate this controller!
-         */
-        bool rtGetErrorState() const;
-
-        /**
-         * @brief Returns the number of used \ref ControlDevice "ControlDevices"
-         * @return The number of used \ref ControlDevice "ControlDevices"
-         */
-        std::size_t rtGetNumberOfUsedControlDevices() const;
-
-        /**
-         * @brief Returns the class name. (the c-string may be used for rt message logging)
-         * @return The class name. (the c-string may be used for rt message logging)
-         */
-        const std::string& rtGetClassName() const;
-
-        /**
-         * @brief Returns the instance name. (the c-string may be used for rt message logging)
-         * @return The instance name. (the c-string may be used for rt message logging)
-         */
-        const std::string& rtGetInstanceName() const;
-
-    protected:
-        /**
-         * @brief This function is called before the controller is activated.
-         * You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
-         */
-        virtual void rtPreActivateController() {}
-        /**
-         * @brief This function is called after the controller is deactivated.
-         * You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
-         */
-        virtual void rtPostDeactivateController() {}
-
-        /**
-         * @brief Sets the error state to true. This will deactivate this controller!
-         */
-        void rtSetErrorState();
-    private:
-        /**
-         * @brief Activates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
-         *
-         * Calls \ref rtPreActivateController
-         * @see rtDeactivateController
-         * @see rtDeactivateControllerBecauseOfError
-         * @see rtPreActivateController
-         */
-        void rtActivateController();
-        /**
-         * @brief Deactivates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
-         *
-         * Calls \ref rtPostDeactivateController
-         * @see rtActivateController
-         * @see rtDeactivateControllerBecauseOfError
-         * @see rtPostDeactivateController
-         */
-        void rtDeactivateController();
-        /**
-         * @brief Deactivates this controller in the \ref RobotUnitModules::ControlThread "ControlThread"
-         * and sets the error flag.
-         *
-         * Calls \ref rtPostDeactivateController
-         * This function is called when this \ref NJointControllerBase produces an error
-         * (e.g.: calculates an invalid \ref ControlTargetBase "ControlTarget", throws an exception)
-         * @see rtActivateController
-         * @see rtDeactivateController
-         * @see rtPostDeactivateController
-         */
-        void rtDeactivateControllerBecauseOfError();
-
-    public:
-
-        static const NJointControllerBasePtr NullPtr;
-
-        NJointControllerBase();
-
-        ~NJointControllerBase() override;
-        //ice interface (must not be called in the rt thread)
-
-        //c++ interface (local calls) (must be called in the rt thread)
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ///////////////////////////////////// used targets ///////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    public:
-        //used control devices
-        StringStringDictionary getControlDeviceUsedControlModeMap(const Ice::Current& = Ice::emptyCurrent) const final override;
-        const std::vector<char>& getControlDeviceUsedBitmap() const;
-        const std::vector<std::size_t>& getControlDeviceUsedIndices() const;
-
-        const std::map<std::string, const JointController*>& getControlDevicesUsedJointController();
-
-        //check for conflict
-        std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
-        std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
-
-        template<class ItT>
-        static std::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last);
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ////////////////////////////////////// publishing ////////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    protected:
-        //publish thread hooks
-        virtual void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
-        virtual void onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
-        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
-    private:
-        void publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
-        void deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // ///////////////////////////////////////// data ///////////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    private:
-        /// @brief reference to the owning \ref RobotUnit
-        RobotUnit& robotUnit;
-        /// @brief Maps names of used \ref ControlDevice "ControlDevices" to the \ref JointController "JointController's" control mode
-        StringStringDictionary controlDeviceControlModeMap;
-        /// @brief Maps names of used \ref ControlDevice "ControlDevices" to the used \ref JointController for this \ref ControlDevice
-        std::map<std::string, const JointController*> controlDeviceUsedJointController;
-        /// @brief Bitmap of used \ref ControlDevice "ControlDevices" (1 == used, 0 == not used)
-        std::vector<char> controlDeviceUsedBitmap;
-        /// @brief Indices of used \ref ControlDevice "ControlDevices"
-        std::vector<std::size_t> controlDeviceUsedIndices;
-
-        std::string rtClassName_;
-        std::string instanceName_;
-
-        //this data is filled by the robot unit to provide convenience functions
-        std::atomic_bool isActive {false};
-        std::atomic_bool isRequested {false};
-        std::atomic_bool deactivatedBecauseOfError {false};
-        std::atomic_bool errorState {false};
-        bool deletable {false};
-        bool internal {false};
-
-        std::atomic_bool publishActive {false};
-
-        std::atomic_bool statusReportedActive {false};
-        std::atomic_bool statusReportedRequested {false};
-
-        VirtualRobot::RobotPtr rtRobot;
-        std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-        // /////////////////////////////////////// friends //////////////////////////////////////// //
-        // //////////////////////////////////////////////////////////////////////////////////////// //
-    private:
-        /**
-        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControllerManagement.
-        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
-        */
-        friend class RobotUnitModule::NJointControllerAttorneyForControllerManagement;
-        /**
-        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControlThread.
-        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
-        */
-        friend class RobotUnitModule::NJointControllerAttorneyForControlThread;
-        /**
-        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::Publisher.
-        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
-        */
-        friend class RobotUnitModule::NJointControllerAttorneyForPublisher;
-        /**
-        * \brief This is required for the factory
-        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
-        */
-        template<class> friend class detail::NJointControllerRegistryEntryHelper;
-    };
-
-    class SynchronousNJointController : public NJointControllerBase
-    {
-    public: ///TODO make protected and use attorneys
-        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
-        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-    };
-    class AsynchronousNJointController : public NJointControllerBase
-    {
-    public: ///TODO make protected and use attorneys
-        virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
-        virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
-    };
-    using NJointController = SynchronousNJointController;
-    using NJointControllerPtr = SynchronousNJointControllerPtr;
-}
-
-#include "NJointController.ipp"
+#include "NJointControllerBase.h"
+#include "NJointControllerRegistry.h"
+#include "NJointControllerWithTripleBuffer.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
deleted file mode 100644
index 4a147cf86218c9c5035ca27474472bca1bf23c54..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ /dev/null
@@ -1,519 +0,0 @@
-/*
- * 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/>.
- *
- * @package    RobotAPI::ArmarXObjects::NJointController
- * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date       2017
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_RobotAPI_NJointController_HPP
-#define _ARMARX_LIB_RobotAPI_NJointController_HPP
-
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
-
-#include "../RobotUnit.h"
-#include "NJointController.h"
-
-namespace armarx::RobotUnitModule
-{
-    class ControllerManagement;
-}
-namespace armarx
-{
-    class NJointControllerRegistryEntry
-    {
-    private:
-        friend class RobotUnitModule::ControllerManagement;
-        virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
-                                               const NJointControllerConfigPtr&,
-                                               const VirtualRobot::RobotPtr&,
-                                               bool deletable,
-                                               bool internal,
-                                               const std::string& instanceName) const = 0;
-        virtual WidgetDescription::WidgetPtr
-        GenerateConfigDescription(const VirtualRobot::RobotPtr&,
-                                  const std::map<std::string, ConstControlDevicePtr>&,
-                                  const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
-        virtual NJointControllerConfigPtr
-        GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
-        virtual bool hasRemoteConfiguration() const = 0;
-
-    protected:
-        static thread_local bool ConstructorIsRunning_;
-
-    public:
-        virtual ~NJointControllerRegistryEntry() = default;
-        static bool
-        ConstructorIsRunning()
-        {
-            return ConstructorIsRunning_;
-        }
-    };
-    using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
-
-    template <class ControllerType>
-    struct NJointControllerRegistration;
-
-    template <typename ControlDataStruct>
-    class NJointControllerWithTripleBuffer : public SynchronousNJointController
-    {
-    public:
-        using MutexType = std::recursive_mutex;
-        using LockGuardType = std::lock_guard<std::recursive_mutex>;
-
-        NJointControllerWithTripleBuffer(
-            const ControlDataStruct& initialCommands = ControlDataStruct()) :
-            controlDataTripleBuffer{initialCommands}
-        {
-        }
-
-        void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
-                                const IceUtil::Time& timeSinceLastIteration) override;
-
-    protected:
-        const ControlDataStruct& rtGetControlStruct() const;
-        bool rtUpdateControlStruct();
-
-        void writeControlStruct();
-        ControlDataStruct& getWriterControlStruct();
-
-        void setControlStruct(const ControlDataStruct& newStruct);
-
-        void reinitTripleBuffer(const ControlDataStruct& initial);
-
-        mutable MutexType controlDataMutex;
-
-    private:
-        WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
-    };
-
-    template <typename ControlDataStruct>
-    using NJointControllerWithTripleBufferPtr =
-        IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
-} // namespace armarx
-
-//inline functions
-namespace armarx
-{
-    template <class T>
-    inline T*
-    NJointControllerBase::useControlTarget(const std::string& deviceName,
-                                           const std::string& controlMode)
-    {
-        static_assert(std::is_base_of<ControlTargetBase, T>::value,
-                      "The given type does not derive ControlTargetBase");
-        ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
-        return ptr ? ptr->asA<T>() : nullptr;
-    }
-    template <class T>
-    inline const T*
-    NJointControllerBase::useSensorValue(const std::string& deviceName) const
-    {
-        static_assert(std::is_base_of<SensorValueBase, T>::value,
-                      "The given type does not derive SensorValueBase");
-        const SensorValueBase* const ptr = useSensorValue(deviceName);
-        return ptr ? ptr->asA<T>() : nullptr;
-    }
-
-    inline void
-    SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
-                                                    const IceUtil::Time& timeSinceLastIteration)
-    {
-        rtRun(sensorValuesTimestamp, timeSinceLastIteration);
-    }
-
-    inline bool
-    NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
-    {
-        return controlDeviceUsedBitmap.at(deviceIndex);
-    }
-
-    inline StringStringDictionary
-    NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
-    {
-        return controlDeviceControlModeMap;
-    }
-
-    inline const std::vector<char>&
-    NJointControllerBase::getControlDeviceUsedBitmap() const
-    {
-        return controlDeviceUsedBitmap;
-    }
-
-    inline const std::vector<std::size_t>&
-    NJointControllerBase::rtGetControlDeviceUsedIndices() const
-    {
-        return controlDeviceUsedIndices;
-    }
-
-    inline const std::vector<std::size_t>&
-    NJointControllerBase::getControlDeviceUsedIndices() const
-    {
-        return controlDeviceUsedIndices;
-    }
-
-    inline const std::map<std::string, const JointController*>&
-    NJointControllerBase::getControlDevicesUsedJointController()
-    {
-        return controlDeviceUsedJointController;
-    }
-
-    inline std::optional<std::vector<char>>
-    NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
-    {
-        return isNotInConflictWith(other->getControlDeviceUsedBitmap());
-    }
-
-    inline std::string
-    NJointControllerBase::getDefaultName() const
-    {
-        return getClassName();
-    }
-
-    inline bool
-    NJointControllerBase::isControllerActive(const Ice::Current&) const
-    {
-        return isActive;
-    }
-
-    inline bool
-    NJointControllerBase::isControllerRequested(const Ice::Current&) const
-    {
-        return isRequested;
-    }
-
-    inline bool
-    NJointControllerBase::isDeletable(const Ice::Current&) const
-    {
-        return deletable;
-    }
-
-    inline bool
-    NJointControllerBase::hasControllerError(const Ice::Current&) const
-    {
-        return deactivatedBecauseOfError;
-    }
-
-    inline std::string
-    NJointControllerBase::getInstanceName(const Ice::Current&) const
-    {
-        return instanceName_;
-    }
-
-    inline WidgetDescription::StringWidgetDictionary
-    NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
-    {
-        return {};
-    }
-
-    inline void
-    NJointControllerBase::callDescribedFunction(const std::string&,
-                                                const StringVariantBaseMap&,
-                                                const Ice::Current&)
-    {
-    }
-
-    inline void
-    NJointControllerBase::rtSetErrorState()
-    {
-        errorState.store(true);
-    }
-
-    inline bool
-    NJointControllerBase::rtGetErrorState() const
-    {
-        return errorState;
-    }
-
-    inline std::size_t
-    NJointControllerBase::rtGetNumberOfUsedControlDevices() const
-    {
-        return controlDeviceUsedIndices.size();
-    }
-
-    inline const std::string&
-    NJointControllerBase::rtGetClassName() const
-    {
-        return rtClassName_;
-    }
-    inline const std::string&
-    NJointControllerBase::rtGetInstanceName() const
-    {
-        return instanceName_;
-    }
-
-    //NJointControllerWithTripleBuffer<ControlDataStruct>
-    template <typename ControlDataStruct>
-    inline void
-    NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
-        const IceUtil::Time& sensorValuesTimestamp,
-        const IceUtil::Time& timeSinceLastIteration)
-    {
-        rtUpdateControlStruct();
-        rtRun(sensorValuesTimestamp, timeSinceLastIteration);
-    }
-
-    template <typename ControlDataStruct>
-    inline const ControlDataStruct&
-    NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
-    {
-        return controlDataTripleBuffer.getReadBuffer();
-    }
-
-    template <typename ControlDataStruct>
-    inline bool
-    NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
-    {
-        return controlDataTripleBuffer.updateReadBuffer();
-    }
-
-    template <typename ControlDataStruct>
-    inline void
-    NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
-    {
-        //just lock to be save and reduce the impact of an error
-        //also this allows code to unlock the mutex before calling this function
-        //(can happen if some lockguard in a scope is used)
-        LockGuardType lock(controlDataMutex);
-        controlDataTripleBuffer.commitWrite();
-    }
-
-    template <typename ControlDataStruct>
-    inline ControlDataStruct&
-    NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
-    {
-        return controlDataTripleBuffer.getWriteBuffer();
-    }
-
-    template <typename ControlDataStruct>
-    inline void
-    NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(
-        const ControlDataStruct& newStruct)
-    {
-        LockGuardType lock{controlDataMutex};
-        getWriterControlStruct() = newStruct;
-        writeControlStruct();
-    }
-
-    template <typename ControlDataStruct>
-    inline void
-    NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(
-        const ControlDataStruct& initial)
-    {
-        controlDataTripleBuffer.reinitAllBuffers(initial);
-    }
-} // namespace armarx
-
-namespace armarx
-{
-    template <class ItT>
-    inline std::optional<std::vector<char>>
-    NJointControllerBase::AreNotInConflict(ItT first, ItT last)
-    {
-        if (first == last)
-        {
-            return std::vector<char>{};
-        }
-        std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
-        std::vector<char> inuse(n, false);
-        while (first != last)
-        {
-            auto r = (*first)->isNotInConflictWith(inuse);
-            if (!r)
-            {
-                return r;
-            }
-            inuse = std::move(*r);
-            ++first;
-        }
-        return inuse;
-    }
-} // namespace armarx
-namespace armarx::detail
-{
-    ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
-        hasGenerateConfigDescription,
-        GenerateConfigDescription,
-        NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
-    ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
-        hasGenerateConfigFromVariants,
-        GenerateConfigFromVariants,
-        NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
-
-    template <class NJointControllerT>
-    class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
-    {
-        static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
-                          hasGenerateConfigFromVariants<NJointControllerT>::value,
-                      "Either overload both GenerateConfigDescription and "
-                      "GenerateConfigFromVariants, or none!");
-        static constexpr bool hasRemoteConfiguration_ =
-            hasGenerateConfigDescription<NJointControllerT>::value;
-
-        NJointControllerBasePtr
-        create(RobotUnitModule::ControllerManagement* cmngr,
-               const NJointControllerConfigPtr& config,
-               const VirtualRobot::RobotPtr& rob,
-               bool deletable,
-               bool internal,
-               const std::string& instanceName) const final override
-        {
-            ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
-
-            using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
-            ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
-            ARMARX_CHECK_EXPRESSION(cfg)
-                << "The configuration is of the wrong type! it has to be an instance of: "
-                << GetTypeString<ConfigPtrT>();
-
-            ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
-                << "Two NJointControllers are created at the same time";
-            NJointControllerBasePtr ptr;
-            {
-                ConstructorIsRunning_ = true;
-                ARMARX_ON_SCOPE_EXIT
-                {
-                    ConstructorIsRunning_ = false;
-                };
-                ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
-            }
-            ptr->deletable = deletable;
-            ptr->internal = internal;
-            ptr->rtClassName_ = ptr->getClassName(Ice::emptyCurrent);
-            ptr->instanceName_ = instanceName;
-            return ptr;
-        }
-
-        WidgetDescription::WidgetPtr
-        GenerateConfigDescription(
-            const VirtualRobot::RobotPtr& robot,
-            const std::map<std::string, ConstControlDevicePtr>& controlDevices,
-            const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
-        {
-            if constexpr (hasRemoteConfiguration_)
-            {
-                try
-                {
-                    return NJointControllerT::GenerateConfigDescription(
-                        robot, controlDevices, sensorDevices);
-                }
-                catch (Ice::UserException& e)
-                {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
-                                 << "::GenerateConfigDescription'"
-                                 << "\n---- file = " << e.ice_file()
-                                 << "\n---- line = " << e.ice_line()
-                                 << "\n---- id   = " << e.ice_id() << "\n---- what:\n"
-                                 << e.what() << "\n---- stacktrace:\n"
-                                 << e.ice_stackTrace();
-                    throw;
-                }
-                catch (std::exception& e)
-                {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
-                                 << "::GenerateConfigDescription'"
-                                 << "\n---- what:\n"
-                                 << e.what();
-                    throw;
-                }
-                catch (...)
-                {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
-                                 << "::GenerateConfigDescription'";
-                    throw;
-                }
-            }
-            else
-            {
-                ARMARX_CHECK_EXPRESSION(!"This function should never be called");
-            }
-        }
-
-        NJointControllerConfigPtr
-        GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
-        {
-            if constexpr (hasRemoteConfiguration_)
-            {
-                try
-                {
-                    return NJointControllerT::GenerateConfigFromVariants(variants);
-                }
-                catch (Ice::UserException& e)
-                {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
-                                 << "::GenerateConfigFromVariants'"
-                                 << "\n---- file = " << e.ice_file()
-                                 << "\n---- line = " << e.ice_line()
-                                 << "\n---- id   = " << e.ice_id() << "\n---- what:\n"
-                                 << e.what() << "\n---- stacktrace:\n"
-                                 << e.ice_stackTrace();
-                    throw;
-                }
-                catch (std::exception& e)
-                {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
-                                 << "::GenerateConfigFromVariants'"
-                                 << "\n---- what:\n"
-                                 << e.what();
-                    throw;
-                }
-                catch (...)
-                {
-                    ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
-                                 << "::GenerateConfigFromVariants'";
-                    throw;
-                }
-            }
-            else
-            {
-                ARMARX_CHECK_EXPRESSION(!"This function should never be called");
-            }
-        }
-
-        bool
-        hasRemoteConfiguration() const final override
-        {
-            return hasRemoteConfiguration_;
-        }
-    };
-} // namespace armarx::detail
-namespace armarx
-{
-    using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
-    template <class ControllerType>
-    struct NJointControllerRegistration
-    {
-        NJointControllerRegistration(const std::string& name)
-        {
-            NJointControllerRegistry::registerElement(
-                name,
-                std::unique_ptr<NJointControllerRegistryEntry>(
-                    new detail::NJointControllerRegistryEntryHelper<ControllerType>));
-        }
-    };
-} // namespace armarx
-#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T)                                     \
-    static_assert(                                                                                 \
-        ::armarx::detail::hasGenerateConfigDescription<T>::value,                                  \
-        #T " does not offer a construction gui (missing: static GenerateConfigDescription)");      \
-    static_assert(                                                                                 \
-        ::armarx::detail::hasGenerateConfigFromVariants<T>::value,                                 \
-        #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
-
-
-#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b18689724a120d17a3eff61158d9e57cfd2ee7a9
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -0,0 +1,1063 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::NJointController
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
+#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <functional>
+#include <optional>
+#include <atomic>
+#include <mutex>
+#include <map>
+
+namespace IceProxy::armarx
+{
+    class DebugDrawerInterface;
+    class DebugObserverInterface;
+    class RobotUnitInterface;
+}
+
+namespace armarx
+{
+    namespace RobotUnitModule
+    {
+        class NJointControllerAttorneyForPublisher;
+        class NJointControllerAttorneyForControlThread;
+        class NJointControllerAttorneyForControllerManagement;
+    }
+    namespace detail
+    {
+        template<class> class NJointControllerRegistryEntryHelper;
+        class ControlThreadOutputBufferEntry;
+    }
+    namespace WidgetDescription
+    {
+        class Widget;
+        typedef ::IceInternal::Handle< ::armarx::WidgetDescription::Widget> WidgetPtr;
+        typedef ::std::map< ::std::string, ::armarx::WidgetDescription::WidgetPtr> StringWidgetDictionary;
+    }
+    using SensorAndControl = detail::ControlThreadOutputBufferEntry;
+    class JointController;
+    class SensorValueBase;
+    class ControlTargetBase;
+    using ConstControlDevicePtr = std::shared_ptr<const class ControlDevice>;
+    using ConstSensorDevicePtr = std::shared_ptr<const class SensorDevice>;
+
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::RobotUnitInterface> RobotUnitInterfacePrx;
+
+    using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
+
+    TYPEDEF_PTRS_HANDLE(NJointControllerBase);
+    TYPEDEF_PTRS_HANDLE(SynchronousNJointController);
+    TYPEDEF_PTRS_HANDLE(AsynchronousNJointController);
+
+    TYPEDEF_PTRS_HANDLE(RobotUnit);
+
+    /**
+     * @defgroup Library-RobotUnit-NJointControllers NJointControllers
+     * @ingroup Library-RobotUnit
+     */
+
+    /**
+    * @ingroup Library-RobotUnit-NJointControllers
+    * @brief A high level controller writing its results into \ref ControlTargetBase "ControlTargets".
+    *
+    * This is the abstract base class for all NJointControllers.
+    * It implements basic management routines required by the RobotUnit and some basic ice calls.
+    * \ref NJointControllerBase "NJointControllers" are instantiated and managed by the \ref RobotUnit.
+    *
+    * A \ref NJointControllerBase calculates \ref ControlTargetBase "ControlTargets" for a set of
+    * \ref ControlDevice "ControlDevices" in a specific ControlMode.
+    * This ControlMode is defined for each \ref ControlDevice during construction.
+    *
+    * \section nj-state Requested and Active
+    * A \ref NJointControllerBase can is requested / not requested and active / inactive.
+    * All four combinations are possible.
+    *
+    * \subsection nj-state-req Requested / Not Requested
+    * If the user wants this \ref NJointControllerBase to be executed it is in a requested state (see \ref isControllerRequested).
+    * Otherwise the controler is not requested.
+    *
+    * Calling \ref activateController sets the state to requested.
+    * Calling \ref deactivateController sets the state to not requested.
+    * If the \ref NJointControllerBase causes an error or a different \ref NJointControllerBase using one or more of the same
+    * \ref ControlDevice's is requested, this \ref NJointControllerBase is deactivated.
+    *
+    *
+    * \subsection nj-state-act Active / Inactive
+    * Is the \ref NJointControllerBase executed by the \ref RobotUnitModules::ControlThread "ControlThread" it is active
+    * (see \ref isControllerActive).
+    *
+    * To be executed by the \ref RobotUnitModules::ControlThread "ControlThread", the \ref NJointControllerBase has to be
+    * requested at some point in the past.
+    *
+    * If a controller is active, it has to write a valid \ref ControlTargetBase "ControlTarget"
+    * for each of its \ref ControlDevice "ControlDevicea" (if it fails, it will be deactivated).
+    *
+    * \section Constructor
+    * In the Constructor, a \ref NJointControllerBase has to declare all \ref ControlDevice "ControlDevices" it uses.
+    *
+    * The constructor takes a pointer to a configuration structure of the type \ref NJointControllerBase::ConfigPtrT.
+    * If an implementation requires a special configuration structure (e.g.: SomeOtherCfg), it has to override this type by calling adding
+    * \code{.cpp}
+    * using ConfigPtrT = SomeOtherCfgPtr;
+    * \endcode
+    * to it's clas definition.
+    * The type SomeOtherCfg has to derive \ref NJointControllerConfigPtr.
+    *
+    * There is a way to generate a small gui widget for controller construction (see this \ref nj-ctor-gui "section").
+    *
+    * \subsection nj-ctor-req-ctrl Using ControlTargets
+    * A \ref NJointControllerBase can use \ref peekControlDevice to examine a \ref ControlDevice before using it
+    * (e.g.: checking the supported \ref ControlTargetBase "ControlTargets").
+    *
+    * If a \ref ControlDevice should be used by this \ref NJointControllerBase, it has to call \ref useControlDevice.
+    * This sets the ControlMode for the \ref ControlDevice and returns a pointer to the \ref ControlTargetBase "ControlTarget".
+    * This pointer has to be used to send commands in each iteration of \ref rtRun.
+    * The ControlMode can't be changed afterwards (A new \ref NJointControllerBase has to be created).
+    *
+    *
+    * \subsection nj-ctor-req-sens Using SensorValues
+    * A \ref NJointControllerBase can use \ref peekSensorDevice to examine a \ref SensorDevice before using it.
+    *
+    * If a \ref SensorDevice should be used by this \ref NJointControllerBase, it has to call \ref useSensorDevice.
+    * This returns a pointer to the \ref SensorValueBase "SensorValue".
+    *
+    * \subsection nj-ctor-rob A synchronized Virtualrobot
+    * If the controller needs a simox robot in \ref rtRun, it should call \ref useSynchronizedRtRobot.
+    * This will provide a simoxRobot via \ref rtGetRobot.
+    * This robot is synchronized with the real robots state before calling \ref rtRun
+    *
+    * \section nj-parts Rt and non Rt
+    * Each \ref NJointControllerBase has two parts:
+    * \li The RT controll loop
+    * \li The NonRT ice communication
+    *
+    * \subsection rtcontrollloop The Rt controll loop (\ref rtRun)
+    * \warning This part has to satisfy realtime conditions!
+    * All realtime functions of \ref NJointControllerBase have the 'rt' prefix.
+    *
+    * Here the \ref NJointControllerBase has access to the robot's current state
+    * and has to write results into \ref ControlTargetBase "ControlTargets".
+    *
+    * It must not:
+    * \li call any blocking operation
+    * \li allocate ram on the heap (since this is a blocking operation)
+    * \li resize any datastructure (e.g. vector::resize) (this sometimes allocates ram on the heap)
+    * \li insert new datafields into datastructures (e.g. vector::push_back) (this sometimes allocates ram on the heap)
+    * \li write to any stream (e.g. ARMARX_VERBOSE, std::cout, print, filestreams) (this sometimes blocks/allocates ram on the heap)
+    * \li make network calls (e.g. through ice) (this blocks/allocates ram on the heap) (using begin_... end_... version of ice calls IS NO SOLUTION)
+    * \li do any expensive calculations (e.g. calculate IK, run some solver, invert big matrices)
+    *
+    * \subsection nonrtpart The NonRT ice communication
+    * This part consits of any ice communication.
+    * Here the \ref NJointControllerBase can get new controll parameters or targets from other components.
+    *
+    * \section rtnrtcomm Communication between RT and NonRT
+    * All communication between RT and NonRT has to be lockfree.
+    * The \ref NJointControllerBase has to use constructs like atomics or
+    * \ref TripleBuffer "TripleBuffers" (See \ref armarx::NJointControllerWithTripleBuffer).
+    *
+    * \image html NJointControllerGeneralDataFlow.svg "General Dataflow in a NJointControllerBase"
+    *
+    * \image html NJointControllerAtomicDataFlow.svg "Dataflow in a NJointControllerBase using atomics for communication between RT and NonRT"
+    *
+    * \image html NJointControllerTripleBufferDataFlow.svg "Dataflow in a NJointControllerBase using a triple buffer for communication between RT and NonRT"
+    *
+    *
+    * \image html NJointControllerDataFlow_Graph.svg "Dataflow in a NJointControllerBase as a Graph of the two participating domains"
+    * The above image shows the two participating domains: one RT thread and multiple ICE threads.
+    * If data has to flow along an arrow, you need some construct for non blocking message passing.
+    *
+    * \warning If you use \ref TrippleBuffer "TrippleBuffers" or \ref WriteBufferedTrippleBuffer "WriteBufferedTrippleBuffers" you need a separate one for each arrow.
+    *
+    * \section expensivework How to do expensive calculations
+    * You have to execute expensive calculations in a different worker thread.
+    * This thread could calculate target values at its own speed (e.g. 100 Hz).
+    *
+    * While rtRun runs at a higher frequency (e.g. 1000 Hz) and:
+    * \li reads target values
+    * \li optionally passes the target values to a PID controller
+    * \li writes them to the targets
+    * \li sends the sensor values to the worker thread.
+    *
+    * If you do some additional calculation in rtRun, you maybe need to pass config parameters from NonRT to RT using a nonblocking method.
+    *
+    * \image html NJointControllerWorkerThreadDataFlow.svg "Dataflow in a NJointControllerBase using a worker thread"
+    *
+    * \image html NJointControllerWorkerThreadDataFlow_Graph.svg "Dataflow in a NJointControllerBase using a worker thread as a Graph of the three participating domains"
+    * The above image shows the three participating domains: one RT thread, one worker trhead and multiple ICE threads.
+    * If data has to flow along an arrow, you need some construct for non blocking message passing.
+    *
+    * \warning If you use \ref TrippleBuffer "TrippleBuffers" or \ref WriteBufferedTrippleBuffer "WriteBufferedTrippleBuffers" you need a separate one for each arrow.
+    *
+    * \section nj-ctor-gui Providing a gui for controller construction.
+    * By implementing these functions:
+    * \code{.cpp}
+     static WidgetDescription::WidgetPtr GenerateConfigDescription
+     (
+        const VirtualRobot::RobotPtr& robot,
+        const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+        const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+     ); //describes how the widget is supposed to look
+     static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap&); // turns the resulting variants into a config
+    * \endcode
+    *
+    * The \ref RobotUnitGui will provide a widget to configure and construct a \ref NJointControllerBase of this type.
+    *
+    * \section Examples
+    *
+    * More examples can be found in the Tutorials Package
+    *
+    * \subsection nj-example-1 A simple pass Position controller
+    * \note The code can be found in the Tutorial package
+    *
+    * \subsection nj-example-1-h Header
+    * Include headers
+    * \code{.cpp}
+        #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+        #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+        #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+    * \endcode
+    *
+    * Open the namespace
+    * \code{.cpp}
+        namespace armarx
+        {
+    * \endcode
+    *
+    * Typedef some pointers
+    * \code{.cpp}
+            TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughController);
+            TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughControllerConfig);
+    * \endcode
+    *
+    * The config has to inherit \ref NJointControllerConfig and consists only of the joint name
+    * \code{.cpp}
+            class NJointPositionPassThroughControllerConfig : virtual public NJointControllerConfig
+            {
+            public:
+                NJointPositionPassThroughControllerConfig(const std::string& name): deviceName {name} {}
+                std::string deviceName;
+            };
+    * \endcode
+    *
+    * The controller class has to inherit \ref NJointControllerBase
+    * \code{.cpp}
+            class NJointPositionPassThroughController: public NJointControllerBase
+            {
+            public:
+    * \endcode
+    *
+    * The \ref NJointControllerBase provides a config widget for the \ref RobotUnitGuiPlugin
+    * \code{.cpp}
+                static WidgetDescription::WidgetPtr GenerateConfigDescription
+                (
+                    const VirtualRobot::RobotPtr&,
+                    const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+                    const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+                );
+                static NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+    * \endcode
+    *
+    * The ctor receives a pointer to the \ref RobotUnit, a pointer to the config and a pointer to a VirtualRobot model.
+    * \code{.cpp}
+                NJointPositionPassThroughController(
+                    const RobotUnitPtr& prov,
+                    NJointControllerConfigPtr config,
+                    const VirtualRobot::RobotPtr& r
+                );
+    * \endcode
+    *
+    * The function to provide the class name.
+    * \code{.cpp}
+                std::string getClassName(const Ice::Current&) const override;
+    * \endcode
+    *
+    * This controller provides widgets for function calls.
+    * \code{.cpp}
+                WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
+                void callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&) override;
+    * \endcode
+    *
+    * The run function executed to calculate new targets
+    * \code{.cpp}
+                void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+    * \endcode
+    *
+    * Hooks for this \ref NJointControllerBase to execute code during publishing.
+    * \code{.cpp}
+                void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+                void onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
+                void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
+    * \endcode
+    *
+    * This \ref NJointControllerBase uses one position \ref SensorValue1DoFActuatorPosition "SensorValue", calculates one
+    * position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and stores the current position target and position value in atomics.
+    * \code{.cpp}
+                const SensorValue1DoFActuatorPosition* sensor;
+                ControlTarget1DoFActuatorPosition* target;
+                std::atomic<float> targetPos {0};
+                std::atomic<float> currentPos {0};
+    * \endcode
+    *
+    * Close the class and the namespace
+    * \code{.cpp}
+            };
+        }
+    * \endcode
+    *
+    * \subsection nj-example-1-c Source file
+    *
+    * Include the required headers.
+    * \code{.cpp}
+        #include "NJointPositionPassThroughController.h"
+        #include <RobotAPI/libraries/core/Pose.h>
+    * \endcode
+    *
+    * Open the namespace
+    * \code{.cpp}
+        namespace armarx
+        {
+    * \endcode
+    *
+    * This generates a config widget used to display a config gui in \ref RobotUnitGui.
+    * It consist out of a \ref HBoxLayout containing a \ref Label and a \ref StringComboBox
+    * \code{.cpp}
+            WidgetDescription::WidgetPtr NJointPositionPassThroughController::GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
+            {
+                using namespace armarx::WidgetDescription;
+                HBoxLayoutPtr layout = new HBoxLayout;
+                LabelPtr label = new Label;
+                label->text = "control device name";
+                layout->children.emplace_back(label);
+                StringComboBoxPtr box = new StringComboBox;
+                box->defaultIndex = 0;
+    * \endcode
+    *
+    * The \ref StringComboBox only contains the names of \ref ControlDevice "ControlDevices" accepting
+    * a position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and providing a position
+    * \ref SensorValue1DoFActuatorPosition "SensorValue"
+    * \code{.cpp}
+                //filter control devices
+                for (const auto& name2dev : controlDevices)
+                {
+                    const ConstControlDevicePtr& dev = name2dev.second;
+                    const auto& name = name2dev.first;
+                    if (
+                        dev->hasJointController(ControlModes::Position1DoF) &&
+                        dev->getJointController(ControlModes::Position1DoF)->getControlTarget()->isA<ControlTarget1DoFActuatorPosition>() &&
+                        sensorDevices.count(name) &&
+                        sensorDevices.at(name)->getSensorValue()->isA<SensorValue1DoFActuatorPosition>()
+                    )
+                    {
+                        box->options.emplace_back(name);
+                    }
+                }
+                box->name = "name";
+                layout->children.emplace_back(box);
+                return layout;
+            }
+    * \endcode
+    *
+    * This turns a map of variants into the config required by this \ref NJointControllerBase.
+    * The \ref Variant for the key 'name' defines the \ref ControlDevice.
+    * \code{.cpp}
+            NJointControllerConfigPtr NJointPositionPassThroughController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+            {
+                //you should check here for types null additional params etc
+                return new NJointPositionPassThroughControllerConfig {values.at("name")->getString()};
+            }
+    * \endcode
+    *
+    * The constructors implementation.
+    * \code{.cpp}
+            NJointPositionPassThroughController::NJointPositionPassThroughController(
+                const RobotUnitPtr& ru,
+                NJointControllerConfigPtr config,
+                const VirtualRobot::RobotPtr&
+            )
+            {
+                ARMARX_CHECK_EXPRESSION(ru);
+    * \endcode
+    *
+    * It checks whether the provided config has the correct type.
+    * \code{.cpp}
+                NJointPositionPassThroughControllerConfigPtr cfg = NJointPositionPassThroughControllerConfigPtr::dynamicCast(config);
+                ARMARX_CHECK_EXPRESSION(cfg) << "The provided config has the wrong type! The type is " << config->ice_id();
+    * \endcode
+    *
+    * Then it retrieves the \ref SensorValue1DoFActuatorPosition "SensorValue" and
+    * \ref ControlTarget1DoFActuatorPosition "ControlTarget", casts then to the required types and stores them to the member variables.
+    * \code{.cpp}
+                const SensorValueBase* sv = useSensorValue(cfg->deviceName);
+                ControlTargetBase* ct = useControlTarget(cfg->deviceName, ControlModes::Position1DoF);
+                ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
+                ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
+                sensor = sv->asA<SensorValue1DoFActuatorPosition>();
+                target = ct->asA<ControlTarget1DoFActuatorPosition>();
+            }
+    * \endcode
+    *
+    * The class name has to be unique, hence the c++ class name should be used.
+    * \code{.cpp}
+            std::string NJointPositionPassThroughController::getClassName(const Ice::Current&) const
+            {
+                return "NJointPositionPassThroughController";
+            }
+
+    * \endcode
+    *
+    * This \ref NJointControllerBase provides two widgets for function calls.
+    * \code{.cpp}
+            WidgetDescription::StringWidgetDictionary NJointPositionPassThroughController::getFunctionDescriptions(const Ice::Current&) const
+            {
+                using namespace armarx::WidgetDescription;
+    * \endcode
+    *
+    * The first widget has a \ref Label and a \ref FloatSpinBox.
+    * \code{.cpp}
+                HBoxLayoutPtr layoutSetPos = new HBoxLayout;
+                {
+                    LabelPtr label = new Label;
+                    label->text = "positiontarget";
+                    layoutSetPos->children.emplace_back(label);
+                    FloatSpinBoxPtr spin = new FloatSpinBox;
+                    spin->defaultValue = 0;
+                    spin->max = 1;
+                    spin->min = -1;
+    * \endcode
+    *
+    * The \ref FloatSpinBox creates a variant called 'spinmearound'
+    * \code{.cpp}
+                    spin->name = "spinmearound";
+                    layoutSetPos->children.emplace_back(spin);
+                }
+    * \endcode
+    *
+    * The second widget also has a \ref Label and a \ref FloatSpinBox.
+    * \code{.cpp}
+                VBoxLayoutPtr layoutSetHalfPos = new VBoxLayout;
+                {
+                    LabelPtr label = new Label;
+                    label->text = "positiontarget / 2";
+                    layoutSetHalfPos->children.emplace_back(label);
+                    FloatSpinBoxPtr spin = new FloatSpinBox;
+                    spin->defaultValue = 0;
+                    spin->max = 0.5;
+                    spin->min = -0.5;
+                    spin->name = "spinmehalfaround";
+                    layoutSetHalfPos->children.emplace_back(spin);
+                }
+    * \endcode
+    *
+    * This returns a map of boths widgets.
+    * The keys will be used to identify the called function.
+    * \code{.cpp}
+                return {{"SetPosition", layoutSetPos}, {"SetPositionHalf", layoutSetHalfPos}};
+            }
+    * \endcode
+    *
+    * This function is called from the \ref RobotUnitGuiPlugin when the prior defined functions are called.
+    * Both functions set the target position.
+    * \code{.cpp}
+            void NJointPositionPassThroughController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&)
+            {
+                if (name == "SetPosition")
+                {
+                    //you should check here for types null additional params etc
+                    targetPos = value.at("spinmearound")->getFloat();
+                }
+                else if (name == "SetPositionHalf")
+                {
+                    //you should check here for types null additional params etc
+                    targetPos = value.at("spinmehalfaround")->getFloat() * 2;
+                }
+                else
+                {
+                    ARMARX_ERROR << "CALLED UNKNOWN REMOTE FUNCTION: " << name;
+                }
+            }
+    * \endcode
+    *
+    * The \ref rtRun function sets the \ref ControlTarget1DoFActuatorPosition "ControlTarget" to the
+    * target value set via the atomic and stores the current
+    * \ref SensorValue1DoFActuatorPosition "SensorValue" to the other atomic.
+    * \code{.cpp}
+            void NJointPositionPassThroughController::rtRun(const IceUtil::Time& t, const IceUtil::Time&)
+            {
+                ARMARX_RT_LOGF_ERROR("A MESSAGE PARAMETER %f", t.toSecondsDouble()).deactivateSpam(1);
+                ARMARX_RT_LOGF_IMPORTANT("A MESSAGE WITHOUT PARAMETERS").deactivateSpam(1);
+                target->position = targetPos;
+                currentPos = sensor->position;
+            }
+    * \endcode
+    *
+    * The publish activation \ref onPublishActivation "hook" does nothing, but could be used to call
+    * \ref DebugDraver::setRobotVisu.
+    * \code{.cpp}
+            void NJointPositionPassThroughController::onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+            {
+                //we could do some setup for the drawer here. e.g. add a robot
+            }
+
+    * \endcode
+    *
+    * The publish deactivation \ref onPublishDeactivation "hook" clears the \ref DebugDrawer layer
+    * \code{.cpp}
+            void NJointPositionPassThroughController::onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
+            {
+                drawer->removeLayer("Layer_" + getInstanceName());
+            }
+
+    * \endcode
+    *
+    * The publish \ref onPublish "hook" draws a sphere with radius of the position stored in the atomic times 2000 to the \ref DebugDrawer.
+    * \code{.cpp}
+            void NJointPositionPassThroughController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
+            {
+                drawer->setSphereVisu("Layer_" + getInstanceName(), "positionball", new Vector3, armarx::DrawColor {1, 1, 1, 1}, currentPos * 2000);
+            }
+    * \endcode
+    *
+    * This registers a factory for this \ref NJointControllerBase.
+    * This factory is used by the \ref RobotUnit to create the \ref NJointControllerBase.
+    * The passed name has to match the name returned by \ref getClassName.
+    * \code{.cpp}
+            NJointControllerRegistration<NJointPositionPassThroughController> registrationControllerNJointPositionPassThroughController("NJointPositionPassThroughController");
+    * \endcode
+    *
+    * This statically asserts a factory for the type NJointPositionPassThroughController is present.
+    * \code{.cpp}
+            ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(NJointPositionPassThroughController);
+    * \endcode
+    * Close the namespace
+    * \code{.cpp}
+        }
+    * \endcode
+    */
+    class NJointControllerBase:
+        virtual public NJointControllerInterface,
+        virtual public ManagedIceObject
+    {
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // /////////////////////////////////////// typedefs /////////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    public:
+        using ConfigPtrT = NJointControllerConfigPtr;
+        using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr(*)
+                (
+                    const VirtualRobot::RobotPtr&,
+                    const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+                    const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+                );
+        template<class ConfigPrtType>
+        using GenerateConfigFromVariantsFunctionSignature = ConfigPrtType(*)(const StringVariantBaseMap&);
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ///////////////////////////// constructor setup functions ////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    public:
+        /**
+         * @brief Get a const ptr to the given \ref SensorDevice
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         * @param deviceName The \ref SensorDevice's name
+         * @return A const ptr to the given \ref SensorDevice
+         */
+        ConstSensorDevicePtr   peekSensorDevice(const std::string& deviceName) const;
+        /**
+         * @brief Get a const ptr to the given \ref ControlDevice
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         * @param deviceName The \ref ControlDevice's name
+         * @return A const ptr to the given \ref ControlDevice
+         */
+        ConstControlDevicePtr peekControlDevice(const std::string& deviceName) const;
+
+        /**
+         * @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
+         * for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         * @param deviceName The \ref ControlDevice's name
+         * @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
+         * @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
+         */
+        ControlTargetBase* useControlTarget(const std::string& deviceName, const std::string& controlMode);
+        /**
+         * @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
+         * for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         * @param deviceName The \ref ControlDevice's name
+         * @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
+         * @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
+         */
+        template<class T>
+        T* useControlTarget(const std::string& deviceName, const std::string& controlMode)
+        {
+            static_assert(std::is_base_of<ControlTargetBase, T>::value,
+                          "The given type does not derive ControlTargetBase");
+            ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
+            return dynamic_cast<T*>(ptr);
+        }
+
+        /**
+         * @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         * @param deviceName The \ref SensorDevice's name
+         * @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
+         */
+        const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
+        /**
+         * @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         * @param deviceName The \ref SensorDevice's name
+         * @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
+         */
+        template<class T>
+        const T* useSensorValue(const std::string& deviceName) const
+        {
+            static_assert(std::is_base_of<SensorValueBase, T>::value,
+                          "The given type does not derive SensorValueBase");
+            const SensorValueBase* const ptr = useSensorValue(deviceName);
+            return dynamic_cast<const T*>(ptr);
+        }
+
+        /**
+         * @brief Requests a VirtualRobot for use in \ref rtRun
+         *          *
+         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
+         *
+         * The robot is updated before \ref rtRun is called and can be accessed via \rtGetRobot
+         * @param updateCollisionModel Whether the robot's collision model should be updated
+         * @see rtGetRobot
+         */
+        const VirtualRobot::RobotPtr& useSynchronizedRtRobot(bool updateCollisionModel = false);
+
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ////////////////////////////////// Component interface ///////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    protected:
+        /// @see Component::getDefaultName
+        std::string getDefaultName() const override
+        {
+            return getClassName();
+        }
+        /// @see Component::onInitComponent
+        void onInitComponent() final;
+        /// @see Component::onConnectComponent
+        void onConnectComponent() final;
+        /// @see Component::onDisconnectComponent
+        void onDisconnectComponent() final;
+        /// @see Component::onExitComponent
+        void onExitComponent() final;
+
+
+        virtual void onInitNJointController() {}
+        virtual void onConnectNJointController() {}
+        virtual void onDisconnectNJointController() {}
+        virtual void onExitNJointController() {}
+
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ////////////////////////////////// ThreadPool functionality///////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    protected:
+        ThreadPoolPtr getThreadPool() const;
+
+        /**
+         * @brief Executes a given task in a separate thread from the Application ThreadPool.
+         * @param taskName Descriptive name of this task to identify it on errors.
+         * @param task std::function object (or lambda) that is to be executed.
+         * @note This task will be joined in onExitComponent of the NJointControllerBase. So make sure it terminates, when the
+         * controller is deactivated or removed!
+         *
+         * @code{.cpp}
+         *  runTask("PlotterTask", [&]
+                {
+                    CycleUtil c(30);
+                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+                    while (getState() == eManagedIceObjectStarted)
+                    {
+                        if (isControllerActive())
+                        {
+                            StringVariantBaseMap errors;
+                            for (size_t i = 0; i < sensors.size(); i++)
+                            {
+                                errors[cfg->jointNames.at(i)] = new Variant(currentError(i));
+                            }
+                            dbgObs->setDebugChannel("TrajectoryController", errors);
+                        }
+                        c.waitForCycleDuration();
+                    }
+                });
+         * @endcode
+         */
+        template < typename Task >
+        void runTask(const std::string& taskName, Task&& task)
+        {
+            std::unique_lock lock(threadHandlesMutex);
+            ARMARX_CHECK_EXPRESSION(!taskName.empty());
+            ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
+            ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
+            ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
+            auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
+            ARMARX_CHECK_EXPRESSION(handlePtr->isValid()) << "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
+            threadHandles[taskName] = handlePtr;
+        }
+        std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
+        std::mutex threadHandlesMutex;
+
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ///////////////////////////////////// ice interface //////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    public:
+        bool isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
+        {
+            return isActive;
+        }
+        bool isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
+        {
+            return isRequested;
+        }
+        bool isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
+        {
+            return deletable;
+        }
+        bool hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
+        {
+            return deactivatedBecauseOfError;
+        }
+
+        std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override  = 0;
+        std::string getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
+        {
+            return instanceName_;
+        }
+
+        NJointControllerDescription getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
+        NJointControllerStatus getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
+        NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
+        //RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = Ice::emptyCurrent) const final override;
+
+        void activateController(const Ice::Current& = Ice::emptyCurrent) final override;
+        void deactivateController(const Ice::Current& = Ice::emptyCurrent) final override;
+        void deleteController(const Ice::Current& = Ice::emptyCurrent) final override;
+        void deactivateAndDeleteController(const Ice::Current& = Ice::emptyCurrent) final override;
+
+        WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
+        {
+            return {};
+        }
+        void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = Ice::emptyCurrent) override
+        {
+
+        }
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ///////////////////////////////////// rt interface ///////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    public: ///TODO make protected and use attorneys
+
+        /**
+         * @brief Returns the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
+         * @return The virtual robot used by this \ref NJointControllerBase in the \ref rtRun
+         * @see useSynchronizedRtRobot
+         * @see rtGetRobotNodes
+         */
+        const VirtualRobot::RobotPtr& rtGetRobot()
+        {
+            return rtRobot;
+        }
+        /**
+         * @brief Returns the nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
+         * @return The nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
+         * @see useSynchronizedRtRobot
+         * @see rtGetRobot
+         */
+        const std::vector<VirtualRobot::RobotNodePtr>& rtGetRobotNodes()
+        {
+            return rtRobotNodes;
+        }
+        /**
+         * @brief Returns whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
+         * for the given \ref ControlDevice
+         * @param deviceIndex The \ref ControlDevice's index
+         * @return Whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
+         * for the given \ref ControlDevice
+         */
+        bool rtUsesControlDevice(std::size_t deviceIndex) const
+        {
+            return controlDeviceUsedBitmap.at(deviceIndex);
+        }
+
+        /**
+         * @brief Returns the indices of all \ref ControlDevice's this \ref NJointControllerBase
+         * calculates a \ref ControlTargetBase "ControlTarget" for.
+         * @return The indices of all \ref ControlDevice's this \ref NJointControllerBase
+         * calculates a \ref ControlTargetBase "ControlTarget" for.
+         */
+        const std::vector<std::size_t>& rtGetControlDeviceUsedIndices() const
+        {
+            return controlDeviceUsedIndices;
+        }
+
+        /**
+         * @brief Sets the error state to true. This will deactivate this controller!
+         */
+        bool rtGetErrorState() const
+        {
+            return errorState;
+        }
+
+        /**
+         * @brief Returns the number of used \ref ControlDevice "ControlDevices"
+         * @return The number of used \ref ControlDevice "ControlDevices"
+         */
+        std::size_t rtGetNumberOfUsedControlDevices() const
+        {
+            return controlDeviceUsedIndices.size();
+        }
+
+        /**
+         * @brief Returns the class name. (the c-string may be used for rt message logging)
+         * @return The class name. (the c-string may be used for rt message logging)
+         */
+        const std::string& rtGetClassName() const
+        {
+            return rtClassName_;
+        }
+
+        /**
+         * @brief Returns the instance name. (the c-string may be used for rt message logging)
+         * @return The instance name. (the c-string may be used for rt message logging)
+         */
+        const std::string& rtGetInstanceName() const
+        {
+            return instanceName_;
+        }
+
+    protected:
+        /**
+         * @brief This function is called before the controller is activated.
+         * You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
+         */
+        virtual void rtPreActivateController() {}
+        /**
+         * @brief This function is called after the controller is deactivated.
+         * You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
+         */
+        virtual void rtPostDeactivateController() {}
+
+        /**
+         * @brief Sets the error state to true. This will deactivate this controller!
+         */
+        void rtSetErrorState()
+        {
+            errorState.store(true);
+        }
+    private:
+        /**
+         * @brief Activates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
+         *
+         * Calls \ref rtPreActivateController
+         * @see rtDeactivateController
+         * @see rtDeactivateControllerBecauseOfError
+         * @see rtPreActivateController
+         */
+        void rtActivateController();
+        /**
+         * @brief Deactivates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
+         *
+         * Calls \ref rtPostDeactivateController
+         * @see rtActivateController
+         * @see rtDeactivateControllerBecauseOfError
+         * @see rtPostDeactivateController
+         */
+        void rtDeactivateController();
+        /**
+         * @brief Deactivates this controller in the \ref RobotUnitModules::ControlThread "ControlThread"
+         * and sets the error flag.
+         *
+         * Calls \ref rtPostDeactivateController
+         * This function is called when this \ref NJointControllerBase produces an error
+         * (e.g.: calculates an invalid \ref ControlTargetBase "ControlTarget", throws an exception)
+         * @see rtActivateController
+         * @see rtDeactivateController
+         * @see rtPostDeactivateController
+         */
+        void rtDeactivateControllerBecauseOfError();
+
+    public:
+
+        static const NJointControllerBasePtr NullPtr;
+
+        NJointControllerBase();
+
+        ~NJointControllerBase() override;
+        //ice interface (must not be called in the rt thread)
+
+        //c++ interface (local calls) (must be called in the rt thread)
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ///////////////////////////////////// used targets ///////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    public:
+        //used control devices
+        StringStringDictionary getControlDeviceUsedControlModeMap(const Ice::Current& = Ice::emptyCurrent) const final override
+        {
+            return controlDeviceControlModeMap;
+        }
+        const std::vector<char>& getControlDeviceUsedBitmap() const
+        {
+            return controlDeviceUsedBitmap;
+        }
+        const std::vector<std::size_t>& getControlDeviceUsedIndices() const
+        {
+            return controlDeviceUsedIndices;
+        }
+
+        const std::map<std::string, const JointController*>& getControlDevicesUsedJointController()
+        {
+            return controlDeviceUsedJointController;
+        }
+
+        //check for conflict
+        std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const
+        {
+            return isNotInConflictWith(other->getControlDeviceUsedBitmap());
+        }
+        std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
+
+        template<class ItT>
+        static std::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last)
+        {
+            if (first == last)
+            {
+                return std::vector<char>{};
+            }
+            std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
+            std::vector<char> inuse(n, false);
+            while (first != last)
+            {
+                auto r = (*first)->isNotInConflictWith(inuse);
+                if (!r)
+                {
+                    return r;
+                }
+                inuse = std::move(*r);
+                ++first;
+            }
+            return inuse;
+        }
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ////////////////////////////////////// publishing ////////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    protected:
+        //publish thread hooks
+        virtual void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
+        virtual void onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
+        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
+    private:
+        void publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
+        void deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // ///////////////////////////////////////// data ///////////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    private:
+        /// @brief reference to the owning \ref RobotUnit
+        RobotUnit& robotUnit;
+        /// @brief Maps names of used \ref ControlDevice "ControlDevices" to the \ref JointController "JointController's" control mode
+        StringStringDictionary controlDeviceControlModeMap;
+        /// @brief Maps names of used \ref ControlDevice "ControlDevices" to the used \ref JointController for this \ref ControlDevice
+        std::map<std::string, const JointController*> controlDeviceUsedJointController;
+        /// @brief Bitmap of used \ref ControlDevice "ControlDevices" (1 == used, 0 == not used)
+        std::vector<char> controlDeviceUsedBitmap;
+        /// @brief Indices of used \ref ControlDevice "ControlDevices"
+        std::vector<std::size_t> controlDeviceUsedIndices;
+
+        std::string rtClassName_;
+        std::string instanceName_;
+
+        //this data is filled by the robot unit to provide convenience functions
+        std::atomic_bool isActive {false};
+        std::atomic_bool isRequested {false};
+        std::atomic_bool deactivatedBecauseOfError {false};
+        std::atomic_bool errorState {false};
+        bool deletable {false};
+        bool internal {false};
+
+        std::atomic_bool publishActive {false};
+
+        std::atomic_bool statusReportedActive {false};
+        std::atomic_bool statusReportedRequested {false};
+
+        VirtualRobot::RobotPtr rtRobot;
+        std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+        // /////////////////////////////////////// friends //////////////////////////////////////// //
+        // //////////////////////////////////////////////////////////////////////////////////////// //
+    private:
+        /**
+        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControllerManagement.
+        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+        */
+        friend class RobotUnitModule::NJointControllerAttorneyForControllerManagement;
+        /**
+        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControlThread.
+        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+        */
+        friend class RobotUnitModule::NJointControllerAttorneyForControlThread;
+        /**
+        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::Publisher.
+        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+        */
+        friend class RobotUnitModule::NJointControllerAttorneyForPublisher;
+        /**
+        * \brief This is required for the factory
+        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+        */
+        template<class> friend class detail::NJointControllerRegistryEntryHelper;
+    };
+
+    class SynchronousNJointController : public NJointControllerBase
+    {
+    public: ///TODO make protected and use attorneys
+        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+        {
+            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+        }
+    };
+    class AsynchronousNJointController : public NJointControllerBase
+    {
+    public: ///TODO make protected and use attorneys
+        virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+        virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+    };
+    using NJointController = SynchronousNJointController;
+    using NJointControllerPtr = SynchronousNJointControllerPtr;
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b49eb4dd9c039d24b13b6ccacbac05ac4f4a293
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
@@ -0,0 +1,245 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::NJointController
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "NJointControllerBase.h"
+
+#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/Registrar.h>
+
+namespace armarx::RobotUnitModule
+{
+    class ControllerManagement;
+}
+
+namespace armarx
+{
+    RobotUnit* getRobotUnit(RobotUnitModule::ControllerManagement* cmgr);
+
+    class NJointControllerRegistryEntry
+    {
+    private:
+        friend class RobotUnitModule::ControllerManagement;
+        virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
+                                               const NJointControllerConfigPtr&,
+                                               const VirtualRobot::RobotPtr&,
+                                               bool deletable,
+                                               bool internal,
+                                               const std::string& instanceName) const = 0;
+        virtual WidgetDescription::WidgetPtr
+        GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+                                  const std::map<std::string, ConstControlDevicePtr>&,
+                                  const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
+        virtual NJointControllerConfigPtr
+        GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
+        virtual bool hasRemoteConfiguration() const = 0;
+
+    protected:
+        static thread_local bool ConstructorIsRunning_;
+
+    public:
+        virtual ~NJointControllerRegistryEntry() = default;
+        static bool
+        ConstructorIsRunning()
+        {
+            return ConstructorIsRunning_;
+        }
+    };
+    using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
+
+    template <class ControllerType>
+    struct NJointControllerRegistration;
+
+    namespace detail
+    {
+        ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+            hasGenerateConfigDescription,
+            GenerateConfigDescription,
+            NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
+        ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+            hasGenerateConfigFromVariants,
+            GenerateConfigFromVariants,
+            NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+
+        template <class NJointControllerT>
+        class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
+        {
+            static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
+                              hasGenerateConfigFromVariants<NJointControllerT>::value,
+                          "Either overload both GenerateConfigDescription and "
+                          "GenerateConfigFromVariants, or none!");
+            static constexpr bool hasRemoteConfiguration_ =
+                hasGenerateConfigDescription<NJointControllerT>::value;
+
+            NJointControllerBasePtr
+            create(RobotUnitModule::ControllerManagement* cmngr,
+                   const NJointControllerConfigPtr& config,
+                   const VirtualRobot::RobotPtr& rob,
+                   bool deletable,
+                   bool internal,
+                   const std::string& instanceName) const final override
+            {
+                ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
+
+                using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
+                ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+                ARMARX_CHECK_EXPRESSION(cfg)
+                    << "The configuration is of the wrong type! it has to be an instance of: "
+                    << GetTypeString<ConfigPtrT>();
+
+                ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
+                    << "Two NJointControllers are created at the same time";
+                NJointControllerBasePtr ptr;
+                {
+                    ConstructorIsRunning_ = true;
+                    ARMARX_ON_SCOPE_EXIT
+                    {
+                        ConstructorIsRunning_ = false;
+                    };
+                    ptr = new NJointControllerT(getRobotUnit(cmngr), cfg, rob);
+                }
+                ptr->deletable = deletable;
+                ptr->internal = internal;
+                ptr->rtClassName_ = ptr->getClassName(Ice::emptyCurrent);
+                ptr->instanceName_ = instanceName;
+                return ptr;
+            }
+
+            WidgetDescription::WidgetPtr
+            GenerateConfigDescription(
+                const VirtualRobot::RobotPtr& robot,
+                const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+                const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+            {
+                if constexpr (hasRemoteConfiguration_)
+                {
+                    try
+                    {
+                        return NJointControllerT::GenerateConfigDescription(
+                            robot, controlDevices, sensorDevices);
+                    }
+                    catch (Ice::UserException& e)
+                    {
+                        ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                     << "::GenerateConfigDescription'"
+                                     << "\n---- file = " << e.ice_file()
+                                     << "\n---- line = " << e.ice_line()
+                                     << "\n---- id   = " << e.ice_id() << "\n---- what:\n"
+                                     << e.what() << "\n---- stacktrace:\n"
+                                     << e.ice_stackTrace();
+                        throw;
+                    }
+                    catch (std::exception& e)
+                    {
+                        ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                     << "::GenerateConfigDescription'"
+                                     << "\n---- what:\n"
+                                     << e.what();
+                        throw;
+                    }
+                    catch (...)
+                    {
+                        ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                     << "::GenerateConfigDescription'";
+                        throw;
+                    }
+                }
+                else
+                {
+                    ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+                }
+            }
+
+            NJointControllerConfigPtr
+            GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
+            {
+                if constexpr (hasRemoteConfiguration_)
+                {
+                    try
+                    {
+                        return NJointControllerT::GenerateConfigFromVariants(variants);
+                    }
+                    catch (Ice::UserException& e)
+                    {
+                        ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                     << "::GenerateConfigFromVariants'"
+                                     << "\n---- file = " << e.ice_file()
+                                     << "\n---- line = " << e.ice_line()
+                                     << "\n---- id   = " << e.ice_id() << "\n---- what:\n"
+                                     << e.what() << "\n---- stacktrace:\n"
+                                     << e.ice_stackTrace();
+                        throw;
+                    }
+                    catch (std::exception& e)
+                    {
+                        ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                     << "::GenerateConfigFromVariants'"
+                                     << "\n---- what:\n"
+                                     << e.what();
+                        throw;
+                    }
+                    catch (...)
+                    {
+                        ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+                                     << "::GenerateConfigFromVariants'";
+                        throw;
+                    }
+                }
+                else
+                {
+                    ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+                }
+            }
+
+            bool
+            hasRemoteConfiguration() const final override
+            {
+                return hasRemoteConfiguration_;
+            }
+        };
+    } // namespace detail
+
+    using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
+    template <class ControllerType>
+    struct NJointControllerRegistration
+    {
+        NJointControllerRegistration(const std::string& name)
+        {
+            NJointControllerRegistry::registerElement(
+                name,
+                std::unique_ptr<NJointControllerRegistryEntry>(
+                    new detail::NJointControllerRegistryEntryHelper<ControllerType>));
+        }
+    };
+} // namespace armarx
+
+#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T)                                     \
+    static_assert(                                                                                 \
+        ::armarx::detail::hasGenerateConfigDescription<T>::value,                                  \
+        #T " does not offer a construction gui (missing: static GenerateConfigDescription)");      \
+    static_assert(                                                                                 \
+        ::armarx::detail::hasGenerateConfigFromVariants<T>::value,                                 \
+        #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd6befc54b45193149ce7106be84fb644599a8cf
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include "NJointControllerBase.h"
+
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+
+namespace armarx
+{
+    template <typename ControlDataStruct>
+    class NJointControllerWithTripleBuffer : public SynchronousNJointController
+    {
+    public:
+        using MutexType = std::recursive_mutex;
+        using LockGuardType = std::lock_guard<std::recursive_mutex>;
+
+        NJointControllerWithTripleBuffer(
+                const ControlDataStruct& initialCommands = ControlDataStruct()) :
+            controlDataTripleBuffer{initialCommands}
+        {
+        }
+
+        void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+                                const IceUtil::Time& timeSinceLastIteration) override
+        {
+            rtUpdateControlStruct();
+            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+        }
+
+    protected:
+        const ControlDataStruct& rtGetControlStruct() const
+        {
+            return controlDataTripleBuffer.getReadBuffer();
+        }
+        bool rtUpdateControlStruct()
+        {
+            return controlDataTripleBuffer.updateReadBuffer();
+        }
+
+        void writeControlStruct()
+        {
+            //just lock to be save and reduce the impact of an error
+            //also this allows code to unlock the mutex before calling this function
+            //(can happen if some lockguard in a scope is used)
+            LockGuardType lock(controlDataMutex);
+            controlDataTripleBuffer.commitWrite();
+        }
+        ControlDataStruct& getWriterControlStruct()
+        {
+            return controlDataTripleBuffer.getWriteBuffer();
+        }
+
+        void setControlStruct(const ControlDataStruct& newStruct)
+        {
+            LockGuardType lock{controlDataMutex};
+            getWriterControlStruct() = newStruct;
+            writeControlStruct();
+        }
+
+        void reinitTripleBuffer(const ControlDataStruct& initial)
+        {
+            controlDataTripleBuffer.reinitAllBuffers(initial);
+        }
+
+        mutable MutexType controlDataMutex;
+
+    private:
+        WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
+    };
+
+    template <typename ControlDataStruct>
+    using NJointControllerWithTripleBufferPtr =
+    IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 06513b41621db1af829262b95c79c6c4634276ec..013af71634177b541922df2733ad495098b435eb 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -23,9 +23,10 @@
  */
 #include "NJointHolonomicPlatformGlobalPositionController.h"
 
-#include <cmath>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
 #include <SimoxUtility/math/periodic/periodic_clamp.h>
+#include <cmath>
 
 namespace armarx
 {
@@ -34,7 +35,7 @@ namespace armarx
 
 
     NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
-        const RobotUnitPtr&,
+        RobotUnit*,
         const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
         const VirtualRobot::RobotPtr&) :
         pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index bf4d338d0e642a870c91fe86772d64371e65f1ce..a343e404f97f759d232669b2a1ed72c5b49bd861 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -30,16 +30,16 @@
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
-#include "NJointController.h"
-#include "../RobotUnit.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include "../SensorValues/SensorValueHolonomicPlatform.h"
 
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
 #include <RobotAPI/libraries/core/PIDController.h>
 
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
-
 #include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+
 namespace armarx
 {
 
@@ -93,7 +93,7 @@ namespace armarx
         using ConfigPtrT = NJointHolonomicPlatformGlobalPositionControllerConfigPtr;
 
         inline NJointHolonomicPlatformGlobalPositionController(
-            const RobotUnitPtr& robotUnit,
+            RobotUnit* robotUnit,
             const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
             const VirtualRobot::RobotPtr&);
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index f5ca656188153d623bd57dee0640c6f842b03e84..40d37c7ae18892dfe18697ada68dcde1297e79ab 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -23,6 +23,10 @@
  */
 #include "NJointHolonomicPlatformRelativePositionController.h"
 
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <Eigen/Geometry>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
@@ -30,7 +34,7 @@ namespace armarx
 
 
     NJointHolonomicPlatformRelativePositionController::NJointHolonomicPlatformRelativePositionController(
-        const RobotUnitPtr&,
+        RobotUnit*,
         const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
         const VirtualRobot::RobotPtr&) :
         pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index abbd25d999ae85f7ba500e49f13479d0a4688700..788a45607361a8fd3f8a36aeb54cbf9042416236 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -30,8 +30,7 @@
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
-#include "NJointController.h"
-#include "../RobotUnit.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include "../SensorValues/SensorValueHolonomicPlatform.h"
 
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
@@ -80,7 +79,7 @@ namespace armarx
         using ConfigPtrT = NJointHolonomicPlatformRelativePositionControllerConfigPtr;
 
         inline NJointHolonomicPlatformRelativePositionController(
-            const RobotUnitPtr& robotUnit,
+            RobotUnit* robotUnit,
             const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
             const VirtualRobot::RobotPtr&);
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index 35fd7551d0f4dfd9b33de0b2cac71ee427ef5f35..fa4b4ad337ed17d35b464a6485ecf45c81f63960 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -22,12 +22,14 @@
 
 #include "NJointHolonomicPlatformUnitVelocityPassThroughController.h"
 
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 namespace armarx
 {
     NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
-        RobotUnitPtr prov,
+        RobotUnit* prov,
         NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
         cfg, const VirtualRobot::RobotPtr&) :
         maxCommandDelay(IceUtil::Time::milliSeconds(500))
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 3c793d040a30b463b4419a3f168d389fa03b2f83..e2c4a3aa4146a573c7fe4e92239272fd02d0372c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,7 +24,7 @@
 
 #include <VirtualRobot/Robot.h>
 
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
 #include "../util.h"
 
@@ -63,7 +63,7 @@ namespace armarx
         using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
 
         NJointHolonomicPlatformUnitVelocityPassThroughController(
-            RobotUnitPtr prov,
+            RobotUnit* prov,
             NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config,
             const VirtualRobot::RobotPtr&);
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index fab7cb647930e4ea2fa71c6ad14c52927e43139c..0d8b30e6b30fc0ab29a6183de6f55a73e01b30c3 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -21,12 +21,15 @@
  */
 
 #include "NJointKinematicUnitPassThroughController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
 
     NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughController(
-        RobotUnitPtr prov,
+        RobotUnit* prov,
         const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
         const VirtualRobot::RobotPtr&)
     {
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index a0fe164c4803ceb322c451b464c0c6acdc48bebf..cbbb83f65750f6aa9e9d1de7d0f7c2e1abe3fdf4 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -28,8 +28,7 @@
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
-#include "NJointController.h"
-#include "../RobotUnit.h"
+#include "NJointControllerBase.h"
 #include "../SensorValues/SensorValue1DoFActuator.h"
 
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
@@ -57,7 +56,7 @@ namespace armarx
         using ConfigPtrT = NJointKinematicUnitPassThroughControllerConfigPtr;
 
         inline NJointKinematicUnitPassThroughController(
-            RobotUnitPtr prov,
+            RobotUnit* prov,
             const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
             const VirtualRobot::RobotPtr&);
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index c67b41d1985b7bcfc2f9404a09878424302da9a3..fcedf27a6a87d533ce5cd67a23cd7c9d36e7774b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -22,9 +22,13 @@
  *             GNU General Public License
  */
 #include "NJointTCPController.h"
-#include <VirtualRobot/RobotNodeSet.h>
 #include "../RobotUnit.h"
 
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+
 #define DEFAULT_TCP_STRING "default TCP"
 
 namespace armarx
@@ -154,7 +158,7 @@ namespace armarx
         return new NJointTCPControllerConfig {values.at("nodeSetName")->getString(), values.at("tcpName")->getString()};
     }
 
-    NJointTCPController::NJointTCPController(RobotUnitPtr robotUnit, NJointTCPControllerConfigPtr config, const VirtualRobot::RobotPtr& r)
+    NJointTCPController::NJointTCPController(RobotUnit* robotUnit, const NJointTCPControllerConfigPtr& config, const VirtualRobot::RobotPtr& r)
     {
         ARMARX_CHECK_EXPRESSION(robotUnit);
         ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 57d2f88812ca75e364ae18f3360bda90063e93c4..41a93a0c368288536ca2eca179b8532fe1d6e01a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -24,9 +24,8 @@
 #pragma once
 
 
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include <VirtualRobot/Robot.h>
-#include "../RobotUnit.h"
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
 #include "../SensorValues/SensorValue1DoFActuator.h"
 #include <VirtualRobot/IK/DifferentialIK.h>
@@ -72,7 +71,7 @@ namespace armarx
     {
     public:
         using ConfigPtrT = NJointTCPControllerConfigPtr;
-        NJointTCPController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+        NJointTCPController(RobotUnit* prov, const NJointTCPControllerConfigPtr& config, const VirtualRobot::RobotPtr& r);
 
         // NJointControllerInterface interface
         std::string getClassName(const Ice::Current&) const override;
@@ -87,10 +86,6 @@ namespace armarx
             const std::map<std::string, ConstSensorDevicePtr>&);
 
         static NJointTCPControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
-        NJointTCPController(
-            RobotUnitPtr prov,
-            NJointTCPControllerConfigPtr config,
-            const VirtualRobot::RobotPtr& r);
 
         // for TCPControlUnit
         void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index d2d10aee0d547b4341c343d368e9c518de86b501..124d66adbc8b24f176cdb7d6acf5df4d3815eccc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -22,6 +22,8 @@
 
 #include "NJointTaskSpaceImpedanceController.h"
 
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
 #include <SimoxUtility/math/convert/mat4f_to_pos.h>
 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index f23c8a04945858b9efdb071f1c5215b719073be0..06d725f856583029d4c99a781b4832a3ee4f3c80 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -22,7 +22,7 @@
 
 #pragma once
 
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
 #include <VirtualRobot/Robot.h>
 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index fa0c743d77a225270c44e767c7e1b0843c7601e3..3b270bd0516b1e00dedfe3c6024c16ecbd6948bc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -1,13 +1,19 @@
 #include "NJointTrajectoryController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
+
 #include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
 
-    NJointTrajectoryController::NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
+    NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
     {
         cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
         ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointTrajectoryControllerConfigPtr";
@@ -39,6 +45,11 @@ namespace armarx
         }
     }
 
+    NJointTrajectoryController::~NJointTrajectoryController()
+    {
+
+    }
+
     std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
     {
         return "NJointTrajectoryController";
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index 560f33100464eb9e2994069059f0262925579e88..072f02567b233234a6669cc1a0b4d366676d0759 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
 #include <VirtualRobot/Robot.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
 #include <RobotAPI/libraries/core/TrajectoryController.h>
@@ -28,7 +28,9 @@ namespace armarx
         public NJointTrajectoryControllerInterface
     {
     public:
-        NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+        NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+        ~NJointTrajectoryController();
 
         // NJointControllerInterface interface
         std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 44c1df4514e42b5f62c8548843971e0b9b7b4ce5..17bdc45e13085d67407d061256319f526e4355c1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -24,4 +24,10 @@
 
 namespace armarx
 {
+
+RobotUnit::~RobotUnit()
+{
+
+}
+
 }
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index dc96e422a047fcdfcc5ebe97d14885191e28b857..22ffd80aefe3191b42626aa0af7875a30188b14e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -23,7 +23,10 @@
 #pragma once
 
 #include "RobotUnitModules/RobotUnitModules.h"
-#include "NJointControllers/NJointController.h"
+
+// This include is not necessary but a lot of other files rely on it being included here
+// Can be removed if other files include what they use
+#include "NJointControllers/NJointControllerBase.h"
 
 /**
 * @defgroup Library-RobotUnit RobotUnit
@@ -187,6 +190,8 @@ namespace armarx
         virtual public RobotUnitModule::ControlThreadDataBuffer
     {
     public:
+        ~RobotUnit();
+
         static RobotUnit& Instance()
         {
             return ModuleBase::Instance<RobotUnit>();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 557f293e3ccb818d63e4fa1918664306c60ac61f..90910c9f28b246abac25b5b15f0c609bdf33fe3e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -26,7 +26,6 @@
 #include <ArmarXCore/core/util/Preprocessor.h>
 #include <ArmarXCore/util/CPPUtility/trace.h>
 
-#include "../NJointControllers/NJointController.h"
 #include "RobotUnitModuleBase.h"
 #include "RobotUnitModules.h"
 
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 6b251b9cbaa0db977e102580deeb17b4b0d2165e..a3269b4d0e11cb792f639e4c477218dcb58183e8 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -29,9 +29,15 @@
 #include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
 #include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
 #include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 
 #include "../Devices/RTThreadTimingsSensorDevice.h"
-#include "../NJointControllers/NJointController.h"
 
 namespace armarx::RobotUnitModule
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index b310e7f57ea397470b683e40b3f9606d2885185e..891c8fbf11edd151630ebb3e6fa4d244b6bc71ea 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -25,7 +25,7 @@
 #include "RobotUnitModuleControllerManagement.h"
 #include "RobotUnitModuleDevices.h"
 
-#include "../NJointControllers/NJointController.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
 
 namespace armarx::RobotUnitModule
 {
@@ -267,7 +267,9 @@ namespace armarx::RobotUnitModule
                 request.jointControllers.at(jointCtrlIndex) = _module<Devices>().getControlDevices().at(jointCtrlIndex)->getJointController(cd2cm.second);
             }
         }
-        for (auto i : getIndices(request.jointControllers))
+
+        std::size_t jointControllersCount = request.jointControllers.size();
+        for (std::size_t i = 0; i < jointControllersCount; ++i)
         {
             JointController*& jointCtrl = request.jointControllers.at(i);
             if (!jointCtrl)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index f03a527d7a2b87c5c5752e2f16cdaa0baaee13c4..60ebf6dbf533c27820ee7b4b950e858004cac60f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -20,11 +20,21 @@
  *             GNU General Public License
  */
 
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
 
 #include "RobotUnitModuleControllerManagement.h"
-#include "../NJointControllers/NJointController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
+
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <ArmarXCore/core/util/algorithm.h>
 
 namespace armarx::RobotUnitModule
 {
@@ -762,6 +772,11 @@ namespace armarx::RobotUnitModule
         nJointControllers.clear();
     }
 
+    ControllerManagement::~ControllerManagement()
+    {
+
+    }
+
     void ControllerManagement::_preOnInitRobotUnit()
     {
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
index 17b5a00e9b28a8e8d2dec03eea1c6aec9b70cdd9..3317540bc5e9adf326ba8410d06df2576d907ec3 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -48,6 +48,9 @@ namespace armarx::RobotUnitModule
         {
             return ModuleBase::Instance<ControllerManagement>();
         }
+
+        ~ControllerManagement();
+
         // //////////////////////////////////////////////////////////////////////////////////////// //
         // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
         // //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 057ca4b175a3eda28066d9289c90caaf95fd7906..fb8b38fc28ad83a8ba07ac395049796ac797d956 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -25,6 +25,7 @@
 #include "RobotUnitModuleRobotData.h"
 
 #include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/core/util/algorithm.h>
 
 namespace armarx::RobotUnitModule
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 4c6de078b00ae810a846c68e829a3da136a6ed79..c07f2d42c7d1d17def0b67a8c4866fe21384284e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -22,8 +22,14 @@
 
 #include "RobotUnitModulePublisher.h"
 
-#include "../NJointControllers/NJointController.h"
-#include "../Units/RobotUnitSubUnit.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
@@ -123,13 +129,6 @@ namespace armarx::RobotUnitModule
 
 namespace armarx::RobotUnitModule
 {
-    const std::map<std::string, NJointControllerBasePtr>& Publisher::getNJointControllers()
-    {
-        ARMARX_TRACE;
-        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
-        return ControllerManagementAttorneyForPublisher::GetNJointControllers(this);
-    }
-
     TimedVariantPtr Publisher::publishNJointClassNames()
     {
         ARMARX_TRACE;
@@ -166,7 +165,7 @@ namespace armarx::RobotUnitModule
         auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
         auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
         NJointControllerStatusSeq allStatus;
-        for (const auto& pair : getNJointControllers())
+        for (const auto& pair : ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
         {
             const auto begInner = TimeUtil::GetTime(true);
             const NJointControllerBasePtr& nJointCtrl = pair.second;
@@ -489,7 +488,7 @@ namespace armarx::RobotUnitModule
             std::this_thread::sleep_for(std::chrono::milliseconds {100});
             ARMARX_DEBUG << "shutting down publisher task done";
         }
-        for (const auto& pair : getNJointControllers())
+        for (const auto& pair : ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
         {
             ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
             NJointControllerAttorneyForPublisher::DeactivatePublishing(pair.second, debugDrawerPrx, debugObserverPrx);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 5844b7ba9efb2bbb4bdcd0f0370ad3def34cddd5..9b68b4c7aeffc6ef77b3aa5e0c2d9ca20c1c8677 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -164,11 +164,6 @@ namespace armarx::RobotUnitModule
         // //////////////////////////////////// implementation //////////////////////////////////// //
         // //////////////////////////////////////////////////////////////////////////////////////// //
     private:
-        /**
-         * @brief Returns a map containing all current NJointControllers.
-         * @return A map containing all current NJointControllers.
-         */
-        const std::map<std::string, NJointControllerBasePtr>& getNJointControllers();
         /**
          * @brief Returns the used RobotUnitObserver
          * @return The used RobotUnitObserver
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index a9d2e8de69625147071a2e2aaa650d43de40cdb9..09b19d38a9ede50f8e7f167aad5aee2681bf30e3 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -26,8 +26,12 @@
 
 #include "RobotUnitModuleSelfCollisionChecker.h"
 #include "RobotUnitModuleRobotData.h"
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
 
-#include "../NJointControllers/NJointController.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
 
 #include <SimoxUtility/algorithm/string/string_tools.h>
 
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index 6e4261c9575753d69b338e7d89c5e99beeae8da7..cdb365742dad36bc50872c5ba86b3a3e58be9d87 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -23,9 +23,7 @@
 
 #include "SensorValueBase.h"
 
-#include <ArmarXCore/core/util/algorithm.h>
 #include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
index 7afa43be267682cfd23bf38eccf073b54f6cdca3..021ab483d56dc0536e28004102fafaacd629677f 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -26,11 +26,10 @@
 #include <string>
 #include <map>
 
-#include <ArmarXCore/observers/variant/TimedVariant.h>
+#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
 
-#include "../util/HeterogenousContinuousContainerMacros.h"
-
-#include "../util.h"
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
+#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
index 7902e41d3ce8023a4dba6f7699086b79a7f04e07..e2ee37caa1e39dd52806b800837a9abdecee7bbd 100644
--- a/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
@@ -20,7 +20,6 @@
  *             GNU General Public License
  */
 
-#include "NJointControllers/NJointController.h"
 #include "RobotUnit.h"
 #include "RobotUnitModules/RobotUnitModuleBase.h"
 
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 086eabf15c900b80d4fd713e6c70ec22a4838fdb..0273f0cf61370e5539dbd8d06d9349d44acec6a7 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -20,8 +20,13 @@
  *             GNU General Public License
  */
 #include "KinematicSubUnit.h"
-#include <VirtualRobot/RobotNodeSet.h>
+
 #include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
 
 armarx::KinematicSubUnit::KinematicSubUnit() :
     reportSkipper(20.0f)
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index 4f320f59ce2d0181473fe7231ed56a5f6ad0a67d..67b7eec6c4b3aa1dec80caa3efc7108adbaef84c 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -25,6 +25,7 @@
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
 
 
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index cb7f6603057bdf2219d3f2aa92506bc6d76d2d53..b952748176b9940a8c5529775faaadb22ce574ab 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -28,6 +28,8 @@
 #include "../NJointControllers/NJointTCPController.h"
 #include "../util.h"
 
+#include <ArmarXCore/core/Component.h>
+
 #include <mutex>
 
 namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index eff567f2748ab7c333deb43537f37a22e961de79..b9649773942d2c539cad4f1b38b03dc4a78d3f40 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -21,10 +21,11 @@
  */
 #pragma once
 
-#include <ArmarXCore/core/util/algorithm.h>
-
 #include "ClassMemberInfoEntry.h"
 
+#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
+
+
 namespace armarx::introspection
 {
     /**
@@ -168,7 +169,14 @@ namespace armarx::introspection
         std::map<std::string, VariantBasePtr> result;
         for (const auto& e : GetEntries().values())
         {
-            mergeMaps(result, e.toVariants(timestamp, ptr), MergeMapsMode::OverrideNoValues);
+            for (auto& elem : e.toVariants(timestamp, ptr))
+            {
+                if (result.count(elem.first))
+                {
+                    throw std::invalid_argument {"mergeMaps: newMap would override values from oldMap"};
+                }
+            }
+            //mergeMaps(result, , MergeMapsMode::OverrideNoValues);
         }
         return result;
     }
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 6188648d33e64d86dbac8a168446bbe5127e6339..1ad242babb61a5f2970b641d0ac3bbb09c338ba1 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -22,6 +22,7 @@
 #pragma once
 
 #include "DataFieldsInfo.h"
+
 namespace armarx::introspection
 {
 
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 29fb7883065ee2f18e68dd5c909e17e8f8d8246c..34f53f63744ff7939c1cfcf94526052d0dad2d71 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -25,7 +25,6 @@
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
 #include <ArmarXCore/observers/variant/TimedVariant.h>
 #include <ArmarXCore/interface/observers/VariantBase.h>
 
diff --git a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
index 1df2874b254aa6eec3688ff645d2a1430df1e85f..c9f30ffe5c0d845c057bf9346ca2425e3e146832 100644
--- a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
@@ -32,7 +32,7 @@ set(COMPONENT_LIBS
     SimpleConfigDialog
 )
 
-set(GUI_RCS icons.qrc)
+qt5_add_resources(GUI_RCS icons.qrc)
 
 if(ArmarXGui_FOUND)
     armarx_gui_library(ArVizGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "${GUI_RCS}" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/CMakeLists.txt b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/CMakeLists.txt
index 8957d94e4797b511b9f377b293b456bb5ec3f07c..4bd6377ed31f44646f8edceee1fcd34d08443a28 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/CMakeLists.txt
@@ -14,7 +14,8 @@ set(HEADERS
 
 set(GUI_MOC_HDRS HomogeneousMatrixCalculatorGuiPlugin.h HomogeneousMatrixCalculatorWidgetController.h)
 set(GUI_UIS HomogeneousMatrixCalculatorWidget.ui)
-set(GUI_RCS icons.qrc)
+#set(GUI_RCS icons.qrc)
+qt5_add_resources(GUI_RCS icons.qrc)
 
 set(COMPONENT_LIBS VirtualRobot)
 
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 68fbc7165c7d318cf68990f779c074d2f9e12cd4..705f0f7584a84aff9354cf2986505436491f98ff 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -232,7 +232,7 @@ namespace armarx
                     item->setText(col++, QString::fromStdString(ss.str()));
                 }
                 item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
-                item->setText(col++, QString::fromStdString(TimeUtil::toStringDateTime(pose.timestamp)));
+                item->setText(col++, QString::fromStdString(pose.timestamp.toDateTimeString()));
 
                 {
                     std::stringstream ss;
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt
index 71694f94d2a53b1e26ec1e9258ed0335c9bb0083..5539182c7d805c3d5d6003273b2d839704d25d8b 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/CMakeLists.txt
@@ -40,7 +40,7 @@ set(GUI_UIS
     QWidgets/NJointControllersWidget.ui
     QWidgets/SensorDevicesWidget.ui
 )
-set(GUI_RCS icons.qrc)
+qt5_add_resources(GUI_RCS icons.qrc)
 
 set(COMPONENT_LIBS
     ArmarXGuiBase
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 749a9e26330f60a2ffaee4ae251a65495887f771..1d427101fc58c6b36acf68979ff0a16c2f890bfc 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -37,7 +37,13 @@ set(GUI_UIS
 )
 
 # Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
-set(COMPONENT_LIBS RobotAPIInterfaces aron skills aronjsonconverter SimpleConfigDialog)
+set(COMPONENT_LIBS
+    RobotAPIInterfaces
+    aron
+    RobotAPISkills
+    aronjsonconverter
+    SimpleConfigDialog
+)
 
 if(ArmarXGui_FOUND)
     armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 15678facbdf134eb61542300528e6aa3290a0dc9..59089db2066050f0ffac50cd63c7af87dbcfea51 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -70,7 +70,7 @@
        <item row="6" column="0">
         <widget class="QPushButton" name="pushButtonStopSkill">
          <property name="text">
-          <string>Stop</string>
+          <string>Stop current skill</string>
          </property>
         </widget>
        </item>
@@ -108,7 +108,7 @@
        <item row="6" column="1">
         <widget class="QPushButton" name="pushButtonExecuteSkill">
          <property name="text">
-          <string>Execute</string>
+          <string>Request Execution</string>
          </property>
         </widget>
        </item>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 4a458d690633e611e64a4402238ec6793ae388d6..431ce92cade5f9d8fc92d8ac7bd80fd402e84c51 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -148,6 +148,15 @@ namespace armarx
 
     void SkillManagerMonitorWidgetController::refreshSkills()
     {
+        static std::map<skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
+            {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
+            {skills::provider::dto::Execution::Status::Failed, "Failed"},
+            {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
+            {skills::provider::dto::Execution::Status::Running, "Running"},
+            {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
+            {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
+        };
+
         if (!connected)
             return;
 
@@ -178,6 +187,7 @@ namespace armarx
                 SkillProviderData providerData;
                 providerData.providerName = providerName;
                 providerData.skillDescriptions = provider->getSkills();
+                providerData.skillProviderPrx = provider;
                 skills.insert(std::make_pair(providerName, providerData));
 
                 newProviders.push_back(providerName);
@@ -185,30 +195,61 @@ namespace armarx
         }
 
         // remove providers from tree
-        for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+        int i = 0;
+        while (i < widget.treeWidgetSkills->topLevelItemCount())
         {
             QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
             if (std::find(removedProviders.begin(), removedProviders.end(), item->text(0).toStdString()) != removedProviders.end())
             {
                 delete widget.treeWidgetSkills->takeTopLevelItem(i);
             }
+            else
+            {
+                ++i;
+            }
         }
 
         // add new providers
         for (const auto& [providerName, provider] : skills)
         {
-            if (std::find(newProviders.begin(), newProviders.end(), providerName) != newProviders.end())
+            if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName); it != newProviders.end())
             {
-                auto it = new QTreeWidgetItem(widget.treeWidgetSkills);
-                it->setText(0, QString::fromStdString(providerName));
+                auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
+                item->setText(0, QString::fromStdString(providerName));
                 for (const auto& [name, sk] : provider.skillDescriptions)
                 {
-                    auto itsk = new QTreeWidgetItem(it);
-                    it->addChild(itsk);
+                    auto itsk = new QTreeWidgetItem(item);
+                    item->addChild(itsk);
                     itsk->setText(0, QString::fromStdString(name));
                 }
             }
         }
+
+        // update status
+        for (int i = 0;  i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+        {
+            try
+            {
+                QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
+                auto providerName = item->text(0).toStdString();
+                for (int j = 0; j < item->childCount(); ++j)
+                {
+                    QTreeWidgetItem* skillItem = item->child(j);
+                    auto skillName = skillItem->text(0).toStdString();
+
+                    auto& providerPrx = skills.at(providerName).skillProviderPrx;
+                    auto statusUpdate = providerPrx->getSkillExecutionStatus(skillName);
+
+                    skillItem->setText(2, QString::fromStdString(ExecutionStatus2String.at(statusUpdate.status)));
+                }
+            }
+            catch (const std::exception& e)
+            {
+                // Perhaps the skill provider died after the check at the beginning of this method
+                // Continue
+                continue;
+            }
+        }
     }
 
     void SkillManagerMonitorWidgetController::executeSkill()
@@ -227,11 +268,14 @@ namespace armarx
         auto data = getConfigAsAron();
 
         skills::manager::dto::SkillExecutionInfo exInfo;
+        exInfo.waitUntilSkillFinished = false;
         exInfo.providerName = selectedSkill.providerName;
         exInfo.skillName = selectedSkill.skillName;
         exInfo.params = aron::data::Dict::ToAronDictDTO(data);
 
         ARMARX_INFO << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
+        // Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
+        //executions.emplace_back([&](){ manager->executeSkill(exInfo); });
         manager->executeSkill(exInfo);
     }
 
@@ -248,9 +292,8 @@ namespace armarx
             return;
         }
 
-        // TODO
         ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
-        //observer->abortSkill(skills.selectedSkill);
+        manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
     }
 
     void SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem*)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 9ba0d8a378411a688f650012ec96d55525a164c2..debeaedff268559d70438cfd3c453cee77f9d3cb 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -22,6 +22,8 @@
 #pragma once
 
 #include <stack>
+#include <vector>
+#include <thread>
 #include <ArmarXCore/core/system/ImportExportComponent.h>
 
 #include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
@@ -95,7 +97,7 @@ namespace armarx
          * Widget Form
          */
         Ui::SkillManagerMonitorWidget  widget;
-        QPointer<SimpleConfigDialog>    dialog;
+        QPointer<SimpleConfigDialog>   dialog;
 
         std::string observerName = "SkillManager";
         skills::manager::dti::SkillManagerInterfacePrx manager = nullptr;
@@ -110,6 +112,7 @@ namespace armarx
         {
             std::string providerName;
             skills::provider::dto::SkillDescriptionMap skillDescriptions;
+            skills::provider::dti::SkillProviderInterfacePrx skillProviderPrx;
         };
 
         // Data taken from observer
@@ -125,6 +128,9 @@ namespace armarx
         // others
         std::atomic_bool connected = false;
         QTimer* refreshSkillsResultTimer;
+
+        // skillExecutions
+        std::vector<std::thread> executions;
     };
 }
 
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 36442cf0bdaa4238a8a832fe39f6c244569fe830..be1193d665db683b844d28d60662e8e3088eec70 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -72,7 +72,6 @@ set(SLICE_FILES
     units/RobotUnit/NJointCartesianWaypointController.ice
     units/RobotUnit/NJointCartesianNaturalPositionController.ice
     units/RobotUnit/RobotUnitInterface.ice
-    units/RobotUnit/GazeController.ice
 
     units/RobotUnit/NJointBimanualForceController.ice
     units/RobotUnit/NJointBimanualObjLevelController.ice
diff --git a/source/RobotAPI/interface/armem/commit.ice b/source/RobotAPI/interface/armem/commit.ice
index 526188ecf649745cb52e1fd00212d48c602cfcee..17260a2136d2cc4c24d1ecd06f5f77ecbcb7f8c0 100644
--- a/source/RobotAPI/interface/armem/commit.ice
+++ b/source/RobotAPI/interface/armem/commit.ice
@@ -34,10 +34,10 @@ module armarx
             {
                 armem::data::MemoryID entityID;
                 aron::data::dto::AronDictSeq instancesData;
-                long timeCreatedMicroSeconds;
+                armarx::core::time::dto::DateTime timeCreated;
 
                 float confidence = 1.0;
-                long timeSentMicroSeconds = -1;
+                armarx::core::time::dto::DateTime timeSent;
             };
             sequence<EntityUpdate> EntityUpdateList;
 
@@ -46,7 +46,7 @@ module armarx
                 bool success = false;
 
                 armem::data::MemoryID snapshotID;
-                long timeArrivedMicroSeconds;
+                armarx::core::time::dto::DateTime timeArrived;
 
                 string errorMessage;
             };
diff --git a/source/RobotAPI/interface/armem/memory.ice b/source/RobotAPI/interface/armem/memory.ice
index a4cdd0cde1de4f553fd4181f6515e1fd4255ae01..617f7043f76019a2c474f588127dbcbd9c4f3991 100644
--- a/source/RobotAPI/interface/armem/memory.ice
+++ b/source/RobotAPI/interface/armem/memory.ice
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <ArmarXCore/interface/core/time.ice>
+
 #include <RobotAPI/interface/aron.ice>
 
 
@@ -15,7 +17,7 @@ module armarx
                 string coreSegmentName = "";
                 string providerSegmentName = "";
                 string entityName = "";
-                long timestampMicroSeconds = -1;
+                armarx::core::time::dto::DateTime timestamp;
                 int instanceIndex = -1;
             }
 
@@ -36,9 +38,9 @@ module armarx
             /// Ice Twin of `armarx::armem::EntityInstanceMetadata`.
             class EntityInstanceMetadata
             {
-                long timeCreatedMicroSeconds;
-                long timeSentMicroSeconds;
-                long timeArrivedMicroSeconds;
+                armarx::core::time::dto::DateTime timeCreated;
+                armarx::core::time::dto::DateTime timeSent;
+                armarx::core::time::dto::DateTime timeArrived;
 
                 float confidence = 1.0;
             };
@@ -57,7 +59,7 @@ module armarx
             {
                 EntityInstanceSeq instances;
             };
-            dictionary<long, EntitySnapshot> EntityHistory;
+            dictionary<armarx::core::time::dto::DateTime, EntitySnapshot> EntityHistory;
 
 
             /// Ice Twin of `armarx::armem::Entity`.
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 936294507f0b05627051fa6974309faa1db4d758..f5555e4f306bf3e73ab3e47719f89a2ddb87268d 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -34,13 +34,13 @@ module armarx
                     /// Get a single snapshot (default for latest).
                     class Single extends EntityQuery
                     {
-                        long timestamp = -1;  // -1 for latest
+                        armarx::core::time::dto::DateTime timestamp;  // -1 for latest
                     };
                     /// Get snapshots based on a time range (default for all).
                     class TimeRange extends EntityQuery
                     {
-                        long minTimestamp = -1;  // -1 for oldest
-                        long maxTimestamp = -1;  // -1 for latest
+                        armarx::core::time::dto::DateTime minTimestamp;  // -1 for oldest
+                        armarx::core::time::dto::DateTime maxTimestamp;  // -1 for latest
                     };
                     /**
                      * @brief Get snapshots based on an index range (default for all).
@@ -78,8 +78,8 @@ module armarx
                      */
                     class TimeApprox extends EntityQuery
                     {
-                        long timestamp = -1;
-                        long eps = -1;
+                        armarx::core::time::dto::DateTime timestamp;
+                        armarx::core::time::dto::Duration eps;
                     };
 
                     /**
@@ -90,7 +90,7 @@ module armarx
                      */
                     class BeforeOrAtTime extends EntityQuery
                     {
-                        long timestamp;
+                        armarx::core::time::dto::DateTime timestamp;
                     }
 
                     /**
@@ -104,7 +104,7 @@ module armarx
                      */
                     class BeforeTime extends EntityQuery
                     {
-                        long timestamp;
+                        armarx::core::time::dto::DateTime timestamp;
                         long maxEntries = 1;
                     }
 
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 4b09fd18ef1c6fe771200b3338ae67073616c42b..35960cce72ba7157a2c3bfbbca61824ef6a26334 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -23,8 +23,8 @@
 
 #pragma once
 
-//fq
 #include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <ArmarXCore/interface/core/time.ice>
 
 #include <RobotAPI/interface/core/PoseBase.ice>
 #include <RobotAPI/interface/core/NameValueMap.ice>
@@ -101,7 +101,7 @@ module armarx
                 /// Confidence in [0, 1] (1 = full, 0 = none).
                 float confidence = 0;
                 /// Source timestamp.
-                long timestampMicroSeconds = -1;
+                armarx::core::time::dto::DateTime timestamp;
 
                 /// [Optional] Object bounding box in object's local coordinate frame.
                 Box localOOBB;
@@ -146,7 +146,7 @@ module armarx
                 /// Confidence in [0, 1] (1 = full, 0 = none).
                 float confidence = 0;
                 /// Source timestamp.
-                long timestampMicroSeconds = -1;
+                armarx::core::time::dto::DateTime timestamp;
 
                 /// Object bounding box in object's local coordinate frame. May be null.
                 Box localOOBB;
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index ea2ffed638075444a61c17b7952da90ae09708e3..9f6bd73148b7de3a7f043359775155d33a3227ff 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -39,6 +39,7 @@ module armarx
                     string providerName;
                     string skillName;
                     aron::data::dto::Dict params;
+                    bool waitUntilSkillFinished;
                 };
 
                 struct ProviderInfo
@@ -57,6 +58,7 @@ module armarx
                     void removeProvider(string providerName);
                     provider::dti::SkillProviderMap getSkillProviders();
                     void executeSkill(dto::SkillExecutionInfo skillExecutionInfo);
+                    void abortSkill(string providerName, string skillName);
                 };
             }
         }
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index ff147cc4a9dcc7447af016d9c0f80251ce02db62..c6eec573654ae0b8a4a6b7811fe273eb189033ad 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -68,6 +68,7 @@ module armarx
                     string skillName;
                     aron::data::dto::Dict params;
                     callback::dti::SkillProviderCallbackInterface* callbackInterface; // use nullptr if you do not want to have callbacks
+                    bool waitUntilSkillFinished;
                 };
 
                 // The status enum of a skill
@@ -108,7 +109,7 @@ module armarx
                     dto::SkillDescriptionMap getSkills();
                     dto::SkillStatusUpdate getSkillExecutionStatus(string name);
                     void executeSkill(dto::SkillExecutionInfo executionInfo);
-                    dto::SkillStatusUpdate abortSkill(string skill);
+                    void abortSkill(string skill, bool waitUntilSkillFinished);
                 };
 
                 dictionary<string, SkillProviderInterface*> SkillProviderMap;
diff --git a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
deleted file mode 100644
index 78f98701af3f230ce2cbbb6c1d97082fa342ec6e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
+++ /dev/null
@@ -1,63 +0,0 @@
-
-/*
-* 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/>.
-*
-* @package    ArmarX::RobotAPI
-* @author     Raphael Grimm
-* @copyright  2019 Humanoids Group, H2T, KIT
-* @license    http://www.gnu.org/licenses/gpl-2.0.txt
-*             GNU General Public License
-*/
-
-#pragma once
-
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
-module armarx { module control { module gaze_controller {
-
-    class GazeControllerConfig extends NJointControllerConfig
-    {
-        string cameraFrameName = "DepthCamera";
-        string yawNodeName = "Neck_1_Yaw";
-        string pitchNodeName = "Neck_2_Pitch";
-        string cameraNodeName = "DepthCamera";
-        string torsoNodeName = "TorsoJoint";
-
-        float Kp = 1.9f;
-        float Ki = 0.0f;
-        float Kd = 0.0f;
-        double maxControlValue = 1.0;
-        double maxDerivation = 0.5;
-
-        float yawAngleTolerance = 0.005;
-        float pitchAngleTolerance = 0.005;
-        bool abortIfUnreachable = false;
-    };
-
-    interface GazeControllerInterface extends
-        NJointControllerInterface
-    {
-        void submitTarget(FramedPositionBase target, long requestTimestamp);
-        void removeTarget();
-        void removeTargetAfter(long durationMilliSeconds);
-    };
-
-    interface GazeControllerListener
-    {
-        void reportGazeTarget(long requestTimestamp, long reachedTimestamp, long releasedTimestamp, long abortedTimestamp, FramedPositionBase target);
-    };
-
-};};};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 712399c5f77ec1a7c116b3e77b0ad2e4df616a87..f02c8e8ee8720f7a45fb730f155986016cdb0331 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -51,10 +51,12 @@ module armarx
     {
         idempotent bool hasReachedTarget();
         idempotent bool hasReachedForceLimit();
+        idempotent int getCurrentWaypointIndex();
 
         void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
         void setWaypoints(Eigen::Matrix4fSeq wps);
         void setWaypoint(Eigen::Matrix4f wp);
+        void setWaypointAx(Ice::FloatSeq wp);
         void setConfigAndWaypoints(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
         void setConfigAndWaypoint(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
         
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointController.ice
index 7e07c41de1e1096a3be5a16bad3a9bd5809c52c4..f1056467092843c4928f63ba9dcba119ac8c0ebd 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointController.ice
@@ -22,7 +22,80 @@
 
 #pragma once
 
+#include <ArmarXGui/interface/WidgetDescription.ice>
 
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+
+module armarx
+{
+    interface NJointControllerInterface;
+
+
+    module RobotUnitControllerNames
+    {
+        const string NJointTrajectoryController = "NJointTrajectoryController";
+        const string NJointGlobalTCPController = "NJointGlobalTCPController"; /*@@@TODO: move NJointGlobalTCPController to RobotAPI */
+        const string NJointTCPController = "NJointTCPController";
+        const string NJointCartesianVelocityController = "NJointCartesianVelocityController";
+    };
+
+
+    class NJointControllerConfig {};
+
+    struct NJointControllerDescription
+    {
+        string instanceName;
+        string className;
+        NJointControllerInterface* controller;
+        StringStringDictionary controlModeAssignment;
+        bool deletable;
+        bool internal;
+    };
+    sequence<NJointControllerDescription> NJointControllerDescriptionSeq;
+
+    struct NJointControllerStatus
+    {
+        string instanceName;
+        bool active = false;
+        bool requested = false;
+        bool error = false;
+        long timestampUSec = 0;
+    };
+    sequence<NJointControllerStatus> NJointControllerStatusSeq;
+
+    struct NJointControllerDescriptionWithStatus
+    {
+        NJointControllerStatus status;
+        NJointControllerDescription description;
+    };
+    sequence<NJointControllerDescriptionWithStatus> NJointControllerDescriptionWithStatusSeq;
+
+    interface NJointControllerInterface
+    {
+        ["cpp:const"] idempotent string getClassName();
+        ["cpp:const"] idempotent string getInstanceName();
+        ["cpp:const"] idempotent StringStringDictionary getControlDeviceUsedControlModeMap();
+        ["cpp:const"] idempotent bool isControllerActive();
+        ["cpp:const"] idempotent bool isControllerRequested();
+        ["cpp:const"] idempotent bool isDeletable();
+        ["cpp:const"] idempotent bool hasControllerError();
+        ["cpp:const"] idempotent NJointControllerStatus getControllerStatus();
+        ["cpp:const"] idempotent NJointControllerDescription getControllerDescription();
+        ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus();
+        // This method is not used by anybody and just produces cyclic dependencies
+        // If you were able to create/get an NJointController, you know the RobotUnit already.
+        //["cpp:const"] idempotent RobotUnitInterface* getRobotUnit();
+
+        void activateController();
+        void deactivateController();
+        void deleteController() throws LogicError;
+        void deactivateAndDeleteController() throws LogicError;
+
+        ["cpp:const"] idempotent WidgetDescription::StringWidgetDictionary getFunctionDescriptions();
+        void callDescribedFunction(string fuinctionName, StringVariantBaseMap values) throws InvalidArgumentException;
+    };
+
+    dictionary<string, NJointControllerInterface*> StringNJointControllerPrxDictionary;
+};
 
-//see RobotUnitInterface.ice (zeroc broke forward declarations)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 84614f0d5d98bb23fc63a14bc4c0a237189b2779..178edf202298befc0236ca0e048cc65456601e44 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -25,6 +25,8 @@
 #include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
 #include <RobotAPI/interface/core/Trajectory.ice>
 
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
 module armarx
 {
     module NJointTaskSpaceDMPControllerMode
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index e6c33056b1d99cf0950cf0fcb5a59a3f893ddc82..f59b430191bb535cf527d0d3a624849102385756 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -41,82 +41,14 @@
 
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.ice>
 #include <RobotAPI/interface/components/RobotHealthInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
 
 #include <ArmarXCore/interface/core/BasicTypes.ice>
 #include <ArmarXCore/interface/core/UserException.ice>
 #include <ArmarXGui/interface/WidgetDescription.ice>
 
 //NJointController
-module armarx
-{
-    interface NJointControllerInterface;
-    interface RobotUnitInterface;
-
-
-    module RobotUnitControllerNames
-    {
-        const string NJointTrajectoryController = "NJointTrajectoryController";
-        const string NJointGlobalTCPController = "NJointGlobalTCPController"; /*@@@TODO: move NJointGlobalTCPController to RobotAPI */
-        const string NJointTCPController = "NJointTCPController";
-        const string NJointCartesianVelocityController = "NJointCartesianVelocityController";
-    };
-
-
-    class NJointControllerConfig {};
-
-    struct NJointControllerDescription
-    {
-        string instanceName;
-        string className;
-        NJointControllerInterface* controller;
-        StringStringDictionary controlModeAssignment;
-        bool deletable;
-        bool internal;
-    };
-    sequence<NJointControllerDescription> NJointControllerDescriptionSeq;
-
-    struct NJointControllerStatus
-    {
-        string instanceName;
-        bool active = false;
-        bool requested = false;
-        bool error = false;
-        long timestampUSec = 0;
-    };
-    sequence<NJointControllerStatus> NJointControllerStatusSeq;
-
-    struct NJointControllerDescriptionWithStatus
-    {
-        NJointControllerStatus status;
-        NJointControllerDescription description;
-    };
-    sequence<NJointControllerDescriptionWithStatus> NJointControllerDescriptionWithStatusSeq;
-
-    interface NJointControllerInterface
-    {
-        ["cpp:const"] idempotent string getClassName();
-        ["cpp:const"] idempotent string getInstanceName();
-        ["cpp:const"] idempotent StringStringDictionary getControlDeviceUsedControlModeMap();
-        ["cpp:const"] idempotent bool isControllerActive();
-        ["cpp:const"] idempotent bool isControllerRequested();
-        ["cpp:const"] idempotent bool isDeletable();
-        ["cpp:const"] idempotent bool hasControllerError();
-        ["cpp:const"] idempotent NJointControllerStatus getControllerStatus();
-        ["cpp:const"] idempotent NJointControllerDescription getControllerDescription();
-        ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus();
-        ["cpp:const"] idempotent RobotUnitInterface* getRobotUnit();
-
-        void activateController();
-        void deactivateController();
-        void deleteController() throws LogicError;
-        void deactivateAndDeleteController() throws LogicError;
-
-        ["cpp:const"] idempotent WidgetDescription::StringWidgetDictionary getFunctionDescriptions();
-        void callDescribedFunction(string fuinctionName, StringVariantBaseMap values) throws InvalidArgumentException;
-    };
 
-    dictionary<string, NJointControllerInterface*> StringNJointControllerPrxDictionary;
-};
 
 //RobotUnit Utility types - ControlDevice
 module armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 20f30486fd3249168f6f80a9353d6dd78cc7b2ea..2b2fb5f9d4ea899930351b1234873b2354cb50bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -30,6 +30,8 @@ set(LIB_FILES
     plugins/ObjectPoseProviderPlugin.cpp
     plugins/ObjectPoseClientPlugin.cpp
     plugins/RequestedObjects.cpp
+
+    util.cpp
 )
 set(LIB_HEADERS
     ArmarXObjects.h
@@ -54,6 +56,8 @@ set(LIB_HEADERS
     plugins/ObjectPoseProviderPlugin.h
     plugins/ObjectPoseClientPlugin.h
     plugins/RequestedObjects.h
+
+    util.h
 )
 
 armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 11e583902b8fbb0963e8f4c1b00746de01356f1b..413d4681c37d51c959573f5a2528a07d66ea4b9b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -7,6 +7,7 @@
 #include <VirtualRobot/RobotConfig.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 #include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
@@ -51,7 +52,7 @@ namespace armarx::objpose
         objpose::fromIce(ice.attachment, this->attachment);
 
         confidence = ice.confidence;
-        timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
+        armarx::core::time::fromIce(ice.timestamp, timestamp);
 
         objpose::fromIce(ice.localOOBB, localOOBB);
     }
@@ -88,7 +89,7 @@ namespace armarx::objpose
         objpose::toIce(ice.attachment, this->attachment);
 
         ice.confidence = confidence;
-        ice.timestampMicroSeconds = timestamp.toMicroSeconds();
+        armarx::core::time::toIce(ice.timestamp, timestamp);
 
         objpose::toIce(ice.localOOBB, localOOBB);
     }
@@ -132,7 +133,7 @@ namespace armarx::objpose
         attachment = std::nullopt;
 
         confidence = provided.confidence;
-        timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+        armarx::core::time::fromIce(provided.timestamp, timestamp);
 
         objpose::fromIce(provided.localOOBB, localOOBB);
     }
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 66c87f22f5f369c154043e83a72146f0d5bbea54..cc8b4c6ebd1f1f112667df6c3eccc1308e5e5416 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -4,7 +4,6 @@
 #include <map>
 #include <vector>
 
-
 #include <Eigen/Core>
 
 #include <SimoxUtility/shapes/OrientedBox.h>
@@ -12,8 +11,7 @@
 #include <VirtualRobot/VirtualRobot.h>
 
 #include <ArmarXCore/core/PackagePath.h>
-
-#include <IceUtil/Time.h>
+#include <ArmarXCore/core/time/DateTime.h>
 
 #include <RobotAPI/interface/objectpose/object_pose_types.h>
 #include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
@@ -87,7 +85,7 @@ namespace armarx::objpose
         /// Confidence in [0, 1] (1 = full, 0 = none).
         float confidence = 0;
         /// Source timestamp.
-        IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
+        DateTime timestamp = DateTime::Invalid();
 
         /// Object bounding box in object's local coordinate frame.
         /// @see oobbRobot(), oobbGlobal()
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index 8710af33c07522713bd833e53c7f1e982754e3ec..13de0bb80cccb9d9e7a63819907d3b7d81688fbd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -1,4 +1,6 @@
 #include "ObjectPoseClient.h"
+#include <optional>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
 
 
 namespace armarx::objpose
@@ -45,7 +47,8 @@ namespace armarx::objpose
     }
 
 
-    ObjectPoseMap ObjectPoseClient::fetchObjectPosesAsMap()
+    ObjectPoseMap
+    ObjectPoseClient::fetchObjectPosesAsMap()
     {
         ObjectPoseMap map;
         for (auto& pose : fetchObjectPoses())
@@ -54,8 +57,24 @@ namespace armarx::objpose
         }
         return map;
     }
+
     
-    ObjectPoseSeq ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName) 
+    std::optional<ObjectPose>
+    ObjectPoseClient::fetchObjectPose(const ObjectID& objectID)
+    {
+        const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
+
+        if(object != nullptr)
+        {
+            return *object;
+        }
+
+        return std::nullopt;
+    }
+    
+
+    ObjectPoseSeq
+    ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
     {
         if (!objectPoseStorage)
         {
@@ -66,7 +85,6 @@ namespace armarx::objpose
     }
 
 
-
     const ObjectPoseStorageInterfacePrx&
     ObjectPoseClient::getObjectPoseStorage() const
     {
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index 33f9a6dd7b40fd933a451d50d093afc937a08635..a5b5fd18d7686e0cc46cd7e0154588ca3b13b40f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -1,6 +1,9 @@
 #pragma once
 
+#include <optional>
+
 #include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
 
@@ -16,28 +19,83 @@ namespace armarx::objpose
     {
     public:
 
+        /// Construct a disconnected client.
         ObjectPoseClient();
-        ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
-                         const ObjectFinder& finder = {});
-
-        void connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
-
-        bool isConnected() const;
-
-
+        /// Construct a client and connect it to the object pose storage.
+        ObjectPoseClient(
+                const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+                const ObjectFinder& finder = {}
+                );
+
+        /**
+         * @brief Connect to the given object pose storage.
+         *
+         * This function can be used after default-constructing the client.
+         *
+         * @param objectPoseStorage The object pose storage.
+         */
+        void
+        connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
+
+        /**
+         * @brief Indicate whether this client is connected to an object pose
+         * storage.
+         *
+         * That is, whether its proxy has been set via the constructor or
+         * `connect()`.
+         *
+         * If false, all `fetch*()` functions will return empty results.
+         *
+         * @return True if connected
+         */
+        bool
+        isConnected() const;
+
+
+        /**
+         * @brief Fetch all known object poses.
+         * @return The known object poses.
+         */
         ObjectPoseSeq
         fetchObjectPoses();
 
+        /**
+         * @brief Fetch all known object poses.
+         * @return The known object poses, with object ID as key.
+         */
         ObjectPoseMap
         fetchObjectPosesAsMap();
 
+        /**
+         * @brief Fetch the pose of a single object.
+         *
+         * This is a network call. If you need multiple object poses, use
+         * `fetchObjectPoses()` instead.
+         *
+         * @param objectID The object's ID.
+         * @return The object's pose, if known.
+         */
+        std::optional<ObjectPose>
+        fetchObjectPose(const ObjectID& objectID);
+
+        /**
+         * @brief Fetch object poses from a specific provider.
+         * @param providerName The provider's name.
+         * @return The object poses from that provider.
+         */
         ObjectPoseSeq
         fetchObjectPosesFromProvider(const std::string& providerName);
 
 
+        /**
+         * @brief Get the object pose storage's proxy.
+         */
         const ObjectPoseStorageInterfacePrx&
         getObjectPoseStorage() const;
 
+        /**
+         * @brief Get the internal object finder.
+         */
         const ObjectFinder&
         getObjectFinder() const;
 
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
index c67af0381b114b1b5e145b60a3c239b464f4ec31..c07cea85884a957218d9a9903ffcdd8c674b37bd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
@@ -1,6 +1,7 @@
 #include "ProvidedObjectPose.h"
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 #include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
@@ -34,7 +35,7 @@ namespace armarx::objpose
         objectJointValues = ice.objectJointValues;
 
         confidence = ice.confidence;
-        timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
+        armarx::core::time::fromIce(ice.timestamp, timestamp);
 
         objpose::fromIce(ice.localOOBB, localOOBB);
     }
@@ -60,7 +61,7 @@ namespace armarx::objpose
         ice.objectJointValues = objectJointValues;
 
         ice.confidence = confidence;
-        ice.timestampMicroSeconds = timestamp.toMicroSeconds();
+        armarx::core::time::toIce(ice.timestamp, timestamp);
 
         objpose::toIce(ice.localOOBB, localOOBB);
     }
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
index c323a879508a60ad1dfd6b4af58dc4c4298ec534..c8d846893dbf857f1ca8bb5dbaabae1ba0b1b14b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
@@ -8,7 +8,7 @@
 
 #include <SimoxUtility/shapes/OrientedBox.h>
 
-#include <IceUtil/Time.h>
+#include <ArmarXCore/core/time/DateTime.h>
 
 #include <RobotAPI/interface/objectpose/object_pose_types.h>
 #include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
@@ -57,7 +57,7 @@ namespace armarx::objpose
         /// Confidence in [0, 1] (1 = full, 0 = none).
         float confidence = 0;
         /// Source timestamp.
-        IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
+        DateTime timestamp = DateTime::Invalid();
 
         /// Object bounding box in object's local coordinate frame.
         /// @see oobbRobot(), oobbGlobal()
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index bb9fb98e66584e8e22889d79c57900df0df1d99f..f21931adabf669f0da8804cf937fd6205c94248b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -104,7 +104,7 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
 
     bo.confidence = dto.confidence;
 
-    bo.timestamp = dto.timestamp;
+    fromAron(dto.timestamp, bo.timestamp);
 
     if (dto.localOOBBValid)
     {
@@ -153,7 +153,7 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
 
     dto.confidence = bo.confidence;
 
-    dto.timestamp = bo.timestamp;
+    toAron(dto.timestamp, bo.timestamp);
 
     if (bo.localOOBB)
     {
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index 6080cfda786c3d6115c6647fe7decd4e5faa6b03..cf42e169e67f0f18aef427c65fd24a73160983bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -43,7 +43,7 @@ void armarx::objpose::to_json(simox::json::json& j, const ObjectPose& op)
     j["robotPose"] = op.robotPose;
 
     j["confidence"] = op.confidence;
-    j["timestampMicroSeconds"] = op.timestamp.toMicroSeconds();
+    j["timestampMicroSeconds"] = op.timestamp.toMicroSecondsSinceEpoch();
 
     if (op.localOOBB)
     {
@@ -68,7 +68,7 @@ void armarx::objpose::from_json(const simox::json::json& j, ObjectPose& op)
     op.robotPose = j.at("robotPose");
 
     op.confidence = j.at("confidence");
-    op.timestamp = IceUtil::Time::microSeconds(j.at("timestampMicroSeconds"));
+    op.timestamp = DateTime(Duration::MicroSeconds(j.at("timestampMicroSeconds")));
 
     if (j.count("localOOBB"))
     {
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.cpp b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dd90c141d81b77fcce1e0b91f8a5963d84a2726
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
@@ -0,0 +1,88 @@
+/**
+ * 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       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "util.h"
+
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+namespace armarx::objpose
+{
+
+    objpose::ObjectPoseSeq
+    filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlocklist)
+    {
+        const auto isBlacklisted = [&datasetBlocklist](const objpose::ObjectPose& objectPose)
+        {
+            const auto dataset = objectPose.objectID.dataset();
+
+            return std::find(datasetBlocklist.begin(), datasetBlocklist.end(), dataset) !=
+                   datasetBlocklist.end();
+        };
+
+        objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
+        return objects;
+    }
+
+
+    VirtualRobot::ManipulationObjectPtr
+    asManipulationObject(const objpose::ObjectPose& objectPose)
+    {
+        ObjectFinder finder;
+
+        VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+        if (auto obstacle = finder.loadManipulationObject(objectPose))
+        {
+            obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+            return obstacle;
+        }
+
+        ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+        return nullptr;
+    }
+
+
+    VirtualRobot::SceneObjectSetPtr
+    asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+    {
+        ObjectFinder finder;
+
+        VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+        for (const auto& objectPose : objectPoses)
+        {
+            if (auto obstacle = finder.loadManipulationObject(objectPose))
+            {
+                obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+                sceneObjects->addSceneObject(obstacle);
+            }
+        }
+
+        return sceneObjects;
+    }
+
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.h b/source/RobotAPI/libraries/ArmarXObjects/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd8ebf0c8397ae1a26b899a8d48c1f9a303377f9
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.h
@@ -0,0 +1,36 @@
+/**
+ * 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       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::objpose
+{
+    objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects,
+                                         const std::vector<std::string>& datasetBlocklist);
+
+    VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
+    VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+  
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 86bdc8fa48e612b256365f1beb1c43bfd1d09ec2..099ad8b4b981aeed1cc9e274c25672a1014ee120 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -23,6 +23,7 @@ add_subdirectory(armem_gui)
 add_subdirectory(armem_motions)
 add_subdirectory(armem_mps)
 add_subdirectory(armem_objects)
+add_subdirectory(armem_reasoning)
 add_subdirectory(armem_robot)
 add_subdirectory(armem_robot_state)
 add_subdirectory(armem_skills)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
index 24d050e646d9bbac8ed3ab5fa30926f6b24f442a..76200b0ab1772d68cd30482e577ae870e711af10 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
@@ -1,13 +1,15 @@
 #include "GraspCandidateWriter.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
 #include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
 #include <RobotAPI/libraries/armem/core/error/mns.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
 
 namespace armarx::armem
 {
 
-
     GraspCandidateWriter::GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem)
         : memoryNameSystem(memoryNameSystem)
     {
@@ -122,7 +124,7 @@ namespace armarx::armem
         std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
         armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
 
-        for (armarx::grasping::BimanualGraspCandidatePtr candidate : candidates)
+        for (const armarx::grasping::BimanualGraspCandidatePtr& candidate : candidates)
         {
             std::string objectName = "UnknownObject";
             if (candidate->sourceInfo)
@@ -161,21 +163,16 @@ namespace armarx::armem
             return false;
         }
 
-        const auto iceTimestamp = Time::microSeconds(timestamp.toMicroSeconds());
-
         const auto providerId = armem::MemoryID(result.segmentID);
         const auto entityID =
-            providerId.withEntityName(entityName).withTimestamp(iceTimestamp);
+            providerId.withEntityName(entityName).withTimestamp(timestamp);
 
         armem::EntityUpdate update;
         update.entityID = entityID;
-
-
-
         update.instancesData = instances;
-        update.timeCreated = iceTimestamp;
+        update.timeCreated = timestamp;
 
-        ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
         ARMARX_DEBUG << updateResult;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 57a714eacc92f1243dcd31d30ea08b1d48648dc0..f5a88c0ae45c0960938f1ef819002a563bc198c1 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -226,6 +226,12 @@ namespace armarx
         return select("ForceTorqueSensor");
     }
 
+    std::string RobotNameHelper::Arm::getForceTorqueSensorFrame() const
+    {
+        ARMARX_TRACE;
+        return select("ForceTorqueSensorFrame");
+    }
+
     std::string RobotNameHelper::Arm::getEndEffector() const
     {
         ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 1c40eac0d8c07c476129e0e852f4e29f57d20144..92330d05d3e18b1447e68a9d36bb4c8b7472ad64 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -73,6 +73,7 @@ namespace armarx
             std::string getTorsoKinematicChain() const;
             std::string getTCP() const;
             std::string getForceTorqueSensor() const;
+            std::string getForceTorqueSensorFrame() const;
             std::string getEndEffector() const;
             std::string getMemoryHandName() const;
             std::string getHandControllerName() const;
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 5feda749b6fc3eb3e1f601812b778e71b20c099b..6b105d8f67ed0ba87bfd5ae17a2fcf54fea79b70 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -9,6 +9,7 @@ set(LIBS
     ArmarXCore
     RemoteGui
     aron
+    aroncommon
 )
 
 set(LIB_FILES
@@ -167,6 +168,7 @@ armarx_add_library(
         "${LIBS}"
     ARON_FILES
         aron/MemoryID.xml
+        aron/MemoryLink.xml
 )
 
 
diff --git a/source/RobotAPI/libraries/armem/aron/MemoryLink.xml b/source/RobotAPI/libraries/armem/aron/MemoryLink.xml
new file mode 100644
index 0000000000000000000000000000000000000000..3f04e393abf0c07747cdebd8288ca655f169e7c5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/aron/MemoryLink.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+    </CodeIncludes>
+    <AronIncludes>
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+    </AronIncludes>
+    <GenerateTypes>
+        <Object name="armarx::armem::arondto::MemoryLink" template="T">
+            <ObjectChild key="memoryID">
+                <armarx::armem::arondto::MemoryID />
+            </ObjectChild>
+            <ObjectChild key="data">
+                <T unique_ptr="true"/>
+            </ObjectChild>
+        </Object>
+    </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem/client/Query.cpp b/source/RobotAPI/libraries/armem/client/Query.cpp
index 52000cf3d25f118ea80cf7dfc7c5ea5830cb36f8..5829c918872623c48a2d2866d84af67b7809d919 100644
--- a/source/RobotAPI/libraries/armem/client/Query.cpp
+++ b/source/RobotAPI/libraries/armem/client/Query.cpp
@@ -1,6 +1,8 @@
 #include "Query.h"
 
 #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
+
 
 namespace armarx::armem::client
 {
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index b7a1c85dd5925353a360058cc5359a312f6e42c0..5bf6dcd0308cfad16c6401bed4c2ae940a972930 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -4,10 +4,14 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <RobotAPI/libraries/armem/aron/MemoryLink.aron.generated.h>
 #include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
 #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 #include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/common/util/object_finders.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h>
 
 #include "query/Builder.h"
 #include "query/query_fns.h"
@@ -81,6 +85,212 @@ namespace armarx::armem::client
     }
 
 
+    QueryResult Reader::query(const QueryInput& input,
+            armem::client::MemoryNameSystem& mns,
+            int recursionDepth) const
+    {
+        return QueryResult::fromIce(query(input.toIce(), mns, recursionDepth));
+    }
+
+
+    armem::query::data::Result Reader::query(const armem::query::data::Input& input,
+            armem::client::MemoryNameSystem& mns,
+            int recursionDepth) const
+    {
+        auto makeErrorMsg = [](auto error)
+        {
+            std::stringstream sstream;
+            sstream << "Memory query failed.\nReason: " << error.what();
+            return sstream.str();
+        };
+
+        armem::query::data::Result result;
+        ARMARX_CHECK_NOT_NULL(memoryPrx);
+        try
+        {
+            result = memoryPrx->query(input);
+            QueryResult bObj = QueryResult::fromIce(result);
+            bObj.memory.forEachInstance(
+                [&bObj, &mns, recursionDepth](armem::wm::EntityInstance& instance)
+                { resolveMemoryLinks(instance, bObj.memory, mns, recursionDepth); });
+            result = bObj.toIce();
+        }
+        catch (const Ice::ConnectionRefusedException& e)
+        {
+            result.errorMessage = makeErrorMsg(e);
+        }
+        catch (const Ice::ConnectionLostException& e)
+        {
+            result.errorMessage = makeErrorMsg(e);
+        }
+        catch (const Ice::NotRegisteredException& e)
+        {
+            result.errorMessage = makeErrorMsg(e);
+        }
+        catch (const exceptions::local::ExpressionException& e)
+        {
+            std::stringstream sstream;
+            sstream << "Encountered malformed MemoryLink: " << e.what();
+            result.errorMessage = sstream.str();
+        }
+
+        return result;
+    }
+
+
+    QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, // NOLINT
+            armem::client::MemoryNameSystem& mns,
+            int recursionDepth) const
+    {
+        return this->query(armem::query::data::MemoryQuerySeq{query}, mns, recursionDepth);
+    }
+
+
+    QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries,
+            armem::client::MemoryNameSystem& mns,
+            int recursionDepth) const
+    {
+        QueryInput input;
+        input.memoryQueries = queries;
+        return this->query(input, mns, recursionDepth);
+    }
+
+
+    QueryResult Reader::query(const QueryBuilder& queryBuilder,
+            armem::client::MemoryNameSystem& mns,
+            int recursionDepth) const
+    {
+        return this->query(queryBuilder.buildQueryInput(), mns, recursionDepth);
+    }
+
+
+    /**
+     * Get the MemoryID and data required to fill in the given MemoryLink.
+     *
+     * Returns nothing if the data could not be retrieved or its type does not
+     * match the MemoryLink's template type.
+     *
+     * @param linkData the data object of the MemoryLink
+     * @param linkType the type object of the MemoryLink
+     * @return a pair of memoryID and data to fill in a MemoryLink, or nothing if not available
+     */
+    std::optional<std::pair<MemoryID, aron::data::VariantPtr>>
+    findDataForLink(const aron::data::VariantPtr& linkData,
+                    const aron::type::VariantPtr& linkType,
+                    armem::client::MemoryNameSystem& mns)
+    {
+        static const aron::Path memoryLinkIDPath{{"memoryID"}};
+        static const aron::Path memoryLinkDataPath{{"data"}};
+        armem::arondto::MemoryID dto;
+        auto memoryIDDict =
+            aron::data::Dict::DynamicCastAndCheck(linkData->navigateAbsolute(memoryLinkIDPath));
+        dto.fromAron(memoryIDDict);
+        auto memoryID = aron::fromAron<armem::MemoryID>(dto);
+        if (!memoryID.isWellDefined() || !memoryID.hasEntityName())
+        {
+            ARMARX_INFO << "Encountered unresolvable MemoryID '" << memoryID.str()
+                        << "', ignoring.";
+            return {};
+        }
+
+        auto nextMemory = resolveID(mns, memoryID);
+        if (!nextMemory)
+        {
+            ARMARX_WARNING << "Could not retrieve data for " << memoryID.str() << ", skipping...";
+            return {};
+        }
+        auto nextDataAndType = extractDataAndType(nextMemory.value(), memoryID);
+        if (!nextDataAndType)
+        {
+            ARMARX_WARNING << "Data or type for " << memoryID.str()
+                           << " not available in memory containing that MemoryID.";
+            return {};
+        }
+        auto [nextData, nextType] = nextDataAndType.value();
+        auto linkTemplate =
+            aron::type::Object::DynamicCastAndCheck(linkType)->getTemplateInstantiations().at(0);
+        std::string nextObjectName = nextType->getObjectNameWithTemplateInstantiations();
+        if (nextType->getObjectName() != linkTemplate)
+        {
+            ARMARX_WARNING << "Linked object " << memoryID.str() << " is of the type "
+                           << nextType->getObjectName() << ", but the link requires the type "
+                           << linkTemplate;
+            return {};
+        }
+        return {{memoryID,
+                 // This is currently the only method to prefix the path to the data correctly.
+                 aron::data::Variant::FromAronDTO(
+                     nextData->toAronDTO(),
+                     linkData->getPath().withElement(memoryLinkDataPath.toString()))}};
+    }
+
+    void
+    Reader::resolveMemoryLinks(armem::wm::EntityInstance& instance,
+                               armem::wm::Memory& input,
+                               armem::client::MemoryNameSystem& mns,
+                               int recursionDepth)
+    {
+        static const aron::Path memoryLinkDataPath{{"data"}};
+        // MemoryID is used for the template instantiation as a placeholder -
+        // you could use any type here.
+        static const std::string linkType =
+            arondto::MemoryLink<arondto::MemoryID>::ToAronType()->getFullName();
+
+        std::set<armem::MemoryID> seenIDs{instance.id()};
+        std::vector<aron::Path> currentPaths{{{""}}};
+        std::vector<aron::Path> nextPaths;
+        int depth = 0;
+
+        auto dataAndType = extractDataAndType(input, instance.id());
+        if (!dataAndType)
+        {
+            ARMARX_INFO << "Instance '" << instance.id().str() << "' does not have a defined type.";
+            return;
+        }
+        auto [instanceData, instanceType] = dataAndType.value();
+        while (!currentPaths.empty() && (recursionDepth == -1 || depth < recursionDepth))
+        {
+            for (const auto& path : currentPaths)
+            {
+                aron::SubObjectFinder finder(linkType);
+                if (path.getFirstElement().empty())
+                {
+                    armarx::aron::data::visitRecursive(finder, instanceData, instanceType);
+                }
+                else
+                {
+                    armarx::aron::data::visitRecursive(finder,
+                                                       instanceData->navigateAbsolute(path),
+                                                       instanceType->navigateAbsolute(path));
+                }
+
+                for (auto& [linkPathStr, linkDataAndType] : finder.getFoundObjects()) // NOLINT
+                {
+                    auto [linkData, linkType] = linkDataAndType;
+                    auto result = findDataForLink(linkData, linkType, mns);
+                    if (result)
+                    {
+                        auto [memoryID, dataToAppend] = result.value();
+                        if (seenIDs.find(memoryID) != seenIDs.end())
+                        {
+                            continue;
+                        }
+                        seenIDs.insert(memoryID);
+
+                        aron::data::Dict::DynamicCastAndCheck(linkData)->setElement(
+                            memoryLinkDataPath.toString(), dataToAppend);
+                        nextPaths.push_back(
+                            linkData->getPath().withElement(memoryLinkDataPath.toString()));
+                    }
+                }
+            }
+            currentPaths = std::move(nextPaths);
+            nextPaths.clear();
+            ++depth;
+        }
+    }
+
+
     QueryResult Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode) const
     {
         using namespace client::query_fns;
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index e9ee1b69afef7d2befdaaf0169b0117a13edbad5..1b957fde328f8c09b1086dc7fa2f0011cecd694b 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -8,6 +8,7 @@
 // RobotAPI
 #include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
 #include <RobotAPI/interface/armem/server/StoringMemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 
 #include "Query.h"
@@ -41,9 +42,60 @@ namespace armarx::armem::client
 
         QueryResult query(const QueryBuilder& queryBuilder) const;
 
+        /** Perform a query with recursive memory link resolution.
+         *
+         * Resolves the links in the query result using the given MemoryNameSystem
+         * and inserts the data into the MemoryLink objects' data fields.
+         * If the inserted data also contains links, those will be resolved as well
+         * up to and including the given recursion depth. Link cycles are detected;
+         * the first repeated memory ID will not be inserted into the data.
+         * Setting the recursion depth to `-1` is equivalent to an infinite
+         * recursion depth.
+         *
+         * Standalone `MemoryID` subobjects are ignored; only MemoryIDs inside of
+         * MemoryLink subobjects are resolved. Empty or unresolvable MemoryIDs
+         * are ignored - this includes MemoryIDs whose Entity fields are not set.
+         * If the data associated with a MemoryID could not be retrieved or
+         * its type does not match the template type of the MemoryLink,
+         * it is ignored.
+         *
+         * @param input the query to perform
+         * @param mns the MemoryNameSystem to use when resolving MemoryLinks
+         * @param recursionDepth how many layers of MemoryLinks to resolve
+         */
+        QueryResult query(const QueryInput& input,
+                          armem::client::MemoryNameSystem& mns,
+                          int recursionDepth = -1) const;
+        /**
+         * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int)
+         */
+        armem::query::data::Result query(const armem::query::data::Input& input,
+                                         armem::client::MemoryNameSystem& mns,
+                                         int recursionDepth = -1) const;
+
+        /**
+         * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int)
+         */
+        QueryResult query(armem::query::data::MemoryQueryPtr query,
+                          armem::client::MemoryNameSystem& mns,
+                          int recursionDepth = -1) const;
+        /**
+         * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int)
+         */
+        QueryResult query(const armem::query::data::MemoryQuerySeq& queries,
+                          armem::client::MemoryNameSystem& mns,
+                          int recursionDepth = -1) const;
+
+        /**
+         * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int)
+         */
+        QueryResult query(const QueryBuilder& queryBuilder,
+                          armem::client::MemoryNameSystem& mns,
+                          int recursionDepth = -1) const;
+
 
         /**
-         * @brief Qeury a specific set of memory IDs.
+         * @brief Query a specific set of memory IDs.
          *
          * Each ID can refer to an entity, a snapshot or an instance. When not
          * referring to an entity instance, the latest snapshot and first
@@ -116,10 +168,16 @@ namespace armarx::armem::client
             return bool(memoryPrx);
         }
 
+    private:
+        static void resolveMemoryLinks(armem::wm::EntityInstance& instance,
+                                       armem::wm::Memory& input,
+                                       armem::client::MemoryNameSystem& mns,
+                                       int recursionDepth);
+
     public:
 
         server::ReadingMemoryInterfacePrx memoryPrx;
 
     };
 
-}
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index 7ffeeca7a71a8ea312565b4e96cf3833300f0fcd..3c4edce62f6cbc0b2d2dc908e673fbe06afe6f53 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -1,6 +1,7 @@
 #include "Writer.h"
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 #include "../error.h"
 
@@ -111,10 +112,10 @@ namespace armarx::armem::client
          * anyway.
          */
 
-        const Time timeSent = armem::Time::now();
+        const Time timeSent = armem::Time::Now();
         for (data::EntityUpdate& update : commit.updates)
         {
-            update.timeSentMicroSeconds = timeSent.toMicroSeconds();
+            toIce(update.timeSent, timeSent);
         }
 
         data::CommitResult result;
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index 0ea7000b229edce38d8a797d3b542d180c2de17a..791a6190c9d344c35113fdb388c8d98f9cdf931b 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,6 +1,9 @@
 #include "selectors.h"
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
 
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
 #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
 
 
@@ -24,7 +27,7 @@ namespace armarx::armem::client::query
     SnapshotSelector& SnapshotSelector::latest()
     {
         auto& q = _addQuery<dq::entity::Single>();
-        q.timestamp = -1;
+        toIce(q.timestamp, Time::Invalid());
         return *this;
     }
 
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index a305de2cbcdd9aa040e6f8b14eb33a905519ec05..bc180c726f4d573fe69a3a77bfba2b0be12fe2c8 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -5,6 +5,7 @@
 #include <ArmarXCore/core/exceptions/LocalException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
 #include <RobotAPI/libraries/armem/core/error.h>
 
 
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 433703a5ac8bc3f05727cffa86be073781c4dcf1..8a1da207528940d0db22dd004faae5b15e3d07f3 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -36,7 +36,7 @@ namespace armarx::armem
          * @brief Time when this entity update was created (e.g. time of image recording).
          * This is the key of the entity's history.
          */
-        Time timeCreated = Time::microSeconds(-1);
+        Time timeCreated = Time::Invalid();
 
 
         // OPTIONAL
@@ -52,14 +52,14 @@ namespace armarx::armem
          *
          * Set automatically when sending the commit.
          */
-        Time timeSent = Time::microSeconds(-1);
+        Time timeSent = Time::Invalid();
 
         /**
          * @brief Time when this update arrived at the memory server.
          *
          * Set by memory server on arrival.
          */
-        Time timeArrived = Time::microSeconds(-1);
+        Time timeArrived = Time::Invalid();
 
 
         friend std::ostream& operator<<(std::ostream& os, const EntityUpdate& rhs);
@@ -74,7 +74,7 @@ namespace armarx::armem
         bool success = false;
 
         MemoryID snapshotID;
-        Time timeArrived = Time::microSeconds(-1);
+        Time timeArrived = Time::Invalid();
 
         std::string errorMessage;
 
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index f3b8c662ddef099b18dc09e03a81f21bd7de2bde..c2a59ff9cb7e653f74153ced3eb73d00ed7572c4 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -13,6 +13,7 @@
 namespace armarx::armem
 {
 
+    // This is constant, not a variable.
     const std::string MemoryID::delimiter = "/";
 
 
@@ -251,7 +252,7 @@ namespace armarx::armem
         {
             return getEntitySnapshotID();
         }
-        if (timestamp != Time::microSeconds(-1))
+        if (timestamp.isValid())
         {
             return getEntityID();
         }
@@ -350,7 +351,7 @@ namespace armarx::armem
 
     std::string MemoryID::timestampStr() const
     {
-        return hasTimestamp() ? std::to_string(timestamp.toMicroSeconds()) : "";
+        return hasTimestamp() ? std::to_string(timestamp.toMicroSecondsSinceEpoch()) : "";
     }
 
     std::string MemoryID::instanceIndexStr() const
@@ -358,9 +359,9 @@ namespace armarx::armem
         return hasInstanceIndex() ? std::to_string(instanceIndex) : "";
     }
 
-    IceUtil::Time MemoryID::timestampFromStr(const std::string& string)
+    Time MemoryID::timestampFromStr(const std::string& string)
     {
-        return Time::microSeconds(parseInteger(string, "timestamp"));
+        return Time(Duration::MicroSeconds(parseInteger(string, "timestamp")));
     }
 
     int MemoryID::instanceIndexFromStr(const std::string& string)
@@ -416,7 +417,7 @@ namespace armarx::armem
     {
         if (string.empty())
         {
-            return -1;
+            return std::numeric_limits<long>::min();
         }
         try
         {
@@ -506,7 +507,7 @@ namespace armarx::armem
             return false;
         }
 
-        if (general.timestamp.toMicroSeconds() < 0)
+        if (general.timestamp.isInvalid())
         {
             return true;
         }
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index d583e752258d69e8271e30be2a2d091493aa18cc..64850ddefa9d2e92cc924e33d765317a8895c067 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <functional>  // for std::hash
 #include <string>
 #include <vector>
 
@@ -52,7 +53,7 @@ namespace armarx::armem
         std::string coreSegmentName = "";
         std::string providerSegmentName = "";
         std::string entityName = "";
-        Time timestamp = Time::microSeconds(-1);
+        Time timestamp = Time::Invalid();
         int instanceIndex = -1;
 
 
@@ -67,7 +68,7 @@ namespace armarx::armem
                  const std::string& coreSegmentName,
                  const std::string& providerSegmentName = "",
                  const std::string& entityName = "",
-                 Time timestamp = Time::microSeconds(-1),
+                 Time timestamp = Time::Invalid(),
                  int instanceIndex = -1);
 
 
@@ -97,27 +98,27 @@ namespace armarx::armem
 
         bool hasMemoryName() const
         {
-            return !memoryName.empty();
+            return not memoryName.empty();
         }
         bool hasCoreSegmentName() const
         {
-            return !coreSegmentName.empty();
+            return not coreSegmentName.empty();
         }
         bool hasProviderSegmentName() const
         {
-            return !providerSegmentName.empty();
+            return not providerSegmentName.empty();
         }
         bool hasEntityName() const
         {
-            return !entityName.empty();
+            return not entityName.empty();
         }
         bool hasTimestamp() const
         {
-            return timestamp.toMicroSeconds() >= 0;
+            return timestamp.isValid();
         }
         void clearTimestamp()
         {
-            timestamp = Time::microSeconds(-1);
+            timestamp = Time::Invalid();
         }
         bool hasInstanceIndex() const
         {
@@ -208,7 +209,7 @@ namespace armarx::armem
         bool operator ==(const MemoryID& other) const;
         inline bool operator !=(const MemoryID& other) const
         {
-            return !(*this == other);
+            return not (*this == other);
         }
 
         bool operator< (const MemoryID& rhs) const;
@@ -218,11 +219,11 @@ namespace armarx::armem
         }
         inline bool operator<=(const MemoryID& rhs) const
         {
-            return !operator> (rhs);
+            return not operator> (rhs);
         }
         inline bool operator>=(const MemoryID& rhs) const
         {
-            return !operator< (rhs);
+            return not operator< (rhs);
         }
 
         friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
diff --git a/source/RobotAPI/libraries/armem/core/Time.cpp b/source/RobotAPI/libraries/armem/core/Time.cpp
index 25bd9a8dc09fcda3c1f86d765e1bbd74f87c6525..80933ac0bd178a17f49c50fe8c89931177d496d5 100644
--- a/source/RobotAPI/libraries/armem/core/Time.cpp
+++ b/source/RobotAPI/libraries/armem/core/Time.cpp
@@ -11,12 +11,12 @@ namespace armarx
     std::string armem::toStringMilliSeconds(const Time& time, int decimals)
     {
         std::stringstream ss;
-        ss << time.toMilliSeconds();
+        ss << time.toMicroSecondsSinceEpoch() / 1000;
         if (decimals > 0)
         {
             int div = int(std::pow(10, 3 - decimals));
             ss << "." << std::setfill('0') << std::setw(decimals)
-               << (time.toMicroSeconds() % 1000) / div;
+               << (time.toMicroSecondsSinceEpoch() % 1000) / div;
         }
         ss << " ms";
         return ss.str();
@@ -27,7 +27,7 @@ namespace armarx
     {
         static const char* mu = "\u03BC";
         std::stringstream ss;
-        ss << time.toMicroSeconds() << " " << mu << "s";
+        ss << time.toMicroSecondsSinceEpoch() << " " << mu << "s";
         return ss.str();
     }
 
@@ -40,7 +40,7 @@ namespace armarx
         {
             int div = int(std::pow(10, 6 - decimals));
             ss << "." << std::setfill('0') << std::setw(decimals)
-               << (time.toMicroSeconds() % int(1e6)) / div;
+               << (time.toMicroSecondsSinceEpoch() % int(1e6)) / div;
         }
 
         return ss.str();
@@ -49,7 +49,7 @@ namespace armarx
 
     armem::Time armem::timeFromStringMicroSeconds(const std::string& microSeconds)
     {
-        return Time::microSeconds(std::stol(microSeconds));
+        return Time(Duration::MicroSeconds(std::stol(microSeconds)), ClockType::Virtual);
     }
 
 }
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index f76f7657dda6e14a9c35bb4ad5edf58471680144..b4e100e91334abeedac42b62efbcfa79ad6b40c6 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -2,13 +2,14 @@
 
 #include <string>
 
-#include <IceUtil/Time.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+
+#include "forward_declarations.h"
 
 
 namespace armarx::armem
 {
-    using Time = IceUtil::Time;
-    using Duration = IceUtil::Time;
 
     /**
      * @brief Returns `time` as e.g. "123456789.012 ms".
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
index 4eafef35a33f746b5c8b898598ac6273cce7030e..62ff85270b07c63907d6a264eee2f73fb0d3c472 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -1,5 +1,6 @@
 #include "aron_conversions.h"
 
+#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
 #include <RobotAPI/libraries/armem/core/MemoryID.h>
 #include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
 
@@ -10,7 +11,7 @@ void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
     bo.coreSegmentName = dto.coreSegmentName;
     bo.providerSegmentName = dto.providerSegmentName;
     bo.entityName = dto.entityName;
-    bo.timestamp = dto.timestamp;
+    fromAron(dto.timestamp, bo.timestamp);
     bo.instanceIndex = dto.instanceIndex;
 }
 
@@ -20,7 +21,7 @@ void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
     dto.coreSegmentName = bo.coreSegmentName;
     dto.providerSegmentName = bo.providerSegmentName;
     dto.entityName = bo.entityName;
-    dto.timestamp = bo.timestamp;
+    toAron(dto.timestamp, bo.timestamp);
     dto.instanceIndex = bo.instanceIndex;
 }
 
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
index 064aafe8d7bd4ea964aa1e020ca692d8eed45bf2..f0b1a26591333440dd41dcd5471a30c061e48f38 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -1,12 +1,16 @@
 #pragma once
 
-#include "forward_declarations.h"
-
 #include <ostream>
 
+#include "forward_declarations.h"
+
 
 namespace armarx::armem
 {
+    // Implemented in <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+    // void fromAron(const IceUtil::Time& dto, Time& bo);
+    // void toAron(IceUtil::Time& dto, const Time& bo);
+
     void fromAron(const arondto::MemoryID& dto, MemoryID& bo);
     void toAron(arondto::MemoryID& dto, const MemoryID& bo);
 }
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 9b34e11bd3d53e896f38cd2c3301c2c35dcdffd9..7395f1447d568aebfa7b1bcbf10325c0ab6728ec 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -311,7 +311,7 @@ namespace armarx::armem::base
          */
         const EntitySnapshotT* findLatestSnapshotBeforeOrAt(const Time& time) const
         {
-            return findLatestSnapshotBefore(time + Time::microSeconds(1));
+            return findLatestSnapshotBefore(time + Duration::MicroSeconds(1));
         }
 
         /**
@@ -339,7 +339,7 @@ namespace armarx::armem::base
          */
         const EntitySnapshotT* findFirstSnapshotAfter(const Time& time) const
         {
-            return findFirstSnapshotAfter(time - Time::microSeconds(1));
+            return findFirstSnapshotAfter(time - Duration::MicroSeconds(1));
         }
 
 
@@ -418,7 +418,7 @@ namespace armarx::armem::base
         template <class FunctionT>
         void forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
         {
-            getSnapshotsBefore(time + Time::microSeconds(1), func);
+            getSnapshotsBefore(time + Duration::MicroSeconds(1), func);
         }
 
 
@@ -432,9 +432,9 @@ namespace armarx::armem::base
         void forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
         {
             // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
-            auto begin = min.toMicroSeconds() > 0 ? this->_container.lower_bound(min) : this->_container.begin();
+            auto begin = min.toMicroSecondsSinceEpoch() > 0 ? this->_container.lower_bound(min) : this->_container.begin();
             // Returns an iterator pointing to the first element that is *greater than* key.
-            auto end = max.toMicroSeconds() > 0 ? this->_container.upper_bound(max) : this->_container.end();
+            auto end = max.toMicroSecondsSinceEpoch() > 0 ? this->_container.upper_bound(max) : this->_container.end();
 
             for (auto it = begin; it != end && it != this->_container.end(); ++it)
             {
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
index 1e19f25a7b08671b1e3f65ea2649f056a107361a..a71881a57851833f91debb68c1f026f2b1fb7859 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
@@ -1,5 +1,7 @@
 #include "ice_conversions.h"
 
+#include <ArmarXCore/core/time/ice_conversions.h>
+
 #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 #include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
 
@@ -50,19 +52,20 @@ namespace armarx::armem::base
 }
 namespace armarx::armem
 {
+
     void base::toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata)
     {
         ice.confidence = metadata.confidence;
-        toIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
-        toIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
-        toIce(ice.timeSentMicroSeconds, metadata.timeSent);
+        toIce(ice.timeArrived, metadata.timeArrived);
+        toIce(ice.timeCreated, metadata.timeCreated);
+        toIce(ice.timeSent, metadata.timeSent);
     }
     void base::fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata)
     {
         metadata.confidence = ice.confidence;
-        fromIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
-        fromIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
-        fromIce(ice.timeSentMicroSeconds, metadata.timeSent);
+        fromIce(ice.timeArrived, metadata.timeArrived);
+        fromIce(ice.timeCreated, metadata.timeCreated);
+        fromIce(ice.timeSent, metadata.timeSent);
     }
 
 
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
index 21316623c9484f46db177d122642170822561c13..b58e820917bd8784f355793c308a8f5fe4d0efdb 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
@@ -15,6 +15,7 @@
 #include <RobotAPI/interface/armem/memory.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 
 namespace armarx::armem::base::detail
@@ -86,7 +87,7 @@ namespace armarx::armem::base
         ice.history.clear();
         entity.forEachSnapshot([&ice](const auto & snapshot)
         {
-            armem::toIce(ice.history[armem::toIce<long>(snapshot.time())], snapshot);
+            armem::toIce(ice.history[armem::toIce<dto::Time>(snapshot.time())], snapshot);
         });
     }
     template <class ...Args>
diff --git a/source/RobotAPI/libraries/armem/core/forward_declarations.h b/source/RobotAPI/libraries/armem/core/forward_declarations.h
index 04cfbfae26533af158ad52c6c39fd49c837f1afb..283988be5244c8b9f49af8ea474faa6713439183 100644
--- a/source/RobotAPI/libraries/armem/core/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem/core/forward_declarations.h
@@ -1,15 +1,17 @@
 #pragma once
 
+#include <ArmarXCore/core/time/forward_declarations.h>
 
-namespace IceUtil
+
+namespace IceInternal
 {
-    class Time;
+    template<typename T> class Handle;
 }
 
 namespace armarx::armem
 {
-    using Time = IceUtil::Time;
-    using Duration = IceUtil::Time;
+    using Time = armarx::core::time::DateTime;
+    using Duration = armarx::core::time::Duration;
 
     class MemoryID;
     class Commit;
@@ -17,6 +19,11 @@ namespace armarx::armem
     class CommitResult;
     class EntityUpdateResult;
 }
+namespace armarx::armem::dto
+{
+    using Time = armarx::core::time::dto::DateTime;
+    using Duration = armarx::core::time::dto::Duration;
+}
 
 namespace armarx::armem::arondto
 {
@@ -62,3 +69,22 @@ namespace armarx::armem::server::wm
     class Memory;
 }
 
+namespace armarx::armem::data
+{
+    struct MemoryID;
+    struct Commit;
+    struct CommitResult;
+    struct EntityUpdate;
+    struct EntityUpdateResult;
+}
+
+namespace armarx::armem::actions
+{
+    class Menu;
+}
+
+namespace armarx::armem::actions::data
+{
+    class Menu;
+    using MenuPtr = ::IceInternal::Handle<Menu>;
+}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index 20df9176bb562ab9025c001451d6fef23793d504..1dd76ae471eab7f3041b7ea835891f53d6df3f3f 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -1,17 +1,20 @@
 #include "ice_conversions.h"
 
+#include <ArmarXCore/core/time/ice_conversions.h>
+
 #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
+#include <RobotAPI/interface/armem/actions.h>
+#include <RobotAPI/interface/armem/commit.h>
+#include <RobotAPI/interface/armem/memory.h>
 
-void IceUtil::toIce(long& ice, const IceUtil::Time& time)
-{
-    ice = time.toMicroSeconds();
-}
+#include "ice_conversions_templates.h"
+
+#include "actions.h"
+#include "Commit.h"
+#include "MemoryID.h"
+#include "Time.h"
 
-void IceUtil::fromIce(const long& ice, IceUtil::Time& time)
-{
-    time = Time::microSeconds(ice);
-}
 
 namespace armarx
 {
@@ -22,7 +25,7 @@ namespace armarx
         ice.coreSegmentName = id.coreSegmentName;
         ice.providerSegmentName = id.providerSegmentName;
         ice.entityName = id.entityName;
-        toIce(ice.timestampMicroSeconds, id.timestamp);
+        toIce(ice.timestamp, id.timestamp);
         ice.instanceIndex = id.instanceIndex;
     }
 
@@ -32,7 +35,7 @@ namespace armarx
         id.coreSegmentName = ice.coreSegmentName;
         id.providerSegmentName = ice.providerSegmentName;
         id.entityName = ice.entityName;
-        fromIce(ice.timestampMicroSeconds, id.timestamp);
+        fromIce(ice.timestamp, id.timestamp);
         id.instanceIndex = ice.instanceIndex;
     }
 
@@ -85,10 +88,10 @@ namespace armarx
         std::transform(ice.instancesData.begin(), ice.instancesData.end(), std::back_inserter(update.instancesData),
                        aron::data::Dict::FromAronDictDTO);
 
-        update.timeCreated = Time::microSeconds(ice.timeCreatedMicroSeconds);
+        fromIce(ice.timeCreated, update.timeCreated);
 
         update.confidence = ice.confidence;
-        update.timeSent = Time::microSeconds(ice.timeSentMicroSeconds);
+        fromIce(ice.timeSent, update.timeSent);
     }
 
     void armem::toIce(data::EntityUpdate& ice, const EntityUpdate& update)
@@ -100,17 +103,17 @@ namespace armarx
         std::transform(update.instancesData.begin(), update.instancesData.end(), std::back_inserter(ice.instancesData),
                        aron::data::Dict::ToAronDictDTO);
 
-        ice.timeCreatedMicroSeconds = update.timeCreated.toMicroSeconds();
+        toIce(ice.timeCreated, update.timeCreated);
 
         ice.confidence = update.confidence;
-        ice.timeSentMicroSeconds = update.timeSent.toMicroSeconds();
+        toIce(ice.timeSent, update.timeSent);
     }
 
     void armem::fromIce(const data::EntityUpdateResult& ice, EntityUpdateResult& result)
     {
         result.success = ice.success;
         fromIce(ice.snapshotID, result.snapshotID);
-        result.timeArrived = Time::microSeconds(ice.timeArrivedMicroSeconds);
+        fromIce(ice.timeArrived, result.timeArrived);
         result.errorMessage = ice.errorMessage;
     }
 
@@ -118,7 +121,7 @@ namespace armarx
     {
         ice.success = result.success;
         toIce(ice.snapshotID, result.snapshotID);
-        ice.timeArrivedMicroSeconds = result.timeArrived.toMicroSeconds();
+        toIce(ice.timeArrived, result.timeArrived);
         ice.errorMessage = result.errorMessage;
     }
 
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.h b/source/RobotAPI/libraries/armem/core/ice_conversions.h
index a8ef35e7938f07f14a81b8ecb731e2470b395c97..2a4daf725a32bb9ca12a5865be8207b87076871a 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.h
@@ -1,23 +1,6 @@
 #pragma once
 
-#include <RobotAPI/interface/armem/actions.h>
-#include <RobotAPI/interface/armem/commit.h>
-#include <RobotAPI/interface/armem/memory.h>
-
-#include "ice_conversions_templates.h"
-
-#include "actions.h"
-#include "Commit.h"
-#include "MemoryID.h"
-#include "Time.h"
-
-
-namespace IceUtil
-{
-    // Define in original namespace to allow ADL.
-    void toIce(long& ice, const Time& time);
-    void fromIce(const long& ice, Time& time);
-}
+#include "forward_declarations.h"
 
 
 namespace armarx::armem
@@ -48,5 +31,6 @@ namespace armarx::armem
 
     void fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu);
     void toIce(actions::data::MenuPtr& ice, const actions::Menu& menu);
+
 }
 
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
index e4da9d89b5e04351e5e528132406a962470364e3..2a6f2a1f6cbff1d7d348d92f9ea7b482b6c25b3b 100644
--- a/source/RobotAPI/libraries/armem/core/json_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
@@ -9,7 +9,7 @@ void armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
     j["coreSegmentName"] = bo.coreSegmentName;
     j["providerSegmentName"] = bo.providerSegmentName;
     j["entityName"] = bo.entityName;
-    j["timestamp_usec"] = bo.timestamp.toMicroSeconds();
+    j["timestamp_usec"] = bo.timestamp.toMicroSecondsSinceEpoch();
     j["timestamp_datetime"] = toDateTimeMilliSeconds(bo.timestamp);
     j["instanceIndex"] = bo.instanceIndex;
 }
@@ -20,7 +20,7 @@ void armarx::armem::from_json(const nlohmann::json& j, MemoryID& bo)
     j.at("coreSegmentName").get_to(bo.coreSegmentName);
     j.at("providerSegmentName").get_to(bo.providerSegmentName);
     j.at("entityName").get_to(bo.entityName);
-    bo.timestamp = Time::microSeconds(j.at("timestamp_usec").get<int64_t>());
+    bo.timestamp = Time(Duration::MicroSeconds(j.at("timestamp_usec").get<int64_t>()));
     // j.at("timestamp_datetime").get_to(toDateTimeMilliSeconds(bo.timestamp));
     j.at("instanceIndex").get_to(bo.instanceIndex);
 }
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
index dacedd5e05177028fea42114a3dd02d4b609ad9a..d24ce36b544670f5e24e72177f64b839be52fc0d 100644
--- a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
@@ -23,13 +23,13 @@ void armarx::armem::from_aron(const aron::data::DictPtr& metadata, const aron::d
     e.data() = data;
 
     auto timeCreated = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
-    m.timeCreated = Time::microSeconds(timeCreated->toAronLongDTO()->value);
+    m.timeCreated = Time(Duration::MicroSeconds(timeCreated->toAronLongDTO()->value));
 
     auto timeSent = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
-    m.timeSent = Time::microSeconds(timeSent->toAronLongDTO()->value);
+    m.timeSent = Time(Duration::MicroSeconds(timeSent->toAronLongDTO()->value));
 
     auto timeArrived = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
-    m.timeArrived = Time::microSeconds(timeArrived->toAronLongDTO()->value);
+    m.timeArrived = Time(Duration::MicroSeconds(timeArrived->toAronLongDTO()->value));
 
     auto confidence = aron::data::Double::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
     m.confidence = static_cast<float>(confidence->toAronDoubleDTO()->value);
@@ -42,20 +42,20 @@ void armarx::armem::to_aron(aron::data::DictPtr& metadata, aron::data::DictPtr&
     metadata = std::make_shared<aron::data::Dict>();
 
     auto timeWrapped = std::make_shared<aron::data::Long>();
-    timeWrapped->setValue(Time::now().toMicroSeconds());
+    timeWrapped->setValue(Time::Now().toMicroSecondsSinceEpoch());
     metadata->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
 
     const wm::EntityInstanceMetadata& m = e.metadata();
     auto timeCreated = std::make_shared<aron::data::Long>();
-    timeCreated->setValue(m.timeCreated.toMicroSeconds());
+    timeCreated->setValue(m.timeCreated.toMicroSecondsSinceEpoch());
     metadata->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
 
     auto timeSent = std::make_shared<aron::data::Long>();
-    timeSent->setValue(m.timeSent.toMicroSeconds());
+    timeSent->setValue(m.timeSent.toMicroSecondsSinceEpoch());
     metadata->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
 
     auto timeArrived = std::make_shared<aron::data::Long>();
-    timeArrived->setValue(m.timeArrived.toMicroSeconds());
+    timeArrived->setValue(m.timeArrived.toMicroSecondsSinceEpoch());
     metadata->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
 
     auto confidence = std::make_shared<aron::data::Double>();
diff --git a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
index ba6c9782e0e2663ee9395fde31956d5fdbfc2ad2..826bf6d9f2f980cd99bbd35a38a2357f585b5f79 100644
--- a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
@@ -1,5 +1,6 @@
 #include "ice_conversions.h"
 
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
 #include <RobotAPI/libraries/armem/core/base/ice_conversions.h>
 
 
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
index b8b2d02c807750aad2cb36c28cd55013b2a0b496..ad3a8e54a3a292f8dfc8dd72ac72b2a7b5564041 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.cpp
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -53,7 +53,7 @@ namespace armarx::armem::mns
         ServerInfo& info = it->second;
         info.name = input.name;
         info.server = input.server;
-        info.timeRegistered = armem::Time::now();
+        info.timeRegistered = armem::Time::Now();
         ARMARX_DEBUG << "Registered memory '" << info.name << "'.";
 
         result.success = true;
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 782e86c28c7355a79ed47ca483ac9deb538782d0..34b02a4da78015ccdb32398a3f1de7f42c0311e3 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -7,11 +7,22 @@ SET(INSTALL_SCRIPT_MSG
     "Please use the installation script in RobotAPI/etc/mongocxx to install libmongocxx and libbsoncxx."
 )
 
-find_package(libmongocxx QUIET)
-armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+
+
+# MongoLTM
+#find_package(libmongocxx QUIET)
+#armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+
+# DiskLTM TODO: switch to FindMinizip file?
+INCLUDE (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+    PKG_CHECK_MODULES(UNZIP minizip)
+endif (PKG_CONFIG_FOUND)
+armarx_build_if(UNZIP_FOUND "MiniZip not available.")
+
+# LTM Encoding stuff
 find_package(libbsoncxx QUIET)
 armarx_build_if(libbsoncxx_FOUND "libbsoncxx not available. ${INSTALL_SCRIPT_MSG}")
-
 armarx_build_if(OpenCV_FOUND "OpenCV not available")
 
 set(LIBS
@@ -21,11 +32,12 @@ set(LIBS
     aron
     RobotAPI::armem
 
-    # Needed for LTM
+    # LTM
     RobotAPI::aron::converter::json
     RobotAPI::aron::converter::opencv
-    ${LIBMONGOCXX_LIBRARIES}
+    #${LIBMONGOCXX_LIBRARIES}
     ${LIBBSONCXX_LIBRARIES}
+    ${UNZIP_LIBRARIES}
 )
 
 set(LIB_FILES
@@ -33,6 +45,7 @@ set(LIB_FILES
     MemoryRemoteGui.cpp
     RemoteGuiAronDataVisitor.cpp
 
+    ltm/base/detail/Processors.cpp
     ltm/base/detail/MemoryItem.cpp
     ltm/base/detail/MemoryBase.cpp
     ltm/base/detail/BufferedMemoryBase.cpp
@@ -44,21 +57,28 @@ set(LIB_FILES
 
     ltm/base/filter/Filter.cpp
     ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
+    ltm/base/filter/equalityFilter/EqualityFilter.cpp
 
     ltm/base/extractor/Extractor.cpp
     ltm/base/extractor/imageExtractor/ImageExtractor.cpp
+    ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp
 
     ltm/base/converter/Converter.cpp
-    ltm/base/converter/dict/Converter.cpp
-    ltm/base/converter/dict/json/JsonConverter.cpp
-    ltm/base/converter/dict/bson/BsonConverter.cpp
+    ltm/base/typeConverter/Converter.cpp
+    ltm/base/typeConverter/json/JsonConverter.cpp
+    ltm/base/converter/object/Converter.cpp
+    ltm/base/converter/object/json/JsonConverter.cpp
+    ltm/base/converter/object/bson/BsonConverter.cpp
     ltm/base/converter/image/Converter.cpp
     ltm/base/converter/image/png/PngConverter.cpp
+    ltm/base/converter/image/exr/ExrConverter.cpp
 
     ltm/base/forgetter/Forgetter.cpp
     ltm/base/forgetter/LRUForgetter/LRUForgetter.cpp
 
-    ltm/disk/detail/Data.cpp
+    ltm/disk/detail/util/util.cpp
+    ltm/disk/detail/util/filesystem_util.cpp
+    ltm/disk/detail/util/minizip_util.cpp
     ltm/disk/detail/DiskStorage.cpp
     ltm/disk/Memory.cpp
     ltm/disk/CoreSegment.cpp
@@ -105,6 +125,7 @@ set(LIB_HEADERS
     MemoryRemoteGui.h
     RemoteGuiAronDataVisitor.h
 
+    ltm/base/detail/Processors.h
     ltm/base/detail/MemoryItem.h
     ltm/base/detail/MemoryBase.h
     ltm/base/detail/BufferedMemoryBase.h
@@ -116,22 +137,29 @@ set(LIB_HEADERS
 
     ltm/base/filter/Filter.h
     ltm/base/filter/frequencyFilter/FrequencyFilter.h
+    ltm/base/filter/equalityFilter/EqualityFilter.h
 
     ltm/base/extractor/Extractor.h
     ltm/base/extractor/imageExtractor/ImageExtractor.h
+    ltm/base/extractor/imageExtractor/DepthImageExtractor.h
 
     ltm/base/converter/Converter.h
-    ltm/base/converter/dict/Converter.h
-    ltm/base/converter/dict/json/JsonConverter.h
-    ltm/base/converter/dict/bson/BsonConverter.h
+    ltm/base/typeConverter/Converter.h
+    ltm/base/typeConverter/json/JsonConverter.h
+    ltm/base/converter/object/Converter.h
+    ltm/base/converter/object/json/JsonConverter.h
+    ltm/base/converter/object/bson/BsonConverter.h
     ltm/base/converter/image/Converter.h
     ltm/base/converter/image/png/PngConverter.h
+    ltm/base/converter/image/exr/ExrConverter.h
 
 
     ltm/base/forgetter/Forgetter.h
     ltm/base/forgetter/LRUForgetter/LRUForgetter.h
 
-    ltm/disk/detail/Data.h
+    ltm/disk/detail/util/util.h
+    ltm/disk/detail/util/filesystem_util.h
+    ltm/disk/detail/util/minizip_util.h
     ltm/disk/detail/DiskStorage.h
     ltm/disk/Memory.h
     ltm/disk/CoreSegment.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 1060a616ff18d9b0a6b7e889aa624f36c1a36f10..a838e9e0f173de79b0a89a9d14bf103757d5b531 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -10,8 +10,9 @@
 
 #include <RobotAPI/libraries/aron/core/Exception.h>
 
-#include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 
 namespace armarx::armem::server
@@ -147,7 +148,7 @@ namespace armarx::armem::server
     MemoryToIceAdapter::commit(const data::Commit& commitIce)
     {
         ARMARX_TRACE;
-        return commit(commitIce, armem::Time::now());
+        return commit(commitIce, armem::Time::Now());
     }
 
 
@@ -221,6 +222,7 @@ namespace armarx::armem::server
 
                         e->addSnapshot(snapshot);
                     }
+                    // store memory
                     longtermMemory->store(m);
                 }
 
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
index dfe5299f7bf37780a034b83f807496c3636728de..d77f34eb719b46c117c64a2e9ec09cd43780dad9 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
@@ -3,6 +3,8 @@
 #include <sstream>
 #include <stack>
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
 
 #include <RobotAPI/libraries/aron/core/data/variant/All.h>
@@ -106,6 +108,12 @@ namespace armarx::armem::server
             this->addValueLabel(key, *aron::data::NDArray::DynamicCastAndCheck(array), "ND Array");
         }
 
+        void visitUnknown(const aron::data::VariantPtr& unknown) override
+        {
+            ARMARX_IMPORTANT << "Unknown data encountered: "
+                             << (unknown ? unknown->getFullName() : "nullptr");
+        }
+
 
     private:
         template <class DataT>
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
index 1e01564f3289a39018862aa24b379e4d2e16ecba..c42e9c0d57d64f4776f7c1f586ea3eba3a4af75a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
@@ -17,16 +17,22 @@ namespace armarx::armem::server::ltm
             Binary
         };
 
-        Converter(const ConverterType t, const std::string& s):
+        Converter(const ConverterType t, const std::string& id, const std::string& s, const aron::type::Descriptor c):
             type(t),
-            suffix(s)
+            identifier(id),
+            suffix(s),
+            convertsType(c)
         {}
         virtual ~Converter() = default;
 
-        virtual std::vector<unsigned char> convert(const aron::data::VariantPtr& data) = 0;
-        virtual aron::data::VariantPtr convert(const std::vector<unsigned char>& data) = 0;
+        virtual std::pair<std::vector<unsigned char>, std::string> convert(const aron::data::VariantPtr& data) = 0;
+        virtual aron::data::VariantPtr convert(const std::vector<unsigned char>& data, const std::string&) = 0;
 
+    public:
         const ConverterType type;
+        const std::string identifier;
         const std::string suffix;
+        const aron::type::Descriptor convertsType;
+        bool enabled = false;
     };
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
deleted file mode 100644
index ea89e7dbc8e04fb69cea10558d0777237730872d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "Converter.h"
-
-namespace armarx::armem::server::ltm
-{
-
-    std::vector<unsigned char> DictConverter::convert(const aron::data::VariantPtr& data)
-    {
-        auto d = aron::data::Dict::DynamicCastAndCheck(data);
-        return _convert(d);
-    }
-
-    aron::data::VariantPtr DictConverter::convert(const std::vector<unsigned char>& data)
-    {
-        auto d = _convert(data);
-        return d;
-    }
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
deleted file mode 100644
index d2965ce0e61f74ff77458e6f770c20b928976f10..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#pragma once
-
-// STD/STL
-#include <memory>
-
-// BaseClass
-#include "../Converter.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-namespace armarx::armem::server::ltm
-{
-    class DictConverter : public Converter
-    {
-    public:
-        using Converter::Converter;
-
-        virtual ~DictConverter() = default;
-
-        std::vector<unsigned char> convert(const aron::data::VariantPtr& data) final;
-        aron::data::VariantPtr convert(const std::vector<unsigned char>& data) final;
-
-    protected:
-        virtual std::vector<unsigned char> _convert(const aron::data::DictPtr& data) = 0;
-        virtual aron::data::DictPtr _convert(const std::vector<unsigned char>& data) = 0;
-    };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
deleted file mode 100644
index 8ef4cbc279c0c4034a5d8d874f42bae445d9a8b6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#pragma once
-
-// Base Class
-#include "../Converter.h"
-
-// Simox
-#include <SimoxUtility/json.h>
-
-namespace armarx::armem::server::ltm::converter::dict
-{
-    class JsonConverter : public DictConverter
-    {
-    public:
-        JsonConverter() :
-            DictConverter(ConverterType::Str, ".json")
-        {}
-
-    protected:
-        std::vector<unsigned char> _convert(const aron::data::DictPtr& data) final;
-        aron::data::DictPtr _convert(const std::vector<unsigned char>& data) final;
-    };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
index a36db667d18750b64847ceed420a2b7d7e5480b3..39b4d94b015841467ccadf14e34bf6ab1bf00ef0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
@@ -3,15 +3,15 @@
 namespace armarx::armem::server::ltm
 {
 
-    std::vector<unsigned char> ImageConverter::convert(const aron::data::VariantPtr& data)
+    std::pair<std::vector<unsigned char>, std::string> ImageConverter::convert(const aron::data::VariantPtr& data)
     {
         auto d = aron::data::NDArray::DynamicCastAndCheck(data);
         return _convert(d);
     }
 
-    aron::data::VariantPtr ImageConverter::convert(const std::vector<unsigned char>& data)
+    aron::data::VariantPtr ImageConverter::convert(const std::vector<unsigned char>& data, const std::string& m)
     {
-        auto d = _convert(data);
+        auto d = _convert(data, m);
         return d;
     }
 
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
index 0dd01ad8466143e040f013c8380e2600ed3e1c5e..a2b2df04565bbe7940cbcc37fa5e49166d554df6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
@@ -14,15 +14,17 @@ namespace armarx::armem::server::ltm
     class ImageConverter : public Converter
     {
     public:
-        using Converter::Converter;
+        ImageConverter(const ConverterType t, const std::string& id, const std::string& s):
+            Converter(t, id, s, aron::type::Descriptor::eImage)
+        {}
 
         virtual ~ImageConverter() = default;
 
-        std::vector<unsigned char> convert(const aron::data::VariantPtr& data) final;
-        aron::data::VariantPtr convert(const std::vector<unsigned char>& data) final;
+        std::pair<std::vector<unsigned char>, std::string> convert(const aron::data::VariantPtr& data) final;
+        aron::data::VariantPtr convert(const std::vector<unsigned char>& data, const std::string&) final;
 
     protected:
-        virtual std::vector<unsigned char> _convert(const aron::data::NDArrayPtr& data) = 0;
-        virtual aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data) = 0;
+        virtual std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::NDArrayPtr& data) = 0;
+        virtual aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data, const std::string&) = 0;
     };
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5eee777b60ac5b7e1c97606e5902107afdb0221
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.cpp
@@ -0,0 +1,33 @@
+#include "ExrConverter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+
+namespace armarx::armem::server::ltm::converter::image
+{
+    std::pair<std::vector<unsigned char>, std::string> ExrConverter::_convert(const aron::data::NDArrayPtr& data)
+    {
+        ARMARX_CHECK_NOT_NULL(data);
+
+        auto img = aron::converter::AronOpenCVConverter::ConvertToMat(data);
+        std::vector<unsigned char> buffer;
+
+        auto shape = data->getShape(); // we know from the extraction that the shape has 3 elements
+        ARMARX_CHECK_EQUAL(shape.size(), 3);
+        ARMARX_CHECK_EQUAL(shape[2], 4);
+
+        cv::imencode(".exr", img, buffer);
+        return std::make_pair(buffer, "");
+    }
+
+    aron::data::NDArrayPtr ExrConverter::_convert(const std::vector<unsigned char>& data, const std::string& m)
+    {
+        cv::Mat img = cv::imdecode(data, cv::IMREAD_ANYDEPTH);
+        return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
+    }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad4b1bfa2f4b517a55136ace72942d4440404c9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.h
@@ -0,0 +1,21 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+namespace armarx::armem::server::ltm::converter::image
+{
+    class ExrConverter : public ImageConverter
+    {
+    public:
+        ExrConverter() :
+            ImageConverter(ConverterType::Binary, "depthimage", ".exr")
+        {
+            enabled = true; // enabled by default
+        }
+
+    protected:
+        std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::NDArrayPtr& data) final;
+        aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
index f2ab4d8db1f0baaf2a2c6524e97e9f17054b88cc..e8536cf64841d23712e7264ce76282039b893189 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
@@ -10,19 +10,52 @@
 
 namespace armarx::armem::server::ltm::converter::image
 {
-    std::vector<unsigned char> PngConverter::_convert(const aron::data::NDArrayPtr& data)
+    std::pair<std::vector<unsigned char>, std::string> PngConverter::_convert(const aron::data::NDArrayPtr& data)
     {
         ARMARX_CHECK_NOT_NULL(data);
 
         auto img = aron::converter::AronOpenCVConverter::ConvertToMat(data);
         std::vector<unsigned char> buffer;
-        cv::imencode(".png", img, buffer);
-        return buffer;
+
+
+        auto shape = data->getShape(); // we know from the extraction that the shape has 3 elements
+        ARMARX_CHECK_EQUAL(shape.size(), 3);
+
+        if (shape[2] == 3) // its probably a rgb image
+        {
+            cv::cvtColor(img, img, CV_RGB2BGR);
+            cv::imencode(suffix, img, buffer);
+            return std::make_pair(buffer, ".rgb");
+        }
+
+        if (shape[2] == 1) // its probably a grayscale image
+        {
+            cv::imencode(suffix, img, buffer);
+            return std::make_pair(buffer, ".gs");
+        }
+
+        // try to export without conversion
+        cv::imencode(suffix, img, buffer);
+        return std::make_pair(buffer, "");
     }
 
-    aron::data::NDArrayPtr PngConverter::_convert(const std::vector<unsigned char>& data)
+    aron::data::NDArrayPtr PngConverter::_convert(const std::vector<unsigned char>& data, const std::string& m)
     {
-        cv::Mat img = cv::imdecode(data, cv::IMREAD_COLOR);
+        if (m == ".rgb")
+        {
+            cv::Mat img = cv::imdecode(data, cv::IMREAD_COLOR);
+            cv::cvtColor(img, img, CV_BGR2RGB);
+            return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
+        }
+
+        if (m == ".gs")
+        {
+            cv::Mat img = cv::imdecode(data, cv::IMREAD_GRAYSCALE);
+            return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
+        }
+
+        // try to load without conversion
+        cv::Mat img = cv::imdecode(data, cv::IMREAD_ANYCOLOR);
         return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
     }
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
index fb1321bc6ba827b7533365dd5665f4e13b193c5a..5b6c80134319a52b77565dba322349985fed2267 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
@@ -9,11 +9,13 @@ namespace armarx::armem::server::ltm::converter::image
     {
     public:
         PngConverter() :
-            ImageConverter(ConverterType::Str, ".png")
-        {}
+            ImageConverter(ConverterType::Binary, "image", ".png")
+        {
+            enabled = true; // enabled by default
+        }
 
     protected:
-        std::vector<unsigned char> _convert(const aron::data::NDArrayPtr& data) final;
-        aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data) final;
+        std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::NDArrayPtr& data) final;
+        aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
     };
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f0808a6f03f1739e76cf6fc02a9a155be334be8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
@@ -0,0 +1,18 @@
+#include "Converter.h"
+
+namespace armarx::armem::server::ltm
+{
+
+    std::pair<std::vector<unsigned char>, std::string> ObjectConverter::convert(const aron::data::VariantPtr& data)
+    {
+        auto d = aron::data::Dict::DynamicCastAndCheck(data);
+        return _convert(d);
+    }
+
+    aron::data::VariantPtr ObjectConverter::convert(const std::vector<unsigned char>& data, const std::string& m)
+    {
+        auto d = _convert(data, m);
+        return d;
+    }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..e5c8e9f80a04d7bba1b2bca9e79986d66fb2d84a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
@@ -0,0 +1,30 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// BaseClass
+#include "../Converter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm
+{
+    class ObjectConverter : public Converter
+    {
+    public:
+        ObjectConverter(const ConverterType t, const std::string& id, const std::string& s):
+            Converter(t, id, s, aron::type::Descriptor::eObject)
+        {}
+
+        virtual ~ObjectConverter() = default;
+
+        std::pair<std::vector<unsigned char>, std::string> convert(const aron::data::VariantPtr& data) final;
+        aron::data::VariantPtr convert(const std::vector<unsigned char>& data, const std::string&) final;
+
+    protected:
+        virtual std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::DictPtr& data) = 0;
+        virtual aron::data::DictPtr _convert(const std::vector<unsigned char>& data, const std::string&) = 0;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
similarity index 72%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
index f183f002f53d586e0529e2e5b56372737c88cba3..8465201f3bd078b7bd93bec2cbd26378dfd119ab 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
@@ -5,14 +5,14 @@
 #include <bsoncxx/builder/stream/document.hpp>
 #include <bsoncxx/builder/stream/array.hpp>
 
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
 {
     namespace bsoncxxbuilder = bsoncxx::builder::stream;
     namespace bsoncxxdoc = bsoncxx::document;
 
-    std::vector<unsigned char> BsonConverter::_convert(const aron::data::DictPtr& data)
+    std::pair<std::vector<unsigned char>, std::string> BsonConverter::_convert(const aron::data::DictPtr& data)
     {
-        std::vector<unsigned char> jsonVec = jsonConverter.convert(data);
+        auto [jsonVec, str] = jsonConverter.convert(data);
         std::string json(jsonVec.begin(), jsonVec.end());
         auto view = bsoncxx::from_json(json).view();
 
@@ -21,16 +21,16 @@ namespace armarx::armem::server::ltm::converter::dict
         {
             std::memcpy(bson.data(), view.data(), view.length());
         }
-        return bson;
+        return std::make_pair(bson, str);
     }
 
-    aron::data::DictPtr BsonConverter::_convert(const std::vector<unsigned char>& data)
+    aron::data::DictPtr BsonConverter::_convert(const std::vector<unsigned char>& data, const std::string& m)
     {
         bsoncxx::document::view view(data.data(), data.size());
         nlohmann::json json = bsoncxx::to_json(view);
         std::string str = json.dump(2);
         std::vector<unsigned char> jsonVec(str.begin(), str.end());
-        auto v = jsonConverter.convert(jsonVec);
+        auto v = jsonConverter.convert(jsonVec, m);
         return aron::data::Dict::DynamicCast(v);
     }
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
similarity index 54%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
index e0021111043acc2141575574b084d6169524c551..6eb183be2c5998a3eb43251ff7018a6e336260ea 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
@@ -6,21 +6,21 @@
 // ArmarX
 #include "../json/JsonConverter.h"
 
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
 {
     class BsonConverter;
     using BsonConverterPtr = std::shared_ptr<BsonConverter>;
 
-    class BsonConverter : public DictConverter
+    class BsonConverter : public ObjectConverter
     {
     public:
         BsonConverter() :
-            DictConverter(ConverterType::Binary, ".bson")
+            ObjectConverter(ConverterType::Binary, "dict", ".bson")
         {}
 
     protected:
-        std::vector<unsigned char> _convert(const aron::data::DictPtr& data) final;
-        aron::data::DictPtr _convert(const std::vector<unsigned char>& data) final;
+        std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::DictPtr& data) final;
+        aron::data::DictPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
 
     private:
         JsonConverter jsonConverter;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
index 1847b87529851b0c41cc0a94f957f55c38306189..f2baa4865ff5d2a832e61acc857863d2c6ca545a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
@@ -2,16 +2,16 @@
 
 #include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
 
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
 {
-    std::vector<unsigned char> JsonConverter::_convert(const aron::data::DictPtr& data)
+    std::pair<std::vector<unsigned char>, std::string> JsonConverter::_convert(const aron::data::DictPtr& data)
     {
         nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(data);
         auto str = j.dump(2);
-        return std::vector<unsigned char>(str.begin(), str.end());
+        return std::make_pair(std::vector<unsigned char>(str.begin(), str.end()), "");
     }
 
-    aron::data::DictPtr JsonConverter::_convert(const std::vector<unsigned char>& data)
+    aron::data::DictPtr JsonConverter::_convert(const std::vector<unsigned char>& data, const std::string&)
     {
         std::string str(data.begin(), data.end());
         nlohmann::json j = nlohmann::json::parse(str);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b9f86ee93cda46983201be6e7910e4596734b3e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
@@ -0,0 +1,24 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+namespace armarx::armem::server::ltm::converter::object
+{
+    class JsonConverter : public ObjectConverter
+    {
+    public:
+        JsonConverter() :
+            ObjectConverter(ConverterType::Str, "dict", ".json")
+        {
+            enabled = true; // always true!
+        }
+
+    protected:
+        std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::DictPtr& data) final;
+        aron::data::DictPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
index 541e2107b001dd0f2872c95560a7d60642945798..4dade14f3b917b81eaf84721c998904f6b3b8be3 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
@@ -26,6 +26,16 @@ namespace armarx::armem::server::ltm
 
         virtual ~BufferedMemoryBase() = default;
 
+        void setMemoryID(const MemoryID& id) override
+        {
+            ARMARX_CHECK_NOT_EMPTY(id.memoryName);
+
+            Base::setMemoryID(id.getMemoryID());
+
+            buffer->id() = id.getMemoryID();
+            to_store->id() = id.getMemoryID();
+        }
+
         armem::wm::Memory getBuffer() const
         {
             std::lock_guard l(bufferMutex);
@@ -35,16 +45,16 @@ namespace armarx::armem::server::ltm
         void directlyStore(const armem::wm::Memory& memory)
         {
             TIMING_START(LTM_Memory_DirectlyStore);
-            for (auto& f : this->filters->memFilters)
+            for (auto& f : this->processors->memFilters)
             {
                 if (!f->accept(memory))
                 {
-                    ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+                    ARMARX_WARNING << deactivateSpam() << "Ignoring to commit a Memory into the LTM because the full commit got filtered.";
                     return;
                 }
             }
             _directlyStore(memory);
-            TIMING_END(LTM_Memory_DirectlyStore);
+            TIMING_END_STREAM(LTM_Memory_DirectlyStore, ARMARX_DEBUG);
         }
 
         void storeBuffer()
@@ -69,7 +79,7 @@ namespace armarx::armem::server::ltm
         {
             Base::createPropertyDefinitions(defs, prefix);
 
-            defs->optional(storeFrequency, prefix + ".buffer.storeFreq", "Frequency to store the buffer to the LTM in Hz.");
+            defs->optional(storeFrequency, prefix + ".buffer.storeFreq", "Frequency to store the buffer to the LTM in Hz.").setMin(1).setMax(1000);
         }
 
     protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
index 73f7b742eb414168ae1f2bbefa229da757b6069d..d34d693af901d09d6cc7f8b9c045522e564501ec 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
@@ -41,6 +41,12 @@ namespace armarx::armem::server::ltm
             _store(coreSeg);
         }
 
+        /// store the type of the core segment
+        void storeType(const armem::wm::CoreSegment& coreSeg)
+        {
+            _storeType(coreSeg);
+        }
+
         /// iterate over all provider segments of this ltm
         virtual bool forEachProviderSegment(std::function<void(ProviderSegmentT&)>&& func) const = 0;
 
@@ -63,6 +69,7 @@ namespace armarx::armem::server::ltm
         virtual void _loadAllReferences(armem::wm::CoreSegment&) = 0;
         virtual void _resolve(armem::wm::CoreSegment&) = 0;
         virtual void _store(const armem::wm::CoreSegment&) = 0;
+        virtual void _storeType(const armem::wm::CoreSegment&) = 0;
 
     protected:
         mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
index 3879f60921f9629300cc99d4ec056e65cd759533..d7ec68da708f72cca8ad0655571c4a73d7f48d0a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
@@ -9,7 +9,6 @@
 #include "CoreSegmentBase.h"
 
 // ArmarX
-#include <ArmarXCore/core/application/properties/Properties.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/logging/LoggingUtil.h>
 
@@ -19,8 +18,6 @@
 #include <RobotAPI/libraries/armem/core/operations.h>
 #include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
-// The default filter
-#include "../converter/dict/Converter.h"
 
 namespace armarx::armem::server::ltm
 {
@@ -53,7 +50,7 @@ namespace armarx::armem::server::ltm
         {
             TIMING_START(LTM_Memory_LoadAll);
             _loadAllReferences(memory);
-            TIMING_END(LTM_Memory_LoadAll);
+            TIMING_END_STREAM(LTM_Memory_LoadAll, ARMARX_DEBUG);
         }
 
         /// return the full ltm as a wm::Memory and resolves the references
@@ -70,7 +67,7 @@ namespace armarx::armem::server::ltm
             TIMING_START(LTM_Memory_LoadAllAndResolve);
             _loadAllReferences(memory);
             _resolve(memory);
-            TIMING_END(LTM_Memory_LoadAllAndResolve);
+            TIMING_END_STREAM(LTM_Memory_LoadAllAndResolve, ARMARX_DEBUG);
         }
 
         /// convert the references of the input into a wm::Memory
@@ -78,23 +75,23 @@ namespace armarx::armem::server::ltm
         {
             TIMING_START(LTM_Memory_Load);
             _resolve(memory);
-            TIMING_END(LTM_Memory_Load);
+            TIMING_END_STREAM(LTM_Memory_Load, ARMARX_DEBUG);
         }
 
         /// append a wm::Memory instance to the ltm
         void store(const armem::wm::Memory& memory)
         {
             TIMING_START(LTM_Memory_Append);
-            for (auto& f : filters->memFilters)
+            for (auto& f : processors->memFilters)
             {
-                if (!f->accept(memory))
+                if (f->enabled && !f->accept(memory))
                 {
-                    ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+                    ARMARX_INFO << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
                     return;
                 }
             }
             _store(memory);
-            TIMING_END(LTM_Memory_Append);
+            TIMING_END_STREAM(LTM_Memory_Append, ARMARX_DEBUG);
         }
 
         /// append a wm::Memory instance to the ltm
@@ -105,27 +102,6 @@ namespace armarx::armem::server::ltm
             this->store(memory);
         }
 
-        void init()
-        {
-            // setup the filters
-            if (mFreqFilterEnabled)
-            {
-                auto mFreqFilter = std::make_unique<filter::MemoryFrequencyFilter>();
-                mFreqFilter->waitingTimeInMs = mFreqFilterWaitingTime;
-                filters->memFilters.push_back(std::move(mFreqFilter));
-            }
-
-            if (snapFreqFilterEnabled)
-            {
-                auto snapFreqFilter = std::make_unique<filter::SnapshotFrequencyFilter>();
-                snapFreqFilter->waitingTimeInMs = snapFreqFilterEnabled;
-                filters->snapFilters.push_back(std::move(snapFreqFilter));
-            }
-
-            // setup converters
-            filters->converters[aron::type::Descriptor::eObject] = std::make_unique<converter::dict::JsonConverter>();
-        }
-
         /// iterate over all core segments of this ltm
         virtual bool forEachCoreSegment(std::function<void(CoreSegmentT&)>&& func) const = 0;
 
@@ -135,10 +111,7 @@ namespace armarx::armem::server::ltm
         /// parameters
         virtual void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
         {
-            defs->optional(mFreqFilterEnabled, prefix + "memFreqFilter.Enabled");
-            defs->optional(mFreqFilterWaitingTime, prefix + "memFreqFilter.WaitingTime", "Waiting time in MS after each LTM update.");
-            defs->optional(snapFreqFilterEnabled, prefix + "memFreqFilter.Enabled");
-            defs->optional(mFreqFilterWaitingTime, prefix + "memSnapFilter.WaitingTime", "Waiting time in MS after each Entity update.");
+            processors->createPropertyDefinitions(defs, prefix);
         }
 
         /// get level name
@@ -158,10 +131,5 @@ namespace armarx::armem::server::ltm
     protected:
         mutable std::recursive_mutex ltm_mutex;
 
-    private:
-        bool mFreqFilterEnabled = false;
-        long mFreqFilterWaitingTime = -1;
-        bool snapFreqFilterEnabled = false;
-        long snapFreqFilterWaitingTime = -1;
     };
 } // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
index bc60bb5d2add9dd67a82b95b18e1b65d042dcbd6..6f38d6ea21317b44d37045c985f15d5efe2103da 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
@@ -8,15 +8,15 @@
 namespace armarx::armem::server::ltm
 {
     MemoryItem::MemoryItem(const MemoryID& id) :
-        MemoryItem(id, std::make_shared<FilterCollection>())
+        processors(std::make_shared<Processors>()),
+        _id(id)
     {
     }
 
-    MemoryItem::MemoryItem(const MemoryID& id, const std::shared_ptr<FilterCollection>& p) :
-        filters(p),
+    MemoryItem::MemoryItem(const MemoryID& id, const std::shared_ptr<Processors>& p) :
+        processors(p),
         _id(id)
     {
-        ARMARX_CHECK_NOT_NULL(p); // There must be a filter stack.
     }
 
     void MemoryItem::setMemoryID(const MemoryID& id)
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
index d8d9a5cedfda00cacbdeff076b40d9c7a3940342..570231509806a4f6a56d55ab4a4a2d7442de8807 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -5,37 +5,16 @@
 #include <optional>
 #include <string>
 
-#include "../filter/frequencyFilter/FrequencyFilter.h"
-#include "../extractor/imageExtractor/ImageExtractor.h"
-#include "../converter/dict/json/JsonConverter.h"
-#include "../converter/image/png/PngConverter.h"
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include "Processors.h"
 
 namespace armarx::armem::server::ltm
 {
-    /// all necessary classes to filter and convert an entry of the ltm to some other format(s)
-    struct FilterCollection
-    {
-        // Memory Filters
-        std::vector<std::unique_ptr<MemoryFilter>> memFilters;
-
-        // Snapshot filters
-        std::vector<std::unique_ptr<SnapshotFilter>> snapFilters;
-
-        // Extractors
-        std::map<aron::data::Descriptor, std::unique_ptr<Extractor>> extractors;
-
-        // Converters
-        std::map<aron::type::Descriptor, std::unique_ptr<Converter>> converters;
-    };
-
     /// @brief Interface functions for the longterm memory classes
     class MemoryItem
     {
     public:
-        MemoryItem(const MemoryID&);
-        MemoryItem(const MemoryID&, const std::shared_ptr<FilterCollection>&);
+        MemoryItem(const MemoryID&); // only used by memory
+        MemoryItem(const MemoryID&, const std::shared_ptr<Processors>&); // used by all other segments
         virtual ~MemoryItem() = default;
 
         MemoryID id() const;
@@ -44,7 +23,7 @@ namespace armarx::armem::server::ltm
         virtual void setMemoryID(const MemoryID&);
 
     protected:
-        std::shared_ptr<FilterCollection> filters;
+        std::shared_ptr<Processors> processors;
 
     private:
         MemoryID _id;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e176bf7914c9d5dc59082f227206c313715c873
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.cpp
@@ -0,0 +1,35 @@
+#include "Processors.h"
+
+namespace armarx::armem::server::ltm
+{
+    Processors::Processors()
+    {
+        // setup containers
+        memFilters.push_back(&memFreqFilter);
+        snapFilters.push_back(&snapFreqFilter);
+        snapFilters.push_back(&snapEqFilter);
+        extractors.push_back(&imageExtractor);
+        extractors.push_back(&depthImageExtractor);
+        converters.insert({pngConverter.identifier, &pngConverter});
+        converters.insert({exrConverter.identifier, &exrConverter});
+    }
+
+    void Processors::createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
+    {
+        // filters
+        defs->optional(memFreqFilter.enabled, prefix + "memFreqFilter.Enabled");
+        defs->optional(memFreqFilter.waitingTimeInMs, prefix + "memFreqFilter.WaitingTime", "Waiting time in MS after each LTM update.");
+        defs->optional(snapFreqFilter.enabled, prefix + "snapFreqFilter.Enabled");
+        defs->optional(snapFreqFilter.waitingTimeInMs, prefix + "snapFreqFilter.WaitingTime", "Waiting time in MS after each Entity update.");
+        defs->optional(snapEqFilter.enabled, prefix + "snapEqFilter.Enabled");
+        defs->optional(snapEqFilter.maxWaitingTimeInMs, prefix + "snapEqFilter.MaxWaitingTime", "Max Waiting time in MS after each Entity update.");
+
+        // extractors
+        defs->optional(imageExtractor.enabled, prefix + "imageExtractor.Enabled");
+        defs->optional(depthImageExtractor.enabled, prefix + "depthImageExtractor.Enabled");
+
+        // converters
+        defs->optional(pngConverter.enabled, prefix + "pngConverter.Enabled");
+        defs->optional(exrConverter.enabled, prefix + "exrConverter.Enabled");
+    }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.h
new file mode 100644
index 0000000000000000000000000000000000000000..cc10b538d5f6b14af4e55fef61bb826508d4ec7d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <map>
+#include <mutex>
+#include <optional>
+#include <string>
+
+#include <ArmarXCore/core/application/properties/Properties.h>
+
+#include "../filter/frequencyFilter/FrequencyFilter.h"
+#include "../filter/equalityFilter/EqualityFilter.h"
+#include "../extractor/imageExtractor/ImageExtractor.h"
+#include "../extractor/imageExtractor/DepthImageExtractor.h"
+#include "../converter/object/json/JsonConverter.h"
+#include "../converter/image/png/PngConverter.h"
+#include "../converter/image/exr/ExrConverter.h"
+#include "../typeConverter/json/JsonConverter.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::server::ltm
+{
+    /// all necessary classes to filter and convert an entry of the ltm to some other format(s)
+    class Processors
+    {
+    public:
+        Processors();
+        void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix);
+
+    public:
+        // Unique Memory Filters
+        std::vector<MemoryFilter*> memFilters;
+        filter::MemoryFrequencyFilter memFreqFilter;
+
+        // Unique Snapshot filters
+        std::vector<SnapshotFilter*> snapFilters;
+        filter::SnapshotFrequencyFilter snapFreqFilter;
+        filter::SnapshotEqualityFilter snapEqFilter;
+
+        // Special Extractors
+        std::vector<Extractor*> extractors;
+        extractor::ImageExtractor imageExtractor;
+        extractor::DepthImageExtractor depthImageExtractor;
+
+        // Special Converters
+        std::map<std::string, Converter*> converters;
+        converter::image::PngConverter pngConverter;
+        converter::image::ExrConverter exrConverter;
+
+        // Default converter
+        converter::object::JsonConverter defaultObjectConverter;
+        converter::type::JsonConverter defaultTypeConverter;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
index e53643cc73768b6d2c51a88930211e49bd35bc10..b3c4be572317fa587b29864fe8981b5e11706cf1 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
@@ -41,6 +41,12 @@ namespace armarx::armem::server::ltm
             _store(provSeg);
         }
 
+        /// store the type of the segment
+        void storeType(const armem::wm::ProviderSegment& coreSeg)
+        {
+            _storeType(coreSeg);
+        }
+
         /// iterate over all core segments of this ltm
         virtual bool forEachEntity(std::function<void(EntityT&)>&& func) const = 0;
 
@@ -61,6 +67,7 @@ namespace armarx::armem::server::ltm
         virtual void _loadAllReferences(armem::wm::ProviderSegment&) = 0;
         virtual void _resolve(armem::wm::ProviderSegment&) = 0;
         virtual void _store(const armem::wm::ProviderSegment&) = 0;
+        virtual void _storeType(const armem::wm::ProviderSegment&) = 0;
 
     protected:
         mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
index 10647ccbe4708a09087673f172eb55ed67cdc68d..0fd5ca5adc0f82010d1a4f0ac85cc836301bac4f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
@@ -18,10 +18,14 @@ namespace armarx::armem::server::ltm
             std::map<std::string, aron::data::VariantPtr> extraction;
         };
 
-        Extractor() = default;
+        Extractor(const aron::type::Descriptor t, const std::string& id) : extractsType(t), identifier(id) {};
         virtual ~Extractor() = default;
 
         virtual Extraction extract(aron::data::DictPtr& data) = 0;
         virtual aron::data::DictPtr merge(Extraction& encoding) = 0;
+
+        const aron::type::Descriptor extractsType;
+        const std::string identifier;
+        bool enabled = false;
     };
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4640cca47ec3401f8effc043536ac1f7244f93fb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp
@@ -0,0 +1,48 @@
+#include "DepthImageExtractor.h"
+
+
+namespace armarx::armem::server::ltm::extractor
+{
+    void DepthImageExtractorVisitor::visitDictOnEnter(Input& data)
+    {
+        ARMARX_CHECK_NOT_NULL(data);
+
+        auto dict = aron::data::Dict::DynamicCastAndCheck(data);
+        for (const auto& [key, child] : dict->getElements())
+        {
+            if (child && child->getDescriptor() == aron::data::Descriptor::eNDArray)
+            {
+                auto ndarray = aron::data::NDArray::DynamicCastAndCheck(child);
+                auto shape = ndarray->getShape();
+                if (shape.size() == 3 && shape[2] == 4 && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
+                {
+                    depthImages[key] = ndarray;
+                    dict->setElement(key, nullptr);
+                }
+            }
+        }
+    }
+
+    void DepthImageExtractorVisitor::visitUnknown(Input&)
+    {
+        // A member is null. Simply ignore...
+    }
+
+    Extractor::Extraction DepthImageExtractor::extract(aron::data::DictPtr& data)
+    {
+        DepthImageExtractorVisitor visitor;
+        aron::data::VariantPtr var = std::static_pointer_cast<aron::data::Variant>(data);
+        aron::data::VariantPtr p;
+        aron::data::visitRecursive(visitor, var);
+
+        Extraction encoding;
+        encoding.dataWithoutExtraction = data;
+        encoding.extraction = visitor.depthImages;
+        return encoding;
+    }
+
+    aron::data::DictPtr DepthImageExtractor::merge(Extraction& encoding)
+    {
+        return encoding.dataWithoutExtraction;
+    }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..09b305e3ca412be13f9b0bf3caf348273fba23e4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.h
@@ -0,0 +1,31 @@
+#pragma once
+
+// Base Class
+#include "../Extractor.h"
+
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::server::ltm::extractor
+{
+    class DepthImageExtractorVisitor : public aron::data::RecursiveVariantVisitor
+    {
+    public:
+        std::map<std::string, aron::data::VariantPtr> depthImages;
+
+        void visitDictOnEnter(Input& data);
+        void visitUnknown(Input& data);
+    };
+
+    class DepthImageExtractor : public Extractor
+    {
+    public:
+        DepthImageExtractor() :
+            Extractor(aron::type::Descriptor::eImage, "depthimage")
+        {
+            enabled = true;
+        };
+
+        virtual Extraction extract(aron::data::DictPtr& data) override;
+        virtual aron::data::DictPtr merge(Extraction& encoding) override;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
index ff86e06243bbee18abfafdfe781ffa50d7126ee6..c646a4f474019742d0db6094245543463ccc553c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
@@ -14,7 +14,7 @@ namespace armarx::armem::server::ltm::extractor
             {
                 auto ndarray = aron::data::NDArray::DynamicCastAndCheck(child);
                 auto shape = ndarray->getShape();
-                if (shape.size() == 3 && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
+                if (shape.size() == 3 && (shape[2] == 3 || shape[2] == 1 /* 3 channel color or grayscale */) && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
                 {
                     images[key] = ndarray;
                     dict->setElement(key, nullptr);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
index 08161d3fc8b81ccab7b927691686d59219454a07..be9c80ba7d8138857aa54edefbb3d61a2d461e39 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
@@ -19,7 +19,11 @@ namespace armarx::armem::server::ltm::extractor
     class ImageExtractor : public Extractor
     {
     public:
-        ImageExtractor() = default;
+        ImageExtractor() :
+            Extractor(aron::type::Descriptor::eImage, "image")
+        {
+            enabled = true;
+        };
 
         virtual Extraction extract(aron::data::DictPtr& data) override;
         virtual aron::data::DictPtr merge(Extraction& encoding) override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
index 6d64ca98c42ac2a014c6bff41f8d73ca927682c2..e75969bfa50b86379b1a0db47ab99c9f533ddc05 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
@@ -16,6 +16,8 @@ namespace armarx::armem::server::ltm
         virtual ~MemoryFilter() = default;
 
         virtual bool accept(const armem::wm::Memory& e) = 0;
+
+        bool enabled = false;
     };
 
     class SnapshotFilter
@@ -25,5 +27,7 @@ namespace armarx::armem::server::ltm
         virtual ~SnapshotFilter() = default;
 
         virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
+
+        bool enabled = false;
     };
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ca0a944c2f2bd59681128ac22f86eaa58e50b51
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
@@ -0,0 +1,52 @@
+#include "EqualityFilter.h"
+
+#include <IceUtil/Time.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+    bool SnapshotEqualityFilter::accept(const armem::wm::EntitySnapshot& e)
+    {
+        auto entityID = e.id().getEntityID();
+        auto genMs = e.time().toMilliSecondsSinceEpoch();
+
+        long lastMs = 0;
+        std::vector<aron::data::DictPtr> lastData;
+        if (timestampLastCommitInMs.count(entityID) > 0)
+        {
+            lastData = dataLastCommit.at(entityID);
+            lastMs = timestampLastCommitInMs.at(entityID);
+        }
+
+        auto timePassedSinceLastStored = genMs - lastMs;
+        if (maxWaitingTimeInMs < 0 || timePassedSinceLastStored > maxWaitingTimeInMs)
+        {
+            bool accept = false;
+            std::vector<aron::data::DictPtr> genData;
+            for (unsigned int i = 0; i != e.size(); ++i)
+            {
+                const auto& d = e.getInstance(i).data();
+                genData.push_back(d);
+
+                if (lastMs == 0 || e.size() != lastData.size()) // nothing stored yet or we cannot compare
+                {
+                    accept = true;
+                    break;
+                }
+
+                const auto& el = lastData.at(i);
+                if ((!d and el) || (d and !el) || (d && el && !(*d == *el))) // data unequal?
+                {
+                    accept = true;
+                    break;
+                }
+            }
+
+            if (!accept) return false;
+
+            dataLastCommit[entityID] = genData;
+            timestampLastCommitInMs[entityID] = genMs;
+            return true;
+        }
+        return false;
+    }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ad630b4fb4add2cd318b6b3e8cdf5eddd52e5fa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <vector>
+#include <map>
+
+// Base Class
+#include "../Filter.h"
+
+// Aron
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+    class SnapshotEqualityFilter :
+            public SnapshotFilter
+    {
+    public:
+        SnapshotEqualityFilter() = default;
+
+        virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+
+    public:
+        int maxWaitingTimeInMs = -1;
+
+    private:
+        std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+        std::map<MemoryID, long> timestampLastCommitInMs;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
index 5650c836523a90c48d5cfb50b9b9e6ef8b307af9..a98aab7f756e42be8d995401964170e64c83e537 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
@@ -6,7 +6,7 @@ namespace armarx::armem::server::ltm::filter
 {
     bool MemoryFrequencyFilter::accept(const armem::wm::Memory& e)
     {
-        auto now = armem::Time::now().toMilliSeconds();
+        auto now = armem::Time::Now().toMilliSecondsSinceEpoch();
         if (waitingTimeInMs < 0 || (now - timestampLastCommitInMs) > waitingTimeInMs)
         {
             timestampLastCommitInMs = now;
@@ -18,7 +18,7 @@ namespace armarx::armem::server::ltm::filter
     bool SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot& e)
     {
         auto entityID = e.id().getEntityID();
-        auto genMs = e.time().toMilliSeconds();
+        auto genMs = e.time().toMilliSecondsSinceEpoch();
 
         long lastMs = 0;
         if (timestampLastCommitInMs.count(entityID) > 0)
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
index 100ccf625e5d962bef40b73367145bf056d67ec6..f8427b2b70e41cd603702cb60b5664ddef3126da 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <map>
+
 // Base Class
 #include "../Filter.h"
 
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c846adcbd45ddb152a4525207a9bbde30bd897af
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.cpp
@@ -0,0 +1,6 @@
+#include "Converter.h"
+
+namespace armarx::armem::server::ltm
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..8290e17c03f3e32e4d7b417b2269699869a85a0a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.h
@@ -0,0 +1,28 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+namespace armarx::armem::server::ltm
+{
+    class TypeConverter
+    {
+    public:
+        TypeConverter(const std::string& id, const std::string& s):
+            identifier(id),
+            suffix(s)
+        {}
+        virtual ~TypeConverter() = default;
+
+        virtual std::pair<std::vector<unsigned char>, std::string> convert(const aron::type::ObjectPtr& data) = 0;
+        virtual aron::type::ObjectPtr convert(const std::vector<unsigned char>& data, const std::string&) = 0;
+
+    public:
+        const std::string identifier;
+        const std::string suffix;
+        bool enabled = false;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc558297e32f82b6db21a2649f283292a0a11518
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.cpp
@@ -0,0 +1,20 @@
+#include "JsonConverter.h"
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+namespace armarx::armem::server::ltm::converter::type
+{
+    std::pair<std::vector<unsigned char>, std::string> JsonConverter::convert(const aron::type::ObjectPtr& data)
+    {
+        nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(data);
+        auto str = j.dump(2);
+        return std::make_pair(std::vector<unsigned char>(str.begin(), str.end()), "");
+    }
+
+    aron::type::ObjectPtr JsonConverter::convert(const std::vector<unsigned char>& data, const std::string&)
+    {
+        std::string str(data.begin(), data.end());
+        nlohmann::json j = nlohmann::json::parse(str);
+        return aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONTypeObject(j);
+    }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f95674b5398d910468b1335617efdb3591a9f7c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.h
@@ -0,0 +1,23 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+namespace armarx::armem::server::ltm::converter::type
+{
+    class JsonConverter : public TypeConverter
+    {
+    public:
+        JsonConverter() :
+            TypeConverter("dict", ".json")
+        {
+            enabled = true; // always true!
+        }
+
+        std::pair<std::vector<unsigned char>, std::string> convert(const aron::type::ObjectPtr& data) final;
+        aron::type::ObjectPtr convert(const std::vector<unsigned char>& data, const std::string&) final;
+    };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
index c22b406d8834f0d633942181f45656b1908e3030..ee45273c7517d48e432b321144afc7a0bc0a364a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
@@ -8,72 +8,72 @@
 
 namespace armarx::armem::server::ltm::disk
 {
-    namespace
-    {
-        MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
-        {
-            util::ensureFolderExists(p);
-
-            MemoryID m;
-            m.memoryName = p.parent_path().filename();
-            m.coreSegmentName = p.filename();
-            return m;
-        }
-    }
-
-    CoreSegment::CoreSegment(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
-        CoreSegmentBase(getMemoryIDFromPath(p), pipe),
-        DiskMemoryItem(p.parent_path())
+    CoreSegment::CoreSegment(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+        CoreSegmentBase(id, filters),
+        DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName))),
+        currentMode(mode),
+        currentExport(e)
     {
     }
 
     bool CoreSegment::forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const
     {
-        if (!checkPath(id().coreSegmentName))
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
             return false;
         }
 
-        std::filesystem::path p = path;
-        p = p / id().coreSegmentName;
-        util::ensureFolderExists(p, false);
-
-        for (const auto& subdir : std::filesystem::directory_iterator(p))
+        for (const auto& subdirName : util::getAllDirectories(mPath, relPath))
         {
-            std::filesystem::path subdirPath = subdir.path();
-            ProviderSegment c(subdirPath, filters);
+            std::string segmentName = UnescapeSegmentName(subdirName);
+            ProviderSegment c(memoryParentPath, id().withProviderSegmentName(segmentName), processors, currentMode, currentExport);
             func(c);
         }
         return true;
     }
 
-    std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& n) const
+    std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& providerSegmentName) const
     {
-        if (!checkPath(id().coreSegmentName))
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
-            return {};
+            return nullptr;
         }
 
-        std::filesystem::path p = path;
-        p = p / id().coreSegmentName;
-        util::ensureFolderExists(p, false);
+        auto c = std::make_shared<ProviderSegment>(memoryParentPath, id().withProviderSegmentName(providerSegmentName), processors, currentMode, currentExport);
+        if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(currentMode)))
+        {
+            return nullptr;
+        }
 
-        std::filesystem::path subpath = p / n;
-        util::ensureFolderExists(subpath, false);
-        auto c = std::make_shared<ProviderSegment>(subpath, filters);
         return c;
     }
 
-    std::string CoreSegment::getExpectedFolderName() const
-    {
-        return name();
-    }
-
     void CoreSegment::_loadAllReferences(armem::wm::CoreSegment& e)
     {
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
+
         e.id() = id();
 
-        forEachProviderSegment([&e](ProviderSegment& x) {
+        auto& conv = processors->defaultTypeConverter;
+        auto relTPath = relPath / (constantes::TYPE_FILENAME + conv.suffix);
+
+        if (util::checkIfFileExists(mPath, relTPath))
+        {
+            auto typeFileContent = util::readDataFromFile(mPath, relTPath);
+            auto typeAron = conv.convert(typeFileContent, "");
+            e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
+        }
+
+        forEachProviderSegment([&e](auto& x) {
             armem::wm::ProviderSegment s;
             x.loadAllReferences(s);
             e.addProviderSegment(s);
@@ -82,22 +82,82 @@ namespace armarx::armem::server::ltm::disk
 
     void CoreSegment::_resolve(armem::wm::CoreSegment& c)
     {
-        c.forEachProviderSegment([this](armem::wm::ProviderSegment& e)
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
+
+        c.forEachProviderSegment([&](auto& e)
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().coreSegmentName / e.id().providerSegmentName, false);
-            ProviderSegment c((std::filesystem::path(path) / id().coreSegmentName / e.id().providerSegmentName), filters);
-            c.resolve(e);
+            ProviderSegment c(memoryParentPath, id().withProviderSegmentName(e.id().providerSegmentName), processors, currentMode, currentExport);
+            if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(currentMode)))
+            {
+                c.resolve(e);
+            }
         });
     }
 
     void CoreSegment::_store(const armem::wm::CoreSegment& c)
     {
-        c.forEachProviderSegment([this](const auto& provSegment)
+        auto currentMaxExport = currentExport;
+        auto encodingModeOfPast = currentMode;
+
+        if (id().coreSegmentName.empty())
+        {
+            ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            id().coreSegmentName = c.id().coreSegmentName;
+        }
+
+        auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+        auto defaultRelPath = getRelativePathForMode(defaultMode);
+        if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().coreSegmentName / provSegment.id().providerSegmentName);
-            ProviderSegment c((std::filesystem::path(path) / id().coreSegmentName / provSegment.id().providerSegmentName), filters);
-            c.store(provSegment);
+            ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+            util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+        }
+
+        c.forEachProviderSegment([&](const auto& prov)
+        {
+            ProviderSegment c(memoryParentPath, id().withProviderSegmentName(prov.id().providerSegmentName), processors, encodingModeOfPast, currentMaxExport);
+            util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode), true);
+
+            c.storeType(prov);
+            c.store(prov);
         });
     }
 
+    void CoreSegment::_storeType(const armem::wm::CoreSegment& c)
+    {
+        if (id().coreSegmentName.empty())
+        {
+            ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            id().coreSegmentName = c.id().coreSegmentName;
+        }
+
+        auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+        auto defaultRelPath = getRelativePathForMode(defaultMode);
+        if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+        {
+            ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+            util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+        }
+
+        if(c.hasAronType())
+        {
+            auto& conv = processors->defaultTypeConverter;
+
+            auto [vec, modeSuffix] = conv.convert(c.aronType());
+            ARMARX_CHECK_EMPTY(modeSuffix);
+            std::filesystem::path relTypePath = defaultRelPath / (constantes::TYPE_FILENAME + conv.suffix);
+            util::writeDataToFileRepeated(defaultMPath, relTypePath, vec);
+        }
+    }
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
index bb16074bcb69c206efc9e06d2fa5188552ad01f9..6dfc00e7e5e37814027cfc10a3be53dd83dff603 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
@@ -16,7 +16,7 @@ namespace armarx::armem::server::ltm::disk
     {
     public:
 
-        CoreSegment(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+        CoreSegment(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
 
         bool forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const override;
 
@@ -26,9 +26,11 @@ namespace armarx::armem::server::ltm::disk
         void _loadAllReferences(armem::wm::CoreSegment&) override;
         void _resolve(armem::wm::CoreSegment&) override;
         void _store(const armem::wm::CoreSegment&) override;
+        void _storeType(const armem::wm::CoreSegment&) override;
 
-        std::string getExpectedFolderName() const override;
-
+    private:
+        MemoryEncodingMode currentMode;
+        unsigned long currentExport;
     };
 
 } // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
index 3854c834c0d3f1aa50e69471ea18722ad553db3c..b64531211cff4343271561180dd312707f1f9847 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
@@ -12,153 +12,330 @@
 
 namespace armarx::armem::server::ltm::disk
 {
-
-    namespace
-    {
-        MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
-        {
-            util::ensureFolderExists(p);
-
-            MemoryID m;
-            m.memoryName = p.parent_path().parent_path().parent_path().filename();
-            m.coreSegmentName = p.parent_path().parent_path().filename();
-            m.providerSegmentName = p.parent_path().filename();
-            m.entityName = p.filename();
-            return m;
-        }
-    }
-
-    Entity::Entity(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
-        EntityBase(getMemoryIDFromPath(p), pipe),
-        DiskMemoryItem(p.parent_path())
+    Entity::Entity(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+        EntityBase(id, filters),
+        DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName)) / EscapeSegmentName(id.providerSegmentName) / EscapeSegmentName(id.entityName)),
+        currentMode(mode),
+        currentExport(e)
     {
     }
 
-    std::string Entity::getExpectedFolderName() const
-    {
-        return name();
-    }
-
     bool Entity::forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const
     {
-        if (!checkPath(id().entityName))
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
             return false;
         }
 
-        std::filesystem::path p = path;
-        p = p / id().entityName;
-        util::ensureFolderExists(p, false);
-
-        for (const auto& subdir : std::filesystem::directory_iterator(p))
+        for (const auto& hName : util::getAllDirectories(mPath, relPath))
         {
-            std::filesystem::path subdirPath = subdir.path();
-            EntitySnapshot c(subdirPath, filters);
-            func(c);
+            if (!util::isNumber(hName))
+            {
+                ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << hName << "'. " <<
+                                  "Ignoring this folder, however this is a bad situation.";
+                continue;
+            }
+
+            // check if this is already a microsec folder (legacy export support)
+            //if (std::stol(secName) > 1647524607 /* the time in us the new export was implemented */)
+            //{
+            //    EntitySnapshot c(memoryParentPath, id().withTimestamp(timeFromStringMicroSeconds(secName)), processors, currentMode, currentExport);
+            //    func(c);
+            //    continue;
+            //}
+
+            auto hRelPath = relPath / hName;
+            for (const auto& secName : util::getAllDirectories(mPath, hRelPath))
+            {
+                if (!util::isNumber(secName))
+                {
+                    ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << secName << "'. " <<
+                                      "Ignoring this folder, however this is a bad situation.";
+                    continue;
+                }
+
+                auto secRelPath = hRelPath / secName;
+                for (const auto& usecName : util::getAllDirectories(mPath, secRelPath))
+                {
+                    if (!util::isNumber(usecName))
+                    {
+                        ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << usecName << "'. " <<
+                                          "Ignoring this folder, however this is a bad situation.";
+                        continue;
+                    }
+
+                    EntitySnapshot c(memoryParentPath, id().withTimestamp(timeFromStringMicroSeconds(usecName)), processors, currentMode, currentExport);
+                    func(c);
+                }
+            }
         }
         return true;
     }
 
     bool Entity::forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const
     {
-        return true;
+        throw LocalException("NOT IMPLEMENTED YET BECAUSE THE DIRECTORY ITERATOR IS UNSORTED!");
     }
 
     bool Entity::forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const
     {
-        return true;
+        auto f = [&](EntitySnapshot& e)
+        {
+            auto ts = e.id().timestamp;
+            if (ts >= min && ts <= max)
+            {
+                func(e);
+            }
+        };
+
+        return forEachSnapshot(std::move(f));
     }
 
     bool Entity::forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
     {
-        return true;
+        auto f = [&](EntitySnapshot& e)
+        {
+            auto ts = e.id().timestamp;
+            if (ts <= time)
+            {
+                func(e);
+            }
+        };
+
+        return forEachSnapshot(std::move(f));
     }
 
     bool Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
     {
-        return true;
+        auto f = [&](EntitySnapshot& e)
+        {
+            auto ts = e.id().timestamp;
+            if (ts < time)
+            {
+                func(e);
+            }
+        };
+
+        return forEachSnapshot(std::move(f));
     }
 
     std::shared_ptr<EntitySnapshot> Entity::findSnapshot(const Time& n) const
     {
-        if (!checkPath(id().entityName))
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
-            return {};
+            return nullptr;
         }
 
-        std::filesystem::path p = path;
-        p = p / id().entityName;
-        util::ensureFolderExists(p, false);
+        auto c = std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(n), processors, currentMode, currentExport);
+        if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(currentMode)))
+        {
+            return nullptr;
+        }
 
-        std::filesystem::path subpath = p / std::to_string(n.toMicroSeconds());
-        util::ensureFolderExists(subpath, false);
-        auto c = std::make_shared<EntitySnapshot>(subpath, filters);
         return c;
     }
 
     std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshot() const
     {
-        return {};
+        Time bestMatch = Time::Invalid();
+        auto f = [&](EntitySnapshot& e) {
+            auto ts = e.id().timestamp;
+            if (ts > bestMatch)
+            {
+                bestMatch = ts;
+            }
+        };
+
+        forEachSnapshot(std::move(f));
+
+        if (bestMatch == Time::Invalid())
+        {
+            return nullptr;
+        }
+
+        return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
     }
 
     std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBefore(const Time& time) const
     {
-        return {};
+        Time bestMatch = Time::Invalid();
+        auto f = [&](EntitySnapshot& e) {
+            auto ts = e.id().timestamp;
+            if (ts < time && ts > bestMatch)
+            {
+                bestMatch = ts;
+            }
+        };
+
+        forEachSnapshot(std::move(f));
+
+        if (bestMatch == Time::Invalid())
+        {
+            return nullptr;
+        }
+
+        return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
     }
 
     std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBeforeOrAt(const Time& time) const
     {
-        return {};
+        Time bestMatch = Time::Invalid();
+        auto f = [&](EntitySnapshot& e) {
+            auto ts = e.id().timestamp;
+            if (ts <= time && ts > bestMatch)
+            {
+                bestMatch = ts;
+            }
+        };
+
+        forEachSnapshot(std::move(f));
+
+        if (bestMatch == Time::Invalid())
+        {
+            return nullptr;
+        }
+
+        return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
     }
 
     std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfter(const Time& time) const
     {
-        return {};
+        Time bestMatch { Duration::MicroSeconds(std::numeric_limits<long>::max()) };
+        auto f = [&](EntitySnapshot& e)
+        {
+            auto ts = e.id().timestamp;
+            if (ts > time && ts < bestMatch)
+            {
+                bestMatch = ts;
+            }
+        };
+
+        forEachSnapshot(std::move(f));
+
+        if (bestMatch == Time(Duration::MicroSeconds(std::numeric_limits<long>::max())))
+        {
+            return nullptr;
+        }
+
+        return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
     }
 
     std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfterOrAt(const Time& time) const
     {
-        return {};
+        Time bestMatch { Duration::MicroSeconds(std::numeric_limits<long>::max()) };
+        auto f = [&](EntitySnapshot& e)
+        {
+            auto ts = e.id().timestamp;
+            if (ts >= time && ts < bestMatch)
+            {
+                bestMatch = ts;
+            }
+        };
+
+        forEachSnapshot(std::move(f));
+
+        if (bestMatch == Time(Duration::MicroSeconds(std::numeric_limits<long>::max())))
+        {
+            return nullptr;
+        }
+
+        return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
     }
 
     void Entity::_loadAllReferences(armem::wm::Entity& e)
     {
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
+
         e.id() = id();
 
-        forEachSnapshot([&e](EntitySnapshotBase& x) {
-            armem::wm::EntitySnapshot s;
-            x.loadAllReferences(s);
-            e.addSnapshot(s);
+        forEachSnapshot([&e](auto& x)
+        {
+            if (!e.hasSnapshot(x.id().timestamp)) // we only load the references if the snapshot is not existant
+            {
+                armem::wm::EntitySnapshot s;
+                x.loadAllReferences(s);
+                e.addSnapshot(s);
+            }
         });
     }
 
     void Entity::_resolve(armem::wm::Entity& p)
     {
-        p.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds()), false);
-            EntitySnapshot c((std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds())), filters);
-            c.resolve(e);
+            return;
+        }
+
+        p.forEachSnapshot([&](auto& e)
+        {
+            EntitySnapshot c(memoryParentPath, id().withTimestamp(e.id().timestamp), processors, currentMode, currentExport);
+            if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(currentMode)))
+            {
+                c.resolve(e);
+            }
         });
     }
 
-    void Entity::_store(const armem::wm::Entity& entity)
+    void Entity::_store(const armem::wm::Entity& c)
     {
-        entity.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+        if (id().entityName.empty())
+        {
+            ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            id().entityName = c.id().entityName;
+        }
+
+        auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+        auto defaultRelPath = getRelativePathForMode(defaultMode);
+        if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+        {
+            ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+            util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+        }
+
+        c.forEachSnapshot([&](const auto& snap)
         {
-            for (auto& f : filters->snapFilters)
+            auto currentMaxExport = currentExport;
+            auto encodingModeOfPast = currentMode;
+
+            for (unsigned long i = 0; i < currentMaxExport; ++i)
             {
-                if (!f->accept(e))
+                MemoryEncodingMode mode = i == 0 ? defaultMode : encodingModeOfPast;
+                auto mPath = getMemoryBasePathForMode(mode, i);
+
+                EntitySnapshot c(memoryParentPath, id().withTimestamp(snap.id().timestamp), processors, mode, i);
+                if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(mode)))
                 {
-                    ARMARX_WARNING << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
+                    ARMARX_INFO << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because the timestamp already existed (we assume snapshots are const and do not change outside the ltm).";
                     return;
                 }
             }
 
-            util::ensureFolderExists(std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds()));
-            EntitySnapshot c((std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds())), filters);
-            c.store(e);
+            for (auto& f : processors->snapFilters)
+            {
+                if (f->enabled && !f->accept(snap))
+                {
+                    ARMARX_INFO << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
+                    return;
+                }
+            }
+
+            EntitySnapshot c(memoryParentPath, id().withTimestamp(snap.id().timestamp), processors, encodingModeOfPast, currentMaxExport);
+            util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode));
+            c.store(snap);
         });
     }
-
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
index e6926cc8caacacd4f604463e48bd84fe3e3093a2..759b22733612c700ec0461541976cf3872d9df07 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
@@ -16,7 +16,7 @@ namespace armarx::armem::server::ltm::disk
             public DiskMemoryItem
     {
     public:
-        Entity(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+        Entity(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
 
         bool forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const override;
         bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const override;
@@ -36,8 +36,9 @@ namespace armarx::armem::server::ltm::disk
         void _resolve(armem::wm::Entity&) override;
         void _store(const armem::wm::Entity&) override;
 
-        std::string getExpectedFolderName() const override;
-
+    private:
+        MemoryEncodingMode currentMode;
+        unsigned long currentExport;
     };
 
 } // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
index 7e37722aac5b8de2f0d601ab41beaf84f37fcf8d..3d43ea8a0b3540afb32863937f4d8114bc9b09cf 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
@@ -12,61 +12,34 @@
 
 namespace armarx::armem::server::ltm::disk
 {
-
-    namespace
-    {
-        MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
-        {
-            util::ensureFolderExists(p);
-
-            MemoryID m;
-            m.memoryName = p.parent_path().parent_path().parent_path().parent_path().filename();
-            m.coreSegmentName = p.parent_path().parent_path().parent_path().filename();
-            m.providerSegmentName = p.parent_path().parent_path().filename();
-            m.entityName = p.parent_path().filename();
-            m.timestamp = IceUtil::Time::microSeconds(std::stol(p.filename()));
-            return m;
-        }
-
-        void writeDataToFile(const std::filesystem::path& path, const std::vector<unsigned char>& data)
-        {
-            std::ofstream dataofs;
-            dataofs.open(path);
-            dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
-            dataofs.close();
-        }
-
-        std::vector<unsigned char> readDataFromFile(const std::filesystem::path path)
-        {
-            std::ifstream dataifs(path);
-            std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)), (std::istreambuf_iterator<char>()));
-            return datafilecontent;
-        }
-    }
-
-    EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
-        EntitySnapshotBase(getMemoryIDFromPath(p), pipe),
-        DiskMemoryItem(p.parent_path())
-    {
-
-    }
-
-    std::string EntitySnapshot::getExpectedFolderName() const
+    EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+        EntitySnapshotBase(id, filters),
+        DiskMemoryItem(p, EscapeSegmentName(id.memoryName),
+                       std::filesystem::path(EscapeSegmentName(id.coreSegmentName))
+                       / EscapeSegmentName(id.providerSegmentName)
+                       / EscapeSegmentName(id.entityName)
+                       / std::to_string(id.timestamp.toSecondsSinceEpoch() / 3600 /* hours */)
+                       / std::to_string(id.timestamp.toSecondsSinceEpoch()) / id.timestampStr()),
+        currentMode(mode),
+        currentExport(e)
     {
-        return name();
     }
 
     void EntitySnapshot::_loadAllReferences(armem::wm::EntitySnapshot& e) const
     {
-        std::filesystem::path p = path;
-        p = p / id().timestampStr();
-        util::ensureFolderExists(p, false);
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
 
         e.id() = id();
 
         for (unsigned int i = 0; i < 1000; ++i) // 1000 is max size for instances in a single timestamp
         {
-            if (!util::checkIfFolderExists(p / std::to_string(i)))
+            std::filesystem::path relIPath = relPath / std::to_string(i) / "";
+            if (!util::checkIfFolderExists(mPath, relIPath))
             {
                 break;
             }
@@ -78,113 +51,174 @@ namespace armarx::armem::server::ltm::disk
 
     void EntitySnapshot::_resolve(armem::wm::EntitySnapshot& e) const
     {
-        ARMARX_IMPORTANT << __PRETTY_FUNCTION__;
-        auto& dictConverter = filters->converters.at(aron::type::Descriptor::eObject);
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
 
-        // Get data from disk
-        std::filesystem::path p = path;
-        p = p / id().timestampStr();
-        util::ensureFolderExists(p, false);
+        auto& dictConverter = processors->defaultObjectConverter;
 
+        // Get data from disk
         for (unsigned int i = 0; i < e.size(); ++i)
         {
-            util::ensureFolderExists(p / std::to_string(i), false);
-
-            std::filesystem::path data = p / std::to_string(i) / (constantes::DATA_FILENAME + dictConverter->suffix);
-            std::filesystem::path metadata = p / std::to_string(i) / (constantes::METADATA_FILENAME + dictConverter->suffix);
-
-            util::ensureFileExists(data);
+            std::filesystem::path relIPath = relPath / std::to_string(i) / "";
+            if (util::checkIfFolderExists(mPath, relIPath))
+            {
+                std::string dataFilename = (constantes::DATA_FILENAME + dictConverter.suffix);
+                std::string metadataFilename = (constantes::METADATA_FILENAME + dictConverter.suffix);
+                std::filesystem::path relDataPath = relIPath / dataFilename;
+                std::filesystem::path relMetadataPath = relIPath / metadataFilename;
 
-            auto& ins = e.getInstance(i);
+                auto& ins = e.getInstance(i);
+                aron::data::DictPtr datadict = nullptr;
+                aron::data::DictPtr metadatadict = nullptr;
 
-            auto datafilecontent = readDataFromFile(data);
-            auto dataaron = dictConverter->convert(datafilecontent);
-            auto datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
+                // get list of all files. This ensures that we only have to call fs::exists once for each file
+                auto allFilesInIndexFolder = util::getAllFiles(mPath, relIPath);
 
-            // check for special members
-            for (const auto& [key, m] : datadict->getElements())
-            {
-                for (auto& [t, f] : filters->converters)
+                if (std::find(allFilesInIndexFolder.begin(), allFilesInIndexFolder.end(), dataFilename) != allFilesInIndexFolder.end())
                 {
-                    if (t == aron::type::Descriptor::eObject)
-                    {
-                        continue;
-                    }
-
-                    std::filesystem::path member = p / std::to_string(i) / (key + f->suffix);
+                    auto datafilecontent = util::readDataFromFile(mPath, relDataPath);
+                    auto dataaron = dictConverter.convert(datafilecontent, "");
+                    datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
 
-                    if (std::filesystem::exists(member) && std::filesystem::is_regular_file(member))
+                    // check for special members
+                    for (const auto& [key, m] : datadict->getElements())
                     {
-                        auto memberfilecontent = readDataFromFile(member);
-                        auto memberaron = f->convert(memberfilecontent);
-                        datadict->setElement(key, memberaron);
+                        for (auto& [t, f] : processors->converters)
+                        {
+                            for (const auto& filename : allFilesInIndexFolder) // iterate over all files and search for matching ones
+                            {
+                                if (simox::alg::starts_with(filename, key) and simox::alg::ends_with(filename, f->suffix))
+                                {
+                                    std::filesystem::path relMemberPath = relIPath / filename;
+                                    std::string mode = simox::alg::remove_suffix(simox::alg::remove_prefix(filename, key), f->suffix);
+
+                                    auto memberfilecontent = util::readDataFromFile(mPath, relMemberPath);
+                                    auto memberaron = f->convert(memberfilecontent, mode);
+                                    datadict->setElement(key, memberaron);
+                                    break;
+                                }
+                            }
+                        }
                     }
                 }
-            }
+                else
+                {
+                    ARMARX_ERROR << "Could not find the data file '" << relDataPath.string() << "'. Continuing without data.";
+                }
 
-            auto metadatafilecontent = readDataFromFile(metadata);
-            auto metadataaron = dictConverter->convert(metadatafilecontent);
-            auto metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
+                if (std::find(allFilesInIndexFolder.begin(), allFilesInIndexFolder.end(), metadataFilename) != allFilesInIndexFolder.end())
+                {
+                    auto metadatafilecontent = util::readDataFromFile(mPath, relMetadataPath);
+                    auto metadataaron = dictConverter.convert(metadatafilecontent, "");
+                    metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
+                }
+                else
+                {
+                    ARMARX_ERROR << "Could not find the metadata file '" << relMetadataPath.string() << "'. Continuing without metadata.";
+                }
 
-            from_aron(metadatadict, datadict, ins);
+                from_aron(metadatadict, datadict, ins);
+            }
+            else
+            {
+                ARMARX_WARNING << "Could not find the index segment folder for segment '" << e.id().str() << "'.";
+            }
         }
     }
 
     void EntitySnapshot::_store(const armem::wm::EntitySnapshot& e) const
     {
-        ARMARX_IMPORTANT << __PRETTY_FUNCTION__;
-        auto& dictConverter = filters->converters.at(aron::type::Descriptor::eObject);
+        //auto currentMaxExport = currentExport;
+        //auto encodingModeOfPast = currentMode;
+
+        if (id().timestampStr().empty())
+        {
+            ARMARX_WARNING << "During storage of segment '" << e.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            id().timestamp = e.id().timestamp;
+        }
 
-        std::filesystem::path p = path;
-        p = p / id().timestampStr();
-        util::ensureFolderExists(p);
+        auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+        auto& dictConverter = processors->defaultObjectConverter;
+
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+        auto defaultRelPath = getRelativePathForMode(defaultMode);
+        if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+        {
+            ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+            util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+        }
 
         for (unsigned int i = 0; i < e.size(); ++i)
         {
-            std::filesystem::path instancePath = p / std::to_string(i);
-            util::ensureFolderExists(instancePath);
+            std::filesystem::path defaultRelIPath = defaultRelPath / std::to_string(i) / "";
 
-            std::filesystem::path dataPath = instancePath / (constantes::DATA_FILENAME + dictConverter->suffix);
-            std::filesystem::path metadataPath = instancePath / (constantes::METADATA_FILENAME + dictConverter->suffix);
+            // For performance reasons we skip to check whether the index folder already exists somewhere.
+            // We already check if the ts exists on entity level.
 
-            if (util::checkIfFileExists(dataPath) or util::checkIfFileExists(metadataPath))
+            if (!util::checkIfFolderExists(defaultMPath, defaultRelIPath))
             {
-                continue; // we ignore if file already exists
-            }
+                util::ensureFolderExists(defaultMPath, defaultRelIPath);
 
-            auto& ins = e.getInstance(i);
+                std::filesystem::path relDataPath = defaultRelIPath / (constantes::DATA_FILENAME + dictConverter.suffix);
+                std::filesystem::path relMetadataPath = defaultRelIPath / (constantes::METADATA_FILENAME + dictConverter.suffix);
 
-            // data
-            auto dataAron = std::make_shared<aron::data::Dict>();
-            auto metadataAron = std::make_shared<aron::data::Dict>();
-            to_aron(metadataAron, dataAron, ins);
+                auto& ins = e.getInstance(i);
 
-            // check special members for special extractions
-            for (auto& [t, x] : filters->extractors)
-            {
-                if (auto it = filters->converters.find(aron::data::defaultconversion::Data2TypeDescriptor.at(t)); it != filters->converters.end())
+                // data
+                auto dataAron = std::make_shared<aron::data::Dict>();
+                auto metadataAron = std::make_shared<aron::data::Dict>();
+                to_aron(metadataAron, dataAron, ins);
+
+                // check special members for special extractions
+                for (auto& x : processors->extractors)
                 {
-                    auto& conv = it->second;
-                    auto dataExt = x->extract(dataAron);
+                    if (!x->enabled) continue;
 
-                    for (const auto& [memberName, var] : dataExt.extraction)
+                    const auto& t = x->identifier;
+
+                    Converter* conv = nullptr; // find suitable converter
+                    for (const auto& [ct, c] : processors->converters)
                     {
-                        ARMARX_CHECK_NOT_NULL(var);
-                        std::filesystem::path extPath = instancePath / (memberName + conv->suffix);
-                        auto extVec = conv->convert(var);
-                        writeDataToFile(extPath, extVec);
+                        if (!c->enabled) continue;
+                        if (t != ct) continue;
+                        conv = c;
                     }
 
-                    dataAron = dataExt.dataWithoutExtraction;
+                    if (conv)
+                    {
+                        auto dataExt = x->extract(dataAron);
+
+                        for (const auto& [memberName, var] : dataExt.extraction)
+                        {
+                            ARMARX_CHECK_NOT_NULL(var);
+
+                            auto [memberDataVec, memberDataModeSuffix] = conv->convert(var);
+                            std::filesystem::path relMemberPath = defaultRelIPath / (memberName + memberDataModeSuffix + conv->suffix);
+
+                            util::writeDataToFileRepeated(defaultMPath, relMemberPath, memberDataVec);
+                        }
+
+                        dataAron = dataExt.dataWithoutExtraction;
+                    }
+                    // else we could not convert the extracted data so it makes no sense to extract it at all...
                 }
-                // else we could not convert the extracted data so it makes no sense to extract it at all...
-            }
 
-            // convert dict and metadata
-            auto dataVec = dictConverter->convert(dataAron);
-            auto metadataVec = dictConverter->convert(metadataAron);
-            writeDataToFile(dataPath, dataVec);
-            writeDataToFile(metadataPath, metadataVec);
+                // convert dict and metadata
+                auto [dataVec, dataVecModeSuffix] = dictConverter.convert(dataAron);
+                auto [metadataVec, metadataVecModeSuffix] = dictConverter.convert(metadataAron);
+                ARMARX_CHECK_EMPTY(dataVecModeSuffix);
+                ARMARX_CHECK_EMPTY(metadataVecModeSuffix);
+
+                util::writeDataToFileRepeated(defaultMPath, relDataPath, dataVec);
+                util::writeDataToFileRepeated(defaultMPath, relMetadataPath, metadataVec);
+            }
+            // Ignore if the full index already exists. Actually this should not happen since the existence of the ts folder is checked on entity level
         }
     }
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
index 2c5eb55cb9168b93ac822b0586c15232ce2c04ba..b91c69b78dca573935b7d51c665ca67e6b499fcc 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
@@ -14,15 +14,16 @@ namespace armarx::armem::server::ltm::disk
             public DiskMemoryItem
     {
     public:
-        EntitySnapshot(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+        EntitySnapshot(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
 
     protected:
         void _loadAllReferences(armem::wm::EntitySnapshot&) const override;
         void _resolve(armem::wm::EntitySnapshot&) const override;
         void _store(const armem::wm::EntitySnapshot&) const override;
 
-        std::string getExpectedFolderName() const override;
-
+    private:
+        MemoryEncodingMode currentMode;
+        unsigned long currentExport;
     };
 
 } // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
index 190699393da470365e30ff4466b06943216fb7ed..0eccbbf1a4435d301e05fea5ea0f64d2abf6eef0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
@@ -5,111 +5,94 @@
 
 #include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
-// ArmarX
-#include "../base/filter/frequencyFilter/FrequencyFilter.h"
-#include "../base/extractor/imageExtractor/ImageExtractor.h"
-#include "../base/converter/dict/json/JsonConverter.h"
-#include "../base/converter/dict/bson/BsonConverter.h"
-#include "../base/converter/image/png/PngConverter.h"
-
 namespace armarx::armem::server::ltm::disk
 {
-    namespace
-    {
-        MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
-        {
-            ARMARX_CHECK(!p.empty());
-
-            MemoryID m;
-            m.memoryName = p.filename();
-            return m;
-        }
-
-        std::filesystem::path getDefaultStoragePath()
-        {
-            /*std::string armarx_home = std::string(getenv("HOME")) + "/.armarx";
-            if (getenv("ARMARX_DEFAULTS_DIR"))
-            {
-                armarx_home = getenv("ARMARX_DEFAULTS_DIR");
-            }
-            path = armarx_home + "/armem/disk/data/db";*/
-            return "/tmp/MemoryExport/Test";
-        }
-    }
-
     void Memory::createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix)
     {
         Base::createPropertyDefinitions(properties, prefix);
-        properties->optional(path, prefix + "storagepath", "The path to the memory storage.");
+        properties->optional(memoryParentPath, prefix + "storagepath", "The path to the memory storage (the memory will be stored in a seperate subfolder).");
+        properties->optional(sizeToCompressDataInMegaBytes, prefix + "sizeToCompressDataInMegaBytes", "The size in MB to compress away the current export. Exports are numbered (lower number means newer).");
     }
 
     Memory::Memory() :
-        Memory(getDefaultStoragePath())
+        Memory(std::filesystem::path("/tmp/MemoryExport"), "Test")
     {
     }
 
-    Memory::Memory(const std::filesystem::path& p) :
-        Base(getMemoryIDFromPath(p)),
-        DiskMemoryItem(p.parent_path())
+    Memory::Memory(const std::filesystem::path& p, const std::string& memoryName /* UNESCAPED */, const MemoryEncodingMode defaultEncodingMode) :
+        Base(MemoryID(memoryName, "")),
+        DiskMemoryItem(p, EscapeSegmentName(memoryName), ""),
+        exportEncodingMode(defaultEncodingMode)
     {
+        setMemoryID(MemoryID(memoryName, "")); // set Memory id and search for old exports
     }
 
     void Memory::setMemoryID(const MemoryID& id)
     {
         ARMARX_CHECK_NOT_EMPTY(id.memoryName);
-
         Base::setMemoryID(id.getMemoryID());
-        buffer->id() = id.getMemoryID();
-        to_store->id() = id.getMemoryID();
-    }
+        escapedMemoryName = EscapeSegmentName(id.memoryName);
 
-    std::string Memory::getExpectedFolderName() const
-    {
-        return id().memoryName;
+        // Discover how many exports already exists
+        for (unsigned long i = 1; i < 10000; ++i)
+        {
+            std::filesystem::path exportPath = getMemoryBasePathForMode(exportEncodingMode, i);
+            if (!std::filesystem::exists(exportPath))
+            {
+                break;
+            }
+            maxExportIndex = i;
+        }
     }
 
     bool Memory::forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const
     {
-        if (!checkPath(id().memoryName))
+        for (unsigned long i = 0; i <= maxExportIndex; ++i)
         {
-            return false;
+            MemoryEncodingMode mode = i == 0 ? MemoryEncodingMode::FILESYSTEM : exportEncodingMode;
+            auto mPath = getMemoryBasePathForMode(mode, i);
+            if (util::checkIfBasePathExists(mPath))
+            {
+                for (const auto& subdirName : util::getAllDirectories(mPath, getRelativePathForMode(mode)))
+                {
+                    std::string segmentName = UnescapeSegmentName(subdirName);
+                    CoreSegment c(memoryParentPath, id().withCoreSegmentName(segmentName), processors, mode, i);
+                    func(c);
+                }
+            }
         }
 
-        std::filesystem::path p = path;
-        p = p / id().memoryName;
-        util::ensureFolderExists(p, false);
-
-        for (const auto& subdir : std::filesystem::directory_iterator(p))
-        {
-            std::filesystem::path subdirPath = subdir.path();
-            CoreSegment c(subdirPath, filters);
-            func(c);
-        }
         return true;
     }
 
-    std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& n) const
+    std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& coreSegmentName) const
     {
-        if (!checkPath(id().memoryName))
+        for (unsigned long i = 0; i <= maxExportIndex; ++i)
         {
-            return {};
-        }
+            MemoryEncodingMode mode = i == 0 ? MemoryEncodingMode::FILESYSTEM : exportEncodingMode;
+
+            auto mPath = getMemoryBasePathForMode(mode, i);
+            if (!util::checkIfBasePathExists(mPath))
+            {
+                continue;
+            }
 
-        std::filesystem::path p = path;
-        p = p / id().memoryName;
-        util::ensureFolderExists(p, false);
+            auto c = std::make_shared<CoreSegment>(memoryParentPath, id().withCoreSegmentName(coreSegmentName), processors, mode, i);
+            if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(mode)))
+            {
+                continue;
+            }
 
-        std::filesystem::path subpath = p / n;
-        util::ensureFolderExists(subpath, false);
-        auto c = std::make_shared<CoreSegment>(subpath, filters);
-        return c;
+            return c;
+        }
+        return nullptr;
     }
 
     void Memory::_loadAllReferences(armem::wm::Memory& m)
     {
         m.id() = id();
 
-        forEachCoreSegment([&m](CoreSegment& x) {
+        forEachCoreSegment([&m](auto& x) {
             armem::wm::CoreSegment s;
             x.loadAllReferences(s);
             m.addCoreSegment(s);
@@ -118,42 +101,98 @@ namespace armarx::armem::server::ltm::disk
 
     void Memory::_resolve(armem::wm::Memory& m)
     {
-        if (!checkPath(id().memoryName))
-        {
-            return;
-        }
+        std::lock_guard l(ltm_mutex); // we cannot load a memory multiple times simultaneously
 
-        std::lock_guard l(ltm_mutex);
-        m.forEachCoreSegment([this](armem::wm::CoreSegment& e)
+        for (unsigned long i = 0; i <= maxExportIndex; ++i)
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().memoryName / e.id().coreSegmentName, false);
-            CoreSegment c((std::filesystem::path(path) / id().memoryName / e.id().coreSegmentName), filters);
-            c.resolve(e);
-        });
+            MemoryEncodingMode mode = i == 0 ? MemoryEncodingMode::FILESYSTEM : exportEncodingMode;
+
+            auto mPath = getMemoryBasePathForMode(mode, i);
+            if (!util::checkIfBasePathExists(mPath))
+            {
+                continue;
+            }
+
+            m.forEachCoreSegment([&](auto& e)
+            {
+                CoreSegment c(memoryParentPath, id().withCoreSegmentName(e.id().coreSegmentName), processors, mode, i);
+                if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(mode)))
+                {
+                    c.resolve(e);
+                }
+            });
+        }
     }
 
     void Memory::_directlyStore(const armem::wm::Memory& memory)
     {
-        if (!checkPath(id().memoryName))
+        std::lock_guard l(ltm_mutex); // we cannot store a memory multiple times simultaneously
+
+        MemoryEncodingMode defaultMode = MemoryEncodingMode::FILESYSTEM;
+        MemoryEncodingMode encodeModeOfPast = exportEncodingMode;
+        // Storage will always be in filesystem mode!
+        // Somehow, minizip was not able to write data to images. It always created a folder named xyz.png without any data in it...
+        // Another problem is that storing data directly in compressed format will require a lot of time when the compressed file is big (>20MB)
+
+        if (id().memoryName.empty())
         {
-            return;
+            ARMARX_WARNING << "During storage of memory '" << memory.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            setMemoryID(memory.id());
         }
 
-        std::lock_guard l(ltm_mutex);
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
 
-        memory.forEachCoreSegment([this](const auto& core)
+        // Check if we have to compress the memory!
+        // See above mentioned issues with directly compressing the data. Therefore we store data in plain text and compress from time to time
+        // using system calls. Also increase the index of all old exports
+        auto size = filesystem::util::getSizeOfDirectory(defaultMPath);
+        //std::cout << "Current maxExportIndex is: " << maxExportIndex << std::endl;
+        if (size >= (sizeToCompressDataInMegaBytes * 1024 * 1024))
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().memoryName / core.id().coreSegmentName);
-            CoreSegment c((std::filesystem::path(path) / id().memoryName / core.id().coreSegmentName), filters);
+            ARMARX_INFO << "Compressen of memory " + id().memoryName + " needed because the size of last export is " + std::to_string(size / 1024.f / 1024.f) + " (>= " + std::to_string(sizeToCompressDataInMegaBytes) + ")";
+
+            // increase index of old memories
+            for (unsigned long i = maxExportIndex; i >= 1; --i)
+            {
+                ARMARX_INFO << "Increasing the index of old compressed memory " + id().memoryName + " (" + std::to_string(i) + " to " + std::to_string(i+1) + ")";
+                auto exportPath = getMemoryBasePathForMode(encodeModeOfPast, i);
+                auto newExportPath = getMemoryBasePathForMode(encodeModeOfPast, i+1);
+                std::string moveCommand = "mv " + exportPath.string() + " " + newExportPath.string();
+                //std::cout << "Exec command: " << moveCommand << std::endl;
+                int ret = system(moveCommand.c_str());
+                (void) ret;
+            }
+
+            // zip away current export
+            ARMARX_INFO << "Compressing the last export of " + id().memoryName;
+            auto newExportPath = getMemoryBasePathForMode(encodeModeOfPast, 1); // 1 will be the new export
+            std::string zipCommand = "cd " + memoryParentPath.string() + " && zip -r " + newExportPath.string() + " " + escapedMemoryName;
+            //std::cout << "Exec command: " << zipCommand << std::endl;
+            int ret = system(zipCommand.c_str());
+            (void) ret;
+
+            // remove unzipped memory export
+            ARMARX_INFO << "Removing the last export of " + id().memoryName;
+            std::string rmCommand = "rm -r " + defaultMPath.string();
+            ret = system(rmCommand.c_str());
+            (void) ret;
+
+            maxExportIndex++;
+        }
+
+        util::ensureBasePathExists(defaultMPath, true); // create if not exists
+
+        memory.forEachCoreSegment([&](const auto& core)
+        {
+            CoreSegment c(memoryParentPath, id().withCoreSegmentName(core.id().coreSegmentName), processors, encodeModeOfPast, 0 /* how far to look back in past on enity level. For full lookup use maxExportIndex. */);
+            util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode), true); // create subfolder
+
+            // 1. store type
+            c.storeType(core);
+
+            // 2. store data
             c.store(core);
         });
     }
-
-    void Memory::setPath(const std::string& ps)
-    {
-        std::filesystem::path p = ps;
-        DiskMemoryItem::setPath(p.parent_path());
-        setMemoryID(getMemoryIDFromPath(p));
-    }
-
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
index 44971a7efa92400aba19b1e85d0260374c993f33..085a56459efd9118d1c9afa4f7bd85f64f6ba38f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
@@ -21,23 +21,26 @@ namespace armarx::armem::server::ltm::disk
         using Base = BufferedMemoryBase<CoreSegment>;
 
         Memory();
-        Memory(const std::filesystem::path&);
+        Memory(const std::filesystem::path&, const std::string&, const MemoryEncodingMode defaultEncodingMode = MemoryEncodingMode::MINIZIP);
 
         void setMemoryID(const MemoryID& id) override;
 
         void createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix) override;
 
         bool forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const override;
-        std::shared_ptr<CoreSegment> findCoreSegment(const std::string&) const override;
-
-        void setPath(const std::string&) override;
+        std::shared_ptr<CoreSegment> findCoreSegment(const std::string& coreSegmentName) const override;
 
     protected:
         void _loadAllReferences(armem::wm::Memory&) override;
         void _resolve(armem::wm::Memory&) override;
         void _directlyStore(const armem::wm::Memory&) override;
 
-        std::string getExpectedFolderName() const override;
+    private:
+        MemoryEncodingMode exportEncodingMode = MemoryEncodingMode::MINIZIP;
+        unsigned long maxExportIndex = 0;
+        unsigned long sizeToCompressDataInMegaBytes = long(1) * 1024; // 1GB
 
+    public:
+        static const int DEPTH_TO_DATA_FILES = 7; // from memory folder = 1 (cseg) + 1 (pseg) + 1 (ent) + 3 (snap) + 1 (inst)
     };
 } // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
index 4d4fdb8f0ecdd658ad767de7016ff5ce5c5c83a4..df52832d1bfdc35e4f19a35c220b2c232a97bdc5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
@@ -10,73 +10,72 @@
 
 namespace armarx::armem::server::ltm::disk
 {
-    namespace
-    {
-        MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
-        {
-            util::ensureFolderExists(p);
-
-            MemoryID m;
-            m.memoryName = p.parent_path().parent_path().filename();
-            m.coreSegmentName = p.parent_path().filename();
-            m.providerSegmentName = p.filename();
-            return m;
-        }
-    }
-
-    ProviderSegment::ProviderSegment(const std::filesystem::path& p, const std::shared_ptr<FilterCollection>& pipe) :
-        ProviderSegmentBase(getMemoryIDFromPath(p), pipe),
-        DiskMemoryItem(p.parent_path())
+    ProviderSegment::ProviderSegment(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+        ProviderSegmentBase(id, filters),
+        DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName)) / EscapeSegmentName(id.providerSegmentName)),
+        currentMode(mode),
+        currentExport(e)
     {
     }
 
     bool ProviderSegment::forEachEntity(std::function<void(Entity&)>&& func) const
     {
-        if (!checkPath(id().providerSegmentName))
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
             return false;
         }
 
-        std::filesystem::path p = path;
-        p = p / id().providerSegmentName;
-        util::ensureFolderExists(p, false);
-
-        for (const auto& subdir : std::filesystem::directory_iterator(p))
+        for (const auto& subdirName : util::getAllDirectories(mPath, relPath))
         {
-            std::filesystem::path subdirPath = subdir.path();
-            Entity c(subdirPath, filters);
+            std::string segmentName = UnescapeSegmentName(subdirName);
+            Entity c(memoryParentPath, id().withEntityName(subdirName), processors, currentMode, currentExport);
             func(c);
         }
         return true;
     }
 
-    std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& n) const
+    std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& entityName) const
     {
-        if (!checkPath(id().providerSegmentName))
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
         {
-            return {};
+            return nullptr;
         }
 
-        std::filesystem::path p = path;
-        p = p / id().providerSegmentName;
-        util::ensureFolderExists(p, false);
+        auto c = std::make_shared<Entity>(memoryParentPath, id().withEntityName(entityName), processors, currentMode, currentExport);
+        if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(currentMode)))
+        {
+            return nullptr;
+        }
 
-        std::filesystem::path subpath = p / n;
-        util::ensureFolderExists(subpath, false);
-        auto c = std::make_shared<Entity>(subpath, filters);
         return c;
     }
 
-    std::string ProviderSegment::getExpectedFolderName() const
-    {
-        return name();
-    }
-
     void ProviderSegment::_loadAllReferences(armem::wm::ProviderSegment& e)
     {
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
+
         e.id() = id();
 
-        forEachEntity([&e](Entity& x) {
+        auto& conv = processors->defaultTypeConverter;
+        auto relTPath = relPath / (constantes::TYPE_FILENAME + conv.suffix);
+
+        if (util::checkIfFileExists(mPath, relTPath))
+        {
+            auto typeFileContent = util::readDataFromFile(mPath, relTPath);
+            auto typeAron = conv.convert(typeFileContent, "");
+            e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
+        }
+
+        forEachEntity([&e](auto& x) {
             armem::wm::Entity s;
             x.loadAllReferences(s);
             e.addEntity(s);
@@ -85,21 +84,80 @@ namespace armarx::armem::server::ltm::disk
 
     void ProviderSegment::_resolve(armem::wm::ProviderSegment& p)
     {
-        p.forEachEntity([this](armem::wm::Entity& e)
+        auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+        auto relPath = getRelativePathForMode(currentMode);
+        if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+        {
+            return;
+        }
+
+        p.forEachEntity([&](auto& e)
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().providerSegmentName / e.id().entityName, false);
-            Entity c((std::filesystem::path(path) / id().providerSegmentName / e.id().entityName), filters);
-            c.resolve(e);
+            Entity c(memoryParentPath, id().withEntityName(e.id().entityName), processors, currentMode, currentExport);
+            if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(currentMode)))
+            {
+                c.resolve(e);
+            }
         });
     }
 
-    void ProviderSegment::_store(const armem::wm::ProviderSegment& providerSegment)
+    void ProviderSegment::_store(const armem::wm::ProviderSegment& p)
     {
-        providerSegment.forEachEntity([this](const auto& entity)
+        auto currentMaxExport = currentExport;
+        auto encodingModeOfPast = currentMode;
+
+        if (id().providerSegmentName.empty())
+        {
+            ARMARX_WARNING << "During storage of segment '" << p.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            id().providerSegmentName = p.id().providerSegmentName;
+        }
+
+        auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+        auto defaultRelPath = getRelativePathForMode(defaultMode);
+        if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
         {
-            util::ensureFolderExists(std::filesystem::path(path) / id().providerSegmentName / entity.id().entityName);
-            Entity c((std::filesystem::path(path) / id().providerSegmentName / entity.id().entityName), filters);
-            c.store(entity);
+            ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+            util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+        }
+
+        p.forEachEntity([&](const auto& e)
+        {
+            Entity c(memoryParentPath, id().withEntityName(e.id().entityName), processors, encodingModeOfPast, currentMaxExport);
+            util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode), true);
+            c.store(e);
         });
     }
+
+    void ProviderSegment::_storeType(const armem::wm::ProviderSegment& p)
+    {
+        if (id().providerSegmentName.empty())
+        {
+            ARMARX_WARNING << "During storage of segment '" << p.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+                              "I set the id of the LTM to the same name, however this should not happen!";
+            id().providerSegmentName = p.id().providerSegmentName;
+        }
+
+        auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+        auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+        auto defaultRelPath = getRelativePathForMode(defaultMode);
+        if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+        {
+            ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+            util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+        }
+
+        if(p.hasAronType())
+        {
+            auto& conv = processors->defaultTypeConverter;
+
+            auto [vec, modeSuffix] = conv.convert(p.aronType());
+            ARMARX_CHECK_EMPTY(modeSuffix);
+            std::filesystem::path relTypePath = defaultRelPath / (constantes::TYPE_FILENAME + conv.suffix);
+            util::writeDataToFileRepeated(defaultMPath, relTypePath, vec);
+        }
+    }
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
index 1d754d868f099a74eb39632534bbc93ad4303074..0402f4b98408b73719ea2d2889d02c195186aac6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
@@ -15,7 +15,7 @@ namespace armarx::armem::server::ltm::disk
             public DiskMemoryItem
     {
     public:
-        ProviderSegment(const std::filesystem::path&, const std::shared_ptr<FilterCollection>& p);
+        ProviderSegment(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
 
         bool forEachEntity(std::function<void(Entity&)>&& func) const override;
 
@@ -25,9 +25,11 @@ namespace armarx::armem::server::ltm::disk
         void _loadAllReferences(armem::wm::ProviderSegment&) override;
         void _resolve(armem::wm::ProviderSegment&) override;
         void _store(const armem::wm::ProviderSegment&) override;
+        void _storeType(const armem::wm::ProviderSegment&) override;
 
-        std::string getExpectedFolderName() const override;
-
+    private:
+        MemoryEncodingMode currentMode;
+        unsigned long currentExport;
     };
 
 } // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp
deleted file mode 100644
index d6eef0874de238366f4946cff7bb673b4b1f16c6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp
+++ /dev/null
@@ -1,7 +0,0 @@
-// Header
-#include "Data.h"
-
-namespace armarx::armem::server::ltm::disk
-{
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
deleted file mode 100644
index b5b2fbf41adc7fffc0f72792ad4b1c09cf023024..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
+++ /dev/null
@@ -1,65 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../../../../core/error.h"
-
-namespace armarx::armem::server::ltm::disk
-{
-    namespace constantes
-    {
-        const std::string TYPE_FILENAME = "type.aron";
-        const std::string DATA_FILENAME = "data.aron";
-        const std::string METADATA_FILENAME = "metadata.aron";
-    }
-
-    namespace util
-    {
-        // check whether a string is a number (e.g. timestamp folders)
-        inline bool isNumber(const std::string& s)
-        {
-            for (char const& ch : s)
-            {
-                if (std::isdigit(ch) == 0)
-                {
-                    return false;
-                }
-            }
-            return true;
-        }
-
-        inline bool checkIfFolderExists(const std::filesystem::path& p)
-        {
-            return std::filesystem::exists(p) and std::filesystem::is_directory(p);
-        }
-
-        inline void ensureFolderExists(const std::filesystem::path& p, bool createIfNotExistent = true)
-        {
-            if (!std::filesystem::exists(p))
-            {
-                if (createIfNotExistent)
-                {
-                    std::filesystem::create_directories(p);
-                }
-            }
-            if (!std::filesystem::is_directory(p))
-            {
-                throw error::ArMemError("Could not find folder: " + p.string());
-            }
-        }
-
-        inline bool checkIfFileExists(const std::filesystem::path& p)
-        {
-            return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
-        }
-
-        inline void ensureFileExists(const std::filesystem::path& p)
-        {
-            if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
-            {
-                // not found
-                throw error::ArMemError("Could not find file: " + p.string());
-            }
-        }
-    }
-} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
index b30b7cde323e011ea9ae715233323542e717dbec..284f289842b88c888195ac4ac8f5d48a196e9a66 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
@@ -1,46 +1,79 @@
 // Header
 #include "DiskStorage.h"
 
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
 // ArmarX
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
 
 namespace armarx::armem::server::ltm::disk
 {
-    DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& p) :
-        path(p.string())
+    //const std::string DiskMemoryItem::Prefix = "_";
+    //const std::string DiskMemoryItem::PrefixEscaped = "__";
+
+    const std::string DiskMemoryItem::MEMORY_EXPORT_SUFFIX = "_";
+    const std::map<std::string, std::string> DiskMemoryItem::EscapeTable = {
+        {"/", "|"}
+    };
+
+    DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& memoryParentPath, const std::string& memoryName, const std::filesystem::path relativePath) :
+        memoryParentPath(memoryParentPath),
+        escapedMemoryName(memoryName),
+        relativeSegmentPath(relativePath)
     {
     }
 
-    bool DiskMemoryItem::checkPath(const std::string& suffix) const
+    std::filesystem::path DiskMemoryItem::getMemoryBasePathForMode(const MemoryEncodingMode m, const unsigned long e) const
     {
-        std::filesystem::path memoryP(path);
-        memoryP = memoryP / suffix;
-
-        // Check connection:
-        if (!std::filesystem::exists(path))
+        std::string escapedMemoryNameSuffixed = escapedMemoryName;
+        if (e > 0)
         {
-            std::filesystem::create_directories(path);
-            return true;
+            escapedMemoryNameSuffixed = escapedMemoryName + MEMORY_EXPORT_SUFFIX + std::to_string(e);
         }
-        else if (std::string expectedFolderName = getExpectedFolderName(); !std::filesystem::is_directory(path) || memoryP.filename() != expectedFolderName)
+
+        switch(m)
         {
-            ARMARX_WARNING << deactivateSpam("LTM_PathError_" + expectedFolderName)
-                           << "The entered path is not valid. Please use a path leading to a memory folder with name: " << expectedFolderName << ". The used path was: " << getPath()
-                           << "\n\n";
-            return false;
+        case MemoryEncodingMode::FILESYSTEM:
+            return memoryParentPath / escapedMemoryNameSuffixed;
+        case MemoryEncodingMode::MINIZIP:
+            return memoryParentPath / (escapedMemoryNameSuffixed + minizip::util::MINIZIP_SUFFIX);
         }
+        return "";
+    }
 
-        return true;
+    std::filesystem::path DiskMemoryItem::getRelativePathForMode(const MemoryEncodingMode m) const
+    {
+        switch(m)  // ensure a tailing "/"
+        {
+        case MemoryEncodingMode::FILESYSTEM:
+            return relativeSegmentPath / "";
+        case MemoryEncodingMode::MINIZIP:
+            return std::filesystem::path(escapedMemoryName) / relativeSegmentPath / "";
+        }
+        return "";
     }
 
-    std::string DiskMemoryItem::getPath() const
+    std::string DiskMemoryItem::EscapeSegmentName(const std::string& segmentName)
     {
-        return path;
+        std::string ret = segmentName;
+        //simox::alg::replace_all(ret, Prefix, PrefixEscaped);
+        for (const auto& [s, r] : EscapeTable)
+        {
+            ret = simox::alg::replace_all(ret, s, r);
+        }
+        return ret;
     }
 
-    void DiskMemoryItem::setPath(const std::string& ps)
+    std::string DiskMemoryItem::UnescapeSegmentName(const std::string& escapedName)
     {
-        path = ps;
+        std::string ret = escapedName;
+        for (const auto& [s, r] : EscapeTable) // Here we assume that noone uses the replaced char usually in the segment name... TODO
+        {
+            ret = simox::alg::replace_all(ret, r, s);
+        }
+        return ret;
     }
 }
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
index 01142b930e6d7d3751c8a01af6d613c2bc3f1501..e4bc43b0b173908bdcc471b4033145cf5fdfd63a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
@@ -1,26 +1,42 @@
 #pragma once
 
 #include <filesystem>
+#include <map>
+#include <string>
 
-#include "Data.h"
+// util
+#include "util/util.h"
 
 namespace armarx::armem::server::ltm::disk
 {
     class DiskMemoryItem
     {
     public:
+        enum class MemoryEncodingMode
+        {
+            FILESYSTEM = 0,
+            MINIZIP = 1
+        };
+
         DiskMemoryItem() = default;
-        DiskMemoryItem(const std::filesystem::path&);
+        DiskMemoryItem(const std::filesystem::path& memoryParentPath, const std::string& escapedMemoryName, const std::filesystem::path relativePath);
         virtual ~DiskMemoryItem() = default;
 
-        virtual void setPath(const std::string&);
-        std::string getPath() const;
+        std::filesystem::path getMemoryBasePathForMode(const MemoryEncodingMode m, const unsigned long e) const;
+        std::filesystem::path getRelativePathForMode(const MemoryEncodingMode m) const;
 
     protected:
-        bool checkPath(const std::string&) const;
-        virtual std::string getExpectedFolderName() const = 0;
+        static std::string EscapeSegmentName(const std::string& segmentName);
+        static std::string UnescapeSegmentName(const std::string& escapedName);
 
     protected:
-        std::string path;
+        //static const std::string PREFIX;
+        //static const std::string PREFIX_ESCAPED;
+        static const std::string MEMORY_EXPORT_SUFFIX;
+        static const std::map<std::string, std::string> EscapeTable;
+
+        std::filesystem::path memoryParentPath;
+        std::string escapedMemoryName;
+        std::filesystem::path relativeSegmentPath;
     };
 } // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..daea3fcfc439326545a79ad00ac53e9133afa195
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
@@ -0,0 +1,117 @@
+#include "filesystem_util.h"
+
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
+
+
+namespace armarx::armem::server::ltm::disk
+{
+    namespace filesystem::util
+    {
+        size_t getSizeOfDirectory(const std::filesystem::path& p)
+        {
+            if (!std::filesystem::exists(p) || !std::filesystem::is_directory(p))
+            {
+                return 0;
+            }
+
+            // command to be executed. Taken from https://stackoverflow.com/questions/15495756/how-can-i-find-the-size-of-all-files-located-inside-a-folder/15501719
+            std::string cmd = "du -sb " + p.string() + " | cut -f1 2>&1";
+
+            // execute above command and get the output
+            FILE *stream = popen(cmd.c_str(), "r");
+            if (stream) {
+                const int max_size = 256;
+                char readbuf[max_size];
+                if (fgets(readbuf, max_size, stream) != NULL)
+                {
+                    return atoll(readbuf);
+                }
+                pclose(stream);
+            }
+
+            // return error val
+            return 0;
+        }
+
+        bool checkIfFolderInFilesystemExists(const std::filesystem::path& p)
+        {
+            return std::filesystem::exists(p) and std::filesystem::is_directory(p);
+        }
+
+        void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent)
+        {
+            if (!std::filesystem::exists(p))
+            {
+                if (createIfNotExistent)
+                {
+                    std::filesystem::create_directories(p);
+                }
+            }
+            if (!std::filesystem::is_directory(p))
+            {
+                throw error::ArMemError("Could not find folder: " + p.string());
+            }
+        }
+
+        bool checkIfFileInFilesystemExists(const std::filesystem::path& p)
+        {
+            return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
+        }
+
+        void ensureFileInFilesystemExists(const std::filesystem::path& p)
+        {
+            if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
+            {
+                // not found
+                throw error::ArMemError("Could not find file: " + p.string());
+            }
+        }
+
+        void writeDataInFilesystemFile(const std::filesystem::path& p, const std::vector<unsigned char>& data)
+        {
+            std::ofstream dataofs;
+            dataofs.open(p);
+            if (!dataofs)
+            {
+                throw error::ArMemError("Could not write data to filesystem file '" + p.string() + "'. Skipping this file.");
+            }
+            dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
+            dataofs.close();
+        }
+
+        std::vector<unsigned char> readDataFromFilesystemFile(const std::filesystem::path path)
+        {
+            std::ifstream dataifs(path);
+            std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)), (std::istreambuf_iterator<char>()));
+            return datafilecontent;
+        }
+
+        std::vector<std::string> getAllFilesystemDirectories(const std::filesystem::path path)
+        {
+            std::vector<std::string> ret;
+            for (const auto& subdir : std::filesystem::directory_iterator(path))
+            {
+                std::filesystem::path subdirPath = subdir.path();
+                if (std::filesystem::is_directory(subdirPath))
+                {
+                    ret.push_back(subdirPath.filename());
+                }
+            }
+            return ret;
+        }
+
+        std::vector<std::string> getAllFilesystemFiles(const std::filesystem::path path)
+        {
+            std::vector<std::string> ret;
+            for (const auto& subdir : std::filesystem::directory_iterator(path))
+            {
+                std::filesystem::path subdirPath = subdir.path();
+                if (std::filesystem::is_regular_file(subdirPath))
+                {
+                    ret.push_back(subdirPath.filename());
+                }
+            }
+            return ret;
+        }
+    }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..78a8f4b1399b8112e204fb891277ed7d52d75ba6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <filesystem>
+#include <fstream>
+#include <iostream>
+#include <vector>
+
+
+namespace armarx::armem::server::ltm::disk
+{
+    namespace filesystem::util
+    {
+        size_t getSizeOfDirectory(const std::filesystem::path& p);
+
+        bool checkIfFolderInFilesystemExists(const std::filesystem::path& p);
+
+        void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent = true);
+
+        bool checkIfFileInFilesystemExists(const std::filesystem::path& p);
+
+        void ensureFileInFilesystemExists(const std::filesystem::path& p);
+
+        void writeDataInFilesystemFile(const std::filesystem::path& p, const std::vector<unsigned char>& data);
+
+        std::vector<unsigned char> readDataFromFilesystemFile(const std::filesystem::path path);
+
+        std::vector<std::string> getAllFilesystemDirectories(const std::filesystem::path path);
+
+        std::vector<std::string> getAllFilesystemFiles(const std::filesystem::path path);
+    }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2995afc21cba2caa75e5dc093398b900bb81e819
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.cpp
@@ -0,0 +1,276 @@
+#include "minizip_util.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+    namespace minizip::util
+    {
+        bool checkZipFile(const std::filesystem::path& pathToZip)
+        {
+            bool ret = false;
+            zipFile z = NULL;
+            if (std::filesystem::exists(pathToZip))
+            {
+                if (std::filesystem::is_regular_file(pathToZip) && pathToZip.extension() == MINIZIP_SUFFIX)
+                {
+                    z = zipOpen64(pathToZip.string().c_str(), APPEND_STATUS_ADDINZIP);
+                    ret = (bool) z;
+                    zipClose(z, NULL);
+                }
+            }
+            return ret;
+        }
+
+        zipFile ensureZipFile(const std::filesystem::path& pathToZip, bool createIfNotExistent)
+        {
+            zipFile z = NULL;
+            if (!checkZipFile(pathToZip))
+            {
+                if (createIfNotExistent)
+                {
+                    z = zipOpen64(pathToZip.string().c_str(), APPEND_STATUS_CREATE);
+                }
+                else
+                {
+                    throw error::ArMemError("Could not find zip file: " + pathToZip.string());
+                }
+            }
+            else
+            {
+                z = zipOpen64(pathToZip.string().c_str(), APPEND_STATUS_ADDINZIP);
+            }
+
+            if (!z)
+            {
+                throw error::ArMemError("Unknown error occured during opening zip file: " + pathToZip.string());
+            }
+            return z;
+        }
+
+        unzFile getUnzFile(const std::filesystem::path& pathToZip)
+        {
+            unzFile z = NULL;
+            if (std::filesystem::exists(pathToZip))
+            {
+                if (std::filesystem::is_regular_file(pathToZip) && pathToZip.extension() == MINIZIP_SUFFIX)
+                {
+                    z = unzOpen64(pathToZip.string().c_str());
+                }
+                else
+                {
+                    throw error::ArMemError("Existing file is not a zip file: " + pathToZip.string());
+                }
+            }
+
+            // if z is NULL then the zip file might be empty
+
+            return z;
+        }
+
+        bool checkIfElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+        {
+            if (!checkZipFile(pathToZip)) return false;
+            auto uzf = getUnzFile(pathToZip);
+
+            bool ret = (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK);
+
+            unzClose(uzf);
+            return ret;
+        }
+
+        void ensureElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent)
+        {
+            auto zf = ensureZipFile(pathToZip, createIfNotExistent);
+            auto uzf = getUnzFile(pathToZip);
+
+            if (auto r = unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY); r != UNZ_OK)
+            {
+                if (createIfNotExistent)
+                {
+                    zip_fileinfo zfi;
+                    zipOpenNewFileInZip64(zf, p.string().c_str(), &zfi, NULL, 0, NULL, 0, NULL, Z_DEFLATED, Z_DEFAULT_COMPRESSION, 1);
+                    zipCloseFileInZip(zf);
+                }
+                else
+                {
+                    unzClose(uzf);
+                    zipClose(zf, NULL);
+                    throw error::ArMemError("Could not find element '" + p.string() + "' in zip file: " + pathToZip.string());
+                }
+            }
+            // else folder exists
+
+            unzClose(uzf);
+            zipClose(zf, NULL);
+        }
+
+        void ensureFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent)
+        {
+            if (!p.filename().empty())
+            {
+                throw error::ArMemError("The specified path is not a folder (it needs tailing /): " + p.string());
+            }
+
+            ensureElementInZipExists(pathToZip, p, createIfNotExistent);
+        }
+
+        void ensureFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+        {
+            if (p.filename().empty())
+            {
+                throw error::ArMemError("The specified path is not a file (it needs a filename): " + p.string());
+            }
+
+            ensureElementInZipExists(pathToZip, p, true);
+        }
+
+        bool checkIfFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+        {
+            if (!p.filename().empty())
+            {
+                throw error::ArMemError("The specified path is not a folder (it needs tailing /): " + p.string());
+            }
+
+            return checkIfElementInZipExists(pathToZip, p);
+        }
+
+        bool checkIfFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+        {
+            if (p.filename().empty())
+            {
+                throw error::ArMemError("The specified path is not a file (it needs a filename): " + p.string());
+            }
+
+            return checkIfElementInZipExists(pathToZip, p);
+        }
+
+        void writeDataInFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p, const std::vector<unsigned char>& data)
+        {
+            auto zf = ensureZipFile(pathToZip);
+
+            zip_fileinfo zfi;
+            if(zipOpenNewFileInZip64(zf, p.string().c_str(), &zfi, NULL, 0, NULL, 0, NULL, Z_DEFLATED, Z_DEFAULT_COMPRESSION, 1) == Z_OK)
+            {
+                if(zipWriteInFileInZip(zf, data.data(), data.size()) != Z_OK)
+                {
+                    throw error::ArMemError("Could not write a file '"+p.string()+"' to zip '" + pathToZip.string() + "'.");
+                }
+            }
+            else
+            {
+                throw error::ArMemError("Could not add a new file '"+p.string()+"' to zip '" + pathToZip.string() + "'.");
+            }
+            zipCloseFileInZip(zf);
+            zipClose(zf, NULL);
+        }
+
+        std::vector<unsigned char> readDataFromFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+        {
+            auto uzf = getUnzFile(pathToZip);
+            if (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK) // set current file
+            {
+                // File located
+                unz_file_info uzfi;
+                if (unzGetCurrentFileInfo(uzf, &uzfi, NULL, 0, NULL, 0, NULL, 0) == UNZ_OK)
+                {
+                    std::vector<unsigned char> data(uzfi.uncompressed_size);
+
+                    if (unzOpenCurrentFile(uzf) == UNZ_OK) // open current file
+                    {
+                        if (unzReadCurrentFile(uzf, data.data(), uzfi.uncompressed_size) == UNZ_OK) // read file in buffer
+                        {
+                            unzCloseCurrentFile(uzf);
+                            unzClose(uzf);
+                            return data;
+                        }
+                    }
+                }
+            }
+            unzClose(uzf);
+            throw error::ArMemError("Could not read data from zip file '" + pathToZip.string() + "' and internal path '" + p.string() + "'.");
+        }
+
+        std::vector<std::string> getAllDirectoriesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p)
+        {
+            std::vector<std::string> ret;
+
+            unzFile uzf = getUnzFile(pathToZip);
+            if (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK) // set current file
+            {
+                while(unzGoToNextFile(uzf) == UNZ_OK)
+                {
+                    unz_file_info uzfi;
+                    unzGetCurrentFileInfo(uzf, &uzfi, NULL, 0, NULL, 0, NULL, 0); // get file info
+                    std::vector<char> filenameVec(uzfi.size_filename);
+                    unzGetCurrentFileInfo(uzf, NULL, filenameVec.data(), uzfi.size_filename, NULL, 0, NULL, 0); // get file name
+                    std::string filename(filenameVec.begin(), filenameVec.end());
+
+                    auto pString = p.string();
+                    if (!simox::alg::starts_with(filename, pString))
+                    {
+                        // we moved out of the folder. Abort
+                        break;
+                    }
+
+                    std::string filenameWithoutPrefix = simox::alg::remove_prefix(filename, pString);
+
+                    if (!simox::alg::ends_with(filenameWithoutPrefix, "/"))
+                    {
+                        // this is not a directory
+                        continue;
+                    }
+
+                    if (simox::alg::count(filenameWithoutPrefix, "/") != 1)
+                    {
+                        // we are in a subfolder of the subfolder
+                        continue;
+                    }
+
+                    ret.push_back(filenameWithoutPrefix);
+                }
+            }
+            return ret;
+        }
+
+        std::vector<std::string> getAllFilesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p)
+        {
+            std::vector<std::string> ret;
+
+            unzFile uzf = getUnzFile(pathToZip);
+            if (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK) // set current file
+            {
+                while(unzGoToNextFile(uzf) == UNZ_OK)
+                {
+                    unz_file_info uzfi;
+                    unzGetCurrentFileInfo(uzf, &uzfi, NULL, 0, NULL, 0, NULL, 0); // get file info
+                    std::vector<char> filenameVec(uzfi.size_filename);
+                    unzGetCurrentFileInfo(uzf, NULL, filenameVec.data(), uzfi.size_filename, NULL, 0, NULL, 0); // get file name
+                    std::string filename(filenameVec.begin(), filenameVec.end());
+
+                    auto pString = p.string();
+                    if (!simox::alg::starts_with(filename, pString))
+                    {
+                        // we moved out of the folder. Abort
+                        break;
+                    }
+
+                    std::string filenameWithoutPrefix = simox::alg::remove_prefix(filename, pString);
+
+                    if (simox::alg::ends_with(filenameWithoutPrefix, "/"))
+                    {
+                        // this is a directory
+                        continue;
+                    }
+
+                    if (simox::alg::count(filenameWithoutPrefix, "/") != 1)
+                    {
+                        // we are in a subfolder of the subfolder
+                        continue;
+                    }
+
+                    ret.push_back(filenameWithoutPrefix);
+                }
+            }
+            return ret;
+        }
+    }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..99ebab93572afd50cccb194c1aa827635a6c585d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.h
@@ -0,0 +1,47 @@
+#pragma once
+
+// STD / STL
+#include <filesystem>
+#include <iostream>
+
+#include <minizip/zip.h>
+#include <minizip/unzip.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include "../../../../../core/error.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+    namespace minizip::util
+    {
+        const std::string MINIZIP_SUFFIX = ".zip";
+        const int MINIZIP_CASE_SENSITIVITY = 1;
+
+        bool checkZipFile(const std::filesystem::path& pathToZip);
+
+        zipFile ensureZipFile(const std::filesystem::path& pathToZip, bool createIfNotExistent = true);
+
+        unzFile getUnzFile(const std::filesystem::path& pathToZip);
+
+        bool checkIfElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+        void ensureElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent = true);
+
+        void ensureFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent = true);
+
+        void ensureFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+        bool checkIfFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+        bool checkIfFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+        void writeDataInFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p, const std::vector<unsigned char>& data);
+
+        std::vector<unsigned char> readDataFromFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+        std::vector<std::string> getAllDirectoriesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p);
+
+        std::vector<std::string> getAllFilesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p);
+    }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8828dc595166c90f7a7217e247428ba0792bf0c4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.cpp
@@ -0,0 +1,143 @@
+#include "util.h"
+
+#include <thread>
+#include <chrono>
+
+namespace armarx::armem::server::ltm::disk
+{
+    namespace util
+    {
+        // check whether a string is a number (e.g. timestamp folders)
+        bool isNumber(const std::string& s)
+        {
+            for (char const& ch : s)
+            {
+                if (std::isdigit(ch) == 0)
+                {
+                    return false;
+                }
+            }
+            return true;
+        }
+
+        bool checkIfBasePathExists(const std::filesystem::path& mPath)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::checkZipFile(mPath);
+            }
+            return filesystem::util::checkIfFolderInFilesystemExists(mPath);
+        }
+
+        void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                auto z = minizip::util::ensureZipFile(mPath, createIfNotExistent);
+                zipClose(z, NULL);
+                return;
+            }
+            return filesystem::util::ensureFolderInFilesystemExists(mPath, createIfNotExistent);
+        }
+
+        bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::checkIfFolderInZipExists(mPath, p);
+            }
+
+            return filesystem::util::checkIfFolderInFilesystemExists(mPath / p);
+        }
+
+        void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::ensureFolderInZipExists(mPath, p, createIfNotExistent);
+            }
+
+            return filesystem::util::ensureFolderInFilesystemExists(mPath / p, createIfNotExistent);
+        }
+
+        bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::checkIfFileInZipExists(mPath, p);
+            }
+
+            return filesystem::util::checkIfFileInFilesystemExists(mPath / p);
+        }
+
+        void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::ensureFileInZipExists(mPath, p);
+            }
+
+            return filesystem::util::ensureFileInFilesystemExists(mPath / p);
+        }
+
+        void writeDataToFile(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::writeDataInFileInZipFile(mPath, p, data);
+            }
+
+            return filesystem::util::writeDataInFilesystemFile(mPath / p, data);
+        }
+
+        void writeDataToFileRepeated(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data, const unsigned int maxTries, const unsigned int sleepTimeMs)
+        {
+            for (unsigned int i = 0; i < maxTries; ++i)
+            {
+                try
+                {
+                    writeDataToFile(mPath, p, data);
+                    return;
+                }
+                catch (const error::ArMemError&)
+                {
+                    // wait a bit to give the filesystem enough time to manage the workload
+                    std::this_thread::sleep_for(std::chrono::milliseconds(sleepTimeMs));
+                }
+            }
+
+            // even after all the tries we did not succeeded. This is very bad!
+            throw error::ArMemError("ATTENTION! Even after " + std::to_string(maxTries) + " tries, the memory was not able to store the instance at path '" + p.string() + "'. This means this instance will be lost!");
+        }
+
+        std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return minizip::util::readDataFromFileInZipFile(mPath, p);
+            }
+
+            return filesystem::util::readDataFromFilesystemFile(mPath / p);
+        }
+
+        std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return {};
+            }
+
+            return filesystem::util::getAllFilesystemDirectories(mPath / p);
+        }
+
+        std::vector<std::string> getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p)
+        {
+            if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+            {
+                return {};
+            }
+
+            return filesystem::util::getAllFilesystemFiles(mPath / p);
+        }
+    }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..e260d70923c7eb36bf0a78ab6e0c3a7d3bf98c52
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include "../../../../../core/error.h"
+
+#include "filesystem_util.h"
+#include "minizip_util.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+    namespace constantes
+    {
+        const std::string TYPE_FILENAME = "type.aron";
+        const std::string DATA_FILENAME = "data.aron";
+        const std::string METADATA_FILENAME = "metadata.aron";
+    }
+
+    namespace util
+    {
+        // check whether a string is a number (e.g. timestamp folders)
+        bool isNumber(const std::string& s);
+
+        bool checkIfBasePathExists(const std::filesystem::path& mPath);
+
+        void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent = true);
+
+        bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+        void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent = true);
+
+        bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+        void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+        void writeDataToFile(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data);
+
+        void writeDataToFileRepeated(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data, const unsigned int maxTries = 100, const unsigned int sleepTimeMs = 10);
+
+        std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+        std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+        std::vector<std::string> getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p);
+    }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 28d09f3e1713d1cee0873de5bc82195d7a62ae3b..4356aed6f4591eb5d33baaa86b0f9d4388f0a628 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -47,9 +47,6 @@ namespace armarx::armem::server::plugins
     {
         Component& parent = this->parent<Component>();
 
-        // init ltm stuff (such as propterty definition dependent filters)
-        longtermMemory.init();
-
         // register to MNS
         if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
         {
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
index d9df528842d11fd66ba38877318785d6c4dd5263..645b8aaa33fa6669135caeb49031e11ca2d8bb6c 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
@@ -7,6 +7,6 @@ namespace armarx::armem::server::query_proc::base
 {
     void detail::checkReferenceTimestampNonNegative(const Time& timestamp)
     {
-        ARMARX_CHECK_NONNEGATIVE(timestamp.toMicroSeconds()) << "Reference timestamp must be non-negative.";
+        ARMARX_CHECK_NONNEGATIVE(timestamp.toMicroSecondsSinceEpoch()) << "Reference timestamp must be non-negative.";
     }
 }
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 6179467a77fa12246df6dd65c6ba9a766c5ef31f..73a77eddd0d7efab69dc16f37e544049e743fc43 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -2,9 +2,12 @@
 
 #include "BaseQueryProcessorBase.h"
 
+#include <ArmarXCore/core/time/ice_conversions.h>
+
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
 
 #include <RobotAPI/interface/armem/query.h>
 
@@ -96,7 +99,7 @@ namespace armarx::armem::server::query_proc::base
                      const armem::query::data::entity::Single& query,
                      const EntityT& entity) const
         {
-            if (query.timestamp < 0)
+            if (query.timestamp.timeSinceEpoch.microSeconds < 0)
             {
                 if (auto snapshot = entity.findLatestSnapshot())
                 {
@@ -105,7 +108,7 @@ namespace armarx::armem::server::query_proc::base
             }
             else
             {
-                const Time time = fromIce<Time>(query.timestamp);
+                const Time time = armem::fromIce<Time>(query.timestamp);
                 if (auto snapshot = entity.findSnapshot(time))
                 {
                     this->addResultSnapshot(result, *snapshot);
@@ -134,10 +137,12 @@ namespace armarx::armem::server::query_proc::base
                      const armem::query::data::entity::TimeRange& query,
                      const EntityT& entity) const
         {
-            if (query.minTimestamp <= query.maxTimestamp || query.minTimestamp < 0 || query.maxTimestamp < 0)
+            if (query.minTimestamp.timeSinceEpoch.microSeconds <= query.maxTimestamp.timeSinceEpoch.microSeconds
+                || query.minTimestamp.timeSinceEpoch.microSeconds < 0
+                || query.maxTimestamp.timeSinceEpoch.microSeconds < 0)
             {
-                Time min = fromIce<Time>(query.minTimestamp);
-                Time max = fromIce<Time>(query.maxTimestamp);
+                const Time min = fromIce<Time>(query.minTimestamp);
+                const Time max = fromIce<Time>(query.maxTimestamp);
                 process(result, min, max, entity, query);
             }
         }
@@ -224,18 +229,11 @@ namespace armarx::armem::server::query_proc::base
             const Time referenceTimestamp = fromIce<Time>(query.timestamp);
             base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
 
-            const float referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
-            const float epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
-
             // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
             const auto isInRange = [&](const Time & t) -> bool
             {
-                if (epsDuration <= 0)
-                {
-                    return true;
-                }
-
-                return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
+                return query.eps.microSeconds <= 0
+                       or std::abs((t - referenceTimestamp).toMicroSeconds()) <= query.eps.microSeconds;
             };
 
             // last element before or at timestamp
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
index b8d25db27074f2033fd2791aa9aca787607d8da5..0a50a1c4fb7baec68c752895eb966d9f689f5778 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
@@ -1,5 +1,6 @@
 #include "wm.h"
 
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 
 namespace armarx::armem::server::query_proc::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
index b4965373da2d7c3443b5748e133e62607b1a198c..ecf13ddc3a54c6000ae7e9d6abcbfa32bec84057 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
@@ -68,9 +68,7 @@ namespace ArMemLTMBenchmark
 
         void storeElementNTimes(const std::string& memoryName, const aron::data::DictPtr& dict, int waitingTimeMs, int n)
         {
-            armem::server::ltm::disk::Memory ltm;
-            ltm.setPath(storagePath / memoryName);
-            ltm.setMemoryID(ltm.id().withMemoryName(memoryName));
+            armem::server::ltm::disk::Memory ltm(storagePath, memoryName);
 
             armem::wm::Memory wm(memoryName);
             auto& core = wm.addCoreSegment("CoreS");
@@ -82,7 +80,7 @@ namespace ArMemLTMBenchmark
                 std::cout << "Store instance " << i << " of memory " << memoryName << std::endl;
 
                 en.clear();
-                auto& snap = en.addSnapshot(IceUtil::Time::now());
+                auto& snap = en.addSnapshot(armem::Time::Now());
                 auto& ins = snap.addInstance();
                 auto cloned = aron::data::Dict::DynamicCastAndCheck(dict->clone());
                 ins.data() = cloned;
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
index 85096741e92f5ede902cdd55879545476cb3facd..7999a6a5752cd1bc9432f9caec95a6f928645a20 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
@@ -116,7 +116,7 @@ namespace ArMemLTMTest
                 }
                 update.entityID = armem::MemoryID::fromString(memoryName + "/TestCoreSegment/TestProvider/TestEntity");
                 update.instancesData = q;
-                update.timeCreated = armem::Time::now();
+                update.timeCreated = armem::Time::Now();
                 BOOST_CHECK_NO_THROW(providerSegment.update(update));
                 BOOST_CHECK_EQUAL(providerSegment.size(), 1);
             }
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
index 3ec604f35f2e586e4cc022a9248e479491b7ecad..8c3052e7d3abebf761431789865f4d0f46633cc7 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
@@ -45,7 +45,7 @@ namespace wm = armarx::armem::wm;
 BOOST_AUTO_TEST_CASE(test_time_to_string)
 {
     // 111111: seconds, 345: milliseconds, 789: microseconds
-    armem::Time time = armem::Time::microSeconds(111111345789);
+    armem::Time time { armem::Duration::MicroSeconds(111111345789) };
 
     BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time), "111111345.789 ms");
     BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time, 0), "111111345 ms");
@@ -61,7 +61,7 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
     BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 6), "1970-01-02 07:51:51.345789");
 
     // 111111: seconds, 000: milliseconds, 789: microseconds
-    time = armem::Time::microSeconds(111111000789);
+    time = armem::Time(armem::Duration::MicroSeconds(111111000789));
     BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time), "111111000.789 ms");
     BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time), "111111000789 " "\u03BC" "s");
     BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time), "1970-01-02 07:51:51.000789");
@@ -69,9 +69,8 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
     BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 3), "1970-01-02 07:51:51.000");
     BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 6), "1970-01-02 07:51:51.000789");
 
-
     // 111111: seconds, 345: milliseconds, 000: microseconds
-    time = armem::Time::microSeconds(111111345000);
+    time = armem::Time(armem::Duration::MicroSeconds(111111345000));
     BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time), "111111345.000 ms");
     BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time), "111111345000 " "\u03BC" "s");
     BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time), "1970-01-02 07:51:51.345000");
@@ -89,7 +88,7 @@ namespace ArMemMemoryTest
     struct APITestFixture
     {
         wm::EntityInstance instance { 0 };
-        wm::EntitySnapshot snapshot { armem::Time::microSeconds(1000) };
+        wm::EntitySnapshot snapshot { armem::Time(armem::Duration::MicroSeconds(1000)) };
         wm::Entity entity { "entity" };
         wm::ProviderSegment provSeg { "provider" };
         wm::CoreSegment coreSeg { "core" };
@@ -111,7 +110,7 @@ namespace ArMemMemoryTest
         {
             BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
             {
-                const armem::Time time = armem::Time::microSeconds(1000);
+                const armem::Time time = armem::Time(armem::Duration::MicroSeconds(1000));
                 BOOST_CHECK_EQUAL(added.time(), time);
                 BOOST_CHECK_EQUAL(parent.size(), 1);
                 BOOST_CHECK(parent.hasSnapshot(time));
@@ -174,7 +173,7 @@ BOOST_AUTO_TEST_CASE(test_add_instance_move)
 
 BOOST_AUTO_TEST_CASE(test_add_snapshot_time)
 {
-    test_add_snapshot(entity.addSnapshot(armem::Time::microSeconds(1000)), entity);
+    test_add_snapshot(entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), entity);
 }
 BOOST_AUTO_TEST_CASE(test_add_snapshot_copy)
 {
@@ -386,8 +385,8 @@ BOOST_AUTO_TEST_CASE(test_key_ctors)
     wm::EntityInstance instance(10);
     BOOST_CHECK_EQUAL(instance.index(), 10);
 
-    wm::EntitySnapshot snapshot(armem::Time::milliSeconds(100));
-    BOOST_CHECK_EQUAL(snapshot.time(), armem::Time::milliSeconds(100));
+    wm::EntitySnapshot snapshot(armem::Time(armem::Duration::MilliSeconds(100)));
+    BOOST_CHECK_EQUAL(snapshot.time(), armem::Time(armem::Duration::MilliSeconds(100)));
 
     wm::Entity entity("entity");
     BOOST_CHECK_EQUAL(entity.name(), "entity");
@@ -518,8 +517,8 @@ struct CustomChecks<armem::d_ltm::Memory>
 
 struct CopyMoveCtorsOpsTestBase
 {
-    const armem::MemoryID id {"M", "C", "P", "E", armem::Time::microSeconds(123000), 1};
-    //const armem::MemoryID idMoved {"", "", "", "", armem::Time::microSeconds(123000), 1};
+    const armem::MemoryID id {"M", "C", "P", "E", armem::Time(armem::Duration::MicroSeconds(123000)), 1};
+    //const armem::MemoryID idMoved {"", "", "", "", armem::Time(armem::Duration::MicroSeconds(123000), 1};
     armem::MemoryID idMoved = id;
 
     std::string typeName;
@@ -653,7 +652,7 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
         }
         {
             armem::EntityUpdate update;
-            update.entityID = armem::MemoryID("M", "C", "P", "E", armem::Time::microSeconds(123000), 0);
+            update.entityID = armem::MemoryID("M", "C", "P", "E", armem::Time(armem::Duration::MicroSeconds(123000)), 0);
             update.timeCreated = update.entityID.timestamp;
             update.instancesData.emplace_back();
             in.update(update);
@@ -815,7 +814,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
         std::make_shared<aron::data::Dict>(),
         std::make_shared<aron::data::Dict>()
     };
-    update.timeCreated = armem::Time::milliSeconds(1000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(1000));
     BOOST_CHECK_NO_THROW(providerSegment.update(update));
 
     BOOST_CHECK_EQUAL(providerSegment.size(), 1);
@@ -834,7 +833,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
     // Another update (on memory).
 
     update.instancesData = { std::make_shared<aron::data::Dict>() };
-    update.timeCreated = armem::Time::milliSeconds(2000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(2000));
     memory.update(update);
     BOOST_CHECK_EQUAL(entity.size(), 2);
     BOOST_CHECK(entity.hasSnapshot(update.timeCreated));
@@ -843,7 +842,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
 
     // A third update (on entity).
     update.instancesData = { std::make_shared<aron::data::Dict>() };
-    update.timeCreated = armem::Time::milliSeconds(3000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(3000));
     entity.update(update);
     BOOST_CHECK_EQUAL(entity.size(), 3);
 
@@ -859,38 +858,38 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
     update.entityID.entityName = entity.name();
 
     // With unlimited history.
-    update.timeCreated = armem::Time::milliSeconds(1000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(1000));
     entity.update(update);
-    update.timeCreated = armem::Time::milliSeconds(2000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(2000));
     entity.update(update);
-    update.timeCreated = armem::Time::milliSeconds(3000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(3000));
     entity.update(update);
     BOOST_CHECK_EQUAL(entity.size(), 3);
 
     // Now with maximum history size.
     entity.setMaxHistorySize(2);
     BOOST_CHECK_EQUAL(entity.size(), 2);
-    BOOST_CHECK(not entity.hasSnapshot(armem::Time::milliSeconds(1000)));
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(2000)));
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(3000)));
+    BOOST_CHECK(not entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(1000))));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(2000))));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(3000))));
 
 
-    update.timeCreated = armem::Time::milliSeconds(4000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(4000));
     entity.update(update);
     BOOST_CHECK_EQUAL(entity.size(), 2);
-    BOOST_CHECK(not entity.hasSnapshot(armem::Time::milliSeconds(2000)));
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(3000)));
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(4000)));
+    BOOST_CHECK(not entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(2000))));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(3000))));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(4000))));
 
     // Disable maximum history size.
     entity.setMaxHistorySize(-1);
 
-    update.timeCreated = armem::Time::milliSeconds(5000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(5000));
     entity.update(update);
     BOOST_CHECK_EQUAL(entity.size(), 3);
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(3000)));
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(4000)));
-    BOOST_CHECK(entity.hasSnapshot(armem::Time::milliSeconds(5000)));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(3000))));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(4000))));
+    BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(5000))));
 }
 
 
@@ -908,15 +907,15 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
     {
         update.entityID.entityName = name;
 
-        update.timeCreated = armem::Time::milliSeconds(1000);
+        update.timeCreated = armem::Time(armem::Duration::MilliSeconds(1000));
         providerSegment.update(update);
-        update.timeCreated = armem::Time::milliSeconds(2000);
+        update.timeCreated = armem::Time(armem::Duration::MilliSeconds(2000));
         providerSegment.update(update);
-        update.timeCreated = armem::Time::milliSeconds(3000);
+        update.timeCreated = armem::Time(armem::Duration::MilliSeconds(3000));
         providerSegment.update(update);
     }
     update.entityID.entityName = entityNames.back();
-    update.timeCreated = armem::Time::milliSeconds(4000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(4000));
     providerSegment.update(update);
 
     BOOST_CHECK_EQUAL(providerSegment.getEntity("A").size(), 3);
@@ -940,11 +939,11 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
     providerSegment.setMaxHistorySize(2);
 
     update.entityID.entityName = "C";
-    update.timeCreated = armem::Time::milliSeconds(1000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(1000));
     providerSegment.update(update);
-    update.timeCreated = armem::Time::milliSeconds(2000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(2000));
     providerSegment.update(update);
-    update.timeCreated = armem::Time::milliSeconds(3000);
+    update.timeCreated = armem::Time(armem::Duration::MilliSeconds(3000));
     providerSegment.update(update);
 
     // Check correctly inherited history size.
@@ -959,7 +958,7 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
     for (const std::string& name : entityNames)
     {
         update.entityID.entityName = name;
-        update.timeCreated = armem::Time::milliSeconds(5000);
+        update.timeCreated = armem::Time(armem::Duration::MilliSeconds(5000));
         providerSegment.update(update);
         BOOST_CHECK_EQUAL(providerSegment.getEntity(name).getMaxHistorySize(), -1);
         BOOST_CHECK_EQUAL(providerSegment.getEntity(name).size(), 3);
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp
index 191cfb6256257006a27bcd46bd25667e0d8b1bb8..c778cbc4fb09e2a590f7a4aa20085f19998e78f4 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp
@@ -65,15 +65,15 @@ namespace ArMemQueryTest
             entity = armem::wm::Entity("entity");
 
             armem::wm::EntitySnapshot snapshot;
-            snapshot.time() = armem::Time::microSeconds(1000);
+            snapshot.time() = armem::Time(armem::Duration::MicroSeconds(1000));
             entity.addSnapshot(snapshot);
-            snapshot.time() = armem::Time::microSeconds(2000);
+            snapshot.time() = armem::Time(armem::Duration::MicroSeconds(2000));
             entity.addSnapshot(snapshot);
-            snapshot.time() = armem::Time::microSeconds(3000);
+            snapshot.time() = armem::Time(armem::Duration::MicroSeconds(3000));
             entity.addSnapshot(snapshot);
-            snapshot.time() = armem::Time::microSeconds(4000);
+            snapshot.time() = armem::Time(armem::Duration::MicroSeconds(4000));
             entity.addSnapshot(snapshot);
-            snapshot.time() = armem::Time::microSeconds(5000);
+            snapshot.time() = armem::Time(armem::Duration::MicroSeconds(5000));
             entity.addSnapshot(snapshot);
         }
         ~Fixture()
@@ -125,30 +125,44 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
     {
         BOOST_CHECK_EQUAL(result.name(), entity.name());
         BOOST_CHECK_EQUAL(result.size(), 1);
-        const armem::wm::EntitySnapshot* first = result.findFirstSnapshotAfterOrAt(armem::Time::microSeconds(0));
+        const armem::wm::EntitySnapshot* first = result.findFirstSnapshotAfterOrAt(armem::Time(armem::Duration::MicroSeconds(0)));
         BOOST_REQUIRE_NE(first, nullptr);
-        BOOST_CHECK_EQUAL(first->time(), armem::Time::microSeconds(5000));
+        BOOST_CHECK_EQUAL(first->time(), armem::Time(armem::Duration::MicroSeconds(5000)));
     }
 }
 
+static armem::dto::Time
+t_usec(long usec)
+{
+    armem::dto::Time t;
+    t.timeSinceEpoch.microSeconds = usec;
+    return t;
+}
+static armem::dto::Duration
+d_usec(long usec)
+{
+    armem::dto::Duration d;
+    d.microSeconds = usec;
+    return d;
+}
 
 BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
 {
-    addResults(query::entity::Single({query::QueryTarget::WM, query::QueryTarget::LTM}, 3000));
+    addResults(query::entity::Single({query::QueryTarget::WM, query::QueryTarget::LTM}, t_usec(3000)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
     {
         BOOST_CHECK_EQUAL(result.name(), entity.name());
         BOOST_REQUIRE_EQUAL(result.size(), 1);
-        BOOST_CHECK_EQUAL(result.getFirstSnapshot().time(), armem::Time::microSeconds(3000));
+        BOOST_CHECK_EQUAL(result.getFirstSnapshot().time(), armem::Time(armem::Duration::MicroSeconds(3000)));
     }
 }
 
 
 BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
 {
-    addResults(query::entity::Single({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500));
+    addResults(query::entity::Single({query::QueryTarget::WM, query::QueryTarget::LTM}, t_usec(3500)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
@@ -183,7 +197,8 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
 {
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, 1500, 3500));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(1500), t_usec(3500)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
@@ -195,7 +210,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
         std::vector<armem::Time> times = result.getTimestamps();
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
+            armem::Time(armem::Duration::MicroSeconds(2000)), armem::Time(armem::Duration::MicroSeconds(3000))
         };
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
@@ -204,7 +219,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
 {
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, 2000, 4000));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(2000), t_usec(4000)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
@@ -216,7 +232,9 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
         std::vector<armem::Time> times = result.getTimestamps();
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+            armem::Time(armem::Duration::MicroSeconds(2000)),
+            armem::Time(armem::Duration::MicroSeconds(3000)),
+            armem::Time(armem::Duration::MicroSeconds(4000))
         };
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
@@ -224,14 +242,15 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
 {
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, 0, 10000));
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, -1, -1));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(0), t_usec(10000)));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(-1), t_usec(-1)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
     {
         BOOST_CHECK_EQUAL(result.name(), entity.name());
-
         BOOST_CHECK_EQUAL(result.size(), entity.size());
 
         const std::vector<armem::Time> times = result.getTimestamps();
@@ -243,8 +262,10 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
 {
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, 2400, 2600));
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, 6000, 1000));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(2400), t_usec(2600)));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(6000), t_usec(1000)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
@@ -258,7 +279,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
 {
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, -1, 2500));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(-1), t_usec(2500)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
@@ -270,7 +292,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
         const std::vector<armem::Time> times = result.getTimestamps();
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(1000), armem::Time::microSeconds(2000)
+            armem::Time(armem::Duration::MicroSeconds(1000)),
+            armem::Time(armem::Duration::MicroSeconds(2000))
         };
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
@@ -279,7 +302,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
 {
-    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM}, 2500, -1));
+    addResults(query::entity::TimeRange({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                        t_usec(2500), t_usec(-1)));
     BOOST_REQUIRE_GT(results.size(), 0);
 
     for (const armem::wm::Entity& result : results)
@@ -290,7 +314,9 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
         const std::vector<armem::Time> times = result.getTimestamps();
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(3000), armem::Time::microSeconds(4000), armem::Time::microSeconds(5000)
+            armem::Time(armem::Duration::MicroSeconds(3000)),
+            armem::Time(armem::Duration::MicroSeconds(4000)),
+            armem::Time(armem::Duration::MicroSeconds(5000))
         };
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
@@ -302,7 +328,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
 BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::BeforeTime({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500, 1));
+    addResults(query::entity::BeforeTime({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3500), 1));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -310,14 +337,15 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
         const std::vector<armem::Time> times = result.getTimestamps();
         BOOST_REQUIRE_EQUAL(times.size(), 1);
 
-        BOOST_CHECK_EQUAL(times.front(), armem::Time::microSeconds(3000));
+        BOOST_CHECK_EQUAL(times.front(), armem::Time(armem::Duration::MicroSeconds(3000)));
     }
 }
 
 BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::BeforeTime({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500, 2));
+    addResults(query::entity::BeforeTime({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3500), 2));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -327,7 +355,8 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
+            armem::Time(armem::Duration::MicroSeconds(2000)),
+            armem::Time(armem::Duration::MicroSeconds(3000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -342,7 +371,8 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
 BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::BeforeOrAtTime({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500));
+    addResults(query::entity::BeforeOrAtTime({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                             t_usec(3500)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -350,14 +380,15 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
         std::vector<armem::Time> times = result.getTimestamps();
         BOOST_REQUIRE_EQUAL(times.size(), 1);
 
-        BOOST_REQUIRE_EQUAL(times.front(),  armem::Time::microSeconds(3000));
+        BOOST_REQUIRE_EQUAL(times.front(), armem::Time(armem::Duration::MicroSeconds(3000)));
     }
 }
 
 BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::BeforeOrAtTime({query::QueryTarget::WM, query::QueryTarget::LTM}, 3000));
+    addResults(query::entity::BeforeOrAtTime({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                             t_usec(3000)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -365,14 +396,15 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
         std::vector<armem::Time> times = result.getTimestamps();
         BOOST_REQUIRE_EQUAL(times.size(), 1);
 
-        BOOST_REQUIRE_EQUAL(times.front(),  armem::Time::microSeconds(3000));
+        BOOST_REQUIRE_EQUAL(times.front(),  armem::Time(armem::Duration::MicroSeconds(3000)));
     }
 }
 
 BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::BeforeOrAtTime({query::QueryTarget::WM, query::QueryTarget::LTM}, 1));
+    addResults(query::entity::BeforeOrAtTime({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                             t_usec(1)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -392,7 +424,8 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500, -1));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3500), d_usec(-1)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -402,7 +435,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+            armem::Time(armem::Duration::MicroSeconds(3000)),
+            armem::Time(armem::Duration::MicroSeconds(4000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -418,7 +452,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500, 600));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3500), d_usec(600)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -428,7 +463,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+            armem::Time(armem::Duration::MicroSeconds(3000)),
+            armem::Time(armem::Duration::MicroSeconds(4000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -444,7 +480,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 3500, 100));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3500), d_usec(100)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -463,7 +500,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 3700, 400));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3700), d_usec(400)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -473,7 +511,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(4000)
+            armem::Time(armem::Duration::MicroSeconds(4000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -488,7 +526,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 3300, 400));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3300), d_usec(400)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -498,7 +537,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(3000)
+            armem::Time(armem::Duration::MicroSeconds(3000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -513,7 +552,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 3000, -1));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(3000), d_usec(-1)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -523,7 +563,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(3000)
+            armem::Time(armem::Duration::MicroSeconds(3000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -538,7 +578,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 1, 1));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(1), d_usec(1)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -556,7 +597,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 10'000, 1));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(10'000), d_usec(1)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -574,7 +616,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
 {
     BOOST_REQUIRE_EQUAL(entity.size(), 5);
-    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, 10'000, -1));
+    addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                         t_usec(10'000), d_usec(-1)));
     BOOST_REQUIRE_EQUAL(results.size(), 2);
 
     for (const auto& result : results)
@@ -584,7 +627,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
 
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(5'000)
+            armem::Time(armem::Duration::MicroSeconds(5'000))
         };
 
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
@@ -593,7 +636,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
 
 BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
 {
-    BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM}, -1, 1)), ::armarx::LocalException);
+    BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox({query::QueryTarget::WM, query::QueryTarget::LTM},
+                                                             t_usec(-1), d_usec(1))), ::armarx::LocalException);
 }
 
 
@@ -668,7 +712,9 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
         std::vector<armem::Time> times = result.getTimestamps();
         std::vector<armem::Time> expected
         {
-            armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+            armem::Time(armem::Duration::MicroSeconds(2000)),
+            armem::Time(armem::Duration::MicroSeconds(3000)),
+            armem::Time(armem::Duration::MicroSeconds(4000))
         };
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
diff --git a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
index 9c29804ebdc56444b175b3de0abd115b3a1067c0..8cb721c4b724e0c6f307a6ee6b796c01c974680a 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
@@ -70,7 +70,7 @@ BOOST_AUTO_TEST_CASE(test_forEach)
                 eids.insert(eid);
                 for (size_t s = 0; s <= e; ++s)
                 {
-                    const MemoryID sid = eid.withTimestamp(Time::microSeconds(int(s)));
+                    const MemoryID sid = eid.withTimestamp(Time(Duration::MicroSeconds(int(s))));
                     sids.insert(sid);
 
                     EntityUpdate update;
@@ -176,8 +176,8 @@ BOOST_AUTO_TEST_CASE(test_forEach_non_bool_func)
     // Check whether this compiles + runs without break.
 
     armem::wm::Entity entity;
-    entity.addSnapshot(armem::Time::microSeconds(500));
-    entity.addSnapshot(armem::Time::microSeconds(1500));
+    entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(500)));
+    entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(1500)));
 
     int i = 0;
     entity.forEachSnapshot([&i](const armem::wm::EntitySnapshot&) -> void
diff --git a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
index 29c12884d15292706ac2be33410352fe832dbc2a..f03b02052ef9956aab079dda96677acaf5aca48d 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
@@ -58,7 +58,7 @@ namespace ArMemGetFindTest
         Fixture()
         {
             {
-                snapshot.time() = armem::Time::microSeconds(1000);
+                snapshot.time() = armem::Time(armem::Duration::MicroSeconds(1000));
                 snapshot.addInstance();
             }
             {
@@ -123,14 +123,14 @@ namespace ArMemGetFindTest
         {
             BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
             {
-                BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time::microSeconds(1000))), true);
-                BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time::microSeconds(2000))), false);
+                BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))), true);
+                BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), false);
 
-                BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(armem::Time::microSeconds(1000))), nullptr);
-                BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(armem::Time::microSeconds(2000))), nullptr);
+                BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))), nullptr);
+                BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), nullptr);
 
-                BOOST_CHECK_NO_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time::microSeconds(1000))));
-                BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time::microSeconds(2000))), armem::error::MissingEntry);
+                BOOST_CHECK_NO_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))));
+                BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), armem::error::MissingEntry);
             }
         }
 
@@ -223,14 +223,14 @@ BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_key)
 
 BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_key)
 {
-    BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time::microSeconds(1000)), true);
-    BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time::microSeconds(2000)), false);
+    BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), true);
+    BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), false);
 
-    BOOST_CHECK_NE(entity.findSnapshot(armem::Time::microSeconds(1000)), nullptr);
-    BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time::microSeconds(2000)), nullptr);
+    BOOST_CHECK_NE(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), nullptr);
+    BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), nullptr);
 
-    BOOST_CHECK_NO_THROW(entity.getSnapshot(armem::Time::microSeconds(1000)));
-    BOOST_CHECK_THROW(entity.getSnapshot(armem::Time::microSeconds(2000)), armem::error::MissingEntry);
+    BOOST_CHECK_NO_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))));
+    BOOST_CHECK_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), armem::error::MissingEntry);
 }
 
 
diff --git a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
index 9fd00a4cf944f50cbd1c0d7bd06b9b56d812c161..07ea4e3acc307aa3d7abb97c7f9bd3f450a4129e 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
@@ -26,7 +26,7 @@
 
 #include <RobotAPI/Test.h>
 #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
 
 #include <iostream>
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
@@ -43,11 +43,11 @@ BOOST_AUTO_TEST_CASE(test_entity)
     std::vector<armem::Time> expectedTimestamps;
 
     armem::wm::EntitySnapshot snapshot;
-    snapshot.time() = armem::Time::milliSeconds(100);
+    snapshot.time() = armem::Time(armem::Duration::MilliSeconds(100));
     expectedTimestamps.push_back(snapshot.time());
     entity.addSnapshot(snapshot);
 
-    snapshot.time() = armem::Time::milliSeconds(300);
+    snapshot.time() = armem::Time(armem::Duration::MilliSeconds(300));
     expectedTimestamps.push_back(snapshot.time());
     entity.addSnapshot(snapshot);
 
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
index 72f09fb225aa22b50ed55a9482e7f973acbf0fd2..987cf20c980e0478056dd529d4ad877de2499ba4 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
@@ -101,7 +101,7 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
     BOOST_CHECK_EQUAL(idIn.coreSegmentName, "Core");
     BOOST_CHECK_EQUAL(idIn.providerSegmentName, "Prov");
     BOOST_CHECK_EQUAL(idIn.entityName, "entity");
-    BOOST_CHECK_EQUAL(idIn.timestamp, IceUtil::Time::microSeconds(2810381));
+    BOOST_CHECK_EQUAL(idIn.timestamp, armem::Time(armem::Duration::MicroSeconds(2810381)));
     BOOST_CHECK_EQUAL(idIn.instanceIndex, 2);
 
 
@@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
     BOOST_CHECK_EQUAL(idIn.coreSegmentName, "AtTheEnd/");
     BOOST_CHECK_EQUAL(idIn.providerSegmentName, "/AtTheStart");
     BOOST_CHECK_EQUAL(idIn.entityName, "YCB/sugar/-1");
-    BOOST_CHECK_EQUAL(idIn.timestamp, IceUtil::Time::microSeconds(-1));
+    BOOST_CHECK_EQUAL(idIn.timestamp, armem::Time::Invalid());
     BOOST_CHECK_EQUAL(idIn.instanceIndex, 2);
 
     BOOST_TEST_CONTEXT(VAROUT(idIn.str()))
@@ -134,7 +134,6 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
         armem::MemoryID idOut(idIn.str());
         BOOST_CHECK_EQUAL(idOut, idIn);
     }
-
 }
 
 
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
index 78d3077c082e41f32bf6dfd398e9e7d474e19dc3..535ce2b80b6b9b5ac7c95fcf3f9847ab943d1729 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
@@ -25,10 +25,12 @@
 #define ARMARX_BOOST_TEST
 
 #include <RobotAPI/Test.h>
+
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
 
 #include <iostream>
 
@@ -97,9 +99,10 @@ BOOST_AUTO_TEST_CASE(test_mixed)
     .entities(withName(entityNames.at(0)),
               withName(entityNames.at(1)))
     .snapshots(latest(),
-               atTime(armem::Time::milliSeconds(3000)),
+               atTime(armem::Time(armem::Duration::MilliSeconds(3000))),
                indexRange(5, -10),
-               timeRange(armem::Time::milliSeconds(1000), armem::Time::milliSeconds(2000)))
+               timeRange(armem::Time(armem::Duration::MilliSeconds(1000)),
+                         armem::Time(armem::Duration::MilliSeconds(2000))))
     ;
 
 
@@ -138,13 +141,13 @@ BOOST_AUTO_TEST_CASE(test_mixed)
         {
             auto latest = query::entity::SinglePtr::dynamicCast(*it);
             BOOST_REQUIRE(latest);
-            BOOST_CHECK_EQUAL(latest->timestamp, -1);
+            BOOST_CHECK_EQUAL(latest->timestamp.timeSinceEpoch.microSeconds, armem::Time::Invalid().toMicroSecondsSinceEpoch());
             ++it;
         }
         {
             auto atTime = query::entity::SinglePtr::dynamicCast(*it);
             BOOST_REQUIRE(atTime);
-            BOOST_CHECK_EQUAL(atTime->timestamp, armem::toIce<long>(armem::Time::milliSeconds(3000)));
+            BOOST_CHECK_EQUAL(atTime->timestamp, armem::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(3000))));
             ++it;
         }
         {
@@ -157,12 +160,11 @@ BOOST_AUTO_TEST_CASE(test_mixed)
         {
             auto timeRange = query::entity::TimeRangePtr::dynamicCast(*it);
             BOOST_REQUIRE(timeRange);
-            BOOST_CHECK_EQUAL(timeRange->minTimestamp, armem::toIce<long>(armem::Time::milliSeconds(1000)));
-            BOOST_CHECK_EQUAL(timeRange->maxTimestamp, armem::toIce<long>(armem::Time::milliSeconds(2000)));
+            BOOST_CHECK_EQUAL(timeRange->minTimestamp, armem::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(1000))));
+            BOOST_CHECK_EQUAL(timeRange->maxTimestamp, armem::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(2000))));
             ++it;
         }
     }
-
 }
 
 
@@ -187,6 +189,7 @@ BOOST_AUTO_TEST_CASE(test_branched_hierarchy)
     .snapshots().latest();
 
 
+    BOOST_TEST_MESSAGE(std::numeric_limits<long>::min());
     armem::client::QueryInput input = qb.buildQueryInput();
     const query::ProviderSegmentQuerySeq& provQueries = input.memoryQueries.front()->coreSegmentQueries.front()->providerSegmentQueries;
 
@@ -211,7 +214,7 @@ BOOST_AUTO_TEST_CASE(test_branched_hierarchy)
         BOOST_CHECK_EQUAL(regex->entityQueries.size(), 1);
         auto snapshotLatest = query::entity::SinglePtr::dynamicCast(regex->entityQueries.at(0));
         BOOST_CHECK(snapshotLatest);
-        BOOST_CHECK_EQUAL(snapshotLatest->timestamp, -1);
+        BOOST_CHECK_EQUAL(snapshotLatest->timestamp.timeSinceEpoch.microSeconds, armem::Time::Invalid().toMicroSecondsSinceEpoch());
 
         ++it;
     }
diff --git a/source/RobotAPI/libraries/armem/util/util.cpp b/source/RobotAPI/libraries/armem/util/util.cpp
index 34f0e2f56d9d47ea7db8b54e198e6faa9d0b25d9..fcaab64e05b6eb35e9c516a9637f77a97b210952 100644
--- a/source/RobotAPI/libraries/armem/util/util.cpp
+++ b/source/RobotAPI/libraries/armem/util/util.cpp
@@ -1,3 +1,68 @@
 #include "util.h"
 
-// intentionally left blank
\ No newline at end of file
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+
+namespace armarx::armem
+{
+
+    std::optional<armarx::armem::wm::Memory>
+    resolveID(armarx::armem::client::MemoryNameSystem& mns, const armarx::armem::MemoryID& memoryID)
+    {
+        armarx::armem::client::Reader reader;
+        try
+        {
+            reader = mns.getReader(memoryID);
+        }
+        catch (const armarx::armem::error::CouldNotResolveMemoryServer& e)
+        {
+            ARMARX_WARNING << armarx::armem::error::CouldNotResolveMemoryServer::makeMsg(memoryID);
+            return {};
+        }
+
+        const armarx::armem::ClientQueryResult result = reader.queryMemoryIDs({memoryID});
+        if (result.success)
+        {
+            return result.memory;
+        }
+        ARMARX_WARNING << "Could not query memory for ID " << memoryID << ", "
+                       << result.errorMessage;
+        return {};
+    }
+
+
+    std::optional<std::pair<armarx::aron::data::DictPtr, armarx::aron::type::ObjectPtr>>
+    extractDataAndType(const armarx::armem::wm::Memory& memory,
+                       const armarx::armem::MemoryID& memoryID)
+    {
+        const auto* instance = memory.findLatestInstance(memoryID);
+        if (instance == nullptr)
+        {
+            return {};
+        }
+
+        armarx::aron::data::DictPtr aronData = instance->data();
+        armarx::aron::type::ObjectPtr aronType;
+        const auto* providerSegment = memory.findProviderSegment(memoryID);
+        if (providerSegment == nullptr)
+        {
+            return {};
+        }
+
+        if (!providerSegment->hasAronType())
+        {
+            const auto* coreSegment = memory.findCoreSegment(memoryID);
+            if (coreSegment == nullptr || !coreSegment->hasAronType())
+            {
+                return {};
+            }
+            aronType = coreSegment->aronType();
+        }
+        else
+        {
+            aronType = providerSegment->aronType();
+        }
+
+        return {{aronData, aronType}};
+    }
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index a337bfafb784725dbfdd7cef9e158361a8911c97..7ffdfbfdc4f874ce264d9a61e2ffb030291827d9 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -26,6 +26,7 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 #include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronGeneratedClass.h>
 
@@ -165,4 +166,37 @@ namespace armarx::armem
         return outV;
     }
 
+    /**
+     * @brief resolve a single MemoryID with the given MemoryNameSystem.
+     *
+     * Returns a Memory containing only the desired segment / entity if it was
+     * successfully queried. If the query was unsuccessful (the MNS could not
+     * resolve the memory server, the MemoryID does not exist, etc.),
+     * nothing is returned.
+     *
+     * @param mns the MemoryNameSystem to use for the query
+     * @param memoryID the MemoryID to query for
+     * @return memory containing the object referenced by the MemoryID if available
+     */
+    std::optional<armarx::armem::wm::Memory>
+    resolveID(armarx::armem::client::MemoryNameSystem& mns,
+              const armarx::armem::MemoryID& memoryID);
+
+
+    /**
+     * @brief get the data and type of the given MemoryID in the given Memory.
+     *
+     * Returns the data and the type of the latest instance corresponding to
+     * the given MemoryID (whatever `memory.findLatestInstance(memoryID)` returns).
+     * If no such instance exists in the memory or no type for the instance is available,
+     * nothing is returned.
+     *
+     * @param memory the Memory to extract the data and type from
+     * @param memoryID the MemoryID of the instance to extract
+     * @return pair of data and type for the instance if available
+     */
+    std::optional<std::pair<armarx::aron::data::DictPtr, armarx::aron::type::ObjectPtr>>
+    extractDataAndType(const armarx::armem::wm::Memory& memory,
+                       const armarx::armem::MemoryID& memoryID);
+
 } // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index e66a190bdf75e652c6505d91f7aae4fe9ae9649a..f1ad93a2c87396e2f178d2ed522d39dcd7de95b9 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -110,7 +110,7 @@ namespace armarx::armem::grasping::segment
     {
         // load data from prior knowledge
         ObjectFinder objectFinder;
-        const auto now = armem::Time::now();
+        const auto now = armem::Time::Now();
 
         const bool checkPaths = false;
         std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 318329d8eb16a348338132ad276e819e631b1cdb..36b2e7036df83d0b99731d6773eb4465e7b75997 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -224,7 +224,7 @@ namespace armarx::armem::gui
         for (auto& [name, reader] : memoryReaders)
         {
             data::StoreInput input;
-            input.query = memoryGroup->queryWidget()->queryInput().toIce();
+            input.query = memoryGroup->queryInput().toIce();
             reader.readAndStore(input);
         }
 
@@ -234,7 +234,7 @@ namespace armarx::armem::gui
     void MemoryViewer::commit()
     {
         TIMING_START(Commit);
-        auto now = armem::Time::now();
+        auto now = armem::Time::Now();
 
         const std::string memoryIDStr = memoryGroup->commitWidget()->getMemoryID();
         const std::string aronJSONStr = memoryGroup->commitWidget()->getAronJSON();
@@ -294,7 +294,7 @@ namespace armarx::armem::gui
     {
         std::string status;
         std::map<std::string, wm::Memory> data =
-            diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
+            diskControl->loadFromDisk(directory, memoryGroup->queryInput(), &status);
 
         memoryWriters = mns.getAllWriters(true);
         for (auto& [name, memory] : data)
@@ -357,24 +357,41 @@ namespace armarx::armem::gui
 
 
     void
-    MemoryViewer::startDueQueries(std::map<std::string, Ice::AsyncResultPtr>& queries,
-                                  const std::map<std::string, armem::client::Reader>& readers)
+    MemoryViewer::startDueQueries(
+        std::map<std::string, std::future<armem::query::data::Result>>& queries,
+        const std::map<std::string, armem::client::Reader>& readers)
     {
-        armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
+        armem::client::QueryInput input = memoryGroup->queryInput();
+        int recursionDepth = memoryGroup->queryWidget()->queryLinkRecursionDepth();
 
-        for (auto& [name, reader] : readers)
+        // Can't use a structured binding here because you can't capture those in a lambda
+        // according to the C++ standard.
+        for (const auto& pair : readers)
         {
+            const auto& name = pair.first;
+            const auto& reader = pair.second;
             if (queries.count(name) == 0)
             {
-                queries[name] = reader.memoryPrx->begin_query(input.toIce());
+                // You could pass the query function itself to async here,
+                // but that caused severe template headaches when I tried it.
+                queries[name] =
+                    std::async(std::launch::async,
+                               [&reader, input, recursionDepth, this]()
+                               {
+                                   // Can't resolve MemoryLinks without data
+                                   return recursionDepth == 0 || input.dataMode == DataMode::NoData
+                                              ? reader.query(input.toIce())
+                                              : reader.query(input.toIce(), mns, recursionDepth);
+                               });
             }
         }
     }
 
 
     std::map<std::string, client::QueryResult>
-    MemoryViewer::collectQueryResults(std::map<std::string, Ice::AsyncResultPtr>& queries,
-                                      const std::map<std::string, client::Reader>& readers)
+    MemoryViewer::collectQueryResults(
+        std::map<std::string, std::future<armem::query::data::Result>>& queries,
+        const std::map<std::string, client::Reader>& readers)
     {
         TIMING_START(tCollectQueryResults)
 
@@ -382,26 +399,25 @@ namespace armarx::armem::gui
         for (auto it = queries.begin(); it != queries.end();)
         {
             const std::string& name = it->first;
-            Ice::AsyncResultPtr& queryPromise = it->second;
+            std::future<armem::query::data::Result>* queryPromise = &it->second;
 
-            if (queryPromise->isCompleted())
+            if (queryPromise->wait_for(std::chrono::seconds(0)) == std::future_status::ready)
             {
                 if (auto jt = memoryReaders.find(name); jt != readers.end())
                 {
                     try
                     {
-                        results[name] = client::QueryResult::fromIce(
-                            jt->second.memoryPrx->end_query(queryPromise));
+                        results[name] = client::QueryResult::fromIce(queryPromise->get());
                     }
                     catch (const Ice::ConnectionRefusedException&)
                     {
-                        // Server is gone (MNS did not now about it yet) => Skip result.
+                        // Server is gone (MNS did not know about it yet) => Skip result.
                     }
                 }
-                // else: Server is gone (MNS new about it) => Skip result.
+                // else: Server is gone (MNS knew about it) => Skip result.
 
                 // Promise is completed => Clean up in any case.
-                it = runningQueries.erase(it);
+                it = queries.erase(it);
             }
             else
             {
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 51cda4cb797911c3afcd1afaf43657b145b9a140..9b0d2a8fff364cd2f702c11a1769243b435ac0ce 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <future>
 #include <string>
 
 #include <QObject>
@@ -120,12 +121,12 @@ namespace armarx::armem::gui
 
         /// Start a query for each entry in `memoryReaders` which is not in `runningQueries`.
         void startDueQueries(
-            std::map<std::string, Ice::AsyncResultPtr>& queries,
+            std::map<std::string, std::future<armem::query::data::Result>>& queries,
             const std::map<std::string, armem::client::Reader>& readers);
 
         std::map<std::string, client::QueryResult>
         collectQueryResults(
-            std::map<std::string, Ice::AsyncResultPtr>& queries,
+            std::map<std::string, std::future<armem::query::data::Result>>& queries,
             const std::map<std::string, armem::client::Reader>& readers);
 
         void applyQueryResults(
@@ -142,7 +143,7 @@ namespace armarx::armem::gui
         std::map<std::string, armem::client::Writer> memoryWriters;
         std::map<std::string, armem::wm::Memory> memoryData;
 
-        std::map<std::string, Ice::AsyncResultPtr> runningQueries;
+        std::map<std::string, std::future<armem::query::data::Result>> runningQueries;
         /// Periodically triggers query result collection.
         QTimer* processQueryResultTimer;
 
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index 215ee34da41b1c26f9ad4dbf9f901f7a4dfcf367..d8755bbfd2e640842b4342c55330a761ce8a362d 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -100,10 +100,7 @@ namespace armarx::armem::gui::disk
                 }
                 else
                 {
-                    std::filesystem::create_directories(path / name);
-
-                    armem::server::ltm::disk::Memory memory((path / name));
-                    memory.init();
+                    armem::server::ltm::disk::Memory memory(path, name);
                     memory.directlyStore(data);
 
                     numStored++;
@@ -147,9 +144,9 @@ namespace armarx::armem::gui::disk
         bool isSingleMemory = false;
         for (auto i = std::filesystem::recursive_directory_iterator(path); i != std::filesystem::recursive_directory_iterator(); ++i)
         {
-            if (i.depth() > 7)
+            if (i.depth() > armem::server::ltm::disk::Memory::DEPTH_TO_DATA_FILES + 2)
             {
-                // After depth 7 we stop searching to not freeze GUI too long
+                // After some depth we stop searching to not freeze GUI too long
                 setStatus("Could not import a memory from " + path.string() + ". Data files were not found until max-depth 7. Skipping import.");
                 return memoryData;
             }
@@ -159,7 +156,7 @@ namespace armarx::armem::gui::disk
             // if one matches it is enough to check
             if (std::filesystem::is_regular_file(dir.path()) && simox::alg::starts_with(dir.path().filename(), "data.aron"))
             {
-                isSingleMemory = (i.depth() == 5);
+                isSingleMemory = (i.depth() == armem::server::ltm::disk::Memory::DEPTH_TO_DATA_FILES);
                 break;
             }
         }
@@ -171,12 +168,10 @@ namespace armarx::armem::gui::disk
         // const query::data::Input queryIce = queryInput.toIce();
 
         int numLoaded = 0;
-        auto loadMemory = [&](const std::filesystem::path& p){
+        auto loadMemory = [&](const std::filesystem::path& p) {
             if (std::filesystem::is_directory(p))
             {
-                armem::server::ltm::disk::Memory ltm(p);
-                ltm.init();
-
+                armem::server::ltm::disk::Memory ltm(p.parent_path(), p.filename());
                 armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references
                 memoryData[memory.name()] = std::move(memory);
 
@@ -190,7 +185,7 @@ namespace armarx::armem::gui::disk
         }
         else
         {
-            // we load multiple memories
+            // we have to load multiple memories (each subfolder)
             for (const auto& dir : std::filesystem::directory_iterator(path))
             {
                 loadMemory(dir.path());
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 708ba87395db5d8e696cf9dbaa4ee174eb23c61b..61918cb20354d5a24a8ab0e8d1636a437966f2b9 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -696,7 +696,10 @@ namespace armarx::armem::gui::instance
         try
         {
             // TODO We cannot know what the str in the pixeltype belongs to (e.g. coming from java, python, c++ it may contain different values!
-            //pixelType = aron::type::Image::pixelTypeFromName(imageData->getType());
+            // pixelType = aron::type::Image::pixelTypeFromName(imageData->getType());
+
+            // For now we assume it comes from c++ where '5' means CV_32FC1 (=5)
+            pixelType = (imageData->getType() == "5" ? PixelType::depth32 : PixelType::rgb24);
         }
         catch (const aron::error::AronException&)
         {
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
index ebb4473bb1941a713526d646b37744f5a3314c1c..5cde16500c5e1dbd05f9f376ccf84f3bd2504f39 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
@@ -36,7 +36,7 @@ namespace armarx::armem::gui::instance
             static const char* mu = "\u03BC";
             std::stringstream ss;
             ss << toDateTimeMilliSeconds(id.timestamp)
-               << " (" << id.timestamp.toMicroSeconds() << " " << mu << "s)";
+               << " (" << id.timestamp.toMicroSecondsSinceEpoch() << " " << mu << "s)";
             child(4)->setText(valueColumn, QString::fromStdString(ss.str()));
         }
     }
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
index 6a132629b65031c1b27225528872bcb4c20cd611..eed9bb9ef66902f3ed15a5ad238bdc24c01b2595 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -87,7 +87,7 @@ namespace armarx::aron
     void TypedDataDisplayVisitor::visitTime(const data::VariantPtr& data, const type::VariantPtr& type)
     {
         auto l = data::Long::DynamicCastAndCheck(data);
-        armem::Time time = armem::Time::microSeconds(l->getValue());
+        armem::Time time { armem::Duration::MicroSeconds(l->getValue()) };
         value << armem::toDateTimeMilliSeconds(time);
     }
 
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
index 5ed578979d2516f3c089685c1bf5bc44843ad157..27cb691cf870d1663305cb25a60f2541dc29e7c6 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -18,7 +18,7 @@ namespace armarx::armem::gui::instance
         builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
         {
             auto child = data->getElement(key);
-            this->update(item, key, child);
+            this->update(item, key, child, data->getPath());
             return true;
         });
 
@@ -27,20 +27,24 @@ namespace armarx::armem::gui::instance
 
     void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::ListPtr& data)
     {
-        auto children = data->getChildren();
 
         ListBuilder builder = getListBuilder();
-        builder.setUpdateItemFn([this, &children](size_t key, QTreeWidgetItem * item)
+        builder.setUpdateItemFn([this, &data](size_t key, QTreeWidgetItem * item)
         {
-            this->update(item, std::to_string(key), children.at(key));
+            auto child = data->getElement(key);
+            this->update(item, std::to_string(key), child, data->getPath());
             return true;
         });
 
-        builder.updateTreeWithContainer(parent, getIndex(children.size()));
+        builder.updateTreeWithContainer(parent, getIndex(data->childrenSize()));
     }
 
 
-    void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data)
+    void
+    DataTreeBuilder::update(QTreeWidgetItem* item,
+                            const std::string& key,
+                            const aron::data::VariantPtr& data,
+                            const aron::Path& parentPath)
     {
         if (data)
         {
@@ -59,8 +63,9 @@ namespace armarx::armem::gui::instance
         {
             // Optional data?
             this->setRowTexts(item, key, "(none)");
+            auto empty = std::make_shared<aron::data::Dict>(parentPath.withElement(key));
+            updateTree(item, empty);
         }
-    }
-
 
+    }
 }
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
index 4ae61c3036195e08055582d5e7251857a30ca650..80773c86aa7074b88b962fa2ae44c7b4f54e65f1 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -22,9 +22,9 @@ namespace armarx::armem::gui::instance
 
 
     protected:
-
-        void update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data);
-
+        void update(QTreeWidgetItem* item,
+                    const std::string& key,
+                    const aron::data::VariantPtr& data,
+                    const aron::Path& parentPath);
     };
-
 }
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index de85c11c08080c88c5f5d72f5b00900547a66fbc..70edd5a5149617618e8ab023a8ccb48f4a629dee 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -213,31 +213,31 @@ namespace armarx::armem::gui::instance
             }
         }
 
-        if (data)
+        // We pass empty containers if data is null so that subitems of the data are deleted.
+        auto emptyDict = aron::data::Dict(type->getPath());
+        auto emptyList = aron::data::List(type->getPath());
+        if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type))
         {
-            if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type))
-            {
-                _updateTree(item, *t, *d);
-            }
-            else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type))
-            {
-                _updateTree(item, *t, *d);
-            }
-            else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type))
-            {
-                _updateTree(item, *t, *d);
-            }
-            else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type))
-            {
-                _updateTree(item, *t, *d);
-            }
-            else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type))
-            {
-                _updateTree(item, *t, *d);
-            }
-            // else???
-            // phesch: else we stop here, since there's no container to recurse into.
+            _updateTree(item, *t, d ? *d : emptyDict);
+        }
+        else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type))
+        {
+            _updateTree(item, *t, d ? *d : emptyDict);
+        }
+        else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type))
+        {
+            _updateTree(item, *t, d ? *d : emptyList);
+        }
+        else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type))
+        {
+            _updateTree(item, *t, d ? *d : emptyList);
+        }
+        else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type))
+        {
+            _updateTree(item, *t, d ? *d : emptyList);
         }
+        // else???
+        // phesch: else we stop here, since there's no container to recurse into.
     }
 
     
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
index 3a3a3b4d6f007a92a009df9d59ddd5ad65d9c070..bc8c27104f78f11547ecc72c6dbc93cded31893b 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
@@ -24,7 +24,7 @@ namespace armarx::armem::gui::instance
     void TreeTypedDataVisitor::streamValueText(const aron::data::Long& data, const aron::type::Time& type, std::stringstream& ss) const
     {
         (void) type;
-        armem::Time time = armem::Time::microSeconds(data.getValue());
+        armem::Time time { armem::Duration::MicroSeconds(data.getValue()) };
         armem::toDateTimeMilliSeconds(time);
         ss << armem::toDateTimeMilliSeconds(time);
     }
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
index f80717342b3bc98dc1ebf9b7a72a4b274ede768a..950f034f8596fe7f3f6034ee675f3f45ffc5f08c 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
@@ -275,7 +275,7 @@ namespace armarx::armem::gui::instance
     {
         const std::string key = elementData->getPath().getLastElement();
         int64_t l = *aron::data::Long::DynamicCastAndCheck(elementData);
-        armem::Time time = armem::Time::microSeconds(l);
+        armem::Time time { armem::Duration::MicroSeconds(l) };
         insertIntoJSON(key, l);
         if (!jsonStack.top().second.is_array())
         {
diff --git a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp
index 27af95f6223db049ff9f26fd41632ac2d7850173..8796557cdcfa2d8f337166cb6181bab6f92111c5 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp
@@ -39,7 +39,13 @@ namespace armarx::armem::gui::memory
             _queryWidget = new armem::gui::QueryWidget();
             _queryWidget->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
 
-            _memoryTabWidget->addTab(_queryWidget, QString("Query"));
+            _memoryTabWidget->addTab(_queryWidget, QString("Query Settings"));
+        }
+        {
+            _snapshotSelectorWidget = new armem::gui::SnapshotSelectorWidget();
+            _snapshotSelectorWidget->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
+
+            _memoryTabWidget->addTab(_snapshotSelectorWidget, QString("Snapshot Selection"));
         }
         {
             _commitWidget = new armem::gui::CommitWidget();
@@ -67,6 +73,11 @@ namespace armarx::armem::gui::memory
         return _memoryTabGroup;
     }
 
+    SnapshotSelectorWidget* GroupBox::snapshotSelectorWidget() const
+    {
+        return _snapshotSelectorWidget;
+    }
+
     CommitWidget* GroupBox::commitWidget() const
     {
         return _commitWidget;
@@ -77,4 +88,16 @@ namespace armarx::armem::gui::memory
         return _tree;
     }
 
-}
+    armem::client::QueryInput GroupBox::queryInput() const
+    {
+        armem::client::query::Builder queryBuilder(_queryWidget->dataMode());
+        queryBuilder.queryTargets(_snapshotSelectorWidget->queryTargets())
+        .coreSegments().all().queryTargets(_snapshotSelectorWidget->queryTargets())
+        .providerSegments().all().queryTargets(_snapshotSelectorWidget->queryTargets())
+        .entities().all().queryTargets(_snapshotSelectorWidget->queryTargets())
+        .snapshots(_snapshotSelectorWidget->selector());
+
+        return queryBuilder.buildQueryInput();
+    }
+
+}  // namespace armarx::armem::gui::memory
diff --git a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h
index 562432d11bb4db79873dd97d200950127bf2e1db..10cf3d5972cc607cfb89fb9b2b0e1962027c9a75 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h
@@ -26,8 +26,11 @@ namespace armarx::armem::gui::memory
         TreeWidget* tree() const;
         QGroupBox* queryGroup() const;
         QueryWidget* queryWidget() const;
+        SnapshotSelectorWidget* snapshotSelectorWidget() const;
         CommitWidget* commitWidget() const;
 
+        armem::client::QueryInput queryInput() const;
+
 
     public slots:
 
@@ -45,6 +48,7 @@ namespace armarx::armem::gui::memory
         QTabWidget* _memoryTabWidget;
         QGroupBox* _memoryTabGroup;
         QueryWidget* _queryWidget;
+        SnapshotSelectorWidget* _snapshotSelectorWidget;
         CommitWidget* _commitWidget;
 
     };
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 858c932bdba6825696a99de538bbdf02284c159c..f5ec0c9139e8db63acdb2ad006b15ec4ab1e32cb 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -116,12 +116,12 @@ namespace armarx::armem::gui::memory
         snapshotBuilder.setMakeItemFn([this](const wm::EntitySnapshot & snapshot)
         {
             QTreeWidgetItem* item = makeItem(toDateTimeMilliSeconds(snapshot.time()), snapshot);
-            item->setData(int(Columns::KEY), Qt::ItemDataRole::UserRole, QVariant(static_cast<qlonglong>(snapshot.time().toMicroSeconds())));
+            item->setData(int(Columns::KEY), Qt::ItemDataRole::UserRole, QVariant(static_cast<qlonglong>(snapshot.time().toMicroSecondsSinceEpoch())));
             return item;
         });
         snapshotBuilder.setCompareFn([](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * item)
         {
-            return armarx::detail::compare(static_cast<qlonglong>(snapshot.time().toMicroSeconds()),
+            return armarx::detail::compare(static_cast<qlonglong>(snapshot.time().toMicroSecondsSinceEpoch()),
                                            item->data(int(Columns::KEY), Qt::ItemDataRole::UserRole).toLongLong());
         });
         snapshotBuilder.setUpdateItemFn([this](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 2bdef0d95aa45845d27170508f83aed51a3e7829..ad07dc5b0de991b1e3296a94c66418caa20bf011 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -1,12 +1,16 @@
 #include "QueryWidget.h"
 
-#include <QWidget>
-#include <QTabWidget>
-#include <QPushButton>
+#include <limits>
+
 #include <QCheckBox>
+#include <QGroupBox>
 #include <QHBoxLayout>
+#include <QLabel>
+#include <QPushButton>
+#include <QSpinBox>
+#include <QTabWidget>
 #include <QVBoxLayout>
-#include <QGroupBox>
+#include <QWidget>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
@@ -16,34 +20,36 @@ namespace armarx::armem::gui
 
     QueryWidget::QueryWidget()
     {
-        QHBoxLayout* hlayout1 = new QHBoxLayout();
-        QHBoxLayout* hlayout2 = new QHBoxLayout();
-        QVBoxLayout* vlayout = new QVBoxLayout();
+        auto* vlayout = new QVBoxLayout();
 
         _dataCheckBox = new QCheckBox("Get Data");
         _dataCheckBox->setChecked(true);
+        vlayout->addWidget(_dataCheckBox);
 
-        _storeInLTMButton = new QPushButton("Store query result in LTM");
-
-        //_tabWidget = new QTabWidget();
-
-        _snapshotSelectorWidget = new SnapshotSelectorWidget();
-        //_tabWidget->addTab(_snapshotSelectorWidget, QString("Snapshots"));
-
-        hlayout1->addWidget(_dataCheckBox);
-        hlayout1->addWidget(_storeInLTMButton);
+        auto* recDepthHLayout = new QHBoxLayout();
+        vlayout->addLayout(recDepthHLayout);
 
-        hlayout2->addWidget(_snapshotSelectorWidget);
+        _recursionDepthSpinner = new QSpinBox(); // NOLINT
+        _recursionDepthSpinner->setRange(-1, std::numeric_limits<int>::max());
+        _recursionDepthSpinner->setValue(0);
+        _recursionDepthSpinner->setEnabled(_dataCheckBox->isChecked());
+        _recursionDepthSpinner->setSpecialValueText(QString("Unlimited"));
+        recDepthHLayout->addWidget(_recursionDepthSpinner);
 
-        const int margin = 0;
-        vlayout->setContentsMargins(margin, margin, margin, margin);
+        _recursionDepthLabel = new QLabel("Link resolution depth");
+        recDepthHLayout->addWidget(_recursionDepthLabel);
+        recDepthHLayout->addStretch();
 
-        vlayout->addLayout(hlayout1);
-        vlayout->addLayout(hlayout2);
+        _storeInLTMButton = new QPushButton("Store query result in LTM");
+        vlayout->addWidget(_storeInLTMButton);
 
         // Public connections.
         connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM);
 
+        // Private connections.
+        connect(
+            _dataCheckBox, &QCheckBox::stateChanged, this, &This::setRecursionDepthSpinnerEnabled);
+
         setLayout(vlayout);
     }
 
@@ -55,16 +61,23 @@ namespace armarx::armem::gui
                : armem::DataMode::NoData;
     }
 
-    armem::client::QueryInput QueryWidget::queryInput()
+    int QueryWidget::queryLinkRecursionDepth() const
     {
-        armem::client::query::Builder qb(dataMode());
-        qb.queryTargets(_snapshotSelectorWidget->queryTargets())
-        .coreSegments().all().queryTargets(_snapshotSelectorWidget->queryTargets())
-        .providerSegments().all().queryTargets(_snapshotSelectorWidget->queryTargets())
-        .entities().all().queryTargets(_snapshotSelectorWidget->queryTargets())
-        .snapshots(_snapshotSelectorWidget->selector());
-
-        return qb.buildQueryInput();
+        return _recursionDepthSpinner->value();
     }
 
+    void
+    QueryWidget::setRecursionDepthSpinnerEnabled(int state)
+    {
+        switch (state)
+        {
+            case Qt::Checked:
+                _recursionDepthSpinner->setEnabled(true);
+                break;
+            case Qt::Unchecked:
+            default:
+                _recursionDepthSpinner->setEnabled(false);
+                break;
+        }
+    }
 }
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
index 2bc7f223d5db52435b7571970bb4cd398aaeec49..e7a98a8026786a7c8e0c2acbfd8dc834cb72f16a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
@@ -23,7 +23,7 @@ namespace armarx::armem::gui
 
         armem::DataMode dataMode() const;
 
-        armem::client::QueryInput queryInput();
+        int queryLinkRecursionDepth() const;
 
 
     public slots:
@@ -36,6 +36,7 @@ namespace armarx::armem::gui
 
 
     private slots:
+        void setRecursionDepthSpinnerEnabled(int state);
 
     signals:
 
@@ -44,12 +45,10 @@ namespace armarx::armem::gui
     private:
 
         QCheckBox* _dataCheckBox;
+        QLabel* _recursionDepthLabel;
+        QSpinBox* _recursionDepthSpinner;
         QPushButton* _storeInLTMButton;
 
-        QTabWidget* _tabWidget;
-
-        SnapshotSelectorWidget* _snapshotSelectorWidget;
-
     };
 
 }
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
index 1d6c0cff39fd69da5e7f29844c64fabc1843f9a4..e6ed6aaaf3d6860c2960dbb83215b5791b8b53de 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
@@ -99,9 +99,9 @@ namespace armarx::armem::gui
     void SnapshotFormSingle::fillEntitySelector(client::query::SnapshotSelector& selector)
     {
         const Time time = latest->isChecked()
-                          ? Time::microSeconds(-1)
-                          : (Time::seconds(dateTime->dateTime().toSecsSinceEpoch()))
-                          + Time::microSeconds(microseconds->value());
+                          ? Time::Invalid()
+                          : (Time(Duration::Seconds(dateTime->dateTime().toSecsSinceEpoch()))
+                             + Duration::MicroSeconds(microseconds->value()));
         selector.atTime(time);
     }
 
@@ -152,17 +152,17 @@ namespace armarx::armem::gui
 
     void SnapshotFormTimeRange::fillEntitySelector(client::query::SnapshotSelector& selector)
     {
-        Time defaults = Time::microSeconds(-1);
+        Time defaults = Time::Invalid();
         Time min = defaults, max = defaults;
 
         if (!fromBegin->isChecked())
         {
-            min = Time::milliSeconds(fromDateTime->dateTime().toMSecsSinceEpoch());
+            min = Time(Duration::MilliSeconds(fromDateTime->dateTime().toMSecsSinceEpoch()));
         }
         if (!toEnd->isChecked())
         {
-            max = Time::milliSeconds(toDateTime->dateTime().toMSecsSinceEpoch())
-                  + Time::microSeconds(int(1e6) - 1);
+            max = Time(Duration::MilliSeconds(toDateTime->dateTime().toMSecsSinceEpoch()))
+                  + Duration::MicroSeconds(int(1e6) - 1);
         }
 
         selector.timeRange(min, max);
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
index 7b83360d50562977bf629a9ca4116f1c91089219..195cc6ebcecdc851b3c259d02172cfd43d6f7d7c 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
@@ -5,6 +5,7 @@
 #include "mdb_conversions.h"
 
 #include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 #include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
@@ -70,14 +71,14 @@ namespace armarx::armem::server::motions::mdb::segment
                             ss << "\n" << attachedFile.fileName << " (" << key << ")";
                         }
                     }
-                    ARMARX_INFO << ss.str();
+                    ARMARX_INFO << ss.str();  // ToDo @Fabian PK: Remove
                     auto& entity = this->segmentPtr->addEntity(std::to_string(op->id));
-                    auto& snapshot = entity.addSnapshot(op->createdDate);
+                    auto& snapshot = entity.addSnapshot(aron::fromAron<armem::Time>(op->createdDate));
 
                     armem::wm::EntityInstance& instance = snapshot.addInstance();
-                    instance.metadata().timeCreated = op->createdDate;
-                    instance.metadata().timeSent = IceUtil::Time::now();
-                    instance.metadata().timeArrived = IceUtil::Time::now();
+                    instance.metadata().timeCreated = aron::fromAron<armem::Time>(op->createdDate);
+                    instance.metadata().timeSent = Time::Now();
+                    instance.metadata().timeArrived = Time::Now();
                     instance.metadata().confidence = 1.0;
                     instance.data() = op->toAron();
                 }
@@ -86,7 +87,6 @@ namespace armarx::armem::server::motions::mdb::segment
                     ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
                 }
             }
-            IceUtil::Time::now();
 
             loadedMotions += allMotions.size();
         }
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
index 8de8754f9f656c61073aea2702910b50bf844cb6..343975886fdf6b6041189500ae40b5662701c5a4 100644
--- a/source/RobotAPI/libraries/armem_mps/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
@@ -3,7 +3,7 @@
 #include <ArmarXCore/interface/core/Profiler.h>
 #include <ArmarXCore/observers/ObserverObjectFactories.h>
 
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
 
 #include <dmp/representation/trajectory.h>
 //#include <dmp
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
index 579e921ca55cb32ebb8031202572ee0b0238390f..a45cd788ff673d0e5a9c78e5a92940d0d2daf3c9 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
@@ -56,21 +56,22 @@ namespace armarx::armem::server::motions::mps::segment
             auto allMotions = motionFinder.findAll("trajectories");
             for (const auto& motionFinderInfo : allMotions)
             {
-              auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID();// / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
-              for(const auto & entry: std::filesystem::directory_iterator(pathToInfoJson)){
-                  if(std::string(entry.path().filename()).rfind("taskspace", 0) == 0){
+                auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID();// / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
+                for (const auto & entry: std::filesystem::directory_iterator(pathToInfoJson))
+                {
+                    if (std::string(entry.path().filename()).rfind("taskspace", 0) == 0)
+                    {
                       //ARMARX_IMPORTANT << entry.path().filename();
                       loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
                       loadedMotions += allMotions.size();
-                  }
-                  /*else if(std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0){
+                    }
+                    /*else if (std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0)
+                    {
                       loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), false);
                       loadedMotions += allMotions.size();
-                  }*/
-              }
-
+                    }*/
+                }
             }
-            IceUtil::Time::now();
 
             loadedMotions += allMotions.size();
         }
@@ -81,46 +82,49 @@ namespace armarx::armem::server::motions::mps::segment
     void MPSegment::loadSingleMotionFinder(const std::string &pathToInfoJson, const std::string &entityName, bool taskspace)
     {
         if (auto op = util::createFromFile(pathToInfoJson, taskspace); op.has_value())
-          {
-               std::stringstream ss;
-              ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
-
-              armem::wm::EntityInstance instance;
-              instance.metadata().timeCreated = IceUtil::Time::now();//op->createdDate;
-              instance.metadata().timeSent = IceUtil::Time::now();
-              instance.metadata().timeArrived = IceUtil::Time::now();
-              instance.metadata().confidence = 1.0;
-
-              if(taskspace){
-                  std::filesystem::path path(pathToInfoJson);
-                  for(const auto & entry: std::filesystem::directory_iterator(path.parent_path())){
-                      std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20);
-                      if(std::string(entry.path().filename()).rfind(newname, 0) == 0){
-                          if (auto op2 = util::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson)
-                          {
-                              op->jointSpace = op2->jointSpace;
-                              instance.data() = op->toAron();
-                              if(this->segmentPtr->hasEntity(entityName)){
-                                   auto& entity = this->segmentPtr->getEntity(entityName);
-                                   auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
-                                   snapshot.addInstance(instance);
-                                }else{
-                                   auto& entity = this->segmentPtr->addEntity(entityName);
-                                   auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
-                                   snapshot.addInstance(instance);
-                                }
-                              ARMARX_IMPORTANT << "Full content trajectory: " << op->name;
-                          }
-                      }
-                  }
-              }
-
-
-
-          }
-          else
-          {
-              ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
-          }
+        {
+            std::stringstream ss;
+            ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
+
+            armem::wm::EntityInstance instance;
+            instance.metadata().timeCreated = armem::Time::Now();  //op->createdDate;
+            instance.metadata().timeSent = armem::Time::Now();
+            instance.metadata().timeArrived = armem::Time::Now();
+            instance.metadata().confidence = 1.0;
+
+            if(taskspace)
+            {
+                std::filesystem::path path(pathToInfoJson);
+                for (const auto & entry: std::filesystem::directory_iterator(path.parent_path()))
+                {
+                    std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20);
+                    if (std::string(entry.path().filename()).rfind(newname, 0) == 0)
+                    {
+                        if (auto op2 = util::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson)
+                        {
+                            op->jointSpace = op2->jointSpace;
+                            instance.data() = op->toAron();
+                            if (this->segmentPtr->hasEntity(entityName))
+                            {
+                                auto& entity = this->segmentPtr->getEntity(entityName);
+                                auto& snapshot = entity.addSnapshot(armem::Time::Now());
+                                snapshot.addInstance(instance);
+                            }
+                            else
+                            {
+                                auto& entity = this->segmentPtr->addEntity(entityName);
+                                auto& snapshot = entity.addSnapshot(armem::Time::Now());
+                                snapshot.addInstance(instance);
+                            }
+                            ARMARX_IMPORTANT << "Full content trajectory: " << op->name;
+                        }
+                    }
+                }
+            }
+        }
+        else
+        {
+          ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
+        }
     }
 }
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
index ab14043ea70b511cacefba9409c8eb001d5f316d..f38c3c114b49e33b7f411450d11f0996f8ac5eb8 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
@@ -8,7 +8,7 @@
 #include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
 
 // ArmarX
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
 
 namespace armarx::armem::server::motions::mps::segment
 {
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
index 080766eafbaf6bc20e84a8fec0a0912ceac6cfc2..32f0302a2b33486f6c4336b278059c6db5cf9208 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -8,8 +8,6 @@
 #include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
-
 #include <dmp/representation/trajectory.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
index 186074ce0dec8222017b0fb4a420beb227890739..72372ca2fa79a39e1a8ed022f4104627353f9727 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
@@ -7,7 +7,7 @@
 #include <optional>
 
 // ArmarX
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
 
 namespace armarx::armem::server::motions::mps::segment::util
 {
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index f26a62736999d28a55001dbaee56d515c6cdd16c..c16ff257603b9fded0a5ea72737b92b40667fdb6 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -24,10 +24,11 @@
 namespace armarx::armem::articulated_object
 {
 
-    VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(const std::string& name,
+    VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(
+            const std::string& name,
             const armem::Time& timestamp)
     {
-        const auto descriptions = queryDescriptions(IceUtil::Time::now());
+        const auto descriptions = queryDescriptions(armem::Time::Now());
 
         ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
 
@@ -45,9 +46,9 @@ namespace armarx::armem::articulated_object
 
         ARMARX_DEBUG << "Object " << name << " available";
 
-        auto obj =
-            VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
-                                             VirtualRobot::RobotIO::eStructure);
+        auto obj = VirtualRobot::RobotIO::loadRobot(
+                    ArmarXDataPath::resolvePath(it->xml.serialize().path),
+                    VirtualRobot::RobotIO::eStructure);
 
         if (not obj)
         {
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
index 5d918ddf474bfb110afaede2c4ce765ae446e931..65e8f2fba2ba962429e040e1d0e1eefc514e2355 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
@@ -10,7 +10,8 @@ namespace armarx::armem::articulated_object
     public:
         using Reader::Reader;
 
-        VirtualRobot::RobotPtr getArticulatedObject(const std::string& name,
+        VirtualRobot::RobotPtr getArticulatedObject(
+                const std::string& name,
                 const armem::Time& timestamp);
     };
 } // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 8caad31be27b227a8df1eb482f568ccf023b0f33..6cbb3ecdcfee0334df79a9d6728080c5bce55f5f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -12,7 +12,8 @@
 
 namespace armarx::armem::articulated_object
 {
-    armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj,
+    armem::articulated_object::ArticulatedObject convert(
+            const VirtualRobot::Robot& obj,
             const armem::Time& timestamp)
     {
         ARMARX_DEBUG << "Filename is " << obj.getFilename();
@@ -37,14 +38,15 @@ namespace armarx::armem::articulated_object
     }
 
     bool
-    ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+    ArticulatedObjectWriter::storeArticulatedObject(
+            const VirtualRobot::RobotPtr& articulatedObject,
             const armem::Time& timestamp)
     {
 
         ARMARX_CHECK_NOT_NULL(articulatedObject);
 
         armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
-            convert(*articulatedObject, IceUtil::Time::now());
+            convert(*articulatedObject, Time::Now());
 
         return store(armemArticulatedObject);
     }
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 05506152012d6527110801adb5bfe5b015a42d1b..fbce08b6323e1a1235bd000aadbd4e73ff6cbf8c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -97,7 +97,7 @@ namespace armarx::armem::articulated_object
 
     void Writer::updateKnownObjects()
     {
-        knownObjects = queryDescriptions(IceUtil::Time::now());
+        knownObjects = queryDescriptions(Time::Now());
 
         ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
     }
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 49dbdcf6140717e63ceec39e8cbf7d0f9b91dd77..861ff14b97addd9b87fee4a834bd605f20b2b681 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -55,6 +55,7 @@ namespace armarx::armem::obj::instance
         }
     }
 
+    /// get the latest object from an memory and cast it to an ObjectInstance
     std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp)
     {
         // clang-format off
@@ -73,6 +74,7 @@ namespace armarx::armem::obj::instance
         return armem::arondto::ObjectInstance::FromAron(instance->data());
     }
 
+    /// Query an object with full name entityName (e.g. Kitchen/green-cup/0)
     std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByEntityID(const std::string& entityName, const armem::Time& timestamp)
     {
         // Query all entities from all provider.
@@ -98,6 +100,7 @@ namespace armarx::armem::obj::instance
         return queryObject(qResult.memory, timestamp);
     }
 
+    /// Query an object by objectId (e.g. Kitchen/green-cup in Entity Kitchen/green-cup/0)
     std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByObjectID(const std::string& objectId, const armem::Time& timestamp)
     {
         // Query all entities from all provider.
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 8db6a1cab642e103dd6c25105b607c47651ccb15..825687d5b6bf17acad4515841e9852c9f020dbed 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -44,15 +44,86 @@ namespace armarx::armem::obj::instance
         void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
         void connect();
 
-
         std::optional<armem::arondto::ObjectInstance> queryObject(const armem::wm::Memory& memory, const armem::Time&);
         std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&);
         std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&);
 
+        // return the class name, e.g. Kitchen/greencup in Kitchen/greencup/0
+        static std::string GetObjectId(const std::string& s)
+        {
+            auto split = simox::alg::split(s, "/");
+            if (simox::alg::starts_with(s, "/"))
+            {
+                split.insert(split.begin(), ""); // sanitize
+            }
+
+            for (auto& e : split)
+            {
+                e = simox::alg::replace_all(e, "/", "");
+            }
+
+            if (IsEntityId(s)) return (split[0] + "/" + split[1]);
+            if (IsObjectId(s)) return s;
+
+            ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
+            return "";
+        }
+
+        // return the class name, e.g. greencup in Kitchen/greencup/0
+        static std::string GetObjectClassName(const std::string& s)
+        {
+            auto split = simox::alg::split(s, "/");
+            if (simox::alg::starts_with(s, "/"))
+            {
+                split.insert(split.begin(), ""); // sanitize
+            }
+
+            for (auto& e : split)
+            {
+                e = simox::alg::replace_all(e, "/", "");
+            }
+
+            if (IsEntityId(s)) return split[1];
+            if (IsObjectId(s)) return split[1];
+
+            ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
+            return "";
+        }
+
+        // check if s matches ??/??/??
+        static bool IsEntityId(const std::string& s)
+        {
+            auto split = simox::alg::split(s, "/");
+            if (simox::alg::starts_with(s, "/"))
+            {
+                split.insert(split.begin(), ""); // sanitize
+            }
+
+            if (split.size() != 3)
+            {
+                return false;
+            }
+            return true;
+        }
+
+        // check if s matches ??/??
+        static bool IsObjectId(const std::string& s)
+        {
+            auto split = simox::alg::split(s, "/");
+            if (simox::alg::starts_with(s, "/"))
+            {
+                split.insert(split.begin(), ""); // sanitize
+            }
+
+            if (split.size() != 2)
+            {
+                return false;
+            }
+            return true;
+        }
 
     private:
 
-
         struct Properties
         {
             std::string memoryName                  = "Object";
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
index ad9c8c83f2ca8889292c8a2023a74fd54528e04a..84b1e4613b8b4bbe8a72938949d503276500e407 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
@@ -65,7 +65,7 @@ namespace armarx::armem::obj::instance
         e.entityID.providerSegmentName = provider;
         e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className + "/" + inst.pose.objectID.instanceName;
         e.timeCreated = t;
-        e.timeSent = IceUtil::Time::now();
+        e.timeSent = armem::Time::Now();
         e.instancesData = { inst.toAron() };
 
         auto res = memoryWriter.commit(c);
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index a0e9bce51558d7fb39caf50a331919dc16034793..3250a1cca34d47df08e4d9218c0bfeda729163b0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -79,7 +79,7 @@ namespace armarx::armem::server::obj::clazz
 
     void Segment::loadByObjectFinder()
     {
-        const Time now = TimeUtil::GetTime();
+        const Time now = Time::Now();
 
         const bool checkPaths = false;
         std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index df3728fb3bbc6149fb329e60a3a94083caa6a441..ac072e19693c213989cd5f6fc8acfb3de1cc8380 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -3,6 +3,7 @@
 #include <SimoxUtility/math/scale_value.h>
 
 #include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
 
 
 namespace armarx::armem::server::obj::instance
@@ -24,7 +25,7 @@ namespace armarx::armem::server::obj::instance
                        "Remove objects whose confidence is lower than this value.");
     }
 
-    void Decay::updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const
+    void Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
     {
         if (pose.attachment or pose.isStatic)
         {
@@ -36,7 +37,7 @@ namespace armarx::armem::server::obj::instance
         }
     }
 
-    void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const
+    void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
     {
         for (objpose::ObjectPose& pose : objectPoses)
         {
@@ -44,7 +45,7 @@ namespace armarx::armem::server::obj::instance
         }
     }
 
-    float Decay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now) const
+    float Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
     {
         const float duration = static_cast<float>((now - localization).toSecondsDouble());
         if (duration < delaySeconds)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
index a44652d5528bceefed921dec184fe8b87fd19bfe..4adf757da588b0c0525a526e70ccd707f95de9a2 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
@@ -1,9 +1,8 @@
 #pragma once
 
-#include <IceUtil/Time.h>
-
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/forward_declarations.h>
 
 #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
 
@@ -23,12 +22,12 @@ namespace armarx::armem::server::obj::instance
 
         void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
 
-        void updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const;
-        void updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
+        void updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const;
+        void updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const;
 
     private:
 
-        float calculateConfidence(IceUtil::Time localization, IceUtil::Time now) const;
+        float calculateConfidence(const DateTime& localization, const DateTime& now) const;
 
 
     public:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
index 973cf5a551228705b918560ad48648db5795ab1f..d1a6c91672c595f2ceba9fa5840f30347d586fff 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
@@ -74,27 +74,27 @@ namespace armarx::armem::server::obj::instance
 
     void RobotHeadMovement::movementStarts(long discardIntervalMs)
     {
-        return movementStarts(IceUtil::Time::milliSeconds(discardIntervalMs));
+        return movementStarts(Duration::MilliSeconds(discardIntervalMs));
     }
-    void RobotHeadMovement::movementStarts(IceUtil::Time discardInterval)
+    void RobotHeadMovement::movementStarts(const Duration& discardInterval)
     {
-        discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+        discardUpdatesUntil = DateTime::Now() + discardInterval;
     }
     void RobotHeadMovement::movementStops(long discardIntervalMs)
     {
-        return movementStops(IceUtil::Time::milliSeconds(discardIntervalMs));
+        return movementStops(Duration::MilliSeconds(discardIntervalMs));
     }
-    void RobotHeadMovement::movementStops(IceUtil::Time discardInterval)
+    void RobotHeadMovement::movementStops(const Duration& discardInterval)
     {
         if (discardInterval.toMilliSeconds() < 0)
         {
             //  Stop discarding.
-            discardUpdatesUntil = IceUtil::Time::milliSeconds(-1);
+            discardUpdatesUntil = DateTime::Invalid();
         }
         else
         {
             // Basically the same as starting.
-            discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+            discardUpdatesUntil = DateTime::Now() + discardInterval;
         }
     }
 
@@ -115,7 +115,7 @@ namespace armarx::armem::server::obj::instance
                 ARMARX_UNEXPECTED_ENUM_VALUE(objpose::HeadMovementAction, input.action);
                 break;
         }
-        output.discardUpdatesUntilMilliSeconds = this->discardUpdatesUntil.toMilliSeconds();
+        output.discardUpdatesUntilMilliSeconds = this->discardUpdatesUntil.toMilliSecondsSinceEpoch();
         return output;
     }
 
@@ -131,7 +131,7 @@ namespace armarx::armem::server::obj::instance
                 // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << discardUpdatesUntil;
                 discard.all = true;
             }
-            else if (TimeUtil::GetTime() < discardUpdatesUntil)
+            else if (DateTime::Now() < discardUpdatesUntil)
             {
                 discard.all = true;
                 // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << discardUpdatesUntil;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
index b6b0acb6da46d593f2da2677e9e7258ce6d7e0c2..b4da82e8239494f191cad7a1da79c39585f4b13d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
@@ -4,10 +4,9 @@
 #include <vector>
 #include <optional>
 
-#include <IceUtil/Time.h>
-
 #include <ArmarXCore/interface/observers/ObserverInterface.h>
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/DateTime.h>
 #include <ArmarXCore/observers/variant/DatafieldRef.h>
 
 #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
@@ -34,16 +33,16 @@ namespace armarx::armem::server::obj::instance
         bool isMoving() const;
 
         void movementStarts(long discardIntervalMs);
-        void movementStarts(IceUtil::Time discardInterval);
+        void movementStarts(const Duration& discardInterval);
         void movementStops(long discardIntervalMs);
-        void movementStops(IceUtil::Time discardInterval);
+        void movementStops(const Duration& discardInterval);
 
         objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input);
 
 
         struct Discard
         {
-            std::optional<IceUtil::Time> updatesUntil;
+            std::optional<DateTime> updatesUntil;
             bool all = false;
         };
         Discard getDiscard();
@@ -60,7 +59,7 @@ namespace armarx::armem::server::obj::instance
 
         KinematicUnitObserverInterfacePrx kinematicUnitObserver;
         std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
-        IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
+        DateTime discardUpdatesUntil = DateTime::Invalid();
 
         DebugObserverInterfacePrx debugObserver;
 
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 0dbe6db72f1b167114b0d211c93c3a37c9202d4a..60fd8f1b5dd3767083ef4c41c8a52ea2188e6dde 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -9,6 +9,7 @@
 
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
 #include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
 #include <RobotAPI/libraries/armem/client/Writer.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
@@ -29,21 +30,19 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <SimoxUtility/algorithm/string.h>
 #include <SimoxUtility/json.h>
 #include <SimoxUtility/math/pose/pose.h>
 
-
 #include <Eigen/Geometry>
-#include <IceUtil/Time.h>
 
 #include <sstream>
 
 
-
 namespace armarx::armem::server::obj::instance
 {
 
@@ -148,7 +147,7 @@ namespace armarx::armem::server::obj::instance
         stats.numUpdated = 0;
         for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
         {
-            const Time timestamp = Time::microSeconds(provided.timestampMicroSeconds);
+            const Time timestamp = armem::fromIce<Time>(provided.timestamp);
 
             // Check whether we have an old snapshot for this object.
             std::optional<objpose::ObjectPose> previousPose;
@@ -230,7 +229,7 @@ namespace armarx::armem::server::obj::instance
 
     void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
     {
-        Time now = TimeUtil::GetTime();
+        Time now = Time::Now();
 
         ARMARX_CHECK_NOT_NULL(segmentPtr);
         const MemoryID coreSegmentID = segmentPtr->id();
@@ -269,7 +268,7 @@ namespace armarx::armem::server::obj::instance
 
 
     objpose::ObjectPoseMap
-    Segment::getObjectPoses(IceUtil::Time now)
+    Segment::getObjectPoses(const DateTime& now)
     {
         ObjectPoseMap objectPoses = getLatestObjectPoses();
         updateObjectPoses(objectPoses, now);
@@ -281,7 +280,7 @@ namespace armarx::armem::server::obj::instance
     objpose::ObjectPoseMap
     Segment::getObjectPosesByProvider(
         const std::string& providerName,
-        IceUtil::Time now)
+        const DateTime& now)
     {
         ARMARX_CHECK_NOT_NULL(segmentPtr);
         ObjectPoseMap objectPoses = getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
@@ -325,7 +324,7 @@ namespace armarx::armem::server::obj::instance
     }
 
 
-    void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, IceUtil::Time now)
+    void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
     {
         bool agentSynchronized = false;
 
@@ -338,7 +337,7 @@ namespace armarx::armem::server::obj::instance
 
     void Segment::updateObjectPoses(
         ObjectPoseMap& objectPoses,
-        IceUtil::Time now,
+        const DateTime& now,
         VirtualRobot::RobotPtr agent,
         bool& agentSynchronized) const
     {
@@ -351,7 +350,7 @@ namespace armarx::armem::server::obj::instance
 
     void Segment::updateObjectPose(
         ObjectPose& objectPose,
-        IceUtil::Time now,
+        const DateTime& now,
         VirtualRobot::RobotPtr agent,
         bool& agentSynchronized) const
     {
@@ -493,7 +492,7 @@ namespace armarx::armem::server::obj::instance
     ::armarx::armem::articulated_object::ArticulatedObjects
     Segment::getArticulatedObjects()
     {
-        objpose::ObjectPoseMap objectPoses = getObjectPoses(IceUtil::Time::now());
+        objpose::ObjectPoseMap objectPoses = getObjectPoses(Time::Now());
 
         ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
 
@@ -507,7 +506,6 @@ namespace armarx::armem::server::obj::instance
             articulatedObject.instance = objectPose.objectID.instanceName();
             articulatedObject.timestamp = objectPose.timestamp;
 
-
             ARMARX_INFO << "Object id is " << objectId.str();
             ARMARX_INFO << "Object id for objectPose is " << objectPose.objectID.str();
 
@@ -537,8 +535,6 @@ namespace armarx::armem::server::obj::instance
                 continue;
             }
 
-
-
             if (not articulatedObject.config.jointMap.empty())
             {
                 objects.push_back(articulatedObject);
@@ -578,7 +574,7 @@ namespace armarx::armem::server::obj::instance
     objpose::AttachObjectToRobotNodeOutput
     Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
     {
-        const armem::Time now = armem::Time::now();
+        const armem::Time now = armem::Time::Now();
 
         objpose::AttachObjectToRobotNodeOutput output;
         output.success = false;  // We are not successful until proven otherwise.
@@ -670,7 +666,7 @@ namespace armarx::armem::server::obj::instance
     Segment::detachObjectFromRobotNode(
         const objpose::DetachObjectFromRobotNodeInput& input)
     {
-        const armem::Time now = armem::Time::now();
+        const armem::Time now = armem::Time::Now();
 
         ObjectID objectID = armarx::fromIce(input.objectID);
         std::string providerName = input.providerName;
@@ -720,7 +716,7 @@ namespace armarx::armem::server::obj::instance
     {
         ARMARX_CHECK_NOT_NULL(segmentPtr);
 
-        const armem::Time now = armem::Time::now();
+        const armem::Time now = armem::Time::Now();
 
         objpose::DetachAllObjectsFromRobotNodesOutput output;
         output.numDetached = 0;
@@ -867,7 +863,7 @@ namespace armarx::armem::server::obj::instance
         std::string filename = _filename;
 
         filename = simox::alg::replace_all(filename, timestampPlaceholder,
-                                           Time::now().toString("%Y-%m-%d_%H-%M-%S"));
+                                           Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
         if (not simox::alg::ends_with(filename, ".json"))
         {
             filename += ".json";
@@ -925,7 +921,7 @@ namespace armarx::armem::server::obj::instance
 
     void Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
     {
-        const Time now = TimeUtil::GetTime();
+        const Time now = Time::Now();
         std::map<ObjectID, int> idCounters;
 
         objpose::ObjectPoseSeq objectPoses;
@@ -975,6 +971,7 @@ namespace armarx::armem::server::obj::instance
                 std::filesystem::path filename = path->filename();
                 filename.replace_extension();  // Removes extension
 
+                // The check seems useless?
                 if (lockMemory)
                 {
                     commitSceneSnapshot(snapshot.value(), filename.string());
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 24f80704f8580c386bfaf346502cccd9fa86c1c6..da97aac841559b845fb308e93ac572739efb7c3c 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -16,6 +16,7 @@
 #include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
 
 #include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 #include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
@@ -24,10 +25,6 @@
 #include "Decay.h"
 
 
-namespace armarx::objects
-{
-    struct Scene;
-}
 namespace armarx::armem::arondto
 {
     class ObjectInstance;
@@ -68,8 +65,8 @@ namespace armarx::armem::server::obj::instance
         void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
 
 
-        objpose::ObjectPoseMap getObjectPoses(IceUtil::Time now);
-        objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+        objpose::ObjectPoseMap getObjectPoses(const DateTime& now);
+        objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, const DateTime& now);
 
         wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
         std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
@@ -115,16 +112,16 @@ namespace armarx::armem::server::obj::instance
 
         void updateObjectPoses(
             ObjectPoseMap& objectPoses,
-            IceUtil::Time now);
+            const DateTime& now);
         void updateObjectPoses(
             ObjectPoseMap& objectPoses,
-            IceUtil::Time now,
+            const DateTime& now,
             VirtualRobot::RobotPtr agent,
             bool& agentSynchronized
         ) const;
         void updateObjectPose(
             ObjectPose& objectPose,
-            IceUtil::Time now,
+            const DateTime& now,
             VirtualRobot::RobotPtr agent,
             bool& agentSynchronized
         ) const;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index ef00638e9ed76d0bbe05de3ab6af98f3687fe9d7..b7f7a63085b31cb3672ad6c0307fc5afe2096c4e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -218,7 +218,7 @@ namespace armarx::armem::server::obj::instance
     {
         TIMING_START(tGetObjectPoses);
 
-        const IceUtil::Time now = TimeUtil::GetTime();
+        const Time now = Time::Now();
         const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now]()
         {
             return simox::alg::get_values(segment.getObjectPoses(now));
@@ -242,7 +242,7 @@ namespace armarx::armem::server::obj::instance
     {
         TIMING_START(GetObjectPoses);
 
-        const IceUtil::Time now = TimeUtil::GetTime();
+        const Time now = Time::Now();
         const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now, &providerName]()
         {
             return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
@@ -451,7 +451,7 @@ namespace armarx::armem::server::obj::instance
 
                     segment.doLocked([this, &objectPoses, &objectFinder]()
                     {
-                        const IceUtil::Time now = TimeUtil::GetTime();
+                        const Time now = Time::Now();
 
                         // Also include decayed objects in result
                         // Store original setting.
diff --git a/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt b/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9da6213e75a9c53e041ab5060537dd9122d9a1c2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME       armem_reasoning)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+    LIBS
+        ArmarXCoreInterfaces ArmarXCore
+        RobotAPICore aron armem
+
+    SOURCES
+        aron_conversions.cpp
+
+    HEADERS
+        aron_conversions.h
+
+    ARON_FILES
+        aron/Anticipation.xml
+)
+
+
+add_library(RobotAPI::armem_reasoning ALIAS ${LIB_NAME})
+
+add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml b/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a3f481c630d573970ace675032a13becc9ca57fe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+    <CodeIncludes>
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+    </CodeIncludes>
+    <AronIncludes>
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+    </AronIncludes>
+
+    <GenerateTypes>
+        <Object name='armarx::reasoning::arondto::Anticipation'>
+             <ObjectChild key='predictions'>
+                <List optional='true'>
+                    	<string />
+                </List>
+            </ObjectChild>
+        </Object>
+    </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e721f12a9349747d7d2b3c6e69a4bc223e5d76ad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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/>.
+ *
+ * @package    VisionX::ArmarXObjects::armem_images_server
+ * @author     Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "aron_conversions.h"
+
+namespace armarx::armem::server::reasoning
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e4d7c8bbd2206ed184cfe7e01c2898d81523b04
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h
@@ -0,0 +1,31 @@
+/*
+ * 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/>.
+ *
+ * @package    VisionX::ArmarXObjects::armem_images_server
+ * @author     Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
+
+namespace armarx::armem::server::reasoning
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf2d89bb748ccf788288bc9632acf55937877125
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
@@ -0,0 +1,39 @@
+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::armem_images_server
+ * @author     Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "AnticipationSegment.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+namespace armarx::armem::server::reasoning
+{
+    AnticipationSegment::AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+        Base(iceMemory, CORE_SEGMENT_NAME)
+    {
+
+    }
+
+    void AnticipationSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        Base::defineProperties(defs, prefix);
+    }
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a05c7adb5b9e7d96d51c2e07a6c18f016bc506a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
@@ -0,0 +1,46 @@
+/*
+ * 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/>.
+ *
+ * @package    VisionX::ArmarXObjects::armem_images_server
+ * @author     Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <string>
+
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+
+#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
+
+namespace armarx::armem::server::reasoning
+{
+    class AnticipationSegment : public armem::server::segment::SpecializedCoreSegment
+    {
+    public:
+        using Base = armem::server::segment::SpecializedCoreSegment;
+
+        AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+        virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+    public:
+        const std::string CORE_SEGMENT_NAME = "Anticipation";
+    };
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..21b041702717541f4def5b8b65705c4dd6463185
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt
@@ -0,0 +1,22 @@
+set(ARMARX_LIB_NAME "" )
+set(ARON_FILES "")
+
+set(LIB_NAME       armem_reasoning_server)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+    LIBS
+        armem_reasoning
+        armem_server
+
+    SOURCES
+	AnticipationSegment.cpp
+
+    HEADERS
+	AnticipationSegment.h
+)
+
+
+add_library(RobotAPI::armem_reasoning_server ALIAS ${LIB_NAME})
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 0ab9ff1a46f9e756471ccf8d257e966dcfb7f2ef..5db2b55603580aa42450938630483d2e8c4270f5 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -53,14 +53,14 @@ namespace armarx::armem::robot
 
     void fromAron(const arondto::RobotState& dto, RobotState& bo)
     {
-        bo.timestamp = dto.timestamp;
+        fromAron(dto.timestamp, bo.timestamp);
         bo.globalPose.matrix() = dto.globalPose;
         bo.jointMap            = dto.jointMap;
     }
 
     void toAron(arondto::RobotState& dto, const RobotState& bo)
     {
-        dto.timestamp = bo.timestamp;
+        toAron(dto.timestamp, bo.timestamp);
         dto.globalPose = bo.globalPose.matrix();
         dto.jointMap   = bo.jointMap;
     }
@@ -97,14 +97,14 @@ namespace armarx::armem
 
     void robot::fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
     {
-        bo.timestamp = dto.timestamp;
+        fromAron(dto.timestamp, bo.timestamp);
         bo.globalPose = dto.objectPoseGlobal;
         bo.jointMap = dto.objectJointValues;
     }
 
     void robot::toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
     {
-        dto.timestamp = bo.timestamp;
+        toAron(dto.timestamp, bo.timestamp);
         dto.objectPoseGlobal = bo.globalPose.matrix();
         dto.objectJointValues = bo.jointMap;
     }
diff --git a/source/RobotAPI/libraries/armem_robot/types.cpp b/source/RobotAPI/libraries/armem_robot/types.cpp
index 01847402c1e1bdde24956203620c13e15328e913..6f09eee1acef62472a1e8cc0027f49c9db6efe46 100644
--- a/source/RobotAPI/libraries/armem_robot/types.cpp
+++ b/source/RobotAPI/libraries/armem_robot/types.cpp
@@ -1,11 +1,24 @@
 #include "types.h"
 
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
 namespace armarx::armem::robot
 {
-  std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs) 
-  {
-     os << "RobotDescription { name: '" << rhs.name << "', xml: '" << rhs.xml << "' }";
-     return os;
-  }
+
+    std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs)
+    {
+        os << "RobotDescription { name: '" << rhs.name << "', xml: '" << rhs.xml << "' }";
+        return os;
+    }
+
+
+    std::string Robot::name() const
+    {
+        ARMARX_CHECK_NOT_EMPTY(description.name) << "The robot name must be set!";
+        ARMARX_CHECK_NOT_EMPTY(instance) << "The robot instance name must be provided!";
+
+        return description.name + "/" + instance;
+    }
   
 }
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 10ca7475f0d119aeb8f40e2f0d674bb3a6450691..345564d4b859af306acecdc82d00096c4a559351 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -1,23 +1,22 @@
 #pragma once
 
+#include <filesystem>
 #include <map>
 #include <vector>
-#include <filesystem>
 
 #include <Eigen/Geometry>
 
-#include <IceUtil/Time.h>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/time/DateTime.h>
 
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <ArmarXCore/core/PackagePath.h>
 
 
 namespace armarx::armem::robot
 {
     struct RobotDescription
     {
-        // IceUtil::Time timestamp;
+        // DateTime timestamp;
 
         std::string name;
         PackagePath xml{"", std::filesystem::path("")};
@@ -46,7 +45,7 @@ namespace armarx::armem::robot
         using JointMap = std::map<std::string, float>;
         using Pose = Eigen::Affine3f;
 
-        IceUtil::Time timestamp;
+        DateTime timestamp;
 
         Pose globalPose;
         JointMap jointMap;
@@ -60,15 +59,9 @@ namespace armarx::armem::robot
 
         RobotState config;
 
-        IceUtil::Time timestamp;
-
-        std::string name() const 
-        {
-            ARMARX_CHECK_NOT_EMPTY(description.name) << "The robot name must be set!";
-            ARMARX_CHECK_NOT_EMPTY(instance) << "The robot instance name must be provided!";
+        DateTime timestamp;
 
-            return description.name + "/" + instance;
-        }
+        std::string name() const;
     };
 
     using Robots = std::vector<Robot>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 03a578fab3b9250f724efcd646c24400d8a898d2..30f3a489cc9b7b2e8bc8f6e80c76d7bbccf15142 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -2,15 +2,15 @@
 
 #include <string>
 
-#include <IceUtil/Time.h>
-
 #include <ArmarXCore/core/logging/Logging.h>
+
 #include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/types.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
+
 namespace armarx::armem
 {
 
@@ -38,7 +38,7 @@ namespace armarx::armem
         aron::toAron(dto.parentFrame, bo.parentFrame);
         aron::toAron(dto.frame, bo.frame);
         aron::toAron(dto.agent, bo.agent);
-        dto.timestamp = bo.timestamp;
+        toAron(dto.timestamp, bo.timestamp);
     }
 
     void
@@ -47,7 +47,7 @@ namespace armarx::armem
         aron::fromAron(dto.parentFrame, bo.parentFrame);
         aron::fromAron(dto.frame, bo.frame);
         aron::fromAron(dto.agent, bo.agent);
-        bo.timestamp = dto.timestamp;
+        fromAron(dto.timestamp, bo.timestamp);
     }
 
     /* JointState */
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index b3dee3440967cf79ae9feee73403116345a23cbf..beebabd2977e8f7e46e976892ee4da3dc18f2bad 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -9,7 +9,10 @@
 #include <ArmarXCore/core/exceptions/LocalException.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
 
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/core/error.h>
@@ -94,13 +97,13 @@ namespace armarx::armem::robot_state
     }
 
     void
-    RobotReader::setSyncTimeout(const armem::Time& duration)
+    RobotReader::setSyncTimeout(const armem::Duration& duration)
     {
         syncTimeout = duration;
     }
 
     void
-    RobotReader::setSleepAfterSyncFailure(const armem::Time& duration)
+    RobotReader::setSleepAfterSyncFailure(const armem::Duration& duration)
     {
         ARMARX_CHECK_NONNEGATIVE(duration.toMicroSeconds());
         sleepAfterFailure = duration;
@@ -109,7 +112,7 @@ namespace armarx::armem::robot_state
     bool
     RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
     {
-        const auto tsStartFunctionInvokation = armem::Time::now();
+        const auto tsStartFunctionInvokation = armem::Time::Now();
 
         while (true)
         {
@@ -120,7 +123,7 @@ namespace armarx::armem::robot_state
                 ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
 
                 // if the syncTime is non-positive there will be no retry
-                const auto elapsedTime = armem::Time::now() - tsStartFunctionInvokation;
+                const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
                 if (elapsedTime > syncTimeout)
                 {
                     ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
@@ -128,8 +131,7 @@ namespace armarx::armem::robot_state
                 }
 
                 ARMARX_INFO << "Retrying to query robot state after failure";
-                std::this_thread::sleep_for(
-                    std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+                Clock::WaitFor(sleepAfterFailure);
             }
 
             obj.config = std::move(*state);
@@ -203,6 +205,31 @@ namespace armarx::armem::robot_state
             .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
     }
 
+    std::optional<robot::RobotState::Pose>
+    RobotReader::queryOdometryPose(const robot::RobotDescription& description,
+                                   const armem::Time& timestamp) const
+    {
+
+        common::robot_state::localization::TransformQuery query
+        {
+            .header = {
+                .parentFrame = OdometryFrame,
+                .frame = "root",
+                .agent = description.name,
+                .timestamp = timestamp
+            }
+        };
+
+        const auto result = transformReader.lookupTransform(query);
+        if (not result)
+        {
+            return std::nullopt;
+        }
+
+        return result.transform.transform;
+    }
+
+
     std::optional<robot::RobotState::JointMap>
     RobotReader::queryJointState(const robot::RobotDescription& description,
                                  const armem::Time& timestamp) const // Why timestamp?!?!
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 46b0924fc2cb06ebee34ff8b61174746939dd7dc..e72b5fc64744a7ad3481990705568e92a8c56208 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -24,9 +24,9 @@
 #include <mutex>
 #include <optional>
 
-#include "RobotAPI/libraries/armem/core/Time.h"
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem_robot/client/interfaces.h>
 #include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
@@ -81,8 +81,8 @@ namespace armarx::armem::robot_state
         queryPlatformState(const robot::RobotDescription& description,
                            const armem::Time& timestamp) const;
 
-        void setSyncTimeout(const armem::Time& duration);
-        void setSleepAfterSyncFailure(const armem::Time& duration);
+        void setSyncTimeout(const armem::Duration& duration);
+        void setSleepAfterSyncFailure(const armem::Duration& duration);
 
         enum class Hand
         {
@@ -99,10 +99,23 @@ namespace armarx::armem::robot_state
                           const armem::Time& start,
                           const armem::Time& end) const;
 
+        /**
+         * @brief retrieve the robot's pose in the odometry frame. 
+         *
+         * This pose is an integration of the robot's platform velocity and undergoes a significant drift.
+         * 
+         * @param description 
+         * @param timestamp 
+         * @return std::optional<robot::RobotState::Pose> 
+         */
+        std::optional<robot::RobotState::Pose>
+        queryOdometryPose(const robot::RobotDescription& description,
+                          const armem::Time& timestamp) const;
+
     protected:
         // by default, no timeout mechanism
-        armem::Time syncTimeout = armem::Time::microSeconds(0);
-        armem::Time sleepAfterFailure = armem::Time::microSeconds(0);
+        armem::Duration syncTimeout = armem::Duration::MicroSeconds(0);
+        armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
 
     private:
         std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 5ef7564f8a0e374060114d0391bfaeebf1df6198..6eeb7e2e9503dd6cf0f876bc4676dc50effc3b50 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -11,6 +11,7 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Clock.h>
 
 
 namespace armarx::armem::robot_state
@@ -100,8 +101,7 @@ namespace armarx::armem::robot_state
             }
 
             ARMARX_INFO << "Retrying to query robot after failure";
-            std::this_thread::sleep_for(
-                std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+            Clock::WaitFor(sleepAfterFailure);
         }
 
         ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index ba34586a1e77390efbecd18abba1e726db6f7467..932f55b0a6ea2ff74762a62ffa74f88d0ff18422 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -94,7 +94,7 @@ namespace armarx::armem::client::robot_state::localization
         // const auto& timestamp = transform.header.timestamp;
         const MemoryID entityID = providerId.withEntityName(
                                       transform.header.parentFrame + "," + transform.header.frame);
-        const Time timestamp = Time::now(); // FIXME remove
+        const Time timestamp = Time::Now();  // FIXME remove
 
         armem::EntityUpdate update;
         update.entityID = entityID;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 67d537d19cf74af3da62dc72ae217f996977433f..e68119affae5664f232dba25c679a8cce9fbdd10 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -123,7 +123,7 @@ namespace armarx::armem::server::robot_state
         {
             if (p.enabled)
             {
-                const Time timestamp = Time::now();
+                const Time timestamp = Time::Now();
                 ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
 
                 try
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 185356425ca3d657014f2b0e0b2e56631e443f4f..f6cba1d8c22086497f7eb537c6a3e16fa2e2faf4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -44,7 +44,7 @@ namespace armarx::armem::server::robot_state::description
 
     void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
     {
-        const Time now = TimeUtil::GetTime();
+        const Time now = Time::Now();
 
         const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name);
         segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 12638d84e18a8a3439b560fb9e2287c3fe233c79..f50c4e5619d70b7910d6d3ef544c2ca4f9d7fdc9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -89,7 +89,7 @@ namespace armarx::armem::server::robot_state::proprioception
 
         RobotUnitData result;
         result.proprioception = converter->convert(data.value(), description);
-        result.timestamp = Time::microSeconds(data->timestampUSec);
+        result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
 
         auto stop = std::chrono::high_resolution_clock::now();
         auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 9bf3f1e8129fe0d2f2194d1f9e223077678e93f0..ca9e43ed9d4a7022dd4c6a4eecba530e81bf7b21 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
         RobotJointPositionMap jointMap;
         int i = 0;
 
-        Duration tFindData = Duration::milliSeconds(0), tReadJointPositions = Duration::milliSeconds(0);
+        Duration tFindData = Duration::MilliSeconds(0), tReadJointPositions = Duration::MilliSeconds(0);
         TIMING_START(tProcessEntities)
         segmentPtr->forEachEntity([&](const wm::Entity & entity)
         {
@@ -97,8 +97,8 @@ namespace armarx::armem::server::robot_state::proprioception
                     data = snapshot->findInstanceData();
                 }
 
-                TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG)
-                tFindData += _tFindData;
+                TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG);
+                tFindData += Duration::MicroSeconds(_tFindData.toMicroSeconds());
             }
             if (data)
             {
@@ -107,7 +107,7 @@ namespace armarx::armem::server::robot_state::proprioception
                 jointMap.emplace(entity.id().providerSegmentName, readJointPositions(*data));
 
                 TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
-                tReadJointPositions += _tReadJointPositions;
+                tReadJointPositions += Duration::MicroSeconds(_tReadJointPositions.toMicroSeconds());
             }
             ++i;
         });
diff --git a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
index 0e539ff08a436fee457a773a7dad2700ca19e8a2..b546afb89779ada2764d83faeb23684cd189ef50 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
@@ -34,7 +34,10 @@ The memory should look like the following:
                 <long />
             </ObjectChild>
 
-            <!-- accepted type as any type -->
+            <ObjectChild key='acceptedType'>
+                <string />
+            </ObjectChild>
+
         </Object>
 
         <Object name='armarx::skills::arondto::SkillExecutionRequest'>
@@ -50,7 +53,10 @@ The memory should look like the following:
                 <String />
             </ObjectChild>
 
-            <!-- ToDo: add params wih any type -->
+            <ObjectChild key='params'>
+                <AnyObject shared_ptr="1" />
+            </ObjectChild>
+
         </Object>
 
         <Object name='armarx::skills::arondto::SkillExecutionEvent'>
@@ -66,8 +72,13 @@ The memory should look like the following:
                 <String />
             </ObjectChild>
 
-            <!-- ToDo: add params with any type -->
-            <!-- ToDo: add result with any type -->
+            <ObjectChild key='params'>
+                <AnyObject shared_ptr="1" />
+            </ObjectChild>
+
+            <ObjectChild key='data'>
+                <AnyObject shared_ptr="1" />
+            </ObjectChild>
         </Object>
 
     </GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
index 1419ddf440ad718bc6c4252c7678b2c607534ab2..745968174eff5fbd83b4d2147fa24a7931515797 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
@@ -40,10 +40,16 @@ namespace armarx::skills::segment
             skillDescription.targets = desc.targets;
             skillDescription.timeoutMs = desc.timeoutMs;
 
+            if (desc.acceptedType)
+            {
+                auto t = aron::type::Object::FromAronObjectDTO(desc.acceptedType);
+                skillDescription.acceptedType = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(t).dump(2);
+            }
+
             armem::Commit commit;
             auto& entityUpdate = commit.add();
             entityUpdate.confidence = 1.0;
-            entityUpdate.timeCreated = armem::Time::now();
+            entityUpdate.timeCreated = armem::Time::Now();
             entityUpdate.instancesData = {skillDescription.toAron()};
             entityUpdate.entityID = provId.withEntityName(skillDescription.skillName);
 
@@ -55,5 +61,7 @@ namespace armarx::skills::segment
     void ExecutableSkillLibraryCoreSegment::removeSkillProvider(const std::string& providerName)
     {
         skills.erase(providerName);
+
+        // TODO also add info about removed provider to memory?
     }
 }
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 43e314fc827148c42a05e89d462964b84fdeed14..af8af24c230d36a91a52e9b15528a6fa9eff7f3a 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -11,7 +11,7 @@ namespace armarx::skills::segment
 {
 
     SkillEventCoreSegment::SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
-        Base(iceMemory, CoreSegmentName)
+        Base(iceMemory, CoreSegmentName, armarx::skills::arondto::SkillExecutionRequest::ToAronType())
     {
     }
 
@@ -42,25 +42,19 @@ namespace armarx::skills::segment
         event.providerName = update.providerName;
         event.skillName = update.skillName;
         event.status = ExecutionStatus2String.at(update.status);
-
-        aron::data::DictPtr aron_params = nullptr;
-        if (update.usedParams) aron_params = std::make_shared<aron::data::Dict>(update.usedParams);
-
-        aron::data::DictPtr aron_data = nullptr;
-        if (update.data) aron_data = std::make_shared<aron::data::Dict>(update.data);
+        event.params = aron::data::Dict::FromAronDictDTO(update.usedParams);
+        event.data = aron::data::Dict::FromAronDictDTO(update.data);
 
         armem::MemoryID commitId = id();
         commitId.providerSegmentName = event.providerName;
         commitId.entityName = event.skillName;
 
         auto aron = event.toAron();
-        aron->addElement("return", aron_data); // how to name?!?
-        aron->addElement("params", aron_params);
 
         armem::Commit comm;
         auto& entityUpdate = comm.add();
         entityUpdate.confidence = 1.0;
-        entityUpdate.timeCreated = armem::Time::now();
+        entityUpdate.timeCreated = armem::Time::Now();
         entityUpdate.instancesData = { aron };
         entityUpdate.entityID = commitId;
 
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index 5068cb04d910100a936b6934ef30fcaafb599f48..9c43b717aca7503962c1eb096202eee2e767bd00 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -12,7 +12,7 @@ namespace armarx::skills::segment
 {
 
     SkillExecutionRequestCoreSegment::SkillExecutionRequestCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
-        Base(iceMemory, CoreSegmentName/*, skills::arondto::SkillExecutionRequest::ToAronType()*/)
+        Base(iceMemory, CoreSegmentName, skills::arondto::SkillExecutionRequest::ToAronType())
     {
     }
 
@@ -35,12 +35,11 @@ namespace armarx::skills::segment
         // we got a skill execution request
         skills::arondto::SkillExecutionRequest request;
         request.fromAron(commitDataAron);
-        auto params = aron::data::Dict::DynamicCastAndCheck(commitDataAron->at("params")); // ToDo remov and add to request
 
         skills::manager::dto::SkillExecutionInfo info;
         info.providerName = request.providerName;
         info.skillName = request.skillName;
-        info.params = aron::data::Dict::ToAronDictDTO(params);
+        info.params = request.params->toAronDictDTO();
         return info;
     }
 
@@ -55,12 +54,10 @@ namespace armarx::skills::segment
         request.clientId = "";
         request.providerName = info.providerName;
         request.skillName = info.skillName;
+        request.params = aron::data::Dict::FromAronDictDTO(info.params);
 
         auto aron = request.toAron();
 
-        aron::data::DictPtr aron_params = aron::data::Dict::FromAronDictDTO(info.params);
-        aron->addElement("params", aron_params); // todo add as any type
-
         armem::MemoryID skillExecutionMemID = id();
         skillExecutionMemID.providerSegmentName = request.providerName;
         skillExecutionMemID.entityName = request.skillName;
@@ -68,7 +65,7 @@ namespace armarx::skills::segment
         entityUpdate.entityID = skillExecutionMemID;
         entityUpdate.instancesData = { aron };
         entityUpdate.confidence = 1.0;
-        entityUpdate.timeCreated = armem::Time::now();
+        entityUpdate.timeCreated = armem::Time::Now();
 
         iceMemory.commit(comm);
     }
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp
index cee0e3a18293dfa15534ece89c8d40f6afdca8cd..46629f7cd066751e6286cb38cb081f5442b6b869 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp
@@ -26,7 +26,7 @@ namespace armarx::skills::segment
     void StatechartListenerProviderSegment::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& t)
     {
         const std::string& entityName = getStatechartName(t.targetStateIdentifier);
-        IceUtil::Time transitionTime = IceUtil::Time::microSeconds(t.timestamp);
+        armem::Time transitionTime = armem::Time(armem::Duration::MicroSeconds(t.timestamp));
 
         armem::EntityUpdate update;
         update.entityID = segmentPtr->id().withEntityName(entityName);
diff --git a/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp b/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp
index 426ab0822c7542b00f38a15cc27d36edf5da6265..fa392c615789ffac9e8ae2b9fd70eda0052e6d27 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp
@@ -63,7 +63,7 @@ namespace armarx::armem::server::systemstate::segment
         EntityUpdate update;
         update.entityID = providerId.withEntityName("CurrentCpuLoad");
         update.confidence = 1.0;
-        update.timeCreated = armem::Time::now();
+        update.timeCreated = armem::Time::Now();
         update.instancesData = { data };
 
         segmentPtr->update(update);
diff --git a/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp b/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp
index a99e1c0b1b407e000ffdd4dee8ec988a1769b5e1..f02b8f4fa263687d52336012322ca640ecd84f1d 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp
@@ -54,7 +54,7 @@ namespace armarx::armem::server::systemstate::segment
         EntityUpdate update;
         update.entityID = providerId.withEntityName("CurrentMemoryLoad");
         update.confidence = 1.0;
-        update.timeCreated = armem::Time::now();
+        update.timeCreated = armem::Time::Now();
         update.instancesData = { data };
 
         segmentPtr->update(update);
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index b4caa2632769a098c970b7ec378bceb49e54f051..4819a27abc53f7b10696da6d452bb2d133a82fd4 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -6,12 +6,14 @@
 
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
 #include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/aron/converter/common/Converter.h>
 #include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
 
 #include "types.h"
 
+
 namespace armarx::armem
 {
 
@@ -20,7 +22,7 @@ namespace armarx::armem
     {
         return {.agent     = aronSensorHeader.agent,
                 .frame     = aronSensorHeader.frame,
-                .timestamp = aronSensorHeader.timestamp};
+                .timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
     }
 
     void fromAron(const arondto::LaserScanStamped& aronLaserScan,
@@ -40,7 +42,7 @@ namespace armarx::armem
 
         // laserScan = fromAron(aronLaserScan.data);
 
-        timestamp = header.timestamp.toMicroSeconds();
+        timestamp = header.timestamp.toMicroSecondsSinceEpoch();
         frame     = header.frame;
         agentName = header.agent;
     }
@@ -54,7 +56,7 @@ namespace armarx::armem
 
     int64_t toAron(const armem::Time& timestamp)
     {
-        return timestamp.toMicroSeconds();
+        return timestamp.toMicroSecondsSinceEpoch();
     }
 
     arondto::SensorHeader toAron(const SensorHeader& sensorHeader)
@@ -63,7 +65,7 @@ namespace armarx::armem
 
         aronSensorHeader.agent     = sensorHeader.agent;
         aronSensorHeader.frame     = sensorHeader.frame;
-        aronSensorHeader.timestamp = sensorHeader.timestamp;
+        toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
 
         return aronSensorHeader;
     }
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
index 861e02c4c9fcabbc40caebba3451b69e81e5016c..0cfe7fb95f4abdf1d5065a6d0f245e516644bc8f 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -65,7 +65,7 @@ namespace armarx::armem::vision::laser_scans::client
             return false;
         }
 
-        const auto iceTimestamp = Time::microSeconds(timestamp);
+        const auto iceTimestamp = Time(Duration::MicroSeconds(timestamp));
 
         const auto providerId = armem::MemoryID(result.segmentID);
         const auto entityID =
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
index 3275d888db9c8841e1dd7a8baf45582d27ab03ed..e781a2582e511b59f3ebefe9cde4a35d425da60a 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -25,7 +25,7 @@ namespace armarx::armem::vision::occupancy_grid::client
             return false;
         }
 
-        const auto iceTimestamp = Time::microSeconds(timestamp);
+        const auto iceTimestamp = Time(Duration::MicroSeconds(timestamp));
 
         const auto providerId = armem::MemoryID(result.segmentID);
         const auto entityID   = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index ea66ba9db549c0f48357b706dfa59f49ee1e8498..aa0a2ea82a6304a4afb3a5bc1b34c3f820b53577 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -13,18 +13,30 @@ armarx_add_library(
 
     HEADERS
         aron_conversions.h
+        forward_declarations.h
+        json_conversions.h
+
         aron_conversions/core.h
         aron_conversions/armarx.h
         aron_conversions/simox.h
         aron_conversions/stl.h
         aron_conversions/eigen.h
 
+        json_conversions/armarx.h
+
+        util/object_finders.h
+
     SOURCES
+
         aron_conversions/core.cpp
         aron_conversions/armarx.cpp
         aron_conversions/simox.cpp
         aron_conversions/stl.cpp
         aron_conversions/eigen.cpp
+
+        json_conversions/armarx.cpp
+
+        util/object_finders.cpp
 )
 
 
@@ -32,11 +44,15 @@ armarx_enable_aron_file_generation_for_target(
     TARGET_NAME
         "${LIB_NAME}"
     ARON_FILES
-        aron/Trajectory.xml
-        aron/Color.xml
-        aron/PackagePath.xml
         aron/AxisAlignedBoundingBox.xml
+        aron/color.xml
+        aron/Color.xml
+        aron/framed.xml
+        aron/Names.xml
         aron/OrientedBox.xml
+        aron/PackagePath.xml
+        aron/time.xml
+        aron/trajectory.xml
 )
 
 add_library(aron::common ALIAS aroncommon)
diff --git a/source/RobotAPI/libraries/aron/common/aron/Color.xml b/source/RobotAPI/libraries/aron/common/aron/Color.xml
index f84c31bbe8e465c63cb0b456c77fdc5b3f831e44..180a9b372464de917400387032bdff9d4c086f79 100644
--- a/source/RobotAPI/libraries/aron/common/aron/Color.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/Color.xml
@@ -2,26 +2,8 @@
 <AronTypeDefinition>
 
   <GenerateTypes>
-    <Object name='armarx::arondto::DrawColor'>
-        <ObjectChild key='r'>
-          <float />
-        </ObjectChild>
-
-        <ObjectChild key='g'>
-          <float />
-        </ObjectChild>
 
-        <ObjectChild key='b'>
-          <float />
-        </ObjectChild>
-
-        <ObjectChild key='a'>
-          <float />
-        </ObjectChild>
-    </Object>
-
-
-    <Object name='armarx::arondto::DrawColor24Bit'>
+    <Object name='simox::arondto::Color'>
         <ObjectChild key='r'>
           <int />
         </ObjectChild>
@@ -33,20 +15,9 @@
         <ObjectChild key='b'>
           <int />
         </ObjectChild>
-    </Object>
-
-
-    <Object name='armarx::arondto::HsvColor'>
-        <ObjectChild key='h'>
-          <float />
-        </ObjectChild>
-
-        <ObjectChild key='s'>
-          <float />
-        </ObjectChild>
 
-        <ObjectChild key='v'>
-          <float />
+        <ObjectChild key='a'>
+          <int />
         </ObjectChild>
     </Object>
 
diff --git a/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ec5aa0c961a11530ad1d6a08372fe2c42cbf10e8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+    <GenerateTypes>
+
+        <Object name="armarx::arondto::FrameHeader">
+            <ObjectChild key='frame'>
+                <string />
+            </ObjectChild>
+            <ObjectChild key='agent'>
+                <string />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedPosition">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameHeader />
+            </ObjectChild>
+            <ObjectChild key='position'>
+                <Position />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedDirection">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameHeader />
+            </ObjectChild>
+            <ObjectChild key='direction'>
+                <Position />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedOrientation">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameHeader />
+            </ObjectChild>
+            <ObjectChild key='orientation'>
+                <Orientation />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedPose">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameHeader />
+            </ObjectChild>
+            <ObjectChild key='pose'>
+                <Pose />
+            </ObjectChild>
+        </Object>
+
+    </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/Names.xml b/source/RobotAPI/libraries/aron/common/aron/Names.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4c57b39981e138cbf83143a06fa71d40019a9dbe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/Names.xml
@@ -0,0 +1,25 @@
+<!--
+Recognized and spoken names of a named entity.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <GenerateTypes>
+
+        <Object name="armarx::arondto::Names">
+
+            <ObjectChild key="recognized">
+                <List>
+                    <String/>
+                </List>
+            </ObjectChild>
+
+            <ObjectChild key="spoken">
+                <List>
+                    <String/>
+                </List>
+            </ObjectChild>
+
+        </Object>
+
+    </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/color.xml b/source/RobotAPI/libraries/aron/common/aron/color.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f84c31bbe8e465c63cb0b456c77fdc5b3f831e44
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/color.xml
@@ -0,0 +1,55 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+  <GenerateTypes>
+    <Object name='armarx::arondto::DrawColor'>
+        <ObjectChild key='r'>
+          <float />
+        </ObjectChild>
+
+        <ObjectChild key='g'>
+          <float />
+        </ObjectChild>
+
+        <ObjectChild key='b'>
+          <float />
+        </ObjectChild>
+
+        <ObjectChild key='a'>
+          <float />
+        </ObjectChild>
+    </Object>
+
+
+    <Object name='armarx::arondto::DrawColor24Bit'>
+        <ObjectChild key='r'>
+          <int />
+        </ObjectChild>
+
+        <ObjectChild key='g'>
+          <int />
+        </ObjectChild>
+
+        <ObjectChild key='b'>
+          <int />
+        </ObjectChild>
+    </Object>
+
+
+    <Object name='armarx::arondto::HsvColor'>
+        <ObjectChild key='h'>
+          <float />
+        </ObjectChild>
+
+        <ObjectChild key='s'>
+          <float />
+        </ObjectChild>
+
+        <ObjectChild key='v'>
+          <float />
+        </ObjectChild>
+    </Object>
+
+  </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/framed.xml b/source/RobotAPI/libraries/aron/common/aron/framed.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2e2e6d45f95defa21e39670d5957dba539d63972
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/framed.xml
@@ -0,0 +1,48 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+    <CodeIncludes>
+        <Include include="<Eigen/Geometry>" />
+    </CodeIncludes>
+
+    <GenerateTypes>
+
+        <Object name="armarx::arondto::FrameID">
+            <ObjectChild key='frame'>
+                <string />
+            </ObjectChild>
+            <ObjectChild key='agent'>
+                <string />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedPosition">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameID />
+            </ObjectChild>
+            <ObjectChild key='position'>
+                <Position />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedOrientation">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameID />
+            </ObjectChild>
+            <ObjectChild key='orientation'>
+                <Orientation />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::FramedPose">
+            <ObjectChild key='header'>
+                <armarx::arondto::FrameID />
+            </ObjectChild>
+            <ObjectChild key='pose'>
+                <Pose />
+            </ObjectChild>
+        </Object>
+
+    </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/time.xml b/source/RobotAPI/libraries/aron/common/aron/time.xml
new file mode 100644
index 0000000000000000000000000000000000000000..390e677cda4a528839abc613b1d147511511e9e9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/time.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+    <GenerateTypes>
+
+        <IntEnum name="armarx::arondto::ClockType">
+            <EnumValue key="Realtime" value="0" />
+            <EnumValue key="Monotonic" value="1" />
+            <EnumValue key="Virtual" value="2" />
+            <EnumValue key="Unknown" value="3" />
+        </IntEnum>
+
+        <Object name="armarx::arondto::Duration">
+            <ObjectChild key='microSeconds'>
+                <long />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::Frequency">
+            <ObjectChild key='cycleDuration'>
+                <armarx::arondto::Duration />
+            </ObjectChild>
+        </Object>
+
+        <Object name="armarx::arondto::DateTime">
+            <ObjectChild key='timeSinceEpoch'>
+                <armarx::arondto::Duration />
+            </ObjectChild>
+            <ObjectChild key='clockType'>
+                <armarx::arondto::ClockType />
+            </ObjectChild>
+            <ObjectChild key='hostname'>
+                <string />
+            </ObjectChild>
+        </Object>
+
+    </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/Trajectory.xml b/source/RobotAPI/libraries/aron/common/aron/trajectory.xml
similarity index 100%
rename from source/RobotAPI/libraries/aron/common/aron/Trajectory.xml
rename to source/RobotAPI/libraries/aron/common/aron/trajectory.xml
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index d408f45ef12fe9ac8624469e6a25326e96498295..beb2f7a552e27e239c9f3b4514cfa4170bd6828a 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -1,34 +1,128 @@
 #include "armarx.h"
 
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
 
-namespace armarx
+
+void IceUtil::fromAron(const long& dto, IceUtil::Time& bo)
 {
+    bo = IceUtil::Time::microSeconds(dto);
+}
 
-    /* PackagePath */
+void IceUtil::toAron(long& dto, const IceUtil::Time& bo)
+{
+    dto = bo.toMicroSeconds();
+}
 
-    void fromAron(const arondto::PackagePath& dto, PackagePath& bo)
-    {
-        bo = {dto.package, dto.path};
-    }
 
-    void toAron(arondto::PackagePath& dto, const PackagePath& bo)
-    {
-        const data::PackagePath icedto = bo.serialize();
-        dto.package                    = icedto.package;
-        dto.path                       = icedto.path;
-    }
+void IceUtil::fromAron(const IceUtil::Time& dto, armarx::DateTime& bo)
+{
+    fromIce(dto, bo);
+}
+
+void IceUtil::toAron(IceUtil::Time& dto, const armarx::DateTime& bo)
+{
+    toIce(dto, bo);
+}
+
+
+/* PackagePath */
+
+void armarx::fromAron(const arondto::PackagePath& dto, PackagePath& bo)
+{
+    bo = {dto.package, dto.path};
+}
+
+void armarx::toAron(arondto::PackagePath& dto, const PackagePath& bo)
+{
+    const data::PackagePath icedto = bo.serialize();
+    dto.package                    = icedto.package;
+    dto.path                       = icedto.path;
+}
+
 
-    void fromAron(const long& dto, IceUtil::Time& bo)
+void armarx::fromAron(const arondto::ClockType& dto, ClockType& bo)
+{
+    switch (dto.value)
     {
-        bo = IceUtil::Time::microSeconds(dto);
+        case arondto::ClockType::Realtime:
+            bo = ClockType::Realtime;
+            break;
+        case arondto::ClockType::Monotonic:
+            bo = ClockType::Monotonic;
+            break;
+        case arondto::ClockType::Virtual:
+            bo = ClockType::Virtual;
+            break;
+        case arondto::ClockType::Unknown:
+            bo = ClockType::Unknown;
+            break;
     }
+}
 
-    void toAron(long& dto, const IceUtil::Time& bo)
+void armarx::toAron(arondto::ClockType& dto, const ClockType& bo)
+{
+    switch (bo)
     {
-        dto = bo.toMicroSeconds();
+        case ClockType::Realtime:
+            dto = arondto::ClockType::Realtime;
+            break;
+        case ClockType::Monotonic:
+            dto = arondto::ClockType::Monotonic;
+            break;
+        case ClockType::Virtual:
+            dto = arondto::ClockType::Virtual;
+            break;
+        case ClockType::Unknown:
+            dto = arondto::ClockType::Unknown;
+            break;
     }
+}
+
+void armarx::fromAron(const arondto::Duration& dto, Duration& bo)
+{
+    bo = Duration::MicroSeconds(dto.microSeconds);
+}
+
+void armarx::toAron(arondto::Duration& dto, const Duration& bo)
+{
+    dto.microSeconds = bo.toMicroSeconds();
+}
 
-} // namespace armarx
+void armarx::fromAron(const arondto::Frequency& dto, Frequency& bo)
+{
+    Duration cycleDuration;
+    fromAron(dto.cycleDuration, cycleDuration);
+    bo = Frequency(cycleDuration);
+}
+
+void armarx::toAron(arondto::Frequency& dto, const Frequency& bo)
+{
+    arondto::Duration cycleDuration;
+    toAron(cycleDuration, bo.toCycleDuration());
+    dto.cycleDuration = cycleDuration;
+}
+
+void armarx::fromAron(const arondto::DateTime& dto, DateTime& bo)
+{
+    Duration timeSinceEpoch;
+    fromAron(dto.timeSinceEpoch, timeSinceEpoch);
+    ClockType clockType;
+    fromAron(dto.clockType, clockType);
+    bo = DateTime(timeSinceEpoch, clockType, dto.hostname);
+}
+
+void armarx::toAron(arondto::DateTime& dto, const DateTime& bo)
+{
+    arondto::Duration timeSinceEpoch;
+    toAron(timeSinceEpoch, bo.toDurationSinceEpoch());
+    dto.timeSinceEpoch = timeSinceEpoch;
+    arondto::ClockType clockType;
+    toAron(clockType, bo.clockType());
+    dto.clockType = clockType;
+    dto.hostname = bo.hostname();
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
index 48c24690503cec91a17da37909a53c3228984e26..807605ec099b5e41110992d28c23bcecb0b92be0 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -1,17 +1,39 @@
 #pragma once
 
 #include <ArmarXCore/core/PackagePath.h>
-
-#include <IceUtil/Time.h>
+#include <ArmarXCore/core/time_minimal.h>
 
 #include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
+
+
+namespace IceUtil
+{
+    class Time;
+
+    void fromAron(const long& dto, IceUtil::Time& bo);
+    void toAron(long& dto, const IceUtil::Time& bo);
 
+    void fromAron(const IceUtil::Time& dto, armarx::DateTime& bo);
+    void toAron(IceUtil::Time& dto, const armarx::DateTime& bo);
+}
 namespace armarx
 {
+
     void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
     void toAron(arondto::PackagePath& dto, const PackagePath& bo);
 
-    void fromAron(const long& dto, IceUtil::Time& bo);
-    void toAron(long& dto, const IceUtil::Time& bo);
+    void fromAron(const arondto::ClockType& dto, ClockType& bo);
+    void toAron(arondto::ClockType& dto, const ClockType& bo);
+
+    void fromAron(const arondto::Duration& dto, Duration& bo);
+    void toAron(arondto::Duration& dto, const Duration& bo);
+
+    void fromAron(const arondto::Frequency& dto, Frequency& bo);
+    void toAron(arondto::Frequency& dto, const Frequency& bo);
+
+    void fromAron(const arondto::DateTime& dto, DateTime& bo);
+    void toAron(arondto::DateTime& dto, const DateTime& bo);
 
 }  // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
index c87d13cdbd7e30aa5b7984cda69cb80261c378a7..f8629190d5f2dac1dd8189f4eb2be2425479464e 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
@@ -1,5 +1,8 @@
 #include "simox.h"
 
+#include <RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/Color.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
 
 
 void simox::fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo)
@@ -15,7 +18,6 @@ void simox::toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundi
 }
 
 
-
 void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
 {
     bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
@@ -28,3 +30,19 @@ void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
     dto.extents = bo.dimensions();
 }
 
+
+void simox::fromAron(const arondto::Color& dto, Color& bo)
+{
+    bo.r = dto.r;
+    bo.g = dto.g;
+    bo.b = dto.b;
+    bo.a = dto.a;
+}
+
+void simox::toAron(arondto::Color& dto, const Color& bo)
+{
+    dto.r = bo.r;
+    dto.g = bo.g;
+    dto.b = bo.b;
+    dto.a = bo.a;
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
index 64e847afbb06c5b58d92db3817c7adccf7e3b563..5440ac1c6e39134c0034ccb67a06c4fff94445fa 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
@@ -1,10 +1,10 @@
 #pragma once
 
+#include <SimoxUtility/color/Color.h>
 #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
-#include <RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>
-
 #include <SimoxUtility/shapes/OrientedBox.h>
-#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
+
+#include <RobotAPI/libraries/aron/common/forward_declarations.h>
 
 
 namespace simox
@@ -14,5 +14,8 @@ namespace simox
 
     void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
     void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+
+    void fromAron(const arondto::Color& dto, Color& bo);
+    void toAron(arondto::Color& dto, const Color& bo);
 }
 
diff --git a/source/RobotAPI/libraries/aron/common/forward_declarations.h b/source/RobotAPI/libraries/aron/common/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..626147c3fc35e827a7b905c8a8b0e76babf3f25e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/forward_declarations.h
@@ -0,0 +1,13 @@
+#pragma once
+
+
+namespace simox::arondto
+{
+    class AxisAlignedBoundingBox;
+    class Color;
+    class OrientedBox;
+}
+namespace armarx::arondto
+{
+    class Names;
+}
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions.h b/source/RobotAPI/libraries/aron/common/json_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..a56bd44287eb16161f84e5d1ef0f5496903e2669
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions.h
@@ -0,0 +1,3 @@
+#pragma once
+
+#include "json_conversions/armarx.h"
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96a26aa0fbe0930037534dbce431fc715669c299
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp
@@ -0,0 +1,16 @@
+#include "armarx.h"
+
+#include <RobotAPI/libraries/aron/common/aron/Names.aron.generated.h>
+
+
+void armarx::arondto::to_json(nlohmann::json& j, const Names& bo)
+{
+    j["recognized"] = bo.recognized;
+    j["spoken"] = bo.spoken;
+}
+
+void armarx::arondto::from_json(const nlohmann::json& j, Names& bo)
+{
+    j.at("recognized").get_to(bo.recognized);
+    j.at("spoken").get_to(bo.spoken);
+}
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h
new file mode 100644
index 0000000000000000000000000000000000000000..4da7677a12e5537ae2d41b0cb67362cc112d49c6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <RobotAPI/libraries/aron/common/forward_declarations.h>
+
+
+namespace armarx::arondto
+{
+
+    void to_json(nlohmann::json& j, const Names& bo);
+    void from_json(const nlohmann::json& j, Names& bo);
+
+}
diff --git a/source/RobotAPI/libraries/aron/common/util/object_finders.cpp b/source/RobotAPI/libraries/aron/common/util/object_finders.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e6bba147d3f274652f3e173667013383d5d56697
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/util/object_finders.cpp
@@ -0,0 +1,60 @@
+/**
+ * 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/>.
+ *
+ * @package    RobotAPI::libraries::aron
+ * @author     Philip Scherer ( ulila@student.kit.edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "object_finders.h"
+
+namespace armarx::aron
+{
+    SubObjectFinder::SubObjectFinder(const std::string& typeNamePrefix) :
+        typeNamePrefix(typeNamePrefix)
+    {
+    }
+
+    const std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>>&
+    SubObjectFinder::getFoundObjects()
+    {
+        return foundSubObjects;
+    }
+
+    void
+    SubObjectFinder::visitObjectOnEnter(DataInput& elementData, TypeInput& elementType)
+    {
+        if (elementType && simox::alg::starts_with(elementType->getFullName(), typeNamePrefix))
+        {
+            foundSubObjects.emplace(elementData->getPath().toString(),
+                                    std::make_pair(elementData, elementType));
+        }
+    }
+
+    void
+    SubObjectFinder::visitUnknown(DataInput& elementData, TypeInput& elementType)
+    {
+        // Ignore (the base class throws an exception here)
+    }
+
+    SubObjectFinder::MapElements
+    SubObjectFinder::getObjectElements(DataInput& elementData, TypeInput& elementType)
+    {
+        return RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(elementData,
+                                                                                elementType);
+    }
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/util/object_finders.h b/source/RobotAPI/libraries/aron/common/util/object_finders.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff67043de5cea4e608dd3f9d488130d80255005d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/util/object_finders.h
@@ -0,0 +1,132 @@
+/**
+ * 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/>.
+ *
+ * @package    RobotAPI::libraries::aron
+ * @author     Philip Scherer ( ulila@student.kit.edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::aron
+{
+    /**
+     * Finds aron objects with a given type name prefix in aron variants.
+     *
+     * Construct it with the prefix you want to search for,
+     * then use `visitRecursive` to run the finder over the variant to search.
+     * Get a map of paths to data and type of the found objects using `getFoundObjects`.
+     */
+    class SubObjectFinder : public armarx::aron::data::RecursiveConstTypedVariantVisitor
+    {
+    public:
+        SubObjectFinder(const std::string& typeNamePrefix);
+
+        ~SubObjectFinder() override = default;
+
+        /**
+         * Get the objects that have been found.
+         *
+         * The keys are strings instead of `aron::Path`s because those don't support comparisons.
+         *
+         * @return map of paths to pair of data and type variants
+         */
+        const std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>>&
+        getFoundObjects();
+
+        void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) override;
+
+        void visitUnknown(DataInput& elementData, TypeInput& elementType) override;
+
+        MapElements getObjectElements(DataInput& elementData, TypeInput& elementType) override;
+
+    private:
+        std::string typeNamePrefix;
+        std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>>
+            foundSubObjects;
+    };
+
+    /**
+     * Finds aron objects with a given type name prefix in aron variants and returns them as BOs.
+     *
+     * Construct it with the prefix you want to search for,
+     * then use `visitRecursive` to run the finder over the variant to search.
+     * Get a map of paths to your BOs using `getFoundObjects`.
+     *
+     * \tparam DTOType the _generated_ aron type of your data.
+     * \tparam BOType the type of your final BO.
+     */
+    template <typename DTOType, typename BOType>
+    class BOSubObjectFinder : public armarx::aron::data::RecursiveConstTypedVariantVisitor
+    {
+    public:
+        BOSubObjectFinder() = default;
+        ~BOSubObjectFinder() override = default;
+
+        /**
+         * Get the objects that have been found.
+         *
+         * The keys are strings instead of `aron::Path`s because those don't support comparisons.
+         *
+         * @return map of paths to BOs.
+         */
+        const std::map<std::string, BOType>&
+        getFoundObjects()
+        {
+            return foundSubObjects;
+        }
+
+        void
+        visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) override
+        {
+            if (elementType->getFullName() == DTOType::ToAronType()->getFullName())
+            {
+                auto dict = armarx::aron::data::Dict::DynamicCastAndCheck(elementData);
+                DTOType dto;
+                dto.fromAron(dict);
+                BOType boObj = armarx::aron::fromAron<BOType, DTOType>(dto);
+                foundSubObjects.emplace(elementData->getPath().toString(), boObj);
+            }
+        }
+
+        void
+        visitUnknown(DataInput& elementData, TypeInput& elementType) override
+        {
+            // Ignore (the base class throws an exception here)
+        }
+
+        MapElements
+        getObjectElements(DataInput& elementData, TypeInput& elementType) override
+        {
+            return RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(elementData,
+                                                                                    elementType);
+        }
+
+    private:
+        std::map<std::string, BOType> foundSubObjects;
+    };
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
index 8eb3599faab31af90f0cf7666494f4eaf946d468..537b389e36191a4a59cd0da08d7ca26e3f889855 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
@@ -12,7 +12,7 @@ namespace armarx::aron::converter
     void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::data::VariantPtr& aron, nlohmann::json& j)
     {
         aron::data::writer::NlohmannJSONWriter dataWriter;
-        j = aron::data::readAndWrite<DataVariant2NlohmannJSONConverterHelper>(aron);
+        j = aron::data::readAndWrite<data::FromVariantConverter<data::writer::NlohmannJSONWriter>>(aron);
     }
 
     nlohmann::json AronNlohmannJSONConverter::ConvertToNlohmannJSON(const type::VariantPtr& aron)
@@ -25,7 +25,7 @@ namespace armarx::aron::converter
     void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::type::VariantPtr& aron, nlohmann::json& j)
     {
         aron::type::writer::NlohmannJSONWriter typeWriter;
-        j = aron::type::readAndWrite<TypeVariant2NlohmannJSONConverterHelper>(aron);
+        j = aron::type::readAndWrite<type::FromVariantConverter<type::writer::NlohmannJSONWriter>>(aron);
     }
 
     data::DictPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(const nlohmann::json& j)
@@ -37,9 +37,21 @@ namespace armarx::aron::converter
         return data::Dict::DynamicCastAndCheck(aron);
     }
 
+    type::ObjectPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSONTypeObject(const nlohmann::json& j)
+    {
+        type::VariantPtr aron = std::make_shared<aron::type::Object>("foo"); // foo is just a placeholder
+        ConvertFromNlohmannJSON(aron, j);
+        return type::Object::DynamicCastAndCheck(aron);
+    }
+
     void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::data::VariantPtr& a, const nlohmann::json& e, const aron::type::VariantPtr& expectedStructure)
     {
-        aron::data::writer::NlohmannJSONWriter dataWriter;
-        a = aron::data::readAndWrite<NlohmannJSON2DataVariantConverterHelper>(e);
+        a = aron::data::readAndWrite<data::FromNlohmannJSONConverter<data::writer::VariantWriter>>(e);
+        ARMARX_CHECK_EXPRESSION(a->fullfillsType(expectedStructure));
+    }
+
+    void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::type::VariantPtr& a, const nlohmann::json& e)
+    {
+        a = aron::type::readAndWrite<type::FromNlohmannJSONConverter<type::writer::VariantWriter>>(e);
     }
 }
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
index 0a4b2a5c39c138c9be0f221f82b1f1c9e699638e..6cce4d7e363795f6a70184faf4fd60110516590f 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
@@ -29,27 +29,6 @@ namespace armarx::aron::converter
 {
     class AronNlohmannJSONConverter
     {
-    private:
-        struct DataVariant2NlohmannJSONConverterHelper :
-                public data::FromVariantConverter<data::writer::NlohmannJSONWriter>
-        {
-        };
-
-        struct NlohmannJSON2DataVariantConverterHelper :
-                public data::FromNlohmannJSONConverter<data::writer::VariantWriter>
-        {
-        };
-
-        struct TypeVariant2NlohmannJSONConverterHelper :
-                public type::FromVariantConverter<type::writer::NlohmannJSONWriter>
-        {
-        };
-
-        struct NlohmannJSON2TypeVariantConverterHelper :
-                public type::FromNlohmannJSONConverter<type::writer::VariantWriter>
-        {
-        };
-
     public:
         AronNlohmannJSONConverter() = delete;
 
@@ -61,5 +40,8 @@ namespace armarx::aron::converter
 
         static data::DictPtr ConvertFromNlohmannJSONObject(const nlohmann::json&);
         static void ConvertFromNlohmannJSON(data::VariantPtr&, const nlohmann::json&, const aron::type::VariantPtr& = nullptr);
+
+        static type::ObjectPtr ConvertFromNlohmannJSONTypeObject(const nlohmann::json& j);
+        static void ConvertFromNlohmannJSON(aron::type::VariantPtr& a, const nlohmann::json& e);
     };
 }
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
index 9fa96dc6af108cc089da8774d7fd59d28291c428..4a3eadd0caaec648db2a175f7a103b33b7a3acad 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
@@ -7,7 +7,9 @@ armarx_build_if(OpenCV_FOUND "OpenCV not available")
 
 set(LIBS
     aron 
-    ${OpenCV_LIBRARIES}
+    opencv_core
+    opencv_imgcodecs
+    opencv_imgproc
 )
 
 set(LIB_FILES
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
index 8e437dd62935c3de5fadf1c93daf10d89270c813..4c0969f0de800c66a6a766fa4a4a80ebf21f6c64 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -211,7 +211,7 @@ namespace armarx::aron::codegenerator::cpp
 
         CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = armarx::aron::type::Maybe::eNone, const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path())", doc.str()));
         CppBlockPtr b = std::make_shared<CppBlock>();
-        b->addLine("using T [[maybe_unused]] = typename WriterT::ReturnType;");
+        b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
 
         std::string dummy;
         b->appendBlock(this->getWriteTypeBlock("", "", Path(), dummy));
@@ -228,7 +228,7 @@ namespace armarx::aron::codegenerator::cpp
 
         CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
         CppBlockPtr b = std::make_shared<CppBlock>();
-        b->addLine("using T [[maybe_unused]] = typename WriterT::ReturnType;");
+        b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
 
         std::string dummy;
         b->appendBlock(this->getWriteBlock("", Path(), dummy));
@@ -245,8 +245,8 @@ namespace armarx::aron::codegenerator::cpp
 
         CppMethodPtr m = CppMethodPtr(new CppMethod("template<class ReaderT>\nvoid read(ReaderT& " + ARON_READER_ACCESSOR + ", typename ReaderT::InputType& input)", doc.str()));
         CppBlockPtr b = std::make_shared<CppBlock>();
-        b->addLine("using T [[maybe_unused]] = typename ReaderT::InputType;");
-        b->addLine("using TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;");
+        b->addLine("using _Aron_T [[maybe_unused]] = typename ReaderT::InputType;");
+        b->addLine("using _Aron_TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;");
 
         b->addLine("this->resetSoft();");
         b->addLine("if (" + ARON_READER_ACCESSOR + ".readNull(input))");
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp
index 16d73e0611df481b0139d895ab10529c2577387e..5d50b76c48b3aaf664722d97152e6390b8145b0e 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp
@@ -72,7 +72,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
 
         const std::string elementsAccessor = variantAccessor + "_dictElements";
-        block_if_data->addLine("std::map<std::string, T> " + elementsAccessor + ";");
+        block_if_data->addLine("std::map<std::string, _Aron_T> " + elementsAccessor + ";");
 
         std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_key";
         std::string accessor_iterator_val = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_value";
@@ -107,7 +107,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictValue";
         std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictKey";
 
-        block_if_data->addLine("std::map<std::string, TNonConst> " + elements_accessor + ";");
+        block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + elements_accessor + ";");
         block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict(" + variantAccessor + ", " + elements_accessor + ");");
         block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_value + "] : " + elements_accessor + ")");
         {
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp
index a69f1767565aa2ea8f242c27a4fd03b4450567c3..76094035446a7f90a3d39afe85a8d1bd16ae3e23 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp
@@ -71,7 +71,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
 
         const std::string elementsAccessor = variantAccessor + "_listElements";
-        block_if_data->addLine("std::vector<T> " + elementsAccessor + ";");
+        block_if_data->addLine("std::vector<_Aron_T> " + elementsAccessor + ";");
 
         std::string accessor_iterator = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_it";
 
@@ -102,7 +102,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listElements";
         std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listValue";
 
-        block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";");
+        block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";");
         block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + variantAccessor + ", " + elements_accessor + ");");
         block_if_data->addLine("for (const auto& " + accessor_iterator_value + " : " + elements_accessor + ")");
         {
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp
index 4f7cde8f0582a191037c5cd49532f785221394e9..d4edd5476373c0c49029c39ccc7537259116192f 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp
@@ -114,7 +114,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         std::string escaped_accessor = EscapeAccessor(cppAccessor);
         std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_pairElements";
 
-        block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";");
+        block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";");
         block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor);
 
         auto child_s1 = FromAronType(*type.getFirstAcceptedType());
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
index 5f2c241cb9024f721f867a54c1bad9a30db96759..84db8b60aab51848000f36d38cc85c8ad6889151 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
@@ -60,7 +60,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
 
         const std::string acceptedTypesAccessor = variantAccessor + "_tupleAcceptedTypes";
-        block_if_data->addLine("std::vector<T> " + acceptedTypesAccessor + ";");
+        block_if_data->addLine("std::vector<_Aron_T> " + acceptedTypesAccessor + ";");
 
         unsigned int i = 0;
         for (const auto& type : type.getAcceptedTypes())
@@ -87,7 +87,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
 
         const std::string elementsAccessor = variantAccessor + "_tupleElements";
-        block_if_data->addLine("std::vector<T> " + elementsAccessor + ";");
+        block_if_data->addLine("std::vector<_Aron_T> " + elementsAccessor + ";");
 
         unsigned int i = 0;
         for (const auto& type : type.getAcceptedTypes())
@@ -114,7 +114,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
         std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_tupleElements";
 
-        block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";");
+        block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";");
         block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor);
 
         unsigned int i = 0;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
index 70df7d108e582d0badc09abd7922296fba769dd5..cf15c169b4afec4e5fd833e40bb16b6e74672dc8 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
@@ -97,7 +97,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
 
         CppBlockPtr b = std::make_shared<CppBlock>();
-        b->addLine("std::map<std::string, T> " + OBJECT_MEMBERS_ACCESSOR + ";");
+        b->addLine("std::map<std::string, _Aron_T> " + OBJECT_MEMBERS_ACCESSOR + ";");
 
         if (type.getExtends() != nullptr)
         {
@@ -152,9 +152,9 @@ namespace armarx::aron::codegenerator::cpp::generator
 
         CppBlockPtr block_if_data = std::make_shared<CppBlock>();
 
-        block_if_data->addLine("std::map<std::string, T> " + OBJECT_MEMBERS_ACCESSOR + ";");
+        block_if_data->addLine("std::map<std::string, _Aron_T> " + OBJECT_MEMBERS_ACCESSOR + ";");
 
-        block_if_data->addLine("std::optional<T> " + OBJECT_EXTENDS_ACCESSOR + ";");
+        block_if_data->addLine("std::optional<_Aron_T> " + OBJECT_EXTENDS_ACCESSOR + ";");
         if (type.getExtends() != nullptr)
         {
             const auto extends_s = FromAronType(*type.getExtends());
@@ -187,7 +187,7 @@ namespace armarx::aron::codegenerator::cpp::generator
         static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
 
         CppBlockPtr block_if_data = std::make_shared<CppBlock>();
-        block_if_data->addLine("std::map<std::string, TNonConst> " + OBJECT_MEMBERS_ACCESSOR + ";");
+        block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + OBJECT_MEMBERS_ACCESSOR + ";");
 
         if (type.getExtends() != nullptr)
         {
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
index 4ff55335a50edba6543b171a6bdc323dc50823e6..0be0b34898308d217d221659d3524d77b2cf8f5c 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
@@ -51,5 +51,7 @@ namespace armarx::aron::data::writer
         data::VariantPtr writeDouble(const double i, const Path& p = Path()) override;
         data::VariantPtr writeString(const std::string& i, const Path& p = Path()) override;
         data::VariantPtr writeBool(const bool i, const Path& p = Path()) override;
+
+        using WriterInterface<data::VariantPtr>::writeDict;
     };
 }
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
index 94ec4a97f6e8159fa0a68f4a16a83330c55441a9..c228a449d39e10ff712942311f4181f6281461f9 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -53,7 +53,8 @@ namespace armarx::aron::data
             case data::Descriptor::eDouble: return std::make_unique<data::Double>(data::dto::AronDoublePtr::dynamicCast(aron), path);
             case data::Descriptor::eString: return std::make_unique<data::String>(data::dto::AronStringPtr::dynamicCast(aron), path);
             case data::Descriptor::eBool: return std::make_unique<data::Bool>(data::dto::AronBoolPtr::dynamicCast(aron), path);
-            default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "", std::to_string((int) descriptor), path);
+            case data::Descriptor::eUnknown: break;
         }
+        throw error::ValueNotValidException(__PRETTY_FUNCTION__, "", std::to_string((int) descriptor), path);
     }
 }
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index 7de89a06a92c1da3e84046c3af98c2d10d602515..fd840a80dfb192ad017fa481a2a167fc9cd872a2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -177,10 +177,7 @@ namespace armarx::aron::data
 
     bool NDArray::fullfillsType(const type::VariantPtr& type) const
     {
-        if (!type)
-        {
-            return false;
-        }
+        if (!type) return true;
 
         type::Descriptor typeDesc = type->getDescriptor();
         switch (typeDesc)
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
index 31677060acad93fe7fbbbe949b4439d434392931..593298a7df80e167b9525ce6280cebdbb132cd59 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
@@ -97,3 +97,12 @@ namespace armarx::aron::data
         virtual bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::NDArrayPtr make_ndarray(_Args&&... args)
+    {
+        return std::make_shared<aron::data::NDArray>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 1745474f386a2e7e05a078c5486a5322067b3dba..12b402fd0cb2d33cb3b3314ee7c9fa373f1128ff 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -208,10 +208,7 @@ namespace armarx::aron::data
 
     bool Dict::fullfillsType(const type::VariantPtr& type) const
     {
-        if(!type)
-        {
-            return false;
-        }
+        if (!type) return true;
 
         type::Descriptor typeDesc = type->getDescriptor();
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
index f022d02645d2b4b9b255de2ba43e982e150fc383..6087ddfac5c8f91123376afa5fe7eb2c60010cf8 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -26,6 +26,7 @@
 // STD/STL
 #include <memory>
 #include <map>
+#include <utility>
 
 // Base class
 #include "../detail/ContainerVariant.h"
@@ -98,3 +99,12 @@ namespace armarx::aron::data
         std::map<std::string, VariantPtr> childrenNavigators;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::DictPtr make_dict(_Args&&... args)
+    {
+        return std::make_shared<aron::data::Dict>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index bb86d4f1baff4013c62c788a7f6ddf0f99f80a1c..74eb78a75c6a38bcee5008025596126ba1a2ce99 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -211,10 +211,7 @@ namespace armarx::aron::data
 
     bool List::fullfillsType(const type::VariantPtr& type) const
     {
-        if (!type)
-        {
-            return false;
-        }
+        if (!type) return true;
 
         type::Descriptor typeDesc = type->getDescriptor();
         switch (typeDesc)
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
index b766ce5aae83416ae632b7e0ff27c30ade72c0e0..4ab656bfa2e11461ffb4914a2fc444f16c5ce6f8 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -97,3 +97,12 @@ namespace armarx::aron::data
         std::vector<VariantPtr> childrenNavigators;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::ListPtr make_list(_Args&&... args)
+    {
+        return std::make_shared<aron::data::List>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 7c5e526822769e56b61e3b0099509274f6ae9224..5b5cb2b0ed8cb7c3f5eb6f276723cbfadbc181da 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -110,6 +110,7 @@ namespace armarx::aron::data
 
     bool Bool::fullfillsType(const type::VariantPtr& type) const
     {
+        if (!type) return true;
         return type->getDescriptor() == type::Descriptor::eBool;
     }
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index 65cfc31a7bec67f63e8f9bf5101f697c7d61e2c7..5e07b8a8e33577b268ea8ae8366291664b3012e6 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
         bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::BoolPtr make_bool(_Args&&... args)
+    {
+        return std::make_shared<aron::data::Bool>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index 729e5bda480e8a21d14e16be1f454a02d1f7cf9b..906afeeea30f14270097d70b1868517dac91acb3 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -103,6 +103,7 @@ namespace armarx::aron::data
 
     bool Double::fullfillsType(const type::VariantPtr& type) const
     {
+        if (!type) return true;
         return type->getDescriptor() == type::Descriptor::eDouble;
     }
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index b02571f1dd7cef61635849b21edddf943469c7cd..fb7a6880a9782397239243d38939d1ac22688fd4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
         bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::DoublePtr make_double(_Args&&... args)
+    {
+        return std::make_shared<aron::data::Double>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index f638882eaf80af2a6a4cba0f9d1fffe43cd6e304..154591f68942d63e02139fd866e7013eaea3f69e 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -100,6 +100,7 @@ namespace armarx::aron::data
 
     bool Float::fullfillsType(const type::VariantPtr& type) const
     {
+        if (!type) return true;
         return type->getDescriptor() == type::Descriptor::eFloat;
     }
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index d7b44e56784ef2a6a1c8b35ebb3b7b3841448426..2d0cecb7e609df5496da3e616ffcd0fe6e249800 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
         bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::FloatPtr make_float(_Args&&... args)
+    {
+        return std::make_shared<aron::data::Float>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index b5f0d39196b750084876ad85a49f9f77cdf23112..14299d05a3f687d58916c4498dc4f50d167dad3a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -103,6 +103,7 @@ namespace armarx::aron::data
 
     bool Int::fullfillsType(const type::VariantPtr& type) const
     {
+        if (!type) return true;
         return type->getDescriptor() == type::Descriptor::eInt || type->getDescriptor() == type::Descriptor::eIntEnum;
     }
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index 9a803a035d0be45cd7f38241a937f67179c2bfa2..5ee4a270f60bd2b21d8e2257c814f5dab8a0b759 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -69,3 +69,12 @@ namespace armarx::aron::data
         bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::IntPtr make_int(_Args&&... args)
+    {
+        return std::make_shared<aron::data::Int>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 1885fc750b5cee8a7b477627167972c37e514db9..c81aaa05054f3734c342a7c174d8c7f31407b9e4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -104,6 +104,7 @@ namespace armarx::aron::data
 
     bool Long::fullfillsType(const type::VariantPtr& type) const
     {
+        if (!type) return true;
         return type->getDescriptor() == type::Descriptor::eLong || type->getDescriptor() == type::Descriptor::eTime;
     }
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index 82b9481be9f957271735d55686659767b60c53ec..0cbcbdb66efa8f6446831ac62617bbfd57119f9b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -69,3 +69,12 @@ namespace armarx::aron::data
         bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::LongPtr make_long(_Args&&... args)
+    {
+        return std::make_shared<aron::data::Long>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index b896221b06d79f3e9cee12b2d84ec9156216dc7f..0c4ec5bd375fcb0e74e7564bd5a9caecbb3b5109 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -100,6 +100,7 @@ namespace armarx::aron::data
 
     bool String::fullfillsType(const type::VariantPtr& type) const
     {
+        if (!type) return true;
         return type->getDescriptor() == type::Descriptor::eString;
     }
 
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
index 33e9ef6ea7d160bc5dab7e64cc043cc7948c1e34..21c23698c114262b3d71658288cef82e12e4747f 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
         bool fullfillsType(const type::VariantPtr&) const override;
     };
 }
+
+namespace armarx::aron
+{
+    template<typename... _Args>
+    aron::data::StringPtr make_string(_Args&&... args)
+    {
+        return std::make_shared<aron::data::String>(args...);
+    }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
index a91f1ac118ba3525ee8a1030915711fb28448c5f..0f6f90a193a9391f943fca4466c294f6ac9d69e9 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
@@ -536,6 +536,31 @@ namespace armarx::aron::data
         return ret;
     }
 
+    RecursiveConstTypedVariantVisitor::MapElements
+    RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(DataInput& o, TypeInput& t)
+    {
+        std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>> ret;
+        auto data = aron::data::Dict::DynamicCastAndCheck(o);
+        auto type = aron::type::Object::DynamicCastAndCheck(t);
+
+        if (data)
+        {
+            for (const auto& [key, e] : data->getElements())
+            {
+                if (type && type->hasMemberType(key))
+                {
+                    auto memberType = type->getMemberType(key);
+                    ret.insert({key, {e, memberType}});
+                }
+                else
+                {
+                    ret.insert({key, {e, nullptr}});
+                }
+            }
+        }
+        return ret;
+    }
+
     RecursiveConstTypedVariantVisitor::MapElements
     RecursiveConstTypedVariantVisitor::GetDictElements(DataInput& o, TypeInput& t)
     {
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
index 8a8cc2a906bfc2e3e37734670468ff8207d308f7..389db80a53d43bdb0514f39cbd5276b8f71291bb 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
@@ -201,6 +201,12 @@ namespace armarx::aron::data
     {
         type::Descriptor getDescriptor(DataInput& o, TypeInput& n) override;
         static MapElements GetObjectElements(DataInput& o, TypeInput& t);
+        /* This override exists for visitors that need to handle untyped members in the hierarchy.
+         * Using it instead of `GetObjectElements` will allow your visitor to visit objects with
+         * a nullptr as their type. However, you will have to handle nullptr types in your
+         * visitor's methods.
+         */
+        static MapElements GetObjectElementsWithNullType(DataInput& o, TypeInput& t);
         static MapElements GetDictElements(DataInput& o, TypeInput& t);
         static ListElements GetListElements(DataInput& o, TypeInput& t);
         static PairElements GetPairElements(DataInput& o, TypeInput& t);
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 246a3698a0b6bd05256643d7086ebdf4641220e5..23bbfa738571caffde68365cf5e62d52fbf2cddf 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -111,7 +111,9 @@ namespace armarx::aron::type::reader
 
         maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
         ndim = input[rw::json::constantes::DIMENSIONS_SLUG];
-        type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+        std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+        type = armarx::aron::type::rw::json::conversion::String2NDArrayType.at(t);
     }
 
     void NlohmannJSONReader::readMatrix(const nlohmann::json& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p)
@@ -122,7 +124,9 @@ namespace armarx::aron::type::reader
         auto list = input[rw::json::constantes::DIMENSIONS_SLUG].get<std::vector<nlohmann::json>>();
         rows = list[0];
         cols = list[1];
-        type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+        std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+        type = armarx::aron::type::rw::json::conversion::String2MatrixType.at(t);
     }
 
     void NlohmannJSONReader::readQuaternion(const nlohmann::json& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p)
@@ -130,7 +134,9 @@ namespace armarx::aron::type::reader
         getAronMetaInformationForType(input, rw::json::constantes::QUATERNION_TYPENAME_SLUG, p);
 
         maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
-        type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+        std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+        type = armarx::aron::type::rw::json::conversion::String2QuaternionType.at(t);
     }
 
     void NlohmannJSONReader::readPointCloud(const nlohmann::json& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p)
@@ -138,7 +144,9 @@ namespace armarx::aron::type::reader
         getAronMetaInformationForType(input, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG, p);
 
         maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
-        type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+        std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+        type = armarx::aron::type::rw::json::conversion::String2VoxelType.at(t);
     }
 
     void NlohmannJSONReader::readImage(const nlohmann::json& input, type::image::PixelType& type, type::Maybe& maybe, Path& p)
@@ -146,7 +154,9 @@ namespace armarx::aron::type::reader
         getAronMetaInformationForType(input, rw::json::constantes::IMAGE_TYPENAME_SLUG, p);
 
         maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
-        type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+        std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+        type = armarx::aron::type::rw::json::conversion::String2PixelType.at(t);
     }
 
     void NlohmannJSONReader::readPosition(const nlohmann::json& input, type::Maybe& maybe, Path& p)
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
index 16fb41506f61ff66a1d7d3770c869858e7f9370d..3d7ca9846e890fe4cd14020b70490788bde2bea4 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -58,8 +58,10 @@ namespace armarx::aron::type
             case type::Descriptor::eString: return std::make_unique<type::String>(dynamic_cast<const type::dto::AronString&>(aron), path);
             case type::Descriptor::eBool: return std::make_unique<type::Bool>(dynamic_cast<const type::dto::AronBool&>(aron), path);
             case type::Descriptor::eTime: return std::make_unique<type::Time>(dynamic_cast<const type::dto::AronTime&>(aron), path);
-            default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Got an unknown descriptor", std::to_string((int) descriptor) + " aka " + defaultconversion::string::Descriptor2String.at(descriptor), path);
+            case type::Descriptor::eAnyObject: return std::make_unique<type::AnyObject>(dynamic_cast<const type::dto::AnyObject&>(aron), path);
+            case type::Descriptor::eUnknown: break;
         }
+        throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Got an unknown descriptor", std::to_string((int) descriptor) + " aka " + defaultconversion::string::Descriptor2String.at(descriptor), path);
     }
 
 }
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 9681636da9a820a53711ed50f327205216a0fcc8..c34d94957311c775b378bcda7e0dda17f38fe1c3 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -24,6 +24,7 @@
 #include "CartesianPositionController.h"
 
 #include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
 
 namespace armarx
 {
@@ -35,12 +36,14 @@ namespace armarx
 
     Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
     {
+        ARMARX_TRACE;
         int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
         int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
         Eigen::VectorXf cartesianVel(posLen + oriLen);
 
         if (posLen)
         {
+            ARMARX_TRACE;
             Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
             Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
             Eigen::Vector3f errPos = targetPos - currentPos;
@@ -54,6 +57,7 @@ namespace armarx
 
         if (oriLen)
         {
+            ARMARX_TRACE;
             Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
             Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
             Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
@@ -72,12 +76,14 @@ namespace armarx
 
     Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
     {
+        ARMARX_TRACE;
         Eigen::VectorXf cartesianVel(3);
         Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
         Eigen::Vector3f errPos = targetPos - currentPos;
         Eigen::Vector3f posVel =  errPos * KpPos;
         if (maxPosVel > 0)
         {
+            ARMARX_TRACE;
             posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
         }
         cartesianVel.block<3, 1>(0, 0) = posVel;
@@ -98,6 +104,7 @@ namespace armarx
             const VirtualRobot::RobotNodePtr& tcp,
             const VirtualRobot::RobotNodePtr& referenceFrame)
     {
+        ARMARX_TRACE;
         Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
         Eigen::Vector3f tcpPos = referenceFrame ? tcp->getPositionInFrame(referenceFrame) : tcp->getPositionInRootFrame();
         Eigen::Vector3f errPos = targetPos - tcpPos;
@@ -108,6 +115,7 @@ namespace armarx
             const VirtualRobot::RobotNodePtr& tcp,
             const VirtualRobot::RobotNodePtr& referenceFrame)
     {
+        ARMARX_TRACE;
         Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
         Eigen::Matrix4f tcpPose = referenceFrame ? tcp->getPoseInFrame(referenceFrame) : tcp->getPoseInRootFrame();
         Eigen::Matrix3f tcpOri = tcpPose.block<3, 3>(0, 0);
@@ -129,6 +137,7 @@ namespace armarx
 
     bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
     {
+        ARMARX_TRACE;
         if (mode & VirtualRobot::IKSolver::Position)
         {
             if (GetPositionError(targetPose, tcp, referenceFrame) > thresholdPosReached)
@@ -148,6 +157,7 @@ namespace armarx
 
     Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
     {
+        ARMARX_TRACE;
         Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
         return targetPos - tcp->getPositionInFrame(referenceFrame);
     }
@@ -159,6 +169,7 @@ namespace armarx
 
     Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
     {
+        ARMARX_TRACE;
         Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
         Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
         Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 9527835a6dd48107f5e11846ec882d30b056c54a..93d3df8e907c5086e00a470d9f4e9bdfe3dfb37f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -22,14 +22,16 @@
  */
 
 #include "CartesianVelocityController.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <Eigen/Core>
 
 #include <RobotAPI/libraries/core/math/MathUtils.h>
 
-#include <VirtualRobot/math/Helpers.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/Robot.h>
+
+#include <Eigen/Core>
 
 using namespace armarx;
 
@@ -48,6 +50,7 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
 
 void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
 {
+    ARMARX_TRACE;
     jacobi = ik->getJacobianMatrix(_tcp, mode);
     _jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
     for (int r = 0; r < jacobi.rows(); r++)
@@ -98,7 +101,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
 
 Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
 {
-
+    ARMARX_TRACE;
     calculateJacobis(mode);
 
 
@@ -106,6 +109,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
 
     if (nullspaceVel.rows() > 0)
     {
+        ARMARX_TRACE;
         //    ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
         Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
         //ARMARX_IMPORTANT << "The rank of the _jacobiWithCosts is " << lu_decomp.rank();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 1c2b0b28ee9607a989e25ca984e12fb893ce6ca4..3e37d44f53308858b756f84423cf06cd65995404 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -23,11 +23,10 @@
 
 #pragma once
 
-#include <VirtualRobot/RobotNodeSet.h>
-
 #include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
-#include <Eigen/Dense>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <Eigen/Core>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index a72fe97e50b3249595840b6fe18e1aca8be36542..ee06ab837f18e01764757e98e9aa372fc63e785f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -77,6 +77,7 @@ namespace armarx
 
     Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
     {
+        ARMARX_TRACE;
         return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
     }
 
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index fe9477986cee0a9d9a9a55b3f94d40206c010e3e..917877b7a5ce1556ece46a9d8353b274dba87465 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -32,6 +32,7 @@
 #include <VirtualRobot/math/Helpers.h>
 #include <VirtualRobot/MathTools.h>
 #include <cfloat>
+#include <ArmarXCore/util/CPPUtility/trace.h>
 
 namespace armarx
 {
@@ -64,21 +65,25 @@ namespace armarx
 
     const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
     {
+        ARMARX_TRACE;
         //calculate cartesian velocity + some management stuff
         if (!isLastWaypoint() && isCurrentTargetNear())
         {
+            ARMARX_TRACE;
             currentWaypointIndex++;
         }
         cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
 
         if (autoClearFeedForward)
         {
+            ARMARX_TRACE;
             clearFeedForwardVelocity();
         }
 
         //calculate joint velocity
         if (nullSpaceControlEnabled)
         {
+            ARMARX_TRACE;
             //avoid joint limits
             _jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
                    ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
@@ -89,9 +94,11 @@ namespace armarx
         }
         else
         {
+            ARMARX_TRACE;
             //don't avoid joint limits
             _jnv *= 0;
         }
+        ARMARX_TRACE;
         _out = ctrlCartesianVelWithRamps.calculate(cartesianVelocity, _jnv, dt);
         return _out;
     }
@@ -178,11 +185,13 @@ namespace armarx
 
     const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
     {
+        ARMARX_TRACE;
         return ::math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
     }
 
     size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
     {
+        ARMARX_TRACE;
         float dist = FLT_MAX;
         size_t minIndex = 0;
         for (size_t i = 0; i < waypoints.size(); i++)
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 416121d847e6cf9dab5cbb280fa0d764fbf263c0..bbf9d1d35d12fff2ceb8b51361e09573986c94a3 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME       skills)
+set(LIB_NAME       RobotAPISkills)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -15,20 +15,26 @@ armarx_add_library(
         ./manager/SkillManagerComponentPlugin.cpp
         ./provider/SkillProviderComponentPlugin.cpp
         ./provider/Skill.cpp
+        ./provider/PeriodicSkill.cpp
         ./provider/SpecializedSkill.cpp
+        ./provider/PeriodicSpecializedSkill.cpp
         ./provider/SkillDescription.cpp
         ./provider/SkillStatusUpdate.cpp
+        ./provider/SkillParameterization.cpp
         ./provider/helper/LambdaSkillImplementation.cpp
         ./provider/detail/SkillImplementationWrapper.cpp
     HEADERS  
         ./manager/SkillManagerComponentPlugin.h
         ./provider/SkillProviderComponentPlugin.h
         ./provider/Skill.h
+        ./provider/PeriodicSkill.h
         ./provider/SpecializedSkill.h
+        ./provider/PeriodicSpecializedSkill.h
         ./provider/SkillDescription.h
         ./provider/SkillStatusUpdate.h
+        ./provider/SkillParameterization.h
         ./provider/helper/LambdaSkillImplementation.h
         ./provider/detail/SkillImplementationWrapper.h
 )
 
-add_library(RobotAPI::skills ALIAS skills)
+add_library(RobotAPI::skills ALIAS RobotAPISkills)
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 514f11ad7712776cc01ac33c71f3122510d78c01..1b97951eb8e7b6c72e5aa7bcc092c966da497a8c 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -30,6 +30,10 @@ namespace armarx
             ARMARX_INFO << "Adding a provider with name '" << info.providerName << "'.";
             skillProviderMap.insert({info.providerName, info.provider});
         }
+        else
+        {
+            ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists.";
+        }
     }
 
     void SkillManagerComponentPluginUser::removeProvider(const std::string& providerName, const Ice::Current&)
@@ -37,8 +41,13 @@ namespace armarx
         std::lock_guard l(skillProviderMapMutex);
         if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
         {
+            ARMARX_INFO << "Removing a provider with name '" << providerName << "'.";
             skillProviderMap.erase(it);
         }
+        else
+        {
+            ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't found.";
+        }
     }
 
     skills::provider::dti::SkillProviderMap SkillManagerComponentPluginUser::getSkillProviders(const Ice::Current&)
@@ -57,6 +66,8 @@ namespace armarx
             exInfo.skillName = info.skillName;
             exInfo.callbackInterface = myPrx;
             exInfo.params = info.params;
+            exInfo.waitUntilSkillFinished = info.waitUntilSkillFinished;
+
             it->second->executeSkill(exInfo);
         }
         else
@@ -65,6 +76,14 @@ namespace armarx
         }
     }
 
+    void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current &current)
+    {
+        if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+        {
+            it->second->abortSkill(skillName, false);
+        }
+    }
+
     void SkillManagerComponentPluginUser::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current&)
     {
         (void) statusUpdate;
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 6fd40f3eb99345a837161c5ffced3161889c28b6..5c59564f6fc16eca8b6e79afafb1d5ba0ef32a4b 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -36,7 +36,7 @@ namespace armarx
         skills::provider::dti::SkillProviderMap getSkillProviders(const Ice::Current &current) override;
         void executeSkill(const skills::manager::dto::SkillExecutionInfo& info, const Ice::Current &current) override;
         void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current &current) override;
-
+        void abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current &current) override;
 
     private:
         armarx::plugins::SkillManagerComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6175015154ea95ecd2229a0b81d16a0035b61265
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -0,0 +1,90 @@
+/**
+ * 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       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "PeriodicSkill.h"
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Frequency.h"
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "RobotAPI/libraries/skills/provider/Skill.h"
+
+namespace armarx::skills
+{
+    PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription,
+                                 const armarx::Frequency& frequency) :
+        Skill(skillDescription), frequency(frequency)
+    {
+    }
+
+    Skill::Status
+    PeriodicSkill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+    {
+        if(not initialize(params))
+        {
+            onFailed();
+            return Skill::Status::Failed;
+        }
+
+        core::time::Metronome metronome(frequency);
+
+        while (not stopped and not timeoutReached)
+        {
+            const auto status = executeOnce(params);
+            switch (status)
+            {
+                case Status::Running:
+                    // nothing to do here
+                    break;
+                case Status::Succeeded:
+                    onSucceeded();
+                    return Skill::Status::Succeeded;
+                case Status::Failed:
+                    onFailed();
+                    return Skill::Status::Failed;
+            }
+
+            const auto sleepDuration = metronome.waitForNextTick();
+            if (not sleepDuration.isPositive())
+            {
+                ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+                            << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+            }
+        }
+
+        if (stopped)
+        {
+            onStopped();
+            return Skill::Status::Stopped;
+        }
+
+        if (timeoutReached)
+        {
+            onTimeoutReached();
+            return Skill::Status::TimeoutReached;
+        }
+
+        throw armarx::LocalException("should not happen.");
+    }
+
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c8f6c81c4d5c1e1718f330803af6d30c61dea7c
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -0,0 +1,62 @@
+/**
+ * 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       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+    class PeriodicSkill : virtual public Skill
+    {
+    public:
+        PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
+
+    protected:
+        typename Skill::Status execute(const aron::data::DictPtr& params,
+                                        const Skill::CallbackT& callback) final;
+
+        enum class Status
+        {
+            Running,
+            Failed,
+            Succeeded
+        };
+
+        virtual bool initialize(const aron::data::DictPtr& params) = 0;
+        virtual Status executeOnce(const aron::data::DictPtr& params) = 0;
+
+        virtual void onSucceeded() = 0;
+        virtual void onStopped() = 0;
+        virtual void onFailed() = 0;
+        virtual void onTimeoutReached() = 0;
+
+    private:
+        const armarx::Frequency frequency;
+    };
+
+
+  
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4c70437ab37e8fd79fcff3cc373d5b349e1cf9f5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
@@ -0,0 +1,28 @@
+/**
+ * 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       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+ 
+ #include "PeriodicSpecializedSkill.h"
+ 
+ namespace armarx::skills
+ {
+ 
+ 
+ } // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..01809566186465638e8601e91e56ce7190ce5d39
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -0,0 +1,116 @@
+/**
+ * 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       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+    template <class AronT>
+    class PeriodicSpecializedSkill : public SpecializedSkill<AronT>
+    {
+    public:
+        using Base = SpecializedSkill<AronT>;
+
+        PeriodicSpecializedSkill() = delete;
+
+        PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+                                 const armarx::Frequency& frequency) :
+            Base(skillDescription), frequency(frequency)
+        {
+        }
+
+    protected:
+        typename Skill::Status
+        execute(const AronT& params, const Skill::CallbackT& callback) final
+        {
+            if (not initialize(params))
+            {
+                onFailed();
+                return Skill::Status::Failed;
+            }
+
+            core::time::Metronome metronome(frequency);
+
+            while (not Skill::stopped and not Skill::timeoutReached)
+            {
+                const auto status = executeOnce(params);
+                switch (status)
+                {
+                    case Status::Running:
+                        // nothing to do here
+                        break;
+                    case Status::Succeeded:
+                        onSucceeded();
+                        return Skill::Status::Succeeded;
+                    case Status::Failed:
+                        onFailed();
+                        return Skill::Status::Failed;
+                }
+
+                const auto sleepDuration = metronome.waitForNextTick();
+                if (not sleepDuration.isPositive())
+                {
+                    ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+                                << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+                }
+            }
+
+            if (Skill::stopped)
+            {
+                onStopped();
+                return Skill::Status::Stopped;
+            }
+
+            if (Skill::timeoutReached)
+            {
+                onTimeoutReached();
+                return Skill::Status::TimeoutReached;
+            }
+
+            throw armarx::LocalException("should not happen.");
+        }
+
+        enum class Status
+        {
+            Running,
+            Failed,
+            Succeeded
+        };
+
+        virtual bool initialize(const AronT& params) = 0;
+        virtual Status executeOnce(const AronT& params) = 0;
+
+        virtual void onSucceeded() = 0;
+        virtual void onStopped() = 0;
+        virtual void onFailed() = 0;
+        virtual void onTimeoutReached() = 0;
+
+    private:
+        const armarx::Frequency frequency;
+    };
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 7a26a5bf92c70d6c3efe0b3b88ba060573207387..c74910e43c3d4b4578c67ad1af06d1cc9fc86f3b 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/Skill.cpp
@@ -17,7 +17,7 @@ namespace armarx
             return std::thread{ [&](){
                     if (description.timeoutMs <= 0) return;
                     long skillStartedAt = IceUtil::Time::now().toMilliSeconds();
-                    while(running)
+                    while(running and !stopRequested())
                     {
                         auto now = IceUtil::Time::now().toMilliSeconds();
                         if ((now - skillStartedAt) >= description.timeoutMs) notifyTimeoutReached();
@@ -30,52 +30,70 @@ namespace armarx
         void Skill::notifyTimeoutReached()
         {
             timeoutReached = true;
-            _notifyTimeoutReached();
         }
 
         void Skill::notifyStopped()
         {
             stopped = true;
-            _notifyStopped();
         }
 
         void Skill::reset()
         {
+            started = IceUtil::Time::milliSeconds(-1);
+            exited = IceUtil::Time::milliSeconds(-1);
+
+            running = false;
             stopped = false;
             timeoutReached = false;
-
-            //provider = nullptr;
-            //manager = nullptr;
-
-            _reset();
         }
 
-        void Skill::_notifyTimeoutReached() { }
-        void Skill::_notifyStopped() { }
-        void Skill::_reset() { }
-
-        Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+        void Skill::init(const aron::data::DictPtr& params)
         {
+            (void) params;
+
+            ARMARX_IMPORTANT << "Initializing Skill '" << description.skillName << "'";
+
+            // always called before execute (should not take longer than 100ms)
             running = true;
-            started = IceUtil::Time::now().toMilliSeconds();
+            started = IceUtil::Time::now();
+            timeoutCheck = installTimeoutCondition();
+        }
 
-            auto timeoutCheck = installTimeoutCondition();
+        void Skill::exit(const aron::data::DictPtr& params)
+        {
+            (void) params;
 
-            auto ret = _execute(params, callback);
+            ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
 
-            started = 0;
+            // always called after execute (should not take longer than 100ms)
             running = false;
-            timeoutCheck.join(); // safely wait for the timeoutcheck to return
-            return ret;
+            if (timeoutCheck.joinable())
+            {
+                timeoutCheck.join();
+            }
+            exited = IceUtil::Time::now();
         }
 
-        Skill::Status Skill::_execute(const aron::data::DictPtr& params, const CallbackT& callback)
+        Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
         {
             (void) params;
 
-            ARMARX_WARNING_S << "You have to override this method!";
+            ARMARX_IMPORTANT << "Executing Skill '" << description.skillName << "'";
             return Status::Succeeded;
         }
 
+        Skill::Status Skill::initExecuteExit(const aron::data::DictPtr& params, const CallbackT& callback)
+        {
+            this->reset();
+            this->init(params);
+            auto ret = this->execute(params);
+            this->exit(params);
+            return ret;
+        }
+
+        bool Skill::stopRequested() const
+        {
+            return (stopped || timeoutReached);
+        }
     }
 }
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 324ccb24e3ea657b5ae879358fa4cb5e62ecc4f7..d4fe0e5cf4fc892cf032f99a7576dfe8d0ab33ab 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/source/RobotAPI/libraries/skills/provider/Skill.h
@@ -36,25 +36,26 @@ namespace armarx
             Skill(const SkillDescription&);
             virtual ~Skill() = default;
 
-            /// Execute Skill
-            Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+            /// Override this method with the actual implementation.
+            virtual void init(const aron::data::DictPtr& params);
 
-            /// Reset all parameters before starting a skill.
-            void reset();
-
-            /// Set the notification bools
-            void notifyTimeoutReached();
-            void notifyStopped();
+            /// Override this method with the actual implementation.
+            virtual void exit(const aron::data::DictPtr& params);
 
             /// Override this method with the actual implementation. The callback is for status updates to the calling instance
-            virtual Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+            virtual Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+
+            /// Do all methods at once.
+            Status initExecuteExit(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
 
-            /// Override if you have special members that needs to be resetted
-            virtual void _reset();
+            /// Override if you have special members that needs to be resetted. It is called before the skill ititializes
+            virtual void reset();
 
             /// Override these methods if you want to do something special when notification comes
-            virtual void _notifyTimeoutReached();
-            virtual void _notifyStopped();
+            virtual void notifyTimeoutReached();
+            virtual void notifyStopped();
+
+            virtual bool stopRequested() const;
 
         private:
             /// install a condition as a seperate thread
@@ -64,17 +65,21 @@ namespace armarx
             const SkillDescription description;
 
             /// running params
-            std::atomic_bool running = false;
-            long started = 0;
+            IceUtil::Time started = IceUtil::Time::milliSeconds(-1);
+            IceUtil::Time exited = IceUtil::Time::milliSeconds(-1);
 
-            /// proxies that called the skills
-            //provider::dti::SkillProviderInterfacePrx provider = nullptr;
-            //manager::dti::SkillManagerInterfacePrx manager = nullptr;
+            /// proxies that called the skills. Will be set from provider and is const afterwards
+            provider::dti::SkillProviderInterfacePrx ownerProvider = nullptr;
+            manager::dti::SkillManagerInterfacePrx ownerManager = nullptr;
 
         protected:
             /// Use conditions during runtime this way
+            std::atomic_bool running = true;
             std::atomic_bool stopped = false;
             std::atomic_bool timeoutReached = false;
+
+        private:
+            std::thread timeoutCheck;
         };
     }
 }
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
index 222c860e47082fc8ea812ac90979738dcf7feb10..2dc791fe3e77495717c0b6fd9f44da19f1ae41f1 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
@@ -4,14 +4,6 @@ namespace armarx
 {
     namespace skills
     {
-        provider::dto::SkillParameterization SkillParameterization::toIce() const
-        {
-            provider::dto::SkillParameterization ret;
-            if (params)
-            {
-                ret.params = params->toAronDictPtr();
-            }
-            return ret;
-        }
+
     }
 }
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
index b42c0e927e2bddf1db91cfc0c54c7f3383ce452a..a6ce6ed4a95c0c090daa698f3cde8c2ebe30af39 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
@@ -12,9 +12,8 @@ namespace armarx
     {
         struct SkillParameterization
         {
-            aron::data::DictPtr params;
-
-            provider::dto::SkillParameterization toIce() const;
+            aron::data::DictPtr                                 usedInputParams = nullptr;
+            callback::dti::SkillProviderCallbackInterfacePrx    usedCallbackInterface = nullptr;
         };
     }
 }
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index c471f140cc312952e960a922ea26be50996eb260..638de458ae31b7abaa7e2cee610a6505b0d0ed63 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -22,11 +22,24 @@ namespace armarx::plugins
         auto& p = parent<SkillProviderComponentPluginUser>();
         std::string providerName = p.getName();
 
+        // update skill ownership
+        for (auto& [skillName, impl] : p.skillImplementations)
+        {
+            impl.skill->ownerProvider = myPrx;
+            impl.skill->ownerManager = ownedBySkillManager;
+
+            impl.statusUpdate.providerName = p.getName();
+            impl.statusUpdate.skillName = skillName;
+        }
+
+        // register to manager
         skills::manager::dto::ProviderInfo i;
         i.provider = myPrx;
         i.providerName = providerName;
         i.providedSkills = p.getSkills();
-        skillManager->addProvider(i);
+        ownedBySkillManager->addProvider(i);
+
+        p.connected = true;
     }
 
     void SkillProviderComponentPlugin::preOnDisconnectComponent()
@@ -34,13 +47,13 @@ namespace armarx::plugins
         auto& p = parent<SkillProviderComponentPluginUser>();
         std::string providerName = p.getName();
 
-        skillManager->removeProvider(providerName);
+        ownedBySkillManager->removeProvider(providerName);
     }
 
     void SkillProviderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
     {
         std::string prefix = "skill.";
-        properties->component(skillManager, "SkillMemory", prefix + "SkillManager", "The name of the SkillManager (or SkillMemory) proxy.");
+        properties->component(ownedBySkillManager, "SkillMemory", prefix + "SkillManager", "The name of the SkillManager (or SkillMemory) proxy this provider belongs to.");
     }
 }
 
@@ -52,14 +65,20 @@ namespace armarx
         addPlugin(plugin);
     }
 
-    void SkillProviderComponentPluginUser::addSkill(const std::shared_ptr<skills::Skill>& skill)
+    void SkillProviderComponentPluginUser::addSkill(std::unique_ptr<skills::Skill>&& skill)
     {
         if (!skill)
         {
             return;
         }
 
-        std::lock_guard l(skillsMutex);
+        if (connected)
+        {
+            ARMARX_WARNING << "The SkillProvider already registered to a manager. The skill '" + skill->description.skillName + "' therefore cannot be added anymore.";
+            return;
+        }
+
+        std::unique_lock l(skillsMutex);
 
         std::string skillName = skill->description.skillName;
         if (skillImplementations.find(skillName) != skillImplementations.end())
@@ -69,19 +88,18 @@ namespace armarx
         }
 
         ARMARX_INFO << "Adding skill " << skillName;
-        skills::detail::SkillImplementationWrapper s(skill);
-        skillImplementations.insert({skillName, s});
+        skillImplementations.emplace(skillName, std::move(skill));
     }
 
     void SkillProviderComponentPluginUser::addSkill(const skills::helper::LambdaSkill::FunT& f, const skills::SkillDescription& desc)
     {
-        auto lambda = std::make_shared<skills::helper::LambdaSkill>(f, desc);
-        addSkill(lambda);
+        auto lambda = std::make_unique<skills::helper::LambdaSkill>(f, desc);
+        addSkill(std::move(lambda));
     }
 
     skills::provider::dto::SkillDescriptionMap SkillProviderComponentPluginUser::getSkills(const Ice::Current &current)
     {
-        std::lock_guard l(skillsMutex);
+        std::shared_lock l(skillsMutex);
         skills::provider::dto::SkillDescriptionMap skillDesciptions;
         for (const auto& [key, skillWrapper] : skillImplementations)
         {
@@ -92,51 +110,54 @@ namespace armarx
 
     skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::getSkillExecutionStatus(const std::string& skill, const Ice::Current &current)
     {
-        std::lock_guard l(skillsMutex);
+        std::shared_lock l(skillsMutex);
         auto& skillWrapper = skillImplementations.at(skill);
 
-        std::lock_guard l2(skillWrapper.skillStatusMutex);
+        std::shared_lock l2(skillWrapper.skillStatusMutex);
         return skillWrapper.statusUpdate.toIce();
     }
 
+    // Please not that this method waits until the skill can be scheduled!
     void SkillProviderComponentPluginUser::executeSkill(const skills::provider::dto::SkillExecutionInfo& info, const Ice::Current &current)
     {
-        std::lock_guard l(skillsMutex);
+        std::shared_lock l(skillsMutex);
         std::string skillName = info.skillName;
         ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
 
         auto& wrapper = skillImplementations.at(skillName);
+
+        skills::SkillParameterization usedParameterization;
+        usedParameterization.usedCallbackInterface = info.callbackInterface;
+        usedParameterization.usedInputParams = aron::data::Dict::FromAronDictDTO(info.params);
+
+        // We have to wait until the last task is finished
         if (wrapper.task.joinable())
         {
             wrapper.task.join();
         }
 
-        // update input params
-        wrapper.reset();
-        wrapper.statusUpdate.usedCallbackInterface = info.callbackInterface;
-        wrapper.statusUpdate.skillName = info.skillName;
-        wrapper.statusUpdate.providerName = getName();
-        wrapper.statusUpdate.usedParameterization = aron::data::Dict::FromAronDictDTO(info.params);
+        // recreate thread and execute skill. A skill can only be executed once
+        wrapper.task = std::thread{ [&] { wrapper.execute(usedParameterization);}};
+        std::this_thread::sleep_for(std::chrono::milliseconds(50)); // somehow we need to wait, otherwise it chrashes
+
 
-        // recreate thread and execute skill.
-        wrapper.task = std::thread{ [&] { wrapper.execute();}};
+        if (info.waitUntilSkillFinished && wrapper.task.joinable())
+        {
+            // wait until task is finished. We hold the shared lock for the whole time.
+            wrapper.task.join();
+        }
     }
 
-    skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current &current)
+    void SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, bool waitUntilSkillFinished, const Ice::Current &current)
     {
-        std::lock_guard l(skillsMutex);
+        std::shared_lock l(skillsMutex);
         ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
 
         auto& wrapper = skillImplementations.at(skillName);
-
-        std::lock_guard l2(wrapper.skillStatusMutex);
-
-        if (wrapper.skill->running)
+        if (waitUntilSkillFinished && !wrapper.skill->stopRequested() && wrapper.task.joinable())
         {
             wrapper.skill->notifyStopped();
             wrapper.task.join();
         }
-
-        return wrapper.statusUpdate.toIce();
     }
 }
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 7e370346cbd8d49f4ec2ac96e638ab85ea23fcea..58895250b3de95d5b3433c92b69fd04c1e201a2b 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <mutex>
+#include <shared_mutex>
 #include <queue>
 #include <thread>
 #include <functional>
@@ -33,7 +33,7 @@ namespace armarx::plugins
         void preOnDisconnectComponent() override;
 
     private:
-        skills::manager::dti::SkillManagerInterfacePrx skillManager;
+        skills::manager::dti::SkillManagerInterfacePrx ownedBySkillManager;
         skills::provider::dti::SkillProviderInterfacePrx myPrx;
     };
 }
@@ -50,17 +50,20 @@ namespace armarx
         skills::provider::dto::SkillDescriptionMap getSkills(const Ice::Current &current = Ice::Current()) override;
         skills::provider::dto::SkillStatusUpdate getSkillExecutionStatus(const std::string& skill, const Ice::Current &current = Ice::Current()) override;
         void executeSkill(const skills::provider::dto::SkillExecutionInfo& executionInfo, const Ice::Current &current = Ice::Current()) override;
-        skills::provider::dto::SkillStatusUpdate abortSkill(const std::string &skill, const Ice::Current &current = Ice::Current()) override;
+        void abortSkill(const std::string &skill, bool waitUntilSkillFinished, const Ice::Current &current = Ice::Current()) override;
 
     protected:
         void addSkill(const skills::helper::LambdaSkill::FunT&, const skills::SkillDescription&);
-        void addSkill(const std::shared_ptr<skills::Skill>&);
+        void addSkill(std::unique_ptr<skills::Skill>&&);
 
     private:
         armarx::plugins::SkillProviderComponentPlugin* plugin = nullptr;
 
     protected:
-        mutable std::mutex skillsMutex;
+        mutable std::shared_mutex skillsMutex;
+
+    public:
+        bool connected = false;
         std::map<std::string, skills::detail::SkillImplementationWrapper> skillImplementations;
     };
 }
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
index cfc9f6fd218acc22e618e5ab264078af053f71da..eb98bb0b01d0eee695b0ff98a5754b3038f881a7 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
@@ -11,8 +11,8 @@ namespace armarx
             ret.skillName = skillName;
             ret.data = aron::data::Dict::ToAronDictDTO(data);
             ret.status = status;
-            ret.usedCallbackInterface = usedCallbackInterface;
-            ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization);
+            ret.usedCallbackInterface = usedParameterization.usedCallbackInterface;
+            ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization.usedInputParams);
             return ret;
         }
     }
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
index 5385c6f0da4d2d1cc86ac664607903652c8a57b2..2b69c7e5d707da474d60285e04a5b01a5fc9a067 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
@@ -6,18 +6,19 @@
 #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 #include <RobotAPI/interface/skills/SkillProviderInterface.h>
 
+#include "SkillParameterization.h"
+
 namespace armarx
 {
     namespace skills
     {
         struct SkillStatusUpdate
         {
-            std::string                                         providerName;
-            std::string                                         skillName;
-            aron::data::DictPtr                                 usedParameterization;
-            callback::dti::SkillProviderCallbackInterfacePrx    usedCallbackInterface;
-            provider::dto::Execution::Status                    status;
-            aron::data::DictPtr                                 data;
+            std::string                                         providerName = "";
+            std::string                                         skillName = "";
+            provider::dto::Execution::Status                    status = provider::dto::Execution::Status::Idle;
+            aron::data::DictPtr                                 data = nullptr;
+            SkillParameterization                               usedParameterization;
 
             provider::dto::SkillStatusUpdate toIce() const;
         };
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index fec98231ad74cf58da95c3bfd9324bc342e99c47..68711db013a4a463af03429befcee867d924cbdd 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -15,6 +15,8 @@ namespace armarx
         class SpecializedSkill : public Skill
         {
         public:
+            using ParamType = AronT;
+
             using Skill::Skill;
             virtual ~SpecializedSkill() = default;
 
@@ -25,22 +27,56 @@ namespace armarx
             }
 
             /// Override this method with the actual implementation. The callback is for status updates to the calling instance
-            virtual Status _execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+            virtual Status execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
             {
                 (void) params;
-
-                ARMARX_WARNING_S << "You have to override this method!";
                 return Status::Succeeded;
             }
 
+            virtual void init(const AronT& params)
+            {
+                (void) params;
+            }
+
+            virtual void exit(const AronT& params)
+            {
+                (void) params;
+            }
+
+            Status initExecuteExit(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+            {
+                return Skill::initExecuteExit(params.toAron(), callback);
+            }
+
             /// Do not use anymore
-            Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
+            Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
             {
+                Skill::execute(params, callback);
+                AronT p;
+                p.fromAron(params);
+
+                return execute(p, callback);
+            }
+
+            /// Do not use anymore
+            void init(const aron::data::DictPtr& params) final
+            {
+                Skill::init(params);
                 AronT p;
                 //ARMARX_IMPORTANT << aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(params).dump(2);
                 p.fromAron(params);
 
-                return _execute(p, callback);
+                return init(p);
+            }
+
+            /// Do not use anymore
+            void exit(const aron::data::DictPtr& params) final
+            {
+                Skill::exit(params);
+                AronT p;
+                p.fromAron(params);
+
+                exit(p);
             }
         };
     }
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 9f9c573b80df17e055baa37737f3f1dc86e4997a..c6add657030fbe8763936fdf28c6ef81bfebeb03 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,25 +4,40 @@ namespace armarx
 {
     namespace skills::detail
     {
-        void SkillImplementationWrapper::reset()
+        void SkillImplementationWrapper::execute(const skills::SkillParameterization parameterization)
         {
-            ARMARX_CHECK_NOT_NULL(skill);
-            std::lock_guard l(skillStatusMutex);
-            statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
-            statusUpdate.data = nullptr;
-            skill->reset();
-        }
-
-        void SkillImplementationWrapper::execute()
-        {
-            ARMARX_CHECK_NOT_NULL(skill);
-            std::lock_guard l(executingMutex);
+            std::unique_lock l(executingMutex);
 
             const std::string skillName = skill->description.skillName;
             ARMARX_INFO_S << "Executing skill: " << skillName;
 
-            // get params and setup variables
-            auto& aron_params = statusUpdate.usedParameterization;
+            // reset Skill
+            {
+                std::unique_lock l2(skillStatusMutex); // skill is not updating
+                statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
+                statusUpdate.data = nullptr;
+                skill->reset();
+            }
+
+            // set params and setup variables
+            {
+                std::lock_guard l(skillStatusMutex);
+                statusUpdate.usedParameterization = parameterization;
+            }
+
+            auto& aron_params = parameterization.usedInputParams;
+            auto updateStatus = [&](const skills::provider::dto::Execution::Status status, const aron::data::DictPtr& data = nullptr){
+                std::lock_guard l(skillStatusMutex);
+                statusUpdate.status = status;
+                statusUpdate.data = data;
+
+                auto& callbackInterface = statusUpdate.usedParameterization.usedCallbackInterface;
+
+                if (callbackInterface)
+                {
+                    callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+                }
+            };
 
             try
             {
@@ -33,28 +48,16 @@ namespace armarx
                 }
 
                 // set scheduled
-                {
-                    std::lock_guard l(skillStatusMutex);
-                    statusUpdate.status = skills::provider::dto::Execution::Status::Scheduled;
-                    updateStatus();
-                }
-
+                updateStatus(skills::provider::dto::Execution::Status::Scheduled);
 
                 // execute
-                {
-                    std::lock_guard l(skillStatusMutex);
-                    statusUpdate.status = skills::provider::dto::Execution::Status::Running;
-                    updateStatus();
-                }
+                updateStatus(skills::provider::dto::Execution::Status::Running);
 
-                auto execution_callback = [&](const aron::data::DictPtr& update)
-                {
-                    statusUpdate.data = update;
-                    updateStatus();
-                };
-                auto result = skill->execute(aron_params, execution_callback);
+                skill->init(aron_params);
+                auto ret = skill->execute(aron_params, [&](const aron::data::DictPtr& update) { updateStatus(statusUpdate.status, update); });
+                skill->exit(aron_params);
 
-                switch (result)
+                switch (ret)
                 {
                     case skills::Skill::Status::Failed:
                         throw armarx::LocalException("The Skill '" + skillName + "' failed during execution.");
@@ -64,36 +67,20 @@ namespace armarx
                         break;
                     case skills::Skill::Status::Stopped:
                     {
-                        std::lock_guard l(skillStatusMutex);
-                        statusUpdate.status = skills::provider::dto::Execution::Status::Aborted;
-                        updateStatus();
+                        updateStatus(skills::provider::dto::Execution::Status::Aborted);
                         break;
                     }
-                    default:
+                    case skills::Skill::Status::Succeeded:
                     {
-                        std::lock_guard l(skillStatusMutex);
-                        statusUpdate.status = skills::provider::dto::Execution::Status::Succeeded;
-                        updateStatus();
+                        updateStatus(skills::provider::dto::Execution::Status::Succeeded);
+                        break;
                     }
                 }
             }
             catch (const std::exception& ex)
             {
                 ARMARX_WARNING_S << "Skill " << skillName << " died with exception:\n" << ex.what();
-
-                std::lock_guard l(skillStatusMutex);
-                statusUpdate.status = skills::provider::dto::Execution::Status::Failed;
-                updateStatus();
-            }
-        }
-
-        void SkillImplementationWrapper::updateStatus() const
-        {
-            auto& callbackInterface = statusUpdate.usedCallbackInterface;
-
-            if (callbackInterface)
-            {
-                callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+                updateStatus(skills::provider::dto::Execution::Status::Failed);
             }
         }
     }
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 35910a968a66026b5eff1399b133484adef1437a..1c70a14e5353bef38401a929783ed8c92b940054 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <shared_mutex>
+
 #include "../SkillDescription.h"
 #include "../SkillStatusUpdate.h"
 #include "../Skill.h"
@@ -16,32 +18,26 @@ namespace armarx
             {
             public:
                 // fixed values. Do not change after skill instantiation
-                const std::shared_ptr<Skill> skill;
+                const std::unique_ptr<Skill> skill;
 
                 // Current execution status. Changes during execution
                 // The status also holds the used parameterization
-                mutable std::mutex skillStatusMutex;
+                // skillName and providerName are const after registering the skill in a provider
+                mutable std::shared_mutex skillStatusMutex;
                 SkillStatusUpdate statusUpdate;
 
                 // Task information. task is recreated every time the skill restarts
-                mutable std::mutex executingMutex;
+                mutable std::shared_mutex executingMutex;
                 std::thread task;
 
-                SkillImplementationWrapper(const std::shared_ptr<skills::Skill> skill) :
-                    skill(skill)
+                SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill) :
+                    skill(std::move(skill))
                 {
-                    reset();
+                    ARMARX_CHECK_NOT_NULL(this->skill);
                 }
 
-                SkillImplementationWrapper(const SkillImplementationWrapper& s) :
-                    SkillImplementationWrapper(s.skill)
-                {}
-
-                void execute();
-                void reset();
-
-            protected:
-                void updateStatus() const;
+                // execute a skill. The parameterization is copied
+                void execute(const skills::SkillParameterization);
             };
         }
     }
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
index 59d51cd16b66a4799f25e0503d2c987bea7ab4cc..9b761b403f51552235b690d601137ff1c11a002b 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
@@ -5,7 +5,7 @@ namespace armarx
     namespace skills::helper
     {
 
-        Skill::Status LambdaSkill::_execute(const aron::data::DictPtr& data, const CallbackT& callback)
+        Skill::Status LambdaSkill::execute(const aron::data::DictPtr& data, const CallbackT& callback)
         {
             (void) callback;
             bool res = fun(data);
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
index 27e1ab0aea43127441d7b4798207433bd7549783..63f537d4cf4c4cfd94633f45fb22ea9223449f5a 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
@@ -18,7 +18,7 @@ namespace armarx
             {};
 
         protected:
-            Skill::Status _execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
+            Skill::Status execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
 
         private:
             FunT fun;
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml
index 31abf88dfe93cad5399c7da4ece19e38c9dad1b9..e04b89d7414961aba30f7d9119fa8dda09477b29 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroup.scgxml
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <StatechartGroup name="SpeechObserverTestGroup" package="RobotAPI" generateContext="true">
 	<Proxies>
-		<Proxy value="RobotAPIInterfaces.speechObserver"/>
+		<Proxy value="ArmarXCoreInterfaces.speechObserver"/>
 		<Proxy value="RobotAPIInterfaces.textToSpeech"/>
 	</Proxies>
 	<State filename="TestTextToSpeech.xml" visibility="public"/>