diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
new file mode 100644
index 0000000000000000000000000000000000000000..af25e698a9aa3820dbc68c346a57aa895f97505c
--- /dev/null
+++ b/.gitlab/CODEOWNERS
@@ -0,0 +1,6 @@
+# ArMem
+/source/RobotAPI/components/armem/ @RainerKartmann @dreher
+/source/RobotAPI/libraries/armem/ @RainerKartmann @dreher
+/source/RobotAPI/interface/armem.ice @RainerKartmann @dreher
+/source/RobotAPI/interface/armem/ @RainerKartmann @dreher
+
diff --git a/data/RobotAPI/ArMemStorage/README b/data/RobotAPI/ArMemStorage/README
new file mode 100644
index 0000000000000000000000000000000000000000..342a90d8a1b226d14c663640bb6d2d8e01683c97
--- /dev/null
+++ b/data/RobotAPI/ArMemStorage/README
@@ -0,0 +1 @@
+This is the default folder where ArMem stores episodes
diff --git a/scenarios/ArMemExample/config/ArMemExampleMemory.cfg b/scenarios/ArMemExample/config/ArMemExampleMemory.cfg
index 5a8764611d35007fb9d7884f7b8101f451016897..a7b234054d462683b2dd20aab5b0f08e23a9af69 100644
--- a/scenarios/ArMemExample/config/ArMemExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ArMemExampleMemory.cfg
@@ -63,10 +63,10 @@
 
 # ArmarX.ArMemExampleMemory.core.DefaultSegments:  Core segments to add on start up.
 #  Attributes:
-#  - Default:            ExampleModality, ExampleData
+#  - Default:            ExampleModality, ExampleConcept
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.ArMemExampleMemory.core.DefaultSegments = ExampleModality, ExampleData
+# ArmarX.ArMemExampleMemory.core.DefaultSegments = ExampleModality, ExampleConcept
 
 
 # ArmarX.ArMemExampleMemory.memory.Name:  Name of this memory.
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index eaf9cc141387cd36585733aab4f987c921b02603..31c31934b335d79b7eb426e6606179c21d910546 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -342,7 +342,7 @@ namespace armarx
     Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
     {
         std::scoped_lock lock(dataMutex);
-        return simox::get_keys(providers);
+        return simox::alg::get_keys(providers);
     }
 
     objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
diff --git a/source/RobotAPI/components/SkillObserver/SkillObserver.cpp b/source/RobotAPI/components/SkillObserver/SkillObserver.cpp
index a30939b9c88d90368a003d7667ed9beb724877bb..db42b9881eccd454fd50190a60446ae7accf0c12 100644
--- a/source/RobotAPI/components/SkillObserver/SkillObserver.cpp
+++ b/source/RobotAPI/components/SkillObserver/SkillObserver.cpp
@@ -80,7 +80,7 @@ namespace armarx::skills
         return s;
     }
 
-    void SkillObserver::skillsProvided(const SkillProviderInterfacePrx& provider, const SkillDescriptionMap &skills, const Ice::Current&)
+    void SkillObserver::skillsProvided(const SkillProviderInterfacePrx& provider, const SkillDescriptionMap& skills, const Ice::Current&)
     {
         std::lock_guard g{_skills_mutex};
         ARMARX_CHECK_NOT_NULL(provider);
@@ -152,7 +152,7 @@ namespace armarx::skills
 
                         }
                     }
-                    if(!to_remove.empty())
+                    if (!to_remove.empty())
                     {
                         ARMARX_INFO << "removing " << to_remove.size() << " skills (timeout on ping)";
                     }
diff --git a/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.cpp b/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.cpp
index 4ebf12662861e3b0a2b400630d37dd9ecf6ebf5d..a2b7c1566cb872e43b65e4c74d2521e5838b21c2 100644
--- a/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.cpp
+++ b/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.cpp
@@ -25,13 +25,11 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 
-#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.h>
 #include <RobotAPI/libraries/armem/component/MemoryRemoteGui.h>
 #include <RobotAPI/libraries/armem/client/QueryBuilder.h>
 #include <RobotAPI/libraries/armem/memory/ice_conversions.h>
 
-#include <RobotAPI/components/armem/ArMemExampleMemory/aron/Primitive.aron.generated.h>
-#include <RobotAPI/components/armem/ArMemExampleMemory/aron/SimpleObject.aron.generated.h>
+#include <RobotAPI/components/armem/ArMemExampleMemory/aron/ExampleData.aron.generated.h>
 
 
 namespace armarx
@@ -103,14 +101,10 @@ namespace armarx
 
     void ArMemExampleClient::run()
     {
-        sleep(2);
         ARMARX_IMPORTANT << "Running now.";
 
-        armem::client::Reader reader(memory);
-        armem::client::Writer writer(memory);
-
-        armem::MemoryID snapshotID = commitSingleSnapshot(writer, entityID);
-        commitMultipleSnapshots(writer, entityID, 3);
+        armem::MemoryID snapshotID = commitSingleSnapshot(entityID);
+        commitMultipleSnapshots(entityID, 3);
 
         if (true)
         {
@@ -118,18 +112,18 @@ namespace armarx
         }
         if (true)
         {
-            querySnapshot(reader, snapshotID);
+            querySnapshot(snapshotID);
         }
         if (true)
         {
-            commitPrimitives(writer);
-            queryPrimitives(reader);
+            commitExampleData();
+            queryExampleData();
         }
 
-        CycleUtil c(500);
+        CycleUtil c(1000);
         while (!task->isStopped())
         {
-            commitSingleSnapshot(writer, entityID);
+            commitSingleSnapshot(entityID);
             c.waitForCycleDuration();
         }
     }
@@ -203,7 +197,7 @@ namespace armarx
         }
     }
 
-    void ArMemExampleClient::querySnapshot(armem::client::Reader& reader, const armem::MemoryID& snapshotID)
+    void ArMemExampleClient::querySnapshot(const armem::MemoryID& snapshotID)
     {
         ARMARX_IMPORTANT
                 << "Querying exact snapshot of entity: (via query builder)"
@@ -217,7 +211,7 @@ namespace armarx
         .entities().withID(snapshotID)
         .snapshots().withID(snapshotID);
 
-        armem::client::QueryResult qResult = reader.query(qb.buildQueryInput());
+        armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
         ARMARX_INFO << "Result: "
                     << "\n- success:       \t" << qResult.success
                     << "\n- error message: \t" << qResult.errorMessage
@@ -287,7 +281,7 @@ namespace armarx
         return armem::MemoryID::fromString(result.segmentID);
     }
 
-    armem::MemoryID ArMemExampleClient::commitSingleSnapshot(armem::client::Writer& writer, const armem::MemoryID& entityID)
+    armem::MemoryID ArMemExampleClient::commitSingleSnapshot(const armem::MemoryID& entityID)
     {
         armem::Commit commit;
         armem::EntityUpdate& update = commit.updates.emplace_back();
@@ -296,15 +290,15 @@ namespace armarx
         update.timeCreated = armem::Time::now();
         update.instancesData =
         {
-            std::make_shared<aron::datanavigator::AronIntDataNavigator>(),
-            std::make_shared<aron::datanavigator::AronIntDataNavigator>()
+            std::make_shared<aron::datanavigator::AronDictDataNavigator>(),
+            std::make_shared<aron::datanavigator::AronDictDataNavigator>()
         };
 
         ARMARX_IMPORTANT
                 << "Committing:"
                 << "\n- entityID:     \t'" << update.entityID.str() << "'"
                 ;
-        armem::CommitResult commitResult = writer.commit(commit);
+        armem::CommitResult commitResult = memoryWriter.commit(commit);
 
         ARMARX_CHECK_EQUAL(commitResult.results.size(), 1);
         armem::EntityUpdateResult& result = commitResult.results.at(0);
@@ -313,7 +307,7 @@ namespace armarx
         return result.snapshotID;
     }
 
-    void ArMemExampleClient::commitMultipleSnapshots(armem::client::Writer& writer, const armem::MemoryID& entityID, int num)
+    void ArMemExampleClient::commitMultipleSnapshots(const armem::MemoryID& entityID, int num)
     {
         armem::Commit commit;
         for (int i = 0; i < num; ++i)
@@ -323,21 +317,21 @@ namespace armarx
             update.timeCreated = armem::Time::now() + armem::Time::seconds(i);
             for (int j = 0; j < i; ++j)
             {
-                update.instancesData.push_back(std::make_shared<aron::datanavigator::AronIntDataNavigator>());
+                update.instancesData.push_back(std::make_shared<aron::datanavigator::AronDictDataNavigator>());
             }
         }
         ARMARX_INFO << "Commiting " << commit.updates.size() << " more updates.";
 
-        armem::CommitResult commitResult = writer.commit(commit);
+        armem::CommitResult commitResult = memoryWriter.commit(commit);
         ARMARX_CHECK_EQUAL(commitResult.results.size(), commit.updates.size());
     }
 
 
-    void ArMemExampleClient::commitPrimitives(armem::client::Writer& writer)
+    void ArMemExampleClient::commitExampleData()
     {
-        ARMARX_IMPORTANT << "Adding segment " << "Primitive" << "/" << getName();
+        ARMARX_IMPORTANT << "Adding segment " << "ExampleData" << "/" << getName();
 
-        auto addSegmentResult = writer.addSegment("Primitive", getName());
+        auto addSegmentResult = memoryWriter.addSegment("ExampleData", getName());
         if (!addSegmentResult.success)
         {
             ARMARX_ERROR << addSegmentResult.errorMessage;
@@ -354,7 +348,7 @@ namespace armarx
             update.entityID.entityName = "default";
             update.timeCreated = time;
 
-            armem::aron::test::Primitive primitive;
+            armem::aron::example::ExampleData primitive;
             update.instancesData = { primitive.toAron() };
         }
         {
@@ -363,24 +357,43 @@ namespace armarx
             update.entityID.entityName = "the answer";
             update.timeCreated = time;
 
-            armem::aron::test::Primitive primitive;
-            primitive.the_bool = true;
-            primitive.the_double = 2 * 42.42;
-            primitive.the_float = 21.5;
-            primitive.the_int = 42;
-            primitive.the_long = 424242;
-            primitive.the_string = "fourty two";
-            update.instancesData = { primitive.toAron() };
+            armem::aron::example::ExampleData data;
+            data.the_bool = true;
+            data.the_double = 2 * 42.42;
+            data.the_float = 21.5;
+            data.the_int = 42;
+            data.the_long = 424242;
+            data.the_string = "fourty two";
+            data.the_float_list = { 21, 42, 84 };
+            data.the_int_list = { 21, 42, 84 };
+            data.the_string_list = simox::alg::multi_to_string(data.the_int_list);
+            data.the_object_list.emplace_back();
+
+            data.the_float_dict =
+            {
+                { "one", 1.0 },
+                { "two", 2.0 },
+                { "three", 3.0 },
+            };
+            data.the_int_dict =
+            {
+                { "one", 1 },
+                { "two", 2 },
+                { "three", 3 },
+            };
+
+
+            update.instancesData = { data.toAron() };
         }
 
-        armem::CommitResult commitResult = writer.commit(commit);
+        armem::CommitResult commitResult = memoryWriter.commit(commit);
         if (!commitResult.allSuccess())
         {
             ARMARX_WARNING << commitResult.allErrorMessages();
         }
     }
 
-    void ArMemExampleClient::queryPrimitives(armem::client::Reader& reader)
+    void ArMemExampleClient::queryExampleData()
     {
         // Query all entities from provider.
         armem::client::QueryBuilder qb;
@@ -390,7 +403,7 @@ namespace armarx
         .entities().all()
         .snapshots().all();
 
-        armem::client::QueryResult result = reader.query(qb.buildQueryInput());
+        armem::client::QueryResult result = memoryReader.query(qb.buildQueryInput());
         if (result)
         {
             tab.queryResult = std::move(result.memory);
diff --git a/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.h b/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.h
index 789dcaf50d73eb0def6f53b01bc5590d4b816876..157b8ec2f4caab32384515a83b156310b1ae3590 100644
--- a/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.h
+++ b/source/RobotAPI/components/armem/ArMemExampleClient/ArMemExampleClient.h
@@ -112,14 +112,14 @@ namespace armarx
         void waitForMemory();
         armem::MemoryID addProviderSegment();
 
-        armem::MemoryID commitSingleSnapshot(armem::client::Writer& writer, const armem::MemoryID& entityID);
-        void commitMultipleSnapshots(armem::client::Writer& writer, const armem::MemoryID& entityID, int num = 3);
+        armem::MemoryID commitSingleSnapshot(const armem::MemoryID& entityID);
+        void commitMultipleSnapshots(const armem::MemoryID& entityID, int num = 3);
 
         void queryLatestRawIce(const armem::MemoryID& entityID);
-        void querySnapshot(armem::client::Reader& reader, const armem::MemoryID& snapshotID);
+        void querySnapshot(const armem::MemoryID& snapshotID);
 
-        void commitPrimitives(armem::client::Writer& writer);
-        void queryPrimitives(armem::client::Reader& reader);
+        void commitExampleData();
+        void queryExampleData();
 
 
     private:
diff --git a/source/RobotAPI/components/armem/ArMemExampleClient/CMakeLists.txt b/source/RobotAPI/components/armem/ArMemExampleClient/CMakeLists.txt
index bfa917947ced2e4d9a240de0952b08d93e1dce7c..9977051593205191473f4faab5eccc46ffc4c788 100644
--- a/source/RobotAPI/components/armem/ArMemExampleClient/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/ArMemExampleClient/CMakeLists.txt
@@ -1,11 +1,16 @@
 armarx_component_set_name("ArMemExampleClient")
 
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
 
 set(COMPONENT_LIBS
     ArmarXCore ArmarXCoreInterfaces  # for DebugObserverInterface
     ArmarXGuiComponentPlugins
     RobotAPICore RobotAPIInterfaces armem
     # RobotAPIComponentPlugins  # for ArViz and other plugins
+
+    ${IVT_LIBRARIES}
 )
 
 set(SOURCES
@@ -17,6 +22,9 @@ set(HEADERS
 
 
 armarx_add_component("${SOURCES}" "${HEADERS}")
+if (IVT_FOUND)
+    target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${IVT_INCLUDE_DIRS})
+endif()
 
 
 # add unit tests
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.cpp b/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.cpp
index 5f60ff6283fc49fb24b58708ab8aa133007b01ad..e52591c31ddb91f4e8d13975bbe4aab2fbbb4349 100644
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.cpp
@@ -29,8 +29,7 @@
 #include <RobotAPI/libraries/armem/error/ArMemError.h>
 #include <RobotAPI/libraries/armem/component/MemoryRemoteGui.h>
 
-#include <RobotAPI/components/armem/ArMemExampleMemory/aron/SimpleObject.aron.generated.h>
-#include <RobotAPI/components/armem/ArMemExampleMemory/aron/Primitive.aron.generated.h>
+#include <RobotAPI/components/armem/ArMemExampleMemory/aron/ExampleData.aron.generated.h>
 
 
 namespace armarx
@@ -74,8 +73,7 @@ namespace armarx
         p.core._defaultSegmentsStr.clear();
         memory.addCoreSegments(p.core.defaultCoreSegments);
 
-        memory.addCoreSegment("Primitive", armem::aron::test::Primitive::toInitialAronType());
-        memory.addCoreSegment("SimpleObject", armem::aron::test::SimpleObject::toInitialAronType());
+        memory.addCoreSegment("ExampleData", armem::aron::example::ExampleData::toInitialAronType());
     }
 
 
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.h b/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.h
index e8b397677bc54a0d62374099aa337b89b9fd996f..7a62894387120b9a0822251801c3b94fac2b6301 100644
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.h
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/ArMemExampleMemory.h
@@ -120,7 +120,7 @@ namespace armarx
 
             struct CoreSegments
             {
-                std::vector<std::string> defaultCoreSegments = { "ExampleModality", "ExampleData" };
+                std::vector<std::string> defaultCoreSegments = { "ExampleModality", "ExampleConcept" };
                 std::string _defaultSegmentsStr;
                 bool addOnUsage = false;
             };
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt b/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt
index 6c823179ccf9cfcbc62406777a864e3d5f0e5998..6cbc35ed322c3c1a941b7ef1c1cecc3e8d9ead81 100644
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt
@@ -1,5 +1,7 @@
 armarx_component_set_name("ArMemExampleMemory")
 
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
 
 
 set(COMPONENT_LIBS
@@ -7,30 +9,29 @@ set(COMPONENT_LIBS
     ArmarXGuiComponentPlugins
     RobotAPICore RobotAPIInterfaces armem
     # RobotAPIComponentPlugins  # for ArViz and other plugins
-)
 
-set(ARON_FILES
-    xmls/SimpleObject.xml
-    xmls/Primitive.xml
+    ${IVT_LIBRARIES}
 )
+
 set(SOURCES
     ArMemExampleMemory.cpp
 )
 set(HEADERS
     ArMemExampleMemory.h
-
-    ${ARON_FILES}
 )
 
 
 armarx_add_component("${SOURCES}" "${HEADERS}")
+if (IVT_FOUND)
+    target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${IVT_INCLUDE_DIRS})
+endif()
 
 
 armarx_enable_aron_file_generation_for_target(
     TARGET_NAME
         ${ARMARX_COMPONENT_NAME}
     ARON_FILES
-        ${ARON_FILES}
+        xmls/ExampleData.xml
 )
 
 
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/ExampleData.xml b/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/ExampleData.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0cd92acebcb91a793a58a7ce09896ac8c2f91214
--- /dev/null
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/ExampleData.xml
@@ -0,0 +1,86 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+  <CodeIncludes>
+    <Include include="<Eigen/Core>" />
+    <Include include="<Image/ByteImage.h>" />
+  </CodeIncludes>
+  <GenerateTypes>
+    <GenerateType name='armarx::armem::aron::example::ExampleData'>
+      <ObjectChild key='the_int'>
+        <Int />
+      </ObjectChild>
+      <ObjectChild key='the_long'>
+        <Long />
+      </ObjectChild>
+      <ObjectChild key='the_float'>
+        <Float />
+      </ObjectChild>
+      <ObjectChild key='the_double'>
+        <Double />
+      </ObjectChild>
+      <ObjectChild key='the_string'>
+        <String />
+      </ObjectChild>
+      <ObjectChild key='the_bool'>
+        <Bool />
+      </ObjectChild>
+
+
+      <ObjectChild key='the_eigen_position'>
+        <EigenMatrix rows="3" cols="1" type="float" />
+      </ObjectChild>
+      <ObjectChild key='the_eigen_pose'>
+        <EigenMatrix rows="4" cols="4" type="float" />
+      </ObjectChild>
+      <ObjectChild key='the_ivt_image'>
+        <IVTCByteImage type="GrayScale" />
+      </ObjectChild>
+
+
+      <ObjectChild key='the_float_list'>
+        <List>
+          <Float />
+        </List>
+      </ObjectChild>
+      <ObjectChild key='the_int_list'>
+        <List>
+          <Int />
+        </List>
+      </ObjectChild>
+      <ObjectChild key='the_string_list'>
+        <List>
+          <String />
+        </List>
+      </ObjectChild>
+
+      <ObjectChild key='the_object_list'>
+        <List>
+          <Object name='ListClass'>
+            <ObjectChild key='element_int'>
+              <Int />
+            </ObjectChild>
+            <ObjectChild key='element_float'>
+              <Float />
+            </ObjectChild>
+            <ObjectChild key='element_string'>
+              <String />
+            </ObjectChild>
+          </Object>
+        </List>
+      </ObjectChild>
+
+      <ObjectChild key='the_float_dict'>
+        <Dict>
+          <Float />
+        </Dict>
+      </ObjectChild>
+      <ObjectChild key='the_int_dict'>
+        <Dict>
+          <Int />
+        </Dict>
+      </ObjectChild>
+
+    </GenerateType>
+  </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/Primitive.xml b/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/Primitive.xml
deleted file mode 100644
index 370957d27fe7f95dcc0f43b5bc807114d4343321..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/Primitive.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<!--Some fancy comment -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
-    <GenerateTypes>
-        <GenerateType name='armarx::armem::aron::test::Primitive'>
-            <objectchild key='the_int'>
-                <int />
-            </ObjectChild>
-            <objectchild key='the_long'>
-                <long />
-            </ObjectChild>
-            <objectchild key='the_float'>
-                <float />
-            </ObjectChild>
-            <objectchild key='the_double'>
-                <double />
-            </ObjectChild>
-            <objectchild key='the_string'>
-                <string />
-            </ObjectChild>
-            <objectchild key='the_bool'>
-                <bool />
-            </ObjectChild>
-        </GenerateType>
-    </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/SimpleObject.xml b/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/SimpleObject.xml
deleted file mode 100644
index 6bff1a70a408f2ff82093a6a6c76773cb5444a1e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/SimpleObject.xml
+++ /dev/null
@@ -1,36 +0,0 @@
-<!--Some fancy comment -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
-    <GenerateTypes>
-
-        <GenerateType name='armarx::SimpleClass'>
-            <objectchild key='the_bool'>
-                <bool />
-            </objectchild>
-        </GenerateType>
-
-        <GenerateType name='armarx::armem::aron::test::SimpleObject'>
-            <objectchild key='local_object'>
-                <object name='OtherFancyObject'>
-                    <objectchild key='blarg'>
-                        <bool />
-                    </objectchild>
-                    <objectchild key='other_blarg'>
-                        <int />
-                    </objectchild>
-                </object>
-            </ObjectChild>
-            <objectchild key='another_local_object'>
-                <object name='VisionClass'>
-                    <objectchild key='blarg'>
-                        <bool />
-                    </objectchild>
-                    <objectchild key='other_object'>
-                        <int />
-                    </objectchild>
-                </object>
-            </ObjectChild>
-        </GenerateType>
-
-    </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index 675ad90c97dd55c2f2c4ddda8856fa3682b2b22f..a0fe164c4803ceb322c451b464c0c6acdc48bebf 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -105,7 +105,7 @@ namespace armarx
                 return *_int16;
             }
 
-            template <bool constPtr = ConstPtr, std::enable_if_t < !constPtr, int> = 0>
+            template < bool constPtr = ConstPtr, std::enable_if_t < !constPtr, int > = 0 >
             void setValue(float val)
             {
                 if (_float)
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 09a7502102c009e07733934a998385dc5703a9db..47dce8075cc3bcf7e80db2273f2a663268e91b3e 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -103,8 +103,8 @@ set(SLICE_FILES
     aron/Aron.ice
 
     armem.ice
-    armem/ReadingInterface.ice
-    armem/WritingInterface.ice
+    armem/ReadingMemoryInterface.ice
+    armem/WritingMemoryInterface.ice
     armem/MemoryInterface.ice
     armem/MemoryListenerInterface.ice
     armem/MemoryNameSystemInterface.ice
diff --git a/source/RobotAPI/interface/armem.ice b/source/RobotAPI/interface/armem.ice
index a01037e52a60b33cf9422b5c69b61eeb858a4e44..c86d4d5fb6588a716a5b1f4cfe0cdbab29066bb8 100644
--- a/source/RobotAPI/interface/armem.ice
+++ b/source/RobotAPI/interface/armem.ice
@@ -1,7 +1,7 @@
 #pragma once
 
-#include <RobotAPI/interface/armem/ReadingInterface.ice>
-#include <RobotAPI/interface/armem/WritingInterface.ice>
+#include <RobotAPI/interface/armem/ReadingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/WritingMemoryInterface.ice>
 #include <RobotAPI/interface/armem/MemoryInterface.ice>
 #include <RobotAPI/interface/armem/MemoryListenerInterface.ice>
 
diff --git a/source/RobotAPI/interface/armem/MemoryInterface.ice b/source/RobotAPI/interface/armem/MemoryInterface.ice
index 90be8cb274e8fa2f136c1fb63b316e2ca6700c1d..b44718d2285757e67c4edf59d8b21f41b7b97d7b 100644
--- a/source/RobotAPI/interface/armem/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/MemoryInterface.ice
@@ -1,7 +1,7 @@
 #pragma once
 
-#include <RobotAPI/interface/armem/ReadingInterface.ice>
-#include <RobotAPI/interface/armem/WritingInterface.ice>
+#include <RobotAPI/interface/armem/ReadingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/WritingMemoryInterface.ice>
 
 
 module armarx
@@ -9,7 +9,7 @@ module armarx
     module armem
     {
 
-        interface MemoryInterface extends ReadingInterface, WritingInterface
+        interface MemoryInterface extends ReadingMemoryInterface, WritingMemoryInterface
         {
         };
 
diff --git a/source/RobotAPI/interface/armem/ReadingInterface.ice b/source/RobotAPI/interface/armem/ReadingMemoryInterface.ice
similarity index 82%
rename from source/RobotAPI/interface/armem/ReadingInterface.ice
rename to source/RobotAPI/interface/armem/ReadingMemoryInterface.ice
index a50056efb2fa78671f62e6173296d352affb5e07..e38e3dc44fd96b76b218c634229a4dcbca8ef4e7 100644
--- a/source/RobotAPI/interface/armem/ReadingInterface.ice
+++ b/source/RobotAPI/interface/armem/ReadingMemoryInterface.ice
@@ -8,7 +8,7 @@ module armarx
     module armem
     {
 
-        interface ReadingInterface
+        interface ReadingMemoryInterface
         {
             data::QueryResult query(data::QueryInput input);
         };
diff --git a/source/RobotAPI/interface/armem/WritingInterface.ice b/source/RobotAPI/interface/armem/WritingMemoryInterface.ice
similarity index 88%
rename from source/RobotAPI/interface/armem/WritingInterface.ice
rename to source/RobotAPI/interface/armem/WritingMemoryInterface.ice
index 902e80dded2f9717bd3570ca3dd1051b308b3fd0..e8e43287b962445609e8209c2057eeaed179bf4c 100644
--- a/source/RobotAPI/interface/armem/WritingInterface.ice
+++ b/source/RobotAPI/interface/armem/WritingMemoryInterface.ice
@@ -1,5 +1,7 @@
 #pragma once
 
+
+#include <RobotAPI/interface/armem/memory.ice>
 #include <RobotAPI/interface/aron.ice>
 
 
@@ -30,8 +32,8 @@ module armarx
 
             struct EntityUpdate
             {
-                string entityID;
-                aron::data::AronDataList instancesData;
+                MemoryID entityID;
+                aron::data::AronDictSeq instancesData;
                 long timeCreatedMicroSeconds;
 
                 float confidence = 1.0;
@@ -43,7 +45,7 @@ module armarx
             {
                 bool success = false;
 
-                string snapshotID;
+                MemoryID snapshotID;
                 long timeArrivedMicroSeconds;
 
                 string errorMessage;
@@ -61,7 +63,7 @@ module armarx
         }
 
 
-        interface WritingInterface
+        interface WritingMemoryInterface
         {
             /// Register multiple core or provider segments.
             data::AddSegmentsResult addSegments(data::AddSegmentsInput input);
diff --git a/source/RobotAPI/interface/armem/memory.ice b/source/RobotAPI/interface/armem/memory.ice
index b501eaf9e54a793dd3188182ba77ad2291af7abe..960d15efae133392c4f18fc5e787b367ea2ff883 100644
--- a/source/RobotAPI/interface/armem/memory.ice
+++ b/source/RobotAPI/interface/armem/memory.ice
@@ -47,7 +47,7 @@ module armarx
             /// Ice Twin of `armarx::armem::EntityInstance`.
             class EntityInstance extends detail::MemoryItem
             {
-                aron::data::AronData data;
+                aron::data::AronDict data;
 
                 EntityInstanceMetadata metadata;
             };
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index bf5e0bd972a0e29d633b8cb45e1975e45fbf752a..cddbcc1558554cdb3ad19e81e981d0480e48b668 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -73,9 +73,11 @@ module armarx
             dictionary<string, AronType> AronTypeDict;
 
             // Container Types (serialize to object/list)
+            class AronContainerType extends AronType {};
+
             // Please note that either elementTypes xor acceptedType is set!!!
-            class AronDictSerializerType extends AronType { AronTypeDict elementTypes; AronType acceptedType;};
-            class AronListSerializerType extends AronType { AronTypeList elementTypes; AronType acceptedType;};
+            class AronDictSerializerType extends AronContainerType { AronTypeDict elementTypes; AronType acceptedType;};
+            class AronListSerializerType extends AronContainerType { AronTypeList elementTypes; AronType acceptedType;};
 
             class AronListType extends AronListSerializerType { };
             class AronTupleType extends AronListSerializerType { };
@@ -84,7 +86,8 @@ module armarx
             class AronDictType extends AronDictSerializerType { };
 
             // Complex Types (serialize to ndarray)
-            class AronNDArraySerializerType extends AronType { AronIntSequence dimensions; string typeName; };
+            class AronComplexType extends AronType {};
+            class AronNDArraySerializerType extends AronComplexType { AronIntSequence dimensions; string typeName; };
             #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
                 class Aron##upperType##Type extends AronNDArraySerializerType { };
 
@@ -92,12 +95,13 @@ module armarx
             #undef RUN_ARON_MACRO
 
             // Primitive Types
+            class AronPrimitiveType extends AronType {};
             #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-                class Aron##upperType##Type extends AronType { };
+                class Aron##upperType##Type extends AronPrimitiveType { };
 
                     HANDLE_PRIMITIVE_TYPES
             #undef RUN_ARON_MACRO
-                };
+        };
 
 
         /*************************
@@ -110,19 +114,34 @@ module armarx
             dictionary<string, AronData> AronDataDict;
 
             // Container Data
-            class AronList extends AronData { AronDataList elements; };
-            class AronDict extends AronData { AronDataDict elements; };
+            class AronContainer extends AronData {};
+            class AronList extends AronContainer { AronDataList elements; };
+            class AronDict extends AronContainer { AronDataDict elements; };
 
             // Complex Data. The NDArray contains the same information as an AronType, but there is no other way to do it
-            class AronNDArray extends AronData { AronIntSequence dimensions; string type; AronByteSequence data; };
+            class AronComplex extends AronData {};
+            class AronNDArray extends AronComplex { AronIntSequence dimensions; string type; AronByteSequence data; };
 
             // Basic Data
+            class AronPrimitive extends AronData {};
             #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-                class Aron##upperType extends AronData { lowerType value; };
+                class Aron##upperType extends AronPrimitive { lowerType value; };
 
                     HANDLE_PRIMITIVE_TYPES
             #undef RUN_ARON_MACRO
-                };
+
+
+            sequence<AronDict> AronDictSeq;
+        };
 
     };
 };
+
+#undef HANDLE_ALL_ARON_TYPES
+#undef HANDLE_PRIMITIVE_TYPES
+#undef HANDLE_COMPLEX_TYPES
+#undef HANDLE_CONTAINER_TYPES
+
+#undef HANDLE_ALL_ARON_DATA
+#undef HANDLE_COMPLEX_DATA
+#undef HANDLE_CONTAINER_DATA
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
index 2a08af75ac30a80201b22ec442ccd14db620b94c..bceb67e36837a6444a2d64249f7088389d019b26 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
@@ -284,7 +284,7 @@ namespace armarx
         else
         {
             objectDMP->flow(deltaT, currentPose, currentTwist);
-//            VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(objectDMP->getTargetPoseMat());
+            //            VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(objectDMP->getTargetPoseMat());
             LockGuardType guard {controlDataMutex};
             getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
             writeControlStruct();
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index e93b5017214036c1e781e82741de96c57e01a971..ef0d4e6cbfaddcb6f7a11de1c92d2366d326b273 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -51,7 +51,7 @@ set(LIB_FILES
     client/QueryBuilder.cpp
     client/query/selectors.cpp
 
-    component/IceMemory.cpp
+    component/MemoryToIceAdapter.cpp
     component/MemoryComponentPlugin.cpp
     component/MemoryRemoteGui.cpp
     component/RemoteGuiAronDataVisitor.cpp
@@ -109,7 +109,7 @@ set(LIB_HEADERS
     client/query/query_fns.h
     client/query/selectors.h
 
-    component/IceMemory.h
+    component/MemoryToIceAdapter.h
     component/MemoryComponentPlugin.h
     component/MemoryRemoteGui.h
     component/RemoteGuiAronDataVisitor.h
diff --git a/source/RobotAPI/libraries/armem/client/QueryBuilder.h b/source/RobotAPI/libraries/armem/client/QueryBuilder.h
index dcfd855cc064a6addccf7b3b57c64c0b9789b4ca..2e38d09865a192123f30f061fdadeeef42d8eee5 100644
--- a/source/RobotAPI/libraries/armem/client/QueryBuilder.h
+++ b/source/RobotAPI/libraries/armem/client/QueryBuilder.h
@@ -1,7 +1,6 @@
 #pragma once
 
 #include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/interface/armem/ReadingInterface.h>
 
 #include "../core/DataMode.h"
 #include "Query.h"
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index e0869b987cfb6d4280706716666ced809c8aec4f..558703dcc1cc1110dc244b6f596cb220d7996f31 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -9,7 +9,7 @@
 namespace armarx::armem::client
 {
 
-    Reader::Reader(ReadingInterfacePrx memory) : memory(memory)
+    Reader::Reader(ReadingMemoryInterfacePrx memory) : memory(memory)
     {
     }
 
@@ -112,7 +112,7 @@ namespace armarx::armem::client
 
 
     void
-    Reader::setReadingMemory(ReadingInterfacePrx memory)
+    Reader::setReadingMemory(ReadingMemoryInterfacePrx memory)
     {
         this->memory = memory;
     }
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index fe293ddbabdf4aaca6856db5bd6fb99dfbe21e04..2a7e4f4c1101b5924f11f9e52faebbc8cad79dcf 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -7,7 +7,7 @@
 #include <vector>
 
 // RobotAPI
-#include <RobotAPI/interface/armem/ReadingInterface.h>
+#include <RobotAPI/interface/armem/ReadingMemoryInterface.h>
 #include <RobotAPI/interface/armem/MemoryListenerInterface.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
 #include <RobotAPI/libraries/armem/memory/Memory.h>
@@ -32,7 +32,7 @@ namespace armarx::armem::client
          * @brief Construct a memory reader.
          * @param memory The memory proxy.
          */
-        Reader(ReadingInterfacePrx memory = nullptr);
+        Reader(ReadingMemoryInterfacePrx memory = nullptr);
 
 
         QueryResult query(const QueryInput& input);
@@ -50,7 +50,7 @@ namespace armarx::armem::client
         void subscribe(const MemoryID& entityID, callback callback);
 
 
-        void setReadingMemory(ReadingInterfacePrx memory);
+        void setReadingMemory(ReadingMemoryInterfacePrx memory);
 
         operator bool() const
         {
@@ -59,7 +59,7 @@ namespace armarx::armem::client
 
     public:
 
-        ReadingInterfacePrx memory;
+        ReadingMemoryInterfacePrx memory;
 
     private:
 
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
index d7eeff0feb7bda07299ce6791643feef38980284..fca6d9442ba419d26a3d6ca9cda23ad90ebb436c 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
@@ -45,7 +45,7 @@ armarx::armem::client::ReaderComponentPluginUser::memoryUpdated(
 
 
 void
-armarx::armem::client::ReaderComponentPluginUser::setReadingMemory(ReadingInterfacePrx memory)
+armarx::armem::client::ReaderComponentPluginUser::setReadingMemory(ReadingMemoryInterfacePrx memory)
 {
     memoryReader.setReadingMemory(memory);
 }
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 66c3cd3631560ba21e441edf10adf191c0a758d1..4ed9bea8fa74e455c2ccf976eb00d97b0828295c 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -49,7 +49,7 @@ namespace armarx::armem::client
 
     protected:
 
-        void setReadingMemory(ReadingInterfacePrx memory);
+        void setReadingMemory(ReadingMemoryInterfacePrx memory);
 
     protected:
 
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index c7a14a15d4816e1991337c41f321188ed18ede5e..f506221179100c532d71d341b1462fa8273d5951 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -8,7 +8,7 @@
 namespace armarx::armem::client
 {
 
-    Writer::Writer(WritingInterfacePrx memory) : memory(memory)
+    Writer::Writer(WritingMemoryInterfacePrx memory) : memory(memory)
     {
     }
 
@@ -108,7 +108,7 @@ namespace armarx::armem::client
 
 
     void
-    Writer::setWritingMemory(WritingInterfacePrx memory)
+    Writer::setWritingMemory(WritingMemoryInterfacePrx memory)
     {
         this->memory = memory;
     }
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 7ed6e44a3964e4003c69e36cba1b4855d699f3fc..67e7f57d0ff3733ebf6d2e7a007309875ba5c03e 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <RobotAPI/interface/armem/WritingInterface.h>
+#include <RobotAPI/interface/armem/WritingMemoryInterface.h>
 
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
 
@@ -19,7 +19,7 @@ namespace armarx::armem::client
          * @brief Construct a memory writer.
          * @param memory The memory proxy.
          */
-        Writer(WritingInterfacePrx memory = nullptr);
+        Writer(WritingMemoryInterfacePrx memory = nullptr);
 
 
         data::AddSegmentResult addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName);
@@ -33,7 +33,7 @@ namespace armarx::armem::client
         EntityUpdateResult commit(const EntityUpdate& update);
 
 
-        void setWritingMemory(WritingInterfacePrx memory);
+        void setWritingMemory(WritingMemoryInterfacePrx memory);
 
         operator bool() const
         {
@@ -46,7 +46,7 @@ namespace armarx::armem::client
 
     public:
 
-        WritingInterfacePrx memory;
+        WritingMemoryInterfacePrx memory;
 
     };
 
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
index f5c648090570b5eb0a1bf4cf6947efdce2aed9d7..5ed4dc1f79cd130f5090411c413bd5a8122c4df3 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
@@ -27,7 +27,7 @@ armarx::armem::client::WriterComponentPluginUser::~WriterComponentPluginUser()
 
 
 void
-armarx::armem::client::WriterComponentPluginUser::setWritingMemory(WritingInterfacePrx memory)
+armarx::armem::client::WriterComponentPluginUser::setWritingMemory(WritingMemoryInterfacePrx memory)
 {
     memoryWriter.setWritingMemory(memory);
 }
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index f93c9a230e26c00bd7d89d51cd37e2afd83ca27f..4f3eeb3bdcbd474bab5c1d068d31d50dc6d66f56 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -38,7 +38,7 @@ namespace armarx::armem::client
 
     protected:
 
-        void setWritingMemory(WritingInterfacePrx memory);
+        void setWritingMemory(WritingMemoryInterfacePrx memory);
 
     protected:
 
diff --git a/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.cpp b/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.cpp
index 47f1d8eee1187917c59b00b5c27de575c839f2d1..fdb50c345ef4f0c33538c04b80483c6bd1427287 100644
--- a/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.cpp
@@ -4,7 +4,7 @@
 
 #include "../error.h"
 
-#include "IceMemory.h"
+#include "MemoryToIceAdapter.h"
 
 
 namespace armarx::armem::plugins
diff --git a/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.h b/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.h
index 89a56ae141c51c733144de66c277a594fe2dbe3b..82aaf5bc8b485463005000492d665feb92134307 100644
--- a/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/component/MemoryComponentPlugin.h
@@ -10,7 +10,7 @@
 
 #include "../memory/Memory.h"
 #include "../mns/MemoryNameSystemClientPlugin.h"
-#include "IceMemory.h"
+#include "MemoryToIceAdapter.h"
 
 
 namespace armarx::armem
@@ -88,7 +88,7 @@ namespace armarx::armem
         Memory memory;
 
         /// Helps connecting `memory` to ice. Used to handle Ice callbacks.
-        IceMemory iceMemory { &memory };
+        MemoryToIceAdapter iceMemory { &memory };
 
 
     private:
diff --git a/source/RobotAPI/libraries/armem/component/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/component/MemoryRemoteGui.cpp
index 6d818d55d9c921affb822c4df0836ce37263a126..46b53f7684bfeaf3e19a1c947bd631028c0f31e3 100644
--- a/source/RobotAPI/libraries/armem/component/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/component/MemoryRemoteGui.cpp
@@ -1,7 +1,5 @@
 #include "MemoryRemoteGui.h"
 
-#include <stack>
-
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 #include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronAllDataNavigators.h>
diff --git a/source/RobotAPI/libraries/armem/component/IceMemory.cpp b/source/RobotAPI/libraries/armem/component/MemoryToIceAdapter.cpp
similarity index 82%
rename from source/RobotAPI/libraries/armem/component/IceMemory.cpp
rename to source/RobotAPI/libraries/armem/component/MemoryToIceAdapter.cpp
index 076e754a4886b3f952d291bf866a516f303ea001..7bee07e7b0f9d1a5f6c686542c70f82df31493fc 100644
--- a/source/RobotAPI/libraries/armem/component/IceMemory.cpp
+++ b/source/RobotAPI/libraries/armem/component/MemoryToIceAdapter.cpp
@@ -1,4 +1,4 @@
-#include "IceMemory.h"
+#include "MemoryToIceAdapter.h"
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
@@ -10,12 +10,12 @@
 namespace armarx::armem
 {
 
-    IceMemory::IceMemory(Memory* memory) : memory(memory)
+    MemoryToIceAdapter::MemoryToIceAdapter(Memory* memory) : memory(memory)
     {
     }
 
 
-    void IceMemory::setMemoryListener(MemoryListenerInterfacePrx memoryListener)
+    void MemoryToIceAdapter::setMemoryListener(MemoryListenerInterfacePrx memoryListener)
     {
         this->memoryListener = memoryListener;
     }
@@ -23,7 +23,7 @@ namespace armarx::armem
 
     // WRITING
 
-    armem::data::AddSegmentResult IceMemory::addSegment(const armem::data::AddSegmentInput& input, bool addCoreSegments)
+    armem::data::AddSegmentResult MemoryToIceAdapter::addSegment(const armem::data::AddSegmentInput& input, bool addCoreSegments)
     {
         ARMARX_DEBUG << "Adding segment '" << input.coreSegmentName << "/" << input.providerSegmentName << "'.";
         ARMARX_CHECK_NOT_NULL(memory);
@@ -78,7 +78,7 @@ namespace armarx::armem
     }
 
 
-    armem::data::AddSegmentsResult IceMemory::addSegments(const armem::data::AddSegmentsInput& input, bool addCoreSegments)
+    armem::data::AddSegmentsResult MemoryToIceAdapter::addSegments(const armem::data::AddSegmentsInput& input, bool addCoreSegments)
     {
         ARMARX_CHECK_NOT_NULL(memory);
 
@@ -90,7 +90,7 @@ namespace armarx::armem
         return output;
     }
 
-    armem::data::CommitResult IceMemory::commit(const armem::data::Commit& commitIce, Time timeArrived)
+    armem::data::CommitResult MemoryToIceAdapter::commit(const armem::data::Commit& commitIce, Time timeArrived)
     {
         ARMARX_CHECK_NOT_NULL(memory);
 
@@ -105,13 +105,13 @@ namespace armarx::armem
     }
 
 
-    armem::data::CommitResult IceMemory::commit(const armem::data::Commit& commitIce)
+    armem::data::CommitResult MemoryToIceAdapter::commit(const armem::data::Commit& commitIce)
     {
         return commit(commitIce, armem::Time::now());
     }
 
 
-    armem::CommitResult IceMemory::commit(const armem::InternalCommit& commit)
+    armem::CommitResult MemoryToIceAdapter::commit(const armem::InternalCommit& commit)
     {
         std::vector<data::MemoryID> updatedIDs;
         const bool publishUpdates = memoryListener;
@@ -152,7 +152,7 @@ namespace armarx::armem
 
     // READING
 
-    armem::data::QueryResult IceMemory::query(const armem::data::QueryInput& input)
+    armem::data::QueryResult MemoryToIceAdapter::query(const armem::data::QueryInput& input)
     {
         ARMARX_CHECK_NOT_NULL(memory);
 
diff --git a/source/RobotAPI/libraries/armem/component/IceMemory.h b/source/RobotAPI/libraries/armem/component/MemoryToIceAdapter.h
similarity index 88%
rename from source/RobotAPI/libraries/armem/component/IceMemory.h
rename to source/RobotAPI/libraries/armem/component/MemoryToIceAdapter.h
index 9c257d2c0915e41811391ae69ecf454eb8d7f8ea..fbd41bd4a39b46cbc5ee5ea5df22814e29412f88 100644
--- a/source/RobotAPI/libraries/armem/component/IceMemory.h
+++ b/source/RobotAPI/libraries/armem/component/MemoryToIceAdapter.h
@@ -16,13 +16,12 @@ namespace armarx::armem
      * This involves conversion of ice types to C++ types as well as
      * catchin exceptions and converting them to error messages
      */
-    // ToDo: Rename to MemoryToIceAdapter
-    class IceMemory
+    class MemoryToIceAdapter
     {
     public:
 
-        /// Construct an IceMemory from an existing Memory.
-        IceMemory(Memory* memory = nullptr);
+        /// Construct an MemoryToIceAdapter from an existing Memory.
+        MemoryToIceAdapter(Memory* memory = nullptr);
 
         void setMemoryListener(MemoryListenerInterfacePrx memoryListener);
 
diff --git a/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.cpp b/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.cpp
index 103aa6f4afd1e5ba8a2146f0d63d0f8362e49ffd..92b786bed827c4d69fe873aa0f81c9df0dc85345 100644
--- a/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.cpp
@@ -1,29 +1,42 @@
 #include "RemoteGuiAronDataVisitor.h"
 
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
 
 namespace armarx::armem
 {
 
+    RemoteGuiAronDataVisitor::Group::Group(const std::string& label)
+    {
+        groupBox.setLabel(label);
+        groupBox.addChild(layout);
+    }
+
     RemoteGuiAronDataVisitor::~RemoteGuiAronDataVisitor()
     {}
 
-    bool RemoteGuiAronDataVisitor::visitEnter()
+    bool RemoteGuiAronDataVisitor::visitEnter(const std::string& key, const std::string& type, size_t size)
     {
-        groups.push(GroupBox());
+        std::stringstream label;
+        label << key << " (" << type << " of size " << size << ")";
+        Group& group = groups.emplace(label.str());
+        (void) group;
         return true;
     }
 
     bool RemoteGuiAronDataVisitor::visitExit()
     {
-        GroupBox group = groups.top();
+        Group group = groups.top();
         groups.pop();
         if (groups.size() > 0)
         {
-            groups.top().addChild(group);
+            groups.top().groupBox.addChild(group.groupBox);
         }
         else
         {
-            result = group;
+            result = group.groupBox;
         }
         return true;
     }
@@ -35,9 +48,16 @@ namespace armarx::armem
 
     void RemoteGuiAronDataVisitor::streamValueText(AronNDArrayDataNavigator& n, std::stringstream& ss)
     {
-        // No useful data in n
-        (void) n;
-        ss << "<NdArray>";
+        ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getDimensions()), ", ") << ")";
+    }
+
+    void RemoteGuiAronDataVisitor::checkGroupsNotEmpty() const
+    {
+        ARMARX_CHECK_POSITIVE(groups.size()) << "Groups must not be empty.";
     }
 
+
+
+
+
 }
diff --git a/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.h b/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.h
index c009416a7068706d88ea93cad079d439650091cc..678718d66e44bc8aaf1829e45a5a8ef7bb31a84a 100644
--- a/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.h
+++ b/source/RobotAPI/libraries/armem/component/RemoteGuiAronDataVisitor.h
@@ -16,77 +16,87 @@ namespace armarx::armem
     struct RemoteGuiAronDataVisitor : public aron::visitor::AronDataVisitor
     {
         using GroupBox = armarx::RemoteGui::Client::GroupBox;
+        using GridLayout = armarx::RemoteGui::Client::GridLayout;
         using Label = armarx::RemoteGui::Client::Label;
 
-        std::stack<GroupBox> groups;
+
+        struct Group
+        {
+            Group(const std::string& label = {});
+
+            GroupBox groupBox;
+            GridLayout layout;
+            int nextRow = 0;
+        };
+
+        std::stack<Group> groups;
         GroupBox result;
 
 
         virtual ~RemoteGuiAronDataVisitor() override;
 
 
-        bool visitEnter(AronDictDataNavigator&) override
+        bool visitEnter(const std::string& key, AronDictDataNavigator& n) override
         {
-            return visitEnter();
+            return visitEnter(key, "dict", n.childrenSize());
         }
         bool visitExit(AronDictDataNavigator&) override
         {
             return visitExit();
         }
 
-        bool visitEnter(AronListDataNavigator& list) override
+        bool visitEnter(const std::string& key, AronListDataNavigator& n) override
         {
-            (void) list;
-            // ARMARX_INFO << "- List Enter (path: '" << list.getPath().toString() << "')";
-            return visitEnter();
+            return visitEnter(key, "list", n.childrenSize());
         }
-        bool visitExit(AronListDataNavigator& list) override
+        bool visitExit(AronListDataNavigator&) override
         {
-            (void) list;
-            // ARMARX_INFO << "- List Exit (path: '" << list.getPath().toString() << "')";
             return visitExit();
         }
 
         // Same for Dict and List
-        bool visitEnter();
+        bool visitEnter(const std::string& key, const std::string& type, size_t size);
         bool visitExit();
 
+        // Do not hide base overloads.
+        using AronDataVisitor::visitEnter;
+        using AronDataVisitor::visitExit;
 
-        bool visit(AronBoolDataNavigator& b) override
+
+        bool visit(const std::string& key, AronBoolDataNavigator& b) override
         {
-            this->addValueLabel(b, "bool");
+            this->addValueLabel(key, b, "bool");
             return true;
         }
-        bool visit(AronDoubleDataNavigator& d) override
+        bool visit(const std::string& key, AronDoubleDataNavigator& d) override
         {
-            this->addValueLabel(d, "double");
+            this->addValueLabel(key, d, "double");
             return true;
         }
-        bool visit(AronFloatDataNavigator& f) override
+        bool visit(const std::string& key, AronFloatDataNavigator& f) override
         {
-            this->addValueLabel(f, "float");
+            this->addValueLabel(key, f, "float");
             return true;
         }
-        bool visit(AronIntDataNavigator& i) override
+        bool visit(const std::string& key, AronIntDataNavigator& i) override
         {
-            this->addValueLabel(i, "int");
+            this->addValueLabel(key, i, "int");
             return true;
         }
-        bool visit(AronLongDataNavigator& l) override
+        bool visit(const std::string& key, AronLongDataNavigator& l) override
         {
-            this->addValueLabel(l, "long");
+            this->addValueLabel(key, l, "long");
             return true;
         }
-        bool visit(AronStringDataNavigator& string) override
+        bool visit(const std::string& key, AronStringDataNavigator& string) override
         {
-            this->addValueLabel(string, "string");
+            this->addValueLabel(key, string, "string");
             return true;
         }
 
-        bool visit(AronNDArrayDataNavigator& array) override
+        bool visit(const std::string& key, AronNDArrayDataNavigator& array) override
         {
-            (void) array;
-            // ARMARX_INFO << "- NdArray (path: '" << array.getPath().toString() << "')";
+            this->addValueLabel(key, array, "ND Array");
             return true;
         }
 
@@ -94,15 +104,22 @@ namespace armarx::armem
     private:
 
         template <class Navigator>
-        void addValueLabel(Navigator& n, const std::string& typeName)
+        void addValueLabel(const std::string& key, Navigator& n, const std::string& typeName)
         {
-            groups.top().addChild(Label(this->makeValueLabelText(n, typeName)));
+            checkGroupsNotEmpty();
+            Group& g = groups.top();
+            g.layout
+            .add(Label(key), {g.nextRow, 0})
+            .add(Label("(" + typeName + ")"), {g.nextRow, 1})
+            .add(Label("= " + getValueText(n)), {g.nextRow, 2})
+            ;
+            ++g.nextRow;
         }
+
         template <class Navigator>
-        std::string makeValueLabelText(Navigator& n, const std::string& typeName)
+        std::string getValueText(Navigator& n)
         {
             std::stringstream ss;
-            ss << n.getPath().getLastElement() << ": " << typeName << " = ";
             streamValueText(n, ss);
             return ss.str();
         }
@@ -115,6 +132,8 @@ namespace armarx::armem
         void streamValueText(AronStringDataNavigator& n, std::stringstream& ss);
         void streamValueText(AronNDArrayDataNavigator& n, std::stringstream& ss);
 
+        void checkGroupsNotEmpty() const;
+
     };
 
 
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 6361aa97cbf73e999c0d3b40ba7f4bc8ec8ce604..cc87c579ad2841f5efeb4b89aa8d8f6d05d67dfc 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -2,7 +2,7 @@
 
 #include <vector>
 
-#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDataNavigator.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h>
 
 #include "../core/MemoryID.h"
 #include "../core/Time.h"
@@ -20,7 +20,7 @@ namespace armarx::armem
         MemoryID entityID;
 
         /// The entity data.
-        std::vector<aron::datanavigator::AronDataNavigatorPtr> instancesData;
+        std::vector<aron::datanavigator::AronDictDataNavigatorPtr> instancesData;
 
         /**
          * @brief Time when this entity update was created (e.g. time of image recording).
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index a5bed94aadc790b46e887056b74537a99b59daa2..f942f44f0036c6b826cf91aa765245c450d0cb2b 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -81,9 +81,13 @@ namespace armarx
 
     void armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update)
     {
-        update.entityID = MemoryID::fromString(ice.entityID);
+        fromIce(ice.entityID, update.entityID);
+
+        update.instancesData.clear();
+        update.instancesData.reserve(ice.instancesData.size());
+        std::transform(ice.instancesData.begin(), ice.instancesData.end(), std::back_inserter(update.instancesData),
+                       aron::datanavigator::AronDictDataNavigator::FromAronDict);
 
-        update.instancesData = aron::datanavigator::AronDataNavigator::FromAronData(ice.instancesData);
         update.timeCreated = Time::microSeconds(ice.timeCreatedMicroSeconds);
 
         update.confidence = ice.confidence;
@@ -92,8 +96,13 @@ namespace armarx
 
     void armem::toIce(data::EntityUpdate& ice, const EntityUpdate& update)
     {
-        ice.entityID = update.entityID.str();
-        ice.instancesData = aron::datanavigator::AronDataNavigator::ToAronData(update.instancesData);
+        toIce(ice.entityID, update.entityID);
+
+        ice.instancesData.clear();
+        ice.instancesData.reserve(update.instancesData.size());
+        std::transform(update.instancesData.begin(), update.instancesData.end(), std::back_inserter(ice.instancesData),
+                       aron::datanavigator::AronDictDataNavigator::ToAronDict);
+
         ice.timeCreatedMicroSeconds = update.timeCreated.toMicroSeconds();
 
         ice.confidence = update.confidence;
@@ -104,7 +113,7 @@ namespace armarx
     void armem::fromIce(const data::EntityUpdateResult& ice, EntityUpdateResult& result)
     {
         result.success = ice.success;
-        result.snapshotID = MemoryID::fromString(ice.snapshotID);
+        fromIce(ice.snapshotID, result.snapshotID);
         result.timeArrived = Time::microSeconds(ice.timeArrivedMicroSeconds);
         result.errorMessage = ice.errorMessage;
     }
@@ -112,7 +121,7 @@ namespace armarx
     void armem::toIce(data::EntityUpdateResult& ice, const EntityUpdateResult& result)
     {
         ice.success = result.success;
-        ice.snapshotID = result.snapshotID.str();
+        toIce(ice.snapshotID, result.snapshotID);
         ice.timeArrivedMicroSeconds = result.timeArrived.toMicroSeconds();
         ice.errorMessage = result.errorMessage;
     }
diff --git a/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.cpp b/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.cpp
index ed3ff03094b9d2cff83bbf81ae08584e7b81a597..e3ade250a80eb9cf5858d6be946b11856e4ca8aa 100644
--- a/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.cpp
+++ b/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.cpp
@@ -13,12 +13,102 @@ namespace armarx::armem::ltm
 
     }
 
-    bool LongTermMemoryLUT::writeOnDisk(const MemoryPtr& m)
+    void LongTermMemoryLUT::writeOnDisk(const MemoryPtr& m)
     {
-        return writer->writeOnDisk(m);
+        io::DiskWriterReturnInformation info = writer->writeOnDisk(m);
+        updateLUT(info.storedElements);
     }
-    bool LongTermMemoryLUT::writeOnDisk(const std::map<std::string, CoreSegmentPtr>& m)
+    void LongTermMemoryLUT::writeOnDisk(const std::map<std::string, CoreSegmentPtr>& m)
     {
-        return writer->writeOnDisk(m);
+        io::DiskWriterReturnInformation info = writer->writeOnDisk(m);
+        updateLUT(info.storedElements);
+    }
+
+    void LongTermMemoryLUT::loadFromDisk()
+    {
+
+    }
+
+    std::string LongTermMemoryLUT::getLUTasString()
+    {
+        std::stringstream ss;
+        for (const auto& [coreKey, coreSegment] : lut)
+        {
+            ss << coreKey << ": " << std::endl;
+            for (const auto& [providerKey, providerSegment] : coreSegment)
+            {
+                ss << "\t" << providerKey << ": " << std::endl;
+                for (const auto& [entityKey, entity] : providerSegment)
+                {
+                    ss << "\t\t" << entityKey << ": " << std::endl;
+                    for (const auto& [entityHistoryTimestamp, entitySnapshot] : entity)
+                    {
+                        ss << "\t\t\t" << entityHistoryTimestamp.toMilliSeconds() << ":" << std::endl;
+                        ss << "\t\t\t\t" << "[";
+                        for (unsigned int i = 0; i < entitySnapshot.size(); ++i)
+                        {
+                            const std::string& newlyGenerate = entitySnapshot[i];
+                            if (i > 0)
+                            {
+                                ss << ", ";
+                            }
+                            ss << "(" << i << " => " << newlyGenerate << ")";
+                        }
+                        ss << "]" << std::endl;
+                    }
+                }
+            }
+        }
+        return ss.str();
+    }
+
+    void LongTermMemoryLUT::updateLUT(const EasyStringMemory& info)
+    {
+        for (const auto& [coreKey, coreSegment] : info)
+        {
+            if (lut.find(coreKey) == lut.end())
+            {
+                lut[coreKey] = coreSegment;
+                continue;
+            }
+
+            EasyStringCoreSegment& lutCoreSegment = lut[coreKey];
+            for (const auto& [providerKey, providerSegment] : coreSegment)
+            {
+                if (lutCoreSegment.find(providerKey) == lutCoreSegment.end())
+                {
+                    lutCoreSegment[providerKey] = providerSegment;
+                    continue;
+                }
+
+                EasyStringProviderSegment& lutProviderSegment = lutCoreSegment[providerKey];
+                for (const auto& [entityKey, entity] : providerSegment)
+                {
+                    if (lutProviderSegment.find(entityKey) == lutProviderSegment.end())
+                    {
+                        lutProviderSegment[entityKey] = entity;
+                        continue;
+                    }
+
+                    EasyStringEntity& lutEntity = lutProviderSegment[entityKey];
+                    for (const auto& [entityHistoryTimestamp, entitySnapshot] : entity)
+                    {
+                        if (lutEntity.find(entityHistoryTimestamp) == lutEntity.end())
+                        {
+                            lutEntity[entityHistoryTimestamp] = entitySnapshot;
+                            continue;
+                        }
+
+                        EasyStringEntityHistory& lutEntitySnapshot = lutEntity[entityHistoryTimestamp];
+                        //for (unsigned int i = 0; i < entitySnapshot.size(); ++i)
+                        //{
+                        //const std::string& newlyGenerate = entitySnapshot[i];
+                        //}
+                        // for now, simply overwrite all values since a history timestamp should not have to different snapshots
+                        lutEntitySnapshot = entitySnapshot;
+                    }
+                }
+            }
+        }
     }
 }
diff --git a/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.h b/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.h
index 0d286eedb45ae8b908f6855689cbbefe27692c40..9f6f937e35a3e245266b31628f69198ab3bf5aa3 100644
--- a/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.h
+++ b/source/RobotAPI/libraries/armem/ltm/LongTermMemoryLUT.h
@@ -34,13 +34,6 @@ namespace armarx::armem
 {
     namespace ltm
     {
-
-        typedef std::vector<std::string> EasyStringEntityHistory; // only store paths to stored files
-        typedef std::map<Time, EasyStringEntityHistory> EasyStringEntity;
-        typedef std::map<std::string, EasyStringEntity> EasyStringProviderSegment;
-        typedef std::map<std::string, EasyStringProviderSegment> EasyStringCoreSegment;
-        typedef std::map<std::string, EasyStringCoreSegment> EasyStringMemory;
-
         class LongTermMemoryLUT;
         typedef std::shared_ptr<LongTermMemoryLUT> LongTermMemoryLUTPtr;
 
@@ -51,8 +44,15 @@ namespace armarx::armem
             LongTermMemoryLUT(const std::string&);
             LongTermMemoryLUT(const io::DiskWriterPtr&);
 
-            bool writeOnDisk(const MemoryPtr&);
-            bool writeOnDisk(const std::map<std::string, CoreSegmentPtr>&);
+            void writeOnDisk(const MemoryPtr&);
+            void writeOnDisk(const std::map<std::string, CoreSegmentPtr>&);
+
+            void loadFromDisk();
+
+            std::string getLUTasString();
+
+        private:
+            void updateLUT(const EasyStringMemory& info);
 
         private:
             EasyStringMemory lut;
diff --git a/source/RobotAPI/libraries/armem/ltm/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h b/source/RobotAPI/libraries/armem/ltm/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h
index 540b3ae7c3fce5c4a930e7b074410a06a969b6c6..1787f168640e81199d7c0a72df9ad420b8d705c6 100644
--- a/source/RobotAPI/libraries/armem/ltm/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h
+++ b/source/RobotAPI/libraries/armem/ltm/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h
@@ -38,7 +38,7 @@ namespace armarx::armem
             typedef std::shared_ptr<NlohmannJSONDiskReader> NlohmannJSONDiskReaderPtr;
 
             class NlohmannJSONDiskReader :
-                    virtual public DiskReader
+                virtual public DiskReader
             {
             public:
                 NlohmannJSONDiskReader() = default;
diff --git a/source/RobotAPI/libraries/armem/ltm/io/DiskWriter/DiskWriter.h b/source/RobotAPI/libraries/armem/ltm/io/DiskWriter/DiskWriter.h
index 4cbd3f01acf8fb033ab18b2debcdf55580a5e2a6..98367758de3c6b35ffa82e1306f4409fbc60b7fa 100644
--- a/source/RobotAPI/libraries/armem/ltm/io/DiskWriter/DiskWriter.h
+++ b/source/RobotAPI/libraries/armem/ltm/io/DiskWriter/DiskWriter.h
@@ -47,8 +47,24 @@ namespace armarx::armem
 {
     namespace ltm
     {
+        typedef std::vector<std::string> EasyStringEntityHistory; // absolute paths to stored files
+        typedef std::map<Time, EasyStringEntityHistory> EasyStringEntity;
+        typedef std::map<std::string, EasyStringEntity> EasyStringProviderSegment;
+        typedef std::map<std::string, EasyStringProviderSegment> EasyStringCoreSegment;
+        typedef std::map<std::string, EasyStringCoreSegment> EasyStringMemory;
+
         namespace io
         {
+            struct DiskWriterReturnInformation
+            {
+                EasyStringMemory storedElements;
+                std::vector<std::string> coreSegmentsError;
+                std::vector<std::string> providerSegmentsError;
+                std::vector<std::string> entitiesError;
+                std::vector<std::string> historyTimestampsError;
+                std::vector<std::string> entityInstancesError;
+            };
+
             class DiskWriter;
             using DiskWriterPtr = std::shared_ptr<DiskWriter>;
 
@@ -56,7 +72,7 @@ namespace armarx::armem
             {
             public:
                 DiskWriter() :
-                    rootPath("/tmp/")
+                    rootPath("/tmp/MemoryExport/")
                 {}
 
                 DiskWriter(const std::string& r) :
@@ -73,57 +89,112 @@ namespace armarx::armem
                     }
                 }
 
-                bool writeOnDisk(const MemoryPtr& m)
+                DiskWriterReturnInformation writeOnDisk(const MemoryPtr& m)
                 {
                     return writeOnDisk(m->coreSegments);
                 }
 
-                bool writeOnDisk(const std::map<std::string, CoreSegmentPtr>& m)
+                DiskWriterReturnInformation writeOnDisk(const std::map<std::string, CoreSegmentPtr>& m)
                 {
+                    DiskWriterReturnInformation ret;
                     for (const auto& [coreKey, coreSegment] : m)
                     {
-                        ensureCoreSegmentPathExists(coreKey);
+                        std::string currentMemoryPath = coreKey;
+                        if (!ensureCoreSegmentPathExists(coreKey))
+                        {
+                            ret.coreSegmentsError.push_back(currentMemoryPath);
+                            continue;
+                        }
+                        ret.storedElements[coreKey] = {};
                         for (const auto& [providerKey, providerSegment] : coreSegment->providerSegments)
                         {
-                            ensureProviderSegmentPathExists(coreKey, providerKey);
-                            for (const auto& [entityKey, entityHistory] : providerSegment->entities)
+                            currentMemoryPath += "/" + providerKey;
+                            if (!ensureProviderSegmentPathExists(coreKey, providerKey))
+                            {
+                                ret.providerSegmentsError.push_back(currentMemoryPath);
+                                continue;
+                            }
+                            ret.storedElements[coreKey][providerKey] = {};
+                            for (const auto& [entityKey, entity] : providerSegment->entities)
                             {
-                                ensureEntityPathExists(coreKey, providerKey, entityKey);
-                                for (const auto& [entityHistoryTimestamp, entitySnapshot] : entityHistory->history)
+                                currentMemoryPath += "/" + entityKey;
+                                if (!ensureEntityPathExists(coreKey, providerKey, entityKey))
+                                {
+                                    ret.entitiesError.push_back(currentMemoryPath);
+                                    continue;
+                                }
+                                ret.storedElements[coreKey][providerKey][entityKey] = {};
+                                for (const auto& [entityHistoryTimestamp, entitySnapshot] : entity->history)
                                 {
-                                    ensureTimestampPathExists(coreKey, providerKey, entityKey, entityHistoryTimestamp);
+                                    currentMemoryPath += "/" + std::to_string(entityHistoryTimestamp.toMilliSeconds());
+                                    if (!ensureTimestampPathExists(coreKey, providerKey, entityKey, entityHistoryTimestamp))
+                                    {
+                                        ret.historyTimestampsError.push_back(currentMemoryPath);
+                                        continue;
+                                    }
+                                    ret.storedElements[coreKey][providerKey][entityKey][entityHistoryTimestamp] = {};
                                     for (unsigned int i = 0; i < entitySnapshot->instances.size(); ++i)
                                     {
+                                        currentMemoryPath += "/" + std::to_string(i);
                                         std::filesystem::path entityElementPath = createEntityElementPath(coreKey, providerKey, entityKey, entityHistoryTimestamp, i);
-                                        if (!entityElementPathExists(entityElementPath))
+                                        std::string val = getDataAsString(wrapData(entitySnapshot->instances[i]));
+                                        if (entityElementPathExists(entityElementPath))
                                         {
-                                            std::ofstream ofs;
-                                            ofs.open(entityElementPath);
-
-
-                                            ofs << getDataAsString(wrapData(entitySnapshot->instances[i]));
-                                            ofs.close();
+                                            if (std::filesystem::is_directory(entityElementPath))
+                                            {
+                                                ret.entityInstancesError.push_back(currentMemoryPath);
+                                                continue;
+                                            }
+                                            std::ifstream ifs(entityElementPath);
+                                            std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+                                            if (file_content == val)
+                                            {
+                                                // already written. skip
+                                                continue;
+                                            }
                                         }
-                                        // check content if file already existent?
+                                        // write
+                                        std::ofstream ofs;
+                                        ofs.open(entityElementPath);
+                                        ofs << val;
+                                        ofs.close();
+                                        ret.storedElements[coreKey][providerKey][entityKey][entityHistoryTimestamp].push_back(entityElementPath);
                                     }
                                 }
                             }
                         }
                     }
-                    return true;
+                    return ret;
                 }
 
             protected:
                 aron::datanavigator::AronDictDataNavigatorPtr wrapData(const EntityInstancePtr& e) const
                 {
                     aron::datanavigator::AronDictDataNavigatorPtr dataWrapped(new aron::datanavigator::AronDictDataNavigator());
-                    dataWrapped->addElement("instanceData", e->data());
+                    dataWrapped->addElement("data", e->data());
 
                     aron::datanavigator::AronLongDataNavigatorPtr timeWrapped(new aron::datanavigator::AronLongDataNavigator());
                     timeWrapped->setValue(Time::now().toMilliSeconds());
                     dataWrapped->addElement("timeWrapped", timeWrapped);
 
-                    // TODO add more metadata
+
+                    const EntityInstanceMetadata& metadata = e->metadata();
+                    aron::datanavigator::AronLongDataNavigatorPtr timeCreated(new aron::datanavigator::AronLongDataNavigator());
+                    timeCreated->setValue(metadata.timeCreated.toMilliSeconds());
+                    dataWrapped->addElement("timeCreated", timeCreated);
+
+                    aron::datanavigator::AronLongDataNavigatorPtr timeSent(new aron::datanavigator::AronLongDataNavigator());
+                    timeSent->setValue(metadata.timeSent.toMilliSeconds());
+                    dataWrapped->addElement("timeSent", timeSent);
+
+                    aron::datanavigator::AronLongDataNavigatorPtr timeArrived(new aron::datanavigator::AronLongDataNavigator());
+                    timeArrived->setValue(metadata.timeArrived.toMilliSeconds());
+                    dataWrapped->addElement("timeArrived", timeArrived);
+
+                    aron::datanavigator::AronFloatDataNavigatorPtr confidence(new aron::datanavigator::AronFloatDataNavigator());
+                    confidence->setValue(metadata.confidence);
+                    dataWrapped->addElement("confidence", confidence);
+
                     return dataWrapped;
                 }
 
diff --git a/source/RobotAPI/libraries/armem/memory/Entity.cpp b/source/RobotAPI/libraries/armem/memory/Entity.cpp
index b8f8d9bca790031a3f3ea96f815cc0bb057414b0..c41a1310a6928ab77a44eb2c3c95c8891f812353 100644
--- a/source/RobotAPI/libraries/armem/memory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/memory/Entity.cpp
@@ -135,7 +135,7 @@ namespace armarx::armem
 
     std::vector<Time> Entity::getTimestamps() const
     {
-        return simox::get_keys(history);
+        return simox::alg::get_keys(history);
     }
 
 
diff --git a/source/RobotAPI/libraries/armem/memory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/memory/EntityInstance.cpp
index 12810b26eb40d3d2745252825427433c6fe1df89..a46096fbd5f7008c591f7f52c3ccf6dbaf145194 100644
--- a/source/RobotAPI/libraries/armem/memory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/memory/EntityInstance.cpp
@@ -23,11 +23,6 @@ namespace armarx::armem
     {
     }
 
-    void EntityInstance::setData(const aron::datanavigator::AronDataNavigatorPtr& data)
-    {
-        this->_data = data;
-    }
-
     void EntityInstance::update(const InternalEntityUpdate& update, int index)
     {
         ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
diff --git a/source/RobotAPI/libraries/armem/memory/EntityInstance.h b/source/RobotAPI/libraries/armem/memory/EntityInstance.h
index d331bb946a91713d78c5dc0ae1b00359ad145724..c9b46753bf5d8335e375d1d1a31aa6bf696bd574 100644
--- a/source/RobotAPI/libraries/armem/memory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/memory/EntityInstance.h
@@ -3,7 +3,7 @@
 #include <memory>
 
 #include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDataNavigator.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h>
 
 #include "../core/Time.h"
 
@@ -66,11 +66,14 @@ namespace armarx::armem
             return _metadata;
         }
 
-        inline armarx::aron::datanavigator::AronDataNavigatorPtr data() const
+        inline armarx::aron::datanavigator::AronDictDataNavigatorPtr data() const
         {
             return _data;
         }
-        void setData(const aron::datanavigator::AronDataNavigatorPtr& data);
+        void setData(const aron::datanavigator::AronDictDataNavigatorPtr& data)
+        {
+            this->_data = data;
+        }
 
 
 
@@ -96,7 +99,7 @@ namespace armarx::armem
         EntityInstanceMetadata _metadata;
 
         /// The data. May be nullptr.
-        armarx::aron::datanavigator::AronDataNavigatorPtr _data;
+        armarx::aron::datanavigator::AronDictDataNavigatorPtr _data;
 
     };
 
diff --git a/source/RobotAPI/libraries/armem/memory/Memory.h b/source/RobotAPI/libraries/armem/memory/Memory.h
index ae0561389b14b1643556719a3f8b127b26aff631..37f34d3c0122cca150421c2cf033392cad34b237 100644
--- a/source/RobotAPI/libraries/armem/memory/Memory.h
+++ b/source/RobotAPI/libraries/armem/memory/Memory.h
@@ -45,7 +45,6 @@ namespace armarx::armem
         using Base::getEntity;
         const Entity& getEntity(const MemoryID& id) const override;
 
-
         /**
          * @brief Add an empty core segment with the given name.
          * @param name The core segment name.
diff --git a/source/RobotAPI/libraries/armem/memory/ice_conversions.cpp b/source/RobotAPI/libraries/armem/memory/ice_conversions.cpp
index 461f9d6b5da4e45ec76cd3807ffbea72b60543a1..de78dc0a4043c3ac9cbdf4dc3e827af93a2bcfce 100644
--- a/source/RobotAPI/libraries/armem/memory/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/memory/ice_conversions.cpp
@@ -54,7 +54,7 @@ namespace armarx
 
         if (data.data())
         {
-            ice.data = data.data()->getResult();
+            ice.data = data.data()->toAronDict();
         }
         toIce(ice.metadata, data.metadata());
     }
@@ -64,7 +64,7 @@ namespace armarx
 
         if (ice.data)
         {
-            data.setData(aron::datanavigator::AronDataNavigator::FromAronData(ice.data));
+            data.setData(aron::datanavigator::AronDictDataNavigator::FromAronDict(ice.data));
         }
         fromIce(ice.metadata, data.metadata());
     }
diff --git a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
index 82b2e8fefe1010703d10225ccc59e97db60c4810..9afa46513fec7b71d43469e8540160df61b52f65 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
@@ -150,4 +150,6 @@ BOOST_AUTO_TEST_CASE(test_memory_heavy_setup_and_export)
     // export memory
     armem::ltm::LongTermMemoryLUT ltm("/tmp/MemoryExport/");
     ltm.writeOnDisk(memory);
+
+    std::cout << ltm.getLUTasString() << std::endl;
 }
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
index e0e62360b163e062d2e7d810d54a4f04a54385f5..f92646cedd9dea94a056a430efdef9c4fac2290d 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
@@ -31,7 +31,7 @@
 
 
 #include <iostream>
-#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h>
 
 
 namespace armem = armarx::armem;
@@ -114,8 +114,8 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
     update.entityID = armem::MemoryID::fromString("Memory/ImageRGB/SomeRGBImageProvider/image");
     update.instancesData =
     {
-        std::make_shared<aron::datanavigator::AronIntDataNavigator>(),
-        std::make_shared<aron::datanavigator::AronIntDataNavigator>()
+        std::make_shared<aron::datanavigator::AronDictDataNavigator>(),
+        std::make_shared<aron::datanavigator::AronDictDataNavigator>()
     };
     update.timeCreated = armem::Time::milliSeconds(1000);
     BOOST_CHECK_NO_THROW(providerSegment.update(update));
@@ -135,7 +135,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
 
     // Another update (on memory).
 
-    update.instancesData = { std::make_shared<aron::datanavigator::AronIntDataNavigator>() };
+    update.instancesData = { std::make_shared<aron::datanavigator::AronDictDataNavigator>() };
     update.timeCreated = armem::Time::milliSeconds(2000);
     memory->update(update);
     BOOST_CHECK_EQUAL(entity.history.size(), 2);
@@ -144,7 +144,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
 
 
     // A third update (on entity).
-    update.instancesData = { std::make_shared<aron::datanavigator::AronIntDataNavigator>() };
+    update.instancesData = { std::make_shared<aron::datanavigator::AronDictDataNavigator>() };
     update.timeCreated = armem::Time::milliSeconds(3000);
     entity.update(update);
     BOOST_CHECK_EQUAL(entity.history.size(), 3);
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 22117ea7ce659254050c84cd679cef0f915dee23..a923ab3e676ea1ecb4dae600031db21c7bbe2ea4 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -187,7 +187,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
 
         BOOST_CHECK_EQUAL(result->history.size(), 2);
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
         std::vector<armem::Time> expected
         {
             armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
@@ -208,7 +208,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
 
         BOOST_CHECK_EQUAL(result->history.size(), 3);
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
         std::vector<armem::Time> expected
         {
             armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
@@ -229,8 +229,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
 
         BOOST_CHECK_EQUAL(result->history.size(), entity->history.size());
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
-        std::vector<armem::Time> expected = simox::get_keys(entity->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
+        std::vector<armem::Time> expected = simox::alg::get_keys(entity->history);
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
 }
@@ -262,7 +262,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
 
         BOOST_CHECK_EQUAL(result->history.size(), 2);
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
         std::vector<armem::Time> expected
         {
             armem::Time::microSeconds(1000), armem::Time::microSeconds(2000)
@@ -283,7 +283,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
 
         BOOST_CHECK_EQUAL(result->history.size(), 3);
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
         std::vector<armem::Time> expected
         {
             armem::Time::microSeconds(3000), armem::Time::microSeconds(4000), armem::Time::microSeconds(5000)
@@ -332,8 +332,8 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
 
         BOOST_CHECK_EQUAL(result->history.size(), entity->history.size());
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
-        std::vector<armem::Time> expected = simox::get_keys(entity->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
+        std::vector<armem::Time> expected = simox::alg::get_keys(entity->history);
         BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
     }
 
@@ -355,7 +355,7 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
 
         BOOST_CHECK_EQUAL(result->history.size(), 3);
 
-        std::vector<armem::Time> times = simox::get_keys(result->history);
+        std::vector<armem::Time> times = simox::alg::get_keys(result->history);
         std::vector<armem::Time> expected
         {
             armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
diff --git a/source/RobotAPI/libraries/aron/aroncore/AronConfig.h b/source/RobotAPI/libraries/aron/aroncore/AronConfig.h
index e1176aa38e43b24b97173d0c4e556237243e2547..a40e3cd54aba357fc66fed2534ae5833e5bed52a 100644
--- a/source/RobotAPI/libraries/aron/aroncore/AronConfig.h
+++ b/source/RobotAPI/libraries/aron/aroncore/AronConfig.h
@@ -84,6 +84,14 @@ namespace armarx
     RUN_ARON_MACRO(Dict, dict, DICT) \
     RUN_ARON_MACRO(Tuple, tuple, TUPLE) \
 
+#define HANDLE_DICT_SERIALIZER_TYPES \
+    RUN_ARON_MACRO(Object, object, OBJECT) \
+    RUN_ARON_MACRO(Dict, dict, DICT) \
+
+#define HANDLE_LIST_SERIALIZER_TYPES \
+    RUN_ARON_MACRO(List, list, LIST) \
+    RUN_ARON_MACRO(Tuple, tuple, TUPLE) \
+
 #define HANDLE_CONTAINER_TYPES_EXCEPT_OBJECT \
     RUN_ARON_MACRO(List, list, LIST) \
     RUN_ARON_MACRO(Dict, dict, DICT) \
diff --git a/source/RobotAPI/libraries/aron/aroncore/AronFactory.h b/source/RobotAPI/libraries/aron/aroncore/AronFactory.h
index 04ab18cf65ba9916666c104d6796bfa53b1c10a7..6ec2b1604afa7fd576895326a9828c6b466647c5 100644
--- a/source/RobotAPI/libraries/aron/aroncore/AronFactory.h
+++ b/source/RobotAPI/libraries/aron/aroncore/AronFactory.h
@@ -50,7 +50,7 @@ namespace armarx
 
         template <typename Input, typename Output>
         class AronPtrInputFactory :
-                virtual public AronFactory<Input, Output>
+            virtual public AronFactory<Input, Output>
         {
         public:
             AronPtrInputFactory() = default;
@@ -61,7 +61,7 @@ namespace armarx
         protected:
             static void CheckIfInputIsNull(const std::string& c, const std::string& m, const AronPath& p, const Input& i)
             {
-                if(i.get() == nullptr)
+                if (i.get() == nullptr)
                 {
                     throw exception::AronExceptionWithPathInfo(c, m, "The used input is NULL", p);
                 }
@@ -70,7 +70,7 @@ namespace armarx
 
         template <typename Input, typename Output>
         class AronPtrOutputFactory :
-                virtual public AronFactory<Input, Output>
+            virtual public AronFactory<Input, Output>
         {
         public:
             AronPtrOutputFactory() = default;
@@ -81,7 +81,7 @@ namespace armarx
         protected:
             static void CheckIfOutputIsNull(const std::string& c, const std::string& m, const AronPath& p, const Output& o)
             {
-                if(o.get() == nullptr)
+                if (o.get() == nullptr)
                 {
                     throw exception::AronExceptionWithPathInfo(c, m, "The used output is NULL", p);
                 }
@@ -90,8 +90,8 @@ namespace armarx
 
         template <typename Input, typename Output>
         class AronPtrInputPtrOutputFactory :
-                virtual public AronPtrInputFactory<Input, Output>,
-                virtual public AronPtrOutputFactory<Input, Output>
+            virtual public AronPtrInputFactory<Input, Output>,
+            virtual public AronPtrOutputFactory<Input, Output>
         {
         public:
             AronPtrInputPtrOutputFactory() = default;
diff --git a/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp b/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp
index a854d7566b7d4657fe9b26e147b541f9821ff023..c22a9cd656de7f76538238e32679e80fdb8e59e9 100644
--- a/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/AronCppWriter.cpp
@@ -50,12 +50,14 @@ namespace armarx
                 void AronTypeClassCppWriter::addToAronMethod()
                 {
                     // The toAron Serializer is visible by default
-                    AronWriterInfoPtr toAron = AronWriterInfoPtr(new AronWriterInfo());
-                    toAron->methodName = "toAron";
-                    toAron->returnType = "armarx::aron::datanavigator::AronDataNavigatorPtr";
-                    toAron->writerClassType = "armarx::aron::io::AronDataNavigatorWriter";
-                    toAron->include = "<RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.h>";
-                    dataWriters.push_back(toAron);
+                    {
+                        AronWriterInfoPtr toAron = AronWriterInfoPtr(new AronWriterInfo());
+                        toAron->methodName = "toAron";
+                        toAron->returnType = "armarx::aron::datanavigator::AronDictDataNavigatorPtr";
+                        toAron->writerClassType = "armarx::aron::io::AronDataNavigatorWriter";
+                        toAron->include = "<RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.h>";
+                        dataWriters.push_back(toAron);
+                    }
                 }
 
                 void AronTypeClassCppWriter::addFromAronMethod()
@@ -64,7 +66,7 @@ namespace armarx
                     {
                         AronReaderInfoPtr fromAron = AronReaderInfoPtr(new AronReaderInfo());
                         fromAron->methodName = "fromAron";
-                        fromAron->argumentType = "armarx::aron::datanavigator::AronDataNavigatorPtr";
+                        fromAron->argumentType = "armarx::aron::datanavigator::AronDictDataNavigatorPtr";
                         fromAron->readerClassType = "armarx::aron::io::AronDataNavigatorReader";
                         fromAron->include = "<RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.h>";
                         dataReaders.push_back(fromAron);
@@ -72,9 +74,8 @@ namespace armarx
                     {
                         AronReaderInfoPtr fromAron = AronReaderInfoPtr(new AronReaderInfo());
                         fromAron->methodName = "fromAron";
-                        fromAron->argumentType = "armarx::aron::data::AronDataPtr";
+                        fromAron->argumentType = "armarx::aron::data::AronDictPtr";
                         fromAron->readerClassType = "armarx::aron::io::AronDataNavigatorReader";
-                        fromAron->include = "<RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.h>";
                         dataReaders.push_back(fromAron);
                     }
                 }
@@ -84,7 +85,7 @@ namespace armarx
                     // The toAron Serializer is visible by default
                     AronWriterInfoPtr toAronType = AronWriterInfoPtr(new AronWriterInfo());
                     toAronType->methodName = "toInitialAronType";
-                    toAronType->returnType = "armarx::aron::typenavigator::AronTypeNavigatorPtr";
+                    toAronType->returnType = "armarx::aron::typenavigator::AronObjectTypeNavigatorPtr";
                     toAronType->writerClassType = "armarx::aron::io::AronTypeNavigatorWriter";
                     toAronType->include = "<RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriter.h>";
                     initialTypeWriters.push_back(toAronType);
@@ -92,7 +93,7 @@ namespace armarx
                     // The toAron Serializer is visible by default
                     AronWriterInfoPtr toAronType2 = AronWriterInfoPtr(new AronWriterInfo());
                     toAronType2->methodName = "toCurrentAronType";
-                    toAronType2->returnType = "armarx::aron::typenavigator::AronTypeNavigatorPtr";
+                    toAronType2->returnType = "armarx::aron::typenavigator::AronObjectTypeNavigatorPtr";
                     toAronType2->writerClassType = "armarx::aron::io::AronTypeNavigatorWriter";
                     currentTypeWriters.push_back(toAronType2);
                 }
@@ -173,7 +174,7 @@ namespace armarx
                             {
                                 c->addInclude(info->include);
                             }
-                            CppMethodPtr convert = objectSerializer->toSpecializedDataWriterMethod(info->returnType, info->methodName, info->writerClassType);
+                            CppMethodPtr convert = objectSerializer->toSpecializedDataWriterMethod(info->returnType, info->methodName, info->writerClassType, "armarx::aron::datanavigator::AronDictDataNavigator::DynamicCast");
                             c->addMethod(convert);
                         }
 
@@ -195,7 +196,7 @@ namespace armarx
                             {
                                 c->addInclude(info->include);
                             }
-                            CppMethodPtr convert = objectSerializer->toSpecializedInitialTypeWriterMethod(info->returnType, info->methodName, info->writerClassType);
+                            CppMethodPtr convert = objectSerializer->toSpecializedInitialTypeWriterMethod(info->returnType, info->methodName, info->writerClassType, "armarx::aron::typenavigator::AronObjectTypeNavigator::DynamicCast");
                             c->addMethod(convert);
                         }
 
@@ -206,7 +207,7 @@ namespace armarx
                             {
                                 c->addInclude(info->include);
                             }
-                            CppMethodPtr convert = objectSerializer->toSpecializedCurrentTypeWriterMethod(info->returnType, info->methodName, info->writerClassType);
+                            CppMethodPtr convert = objectSerializer->toSpecializedCurrentTypeWriterMethod(info->returnType, info->methodName, info->writerClassType, "armarx::aron::typenavigator::AronObjectTypeNavigator::DynamicCast");
                             c->addMethod(convert);
                         }
 
@@ -217,7 +218,7 @@ namespace armarx
                             {
                                 c->addInclude(info->include);
                             }
-                            CppMethodPtr convert = objectSerializer->toSpecializedTypeReaderMethod(info->argumentType, info->methodName, info->readerClassType);
+                            CppMethodPtr convert = objectSerializer->toSpecializedCurrentTypeReaderMethod(info->argumentType, info->methodName, info->readerClassType);
                             c->addMethod(convert);
                         }
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.cpp b/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.cpp
index 9dc61382a425e2451ce636dd5d71b7c7165a728b..a64a9a0a962bf587782b552f1fcd6e2615bcab2b 100644
--- a/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.cpp
@@ -244,7 +244,7 @@ namespace armarx
                         return m;
                     }
 
-                    CppMethodPtr AronTypeCppSerializer::toSpecializedDataWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName) const
+                    CppMethodPtr AronTypeCppSerializer::toSpecializedDataWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
                     {
                         std::stringstream doc;
                         doc << "@brief specializedDataWrite() - This method returns a new data from the member data types using a writer implementation. \n";
@@ -253,23 +253,23 @@ namespace armarx
                         CppMethodPtr m = CppMethodPtr(new CppMethod(returnname + " " + methodname + "() const", doc.str()));
                         m->addLine(writerName + " writer;");
                         m->addLine("this->write(writer);");
-                        m->addLine("return writer.getResult();");
+                        m->addLine("return " + enforceConversion + "(writer.getResult());");
                         return m;
                     }
 
-                    CppMethodPtr AronTypeCppSerializer::toSpecializedDataReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName) const
+                    CppMethodPtr AronTypeCppSerializer::toSpecializedDataReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName, const std::string& enforceConversion) const
                     {
                         std::stringstream doc;
                         doc << "@brief specializedDataRead() - This method sets the struct members to new values given in a reader implementation. \n";
                         doc << "@return - nothing";
 
                         CppMethodPtr m = CppMethodPtr(new CppMethod("void " + methodname + "(const " + argumentname + "& input)", doc.str()));
-                        m->addLine(readerName + " reader(input);");
+                        m->addLine(readerName + " reader(" + enforceConversion + "(input));");
                         m->addLine("this->read(reader);");
                         return m;
                     }
 
-                    CppMethodPtr AronTypeCppSerializer::toSpecializedInitialTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName) const
+                    CppMethodPtr AronTypeCppSerializer::toSpecializedInitialTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
                     {
                         std::stringstream doc;
                         doc << "@brief specializedTypeWrite() - This method returns a new type from the member data types using a writer implementation. \n";
@@ -278,11 +278,11 @@ namespace armarx
                         CppMethodPtr m = CppMethodPtr(new CppMethod("static " + returnname + " " + methodname + "()", doc.str()));
                         m->addLine(writerName + " writer;");
                         m->addLine(getCppTypename() + "::writeInitialType(writer);");
-                        m->addLine("return writer.getResult();");
+                        m->addLine("return " + enforceConversion + "(writer.getResult());");
                         return m;
                     }
 
-                    CppMethodPtr AronTypeCppSerializer::toSpecializedCurrentTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName) const
+                    CppMethodPtr AronTypeCppSerializer::toSpecializedCurrentTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
                     {
                         std::stringstream doc;
                         doc << "@brief specializedTypeWrite() - This method returns a new type from the current member data types using a writer implementation. \n";
@@ -291,18 +291,18 @@ namespace armarx
                         CppMethodPtr m = CppMethodPtr(new CppMethod(returnname + " " + methodname + "()", doc.str()));
                         m->addLine(writerName + " writer;");
                         m->addLine("this->writeCurrentType(writer);");
-                        m->addLine("return writer.getResult();");
+                        m->addLine("return " + enforceConversion + "(writer.getResult());");
                         return m;
                     }
 
-                    CppMethodPtr AronTypeCppSerializer::toSpecializedTypeReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName) const
+                    CppMethodPtr AronTypeCppSerializer::toSpecializedCurrentTypeReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName, const std::string& enforceConversion) const
                     {
                         std::stringstream doc;
                         doc << "@brief specializedTypeRead() - This method sets the structure of the members to new values given in a reader implementation. \n";
                         doc << "@return - nothing";
 
                         CppMethodPtr m = CppMethodPtr(new CppMethod("void " + methodname + "(const " + argumentname + "& input)", doc.str()));
-                        m->addLine(readerName + " reader(input);");
+                        m->addLine(readerName + " reader(" + enforceConversion + "(input));");
                         m->addLine("this->readType(reader);");
                         return m;
                     }
diff --git a/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.h b/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.h
index aaecbf40dcd6285b67d1b7c6a90039ab268bfcac..51bab528dacaf7e873dfe795c510540886412a48 100644
--- a/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.h
+++ b/source/RobotAPI/libraries/aron/aroncore/codegenerator/codeWriters/cppWriter/typeSerializers/AronTypeCppSerializer.h
@@ -78,12 +78,12 @@ namespace armarx
                         std::string getAronTypeTypename() const;
                         std::string getAronTypePtrTypename() const;
 
-                        CppMethodPtr toSpecializedDataWriterMethod(const std::string&, const std::string&, const std::string&) const;
-                        CppMethodPtr toSpecializedDataReaderMethod(const std::string&, const std::string&, const std::string&) const;
+                        CppMethodPtr toSpecializedDataWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
+                        CppMethodPtr toSpecializedDataReaderMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
 
-                        CppMethodPtr toSpecializedInitialTypeWriterMethod(const std::string&, const std::string&, const std::string&) const;
-                        CppMethodPtr toSpecializedCurrentTypeWriterMethod(const std::string&, const std::string&, const std::string&) const;
-                        CppMethodPtr toSpecializedTypeReaderMethod(const std::string&, const std::string&, const std::string&) const;
+                        CppMethodPtr toSpecializedInitialTypeWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
+                        CppMethodPtr toSpecializedCurrentTypeWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
+                        CppMethodPtr toSpecializedCurrentTypeReaderMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
 
                         // virtual override definitions
                         virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const = 0;
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/AronDataClassReaderToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/AronDataClassReaderToken.h
index 2e52bc5aa810fb2560d2379ddb6724e88fcf220f..11daabc82189526f25850bf577777b32d0c194fc 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/AronDataClassReaderToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/AronDataClassReaderToken.h
@@ -39,24 +39,24 @@ namespace armarx
     {
         namespace io
         {
-            template<typename ElementPtrTypename>
+            template<typename ElementReturnPtrTypename, typename ElementAddTypename>
             class AronDataClassReaderToken;
 
-            template<typename ElementPtrTypename>
-            using AronDataClassReaderTokenPtr = std::shared_ptr<AronDataClassReaderToken<ElementPtrTypename>>;
+            template<typename ElementReturnPtrTypename, typename ElementAddTypename>
+            using AronDataClassReaderTokenPtr = std::shared_ptr<AronDataClassReaderToken<ElementReturnPtrTypename, ElementAddTypename>>;
 
-            template<typename ElementPtrTypename>
+            template<typename ElementReturnPtrTypename, typename ElementAddTypename>
             class AronDataClassReaderToken :
-                virtual public AronReaderToken<AronDataDescriptor, ElementPtrTypename>
+                virtual public AronReaderToken<AronDataDescriptor, ElementReturnPtrTypename, ElementAddTypename>
             {
             public:
-                using PointerType = AronDataClassReaderToken<ElementPtrTypename>;
+                using PointerType = AronDataClassReaderTokenPtr<ElementReturnPtrTypename, ElementAddTypename>;
 
             public:
                 // constructors
                 AronDataClassReaderToken() = delete;
-                AronDataClassReaderToken(const AronDataDescriptor desc, const ElementPtrTypename& data) :
-                    AronReaderToken<AronDataDescriptor, ElementPtrTypename>(desc, data)
+                AronDataClassReaderToken(const AronDataDescriptor desc, const ElementAddTypename& data) :
+                    AronReaderToken<AronDataDescriptor, ElementReturnPtrTypename, ElementAddTypename>(desc, data)
                 { }
 
                 virtual std::string resolveCurrentKey() const override
@@ -78,7 +78,7 @@ namespace armarx
                     return this->currentKey;
                 }
 
-                virtual ElementPtrTypename getElementAndIncreaseCnt() override = 0;
+                virtual ElementReturnPtrTypename getElementAndIncreaseCnt() override = 0;
 
             private:
             };
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.cpp b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.cpp
index 018bbf0eca9476cf287591229f2d4620c59edd6e..b37dde676c4bd036a5b4d0347a1f3272906cac4d 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.cpp
@@ -38,14 +38,14 @@ namespace armarx
     {
         namespace io
         {
-            AronDataNavigatorReader::AronDataNavigatorReader(const datanavigator::AronDataNavigatorPtr& n) :
-                AronDataClassReader<datanavigator::AronDataNavigatorPtr, AronDataNavigatorReaderTokenPtr>(n)
+            AronDataNavigatorReader::AronDataNavigatorReader(const datanavigator::AronContainerDataNavigatorPtr& n) :
+                AronDataClassReader<datanavigator::AronContainerDataNavigatorPtr, AronDataNavigatorReaderTokenPtr>(n)
             {
             }
 
-            AronDataNavigatorReader::AronDataNavigatorReader(const data::AronDataPtr& n) :
-                AronDataClassReader<datanavigator::AronDataNavigatorPtr, AronDataNavigatorReaderTokenPtr>(
-                    datanavigator::AronDataNavigator::FromAronData(n)
+            AronDataNavigatorReader::AronDataNavigatorReader(const data::AronContainerPtr& n) :
+                AronDataClassReader<datanavigator::AronContainerDataNavigatorPtr, AronDataNavigatorReaderTokenPtr>(
+                    datanavigator::AronContainerDataNavigator::DynamicCast(datanavigator::AronDataNavigator::FromAronData(n))
                 )
             {
             }
@@ -65,8 +65,15 @@ namespace armarx
     int AronDataNavigatorReader::readStart##upperType() \
     { \
         datanavigator::AronDataNavigatorPtr current_nav = getNextNavigator(); \
+        auto desc = current_nav->getDescriptor(); \
+        if (desc != AronDataDescriptor::eAron##upperType) \
+        { \
+            throw exception::AronDataDescriptorNotValidException("AronDataNavigatorReader", "readStart" + std::string(#upperType), "A token in the stack has the wrong type. Expected " + std::string(#upperType), desc); \
+            return false; \
+        } \
+        datanavigator::Aron##upperType##DataNavigatorPtr current_nav_casted = datanavigator::Aron##upperType##DataNavigator::DynamicCast(current_nav); \
         int c = current_nav->childrenSize(); \
-        AronDataNavigatorReaderTokenPtr newToken = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(current_nav->getDescriptor(), current_nav)); \
+        AronDataNavigatorReaderTokenPtr newToken = AronDataNavigatorReaderTokenPtr(new AronDataNavigatorReaderToken(current_nav->getDescriptor(), current_nav_casted)); \
         stack.push(newToken); \
         return c; \
     } \
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.h
index 79fd1db6729ae05cb6344139ff59eb03a2f0db02..e6edac332832c30c706648fe29ff1eecb095e53d 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReader.h
@@ -41,7 +41,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataNavigatorReader> AronDataNavigatorReaderPtr;
 
             class AronDataNavigatorReader :
-                virtual public AronDataClassReader<datanavigator::AronDataNavigatorPtr, AronDataNavigatorReaderTokenPtr>
+                virtual public AronDataClassReader<datanavigator::AronContainerDataNavigatorPtr, AronDataNavigatorReaderTokenPtr>
             {
             public:
                 using PointerType = AronDataNavigatorReaderPtr;
@@ -49,8 +49,8 @@ namespace armarx
             public:
                 // constructors
                 AronDataNavigatorReader() = delete;
-                AronDataNavigatorReader(const datanavigator::AronDataNavigatorPtr& n);
-                AronDataNavigatorReader(const data::AronDataPtr& n);
+                AronDataNavigatorReader(const datanavigator::AronContainerDataNavigatorPtr& n);
+                AronDataNavigatorReader(const data::AronContainerPtr& n);
 
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     virtual int  readStart##upperType() override; \
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReaderToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReaderToken.h
index 28c4488b9a725b289924d31d4cf7fafbfd85bde4..be68ba2a5d427da87052481610f77228d90e29c8 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReaderToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NavigatorReader/AronDataNavigatorReaderToken.h
@@ -42,7 +42,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataNavigatorReaderToken> AronDataNavigatorReaderTokenPtr;
 
             class AronDataNavigatorReaderToken :
-                virtual public AronDataClassReaderToken<datanavigator::AronDataNavigatorPtr>
+                virtual public AronDataClassReaderToken<datanavigator::AronDataNavigatorPtr, datanavigator::AronContainerDataNavigatorPtr>
             {
             public:
                 using PointerType = AronDataNavigatorReaderTokenPtr;
@@ -50,9 +50,9 @@ namespace armarx
             public:
                 // constructors
                 AronDataNavigatorReaderToken() = delete;
-                AronDataNavigatorReaderToken(const AronDataDescriptor desc, const datanavigator::AronDataNavigatorPtr& data) :
-                    AronReaderToken<AronDataDescriptor, datanavigator::AronDataNavigatorPtr>(desc, data),
-                    AronDataClassReaderToken<datanavigator::AronDataNavigatorPtr>(desc, data)
+                AronDataNavigatorReaderToken(const AronDataDescriptor desc, const datanavigator::AronContainerDataNavigatorPtr& data) :
+                    AronReaderToken<AronDataDescriptor, datanavigator::AronDataNavigatorPtr, datanavigator::AronContainerDataNavigatorPtr>(desc, data),
+                    AronDataClassReaderToken<datanavigator::AronDataNavigatorPtr, datanavigator::AronContainerDataNavigatorPtr>(desc, data)
                 {
                     switch (descriptor)
                     {
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NlohmannJSONReader/AronDataNlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NlohmannJSONReader/AronDataNlohmannJSONReaderToken.h
index 42f81e4b84764249e2b1df34d270cac4c42c02b3..3ee8d3c541761c333adf090007a3549bbd8752bf 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NlohmannJSONReader/AronDataNlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classReaders/NlohmannJSONReader/AronDataNlohmannJSONReaderToken.h
@@ -44,7 +44,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataNlohmannJSONReaderToken> AronDataNlohmannJSONReaderTokenPtr;
 
             class AronDataNlohmannJSONReaderToken :
-                virtual public AronDataClassReaderToken<nlohmann::json>
+                virtual public AronDataClassReaderToken<nlohmann::json, nlohmann::json>
             {
             public:
                 using PointerType = AronDataNlohmannJSONReaderTokenPtr;
@@ -53,8 +53,8 @@ namespace armarx
                 // constructors
                 AronDataNlohmannJSONReaderToken() = delete;
                 AronDataNlohmannJSONReaderToken(const AronDataDescriptor desc, const nlohmann::json& data) :
-                    AronReaderToken<AronDataDescriptor, nlohmann::json>(desc, data),
-                    AronDataClassReaderToken<nlohmann::json>(desc, data)
+                    AronReaderToken<AronDataDescriptor, nlohmann::json, nlohmann::json>(desc, data),
+                    AronDataClassReaderToken<nlohmann::json, nlohmann::json>(desc, data)
                 {
                     switch (descriptor)
                     {
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/AronDataClassWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/AronDataClassWriterToken.h
index e15fa0cdd18a296db4fe76a36e9032967a822427..141e1de4243fde84d1594697c8d29d9317e26a0d 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/AronDataClassWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/AronDataClassWriterToken.h
@@ -37,28 +37,28 @@ namespace armarx
     {
         namespace io
         {
-            template<typename ElementTypename>
+            template<typename ElementTypename, typename ElementAddTypename>
             class AronDataClassWriterToken;
 
-            template<typename ElementTypename>
-            using AronDataClassWriterTokenPtr = std::shared_ptr<AronDataClassWriterToken<ElementTypename>>;
+            template<typename ElementTypename, typename ElementAddTypename>
+            using AronDataClassWriterTokenPtr = std::shared_ptr<AronDataClassWriterToken<ElementTypename, ElementAddTypename>>;
 
-            template<typename ElementTypename>
+            template<typename ElementTypename, typename ElementAddTypename>
             class AronDataClassWriterToken :
-                virtual public AronWriterToken<AronDataDescriptor, ElementTypename>
+                virtual public AronWriterToken<AronDataDescriptor, ElementTypename, ElementAddTypename>
             {
             public:
-                using PointerType = AronDataClassWriterToken<ElementTypename>;
+                using PointerType = AronDataClassWriterToken<ElementTypename, ElementAddTypename>;
 
             public:
                 // constructor
                 AronDataClassWriterToken() = delete;
                 AronDataClassWriterToken(const AronDataDescriptor desc, const ElementTypename& n) :
-                    AronWriterToken<AronDataDescriptor, ElementTypename>(desc, n)
+                    AronWriterToken<AronDataDescriptor, ElementTypename, ElementAddTypename>(desc, n)
                 {}
 
                 // public member functions
-                virtual void addElement(const ElementTypename&) override = 0;
+                virtual void addElement(const ElementAddTypename&) override = 0;
 
                 virtual std::string toElementAccessor() const override
                 {
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.cpp b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.cpp
index 474139dbf2a803476ab1956e0442e003f34dd129..a39b26b541809f6e5e2cfcadc50bc6005f50d059 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.cpp
@@ -57,7 +57,7 @@ namespace armarx
     bool AronDataNavigatorWriter::writeStart##upperType() \
     { \
         AronPath path = generateAronPath(); \
-        datanavigator::AronDataNavigatorPtr data = datanavigator::AronDataNavigatorPtr(new datanavigator::Aron##upperType##DataNavigator(path)); \
+        datanavigator::Aron##upperType##DataNavigatorPtr data = datanavigator::Aron##upperType##DataNavigatorPtr(new datanavigator::Aron##upperType##DataNavigator(path)); \
         AronDataNavigatorWriterTokenPtr new_token = AronDataNavigatorWriterTokenPtr(new AronDataNavigatorWriterToken(data->getDescriptor(), data)); \
         stack.push(new_token); \
         return true; \
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.h
index 93126c07a01c1172720869717daf0a37524508e7..ab1022c8f33f8dc7e1558e08e268f487d5f796e9 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.h
@@ -42,7 +42,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataNavigatorWriter> AronDataNavigatorWriterPtr;
 
             class AronDataNavigatorWriter :
-                virtual public AronDataClassWriter<datanavigator::AronDataNavigatorPtr, AronDataNavigatorWriterTokenPtr>
+                virtual public AronDataClassWriter<datanavigator::AronContainerDataNavigatorPtr, AronDataNavigatorWriterTokenPtr>
             {
             public:
                 AronDataNavigatorWriter() = default;
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriterToken.h
index bc6fd30ba0b618eef71a06dd02ba55010780caf6..5c19455997000d2f3d48e9ac98c6f185b406bace 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriterToken.h
@@ -41,7 +41,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataNavigatorWriterToken> AronDataNavigatorWriterTokenPtr;
 
             class AronDataNavigatorWriterToken :
-                virtual public AronDataClassWriterToken<datanavigator::AronDataNavigatorPtr>
+                virtual public AronDataClassWriterToken<datanavigator::AronContainerDataNavigatorPtr, datanavigator::AronDataNavigatorPtr>
             {
             public:
                 using PointerType = AronDataNavigatorWriterTokenPtr;
@@ -49,9 +49,9 @@ namespace armarx
             public:
                 // constructor
                 AronDataNavigatorWriterToken() = delete;
-                AronDataNavigatorWriterToken(const AronDataDescriptor desc, const datanavigator::AronDataNavigatorPtr& d) :
-                    AronWriterToken<AronDataDescriptor, datanavigator::AronDataNavigatorPtr>(desc, d),
-                    AronDataClassWriterToken<datanavigator::AronDataNavigatorPtr>(desc, d)
+                AronDataNavigatorWriterToken(const AronDataDescriptor desc, const datanavigator::AronContainerDataNavigatorPtr& d) :
+                    AronWriterToken<AronDataDescriptor, datanavigator::AronContainerDataNavigatorPtr, datanavigator::AronDataNavigatorPtr>(desc, d),
+                    AronDataClassWriterToken<datanavigator::AronContainerDataNavigatorPtr, datanavigator::AronDataNavigatorPtr>(desc, d)
                 {
 
                 }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NlohmannJSONWriter/AronDataNlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NlohmannJSONWriter/AronDataNlohmannJSONWriterToken.h
index 455b8b23ede0fe61c3a108fca218a0eca48d7040..4246f10e36d3735c8ac55873976fe28cc1a09181 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NlohmannJSONWriter/AronDataNlohmannJSONWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/NlohmannJSONWriter/AronDataNlohmannJSONWriterToken.h
@@ -43,7 +43,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataNlohmannJSONWriterToken> AronDataNlohmannJSONWriterTokenPtr;
 
             class AronDataNlohmannJSONWriterToken :
-                virtual public AronDataClassWriterToken<nlohmann::json>
+                virtual public AronDataClassWriterToken<nlohmann::json, nlohmann::json>
             {
             public:
                 using PointerType = AronDataNlohmannJSONWriterTokenPtr;
@@ -52,8 +52,8 @@ namespace armarx
                 // constructor
                 AronDataNlohmannJSONWriterToken() = delete;
                 AronDataNlohmannJSONWriterToken(const AronDataDescriptor desc, const nlohmann::json& d) :
-                    AronWriterToken<AronDataDescriptor, nlohmann::json>(desc, d),
-                    AronDataClassWriterToken<nlohmann::json>(desc, d)
+                    AronWriterToken<AronDataDescriptor, nlohmann::json, nlohmann::json>(desc, d),
+                    AronDataClassWriterToken<nlohmann::json, nlohmann::json>(desc, d)
                 {
 
                 }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriter.cpp b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriter.cpp
index 944ec31b97918e1f82d52156b8bc43ccceb40e16..df6afd5b61fe220e2c23087edd70f671225963b6 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriter.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriter.cpp
@@ -31,7 +31,7 @@ namespace armarx
     {
         namespace io
         {
-        // Containers
+            // Containers
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronDataRapidXMLWriter::writeStart##upperType() \
     { \
@@ -43,17 +43,17 @@ namespace armarx
         return true; \
     }
 
-        HANDLE_CONTAINER_DATA
+            HANDLE_CONTAINER_DATA
 #undef RUN_ARON_MACRO
 
-        // Complex Types
+            // Complex Types
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronDataRapidXMLWriter::write##upperType(const std::vector<int>& dims, const std::string& t, const unsigned char* data) \
     { \
         return true; \
     }
 
-        HANDLE_COMPLEX_DATA
+            HANDLE_COMPLEX_DATA
 #undef RUN_ARON_MACRO
 
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
@@ -62,7 +62,7 @@ namespace armarx
         return true; \
     }
 
-        HANDLE_PRIMITIVE_TYPES
+            HANDLE_PRIMITIVE_TYPES
 #undef RUN_ARON_MACRO
 
         }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriterToken.h
index 2e82c9326b67152459f0e843576788d5018d3ee0..9e2959e8d17caeb60eb03150773e394633137a32 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronDataIO/classWriters/RapidXMLWriter/AronDataRapidXMLWriterToken.h
@@ -44,7 +44,7 @@ namespace armarx
             typedef std::shared_ptr<AronDataRapidXMLWriterToken> AronDataRapidXMLWriterTokenPtr;
 
             class AronDataRapidXMLWriterToken :
-                virtual public AronDataClassWriterToken<RapidXmlReaderNode>
+                virtual public AronDataClassWriterToken<RapidXmlReaderNode, RapidXmlReaderNode>
             {
             public:
                 using PointerType = AronDataRapidXMLWriterTokenPtr;
@@ -53,8 +53,8 @@ namespace armarx
                 // constructor
                 AronDataRapidXMLWriterToken() = delete;
                 AronDataRapidXMLWriterToken(const AronDataDescriptor desc, const RapidXmlReaderNode& d) :
-                    AronWriterToken<AronDataDescriptor, RapidXmlReaderNode>(desc, d),
-                    AronDataClassWriterToken<RapidXmlReaderNode>(desc, d)
+                    AronWriterToken<AronDataDescriptor, RapidXmlReaderNode, RapidXmlReaderNode>(desc, d),
+                    AronDataClassWriterToken<RapidXmlReaderNode, RapidXmlReaderNode>(desc, d)
                 {
 
                 }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronReaderToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronReaderToken.h
index abac79059b800e7f1a65597ebd3fd544a5dbe1b1..39ebce21cacdd9eded7c72c8f2e693f350780a0a 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronReaderToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronReaderToken.h
@@ -36,13 +36,13 @@ namespace armarx
     {
         namespace io
         {
-            template<typename AronDescriptorTypename, typename ElementTypename>
+            template<typename AronDescriptorTypename, typename ElementReturnPtrTypename, typename ElementTypename>
             class AronReaderToken;
 
-            template<typename AronDescriptorTypename, typename ElementTypename>
-            using AronReaderTokenPtr = std::shared_ptr<AronReaderToken<AronDescriptorTypename, ElementTypename>> ;
+            template<typename AronDescriptorTypename, typename ElementReturnPtrTypename, typename ElementTypename>
+            using AronReaderTokenPtr = std::shared_ptr<AronReaderToken<AronDescriptorTypename, ElementReturnPtrTypename, ElementTypename>> ;
 
-            template<typename AronDescriptorTypename, typename ElementTypename>
+            template<typename AronDescriptorTypename, typename ElementReturnPtrTypename, typename ElementTypename>
             class AronReaderToken
             {
             public:
@@ -73,7 +73,7 @@ namespace armarx
 
                 virtual std::string resolveCurrentKey() const = 0;
 
-                virtual ElementTypename getElementAndIncreaseCnt() = 0;
+                virtual ElementReturnPtrTypename getElementAndIncreaseCnt() = 0;
 
             protected:
                 // members
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTextReader.h b/source/RobotAPI/libraries/aron/aroncore/io/AronTextReader.h
index c1bb433ed1de38c6a03b1baf8eab5614dbb73738..0b2aa76f1503762dba03c4505a3ad18735cec905 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTextReader.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTextReader.h
@@ -42,6 +42,6 @@ namespace armarx
             {
             public:
 
+            }
         }
     }
-}
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/AronTypeClassWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/AronTypeClassWriterToken.h
index 6ca01e5481b9709065a62346611e44376685288f..5b016ca9f3e6245200500feb6d3be97cd6aae53b 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/AronTypeClassWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/AronTypeClassWriterToken.h
@@ -36,28 +36,28 @@ namespace armarx
     {
         namespace io
         {
-            template<typename ElementPtrTypename>
+            template<typename ElementPtrTypename, typename ElementAddTypename>
             class AronTypeClassWriterToken;
 
-            template<typename ElementPtrTypename>
-            using AronTypeClassWriterTokenPtr = std::shared_ptr<AronTypeClassWriterToken<ElementPtrTypename>>;
+            template<typename ElementPtrTypename, typename ElementAddTypename>
+            using AronTypeClassWriterTokenPtr = std::shared_ptr<AronTypeClassWriterToken<ElementPtrTypename, ElementAddTypename>>;
 
-            template<typename ElementPtrTypename>
+            template<typename ElementPtrTypename, typename ElementAddTypename>
             class AronTypeClassWriterToken :
-                virtual public AronWriterToken<AronTypeDescriptor, ElementPtrTypename>
+                virtual public AronWriterToken<AronTypeDescriptor, ElementPtrTypename, ElementAddTypename>
             {
             public:
-                using PointerType = AronTypeClassWriterToken<ElementPtrTypename>;
+                using PointerType = AronTypeClassWriterToken<ElementPtrTypename, ElementAddTypename>;
 
             public:
                 // constructor
                 AronTypeClassWriterToken() = delete;
                 AronTypeClassWriterToken(const AronTypeDescriptor desc, const ElementPtrTypename& n) :
-                    AronWriterToken<AronTypeDescriptor, ElementPtrTypename>(desc, n)
+                    AronWriterToken<AronTypeDescriptor, ElementPtrTypename, ElementAddTypename>(desc, n)
                 {}
 
                 // public member functions
-                virtual void addElement(const ElementPtrTypename&) = 0;
+                virtual void addElement(const ElementAddTypename&) = 0;
 
                 virtual std::string toElementAccessor() const override
                 {
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriter.cpp b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriter.cpp
index 4ed6df6307d4ef5390a83d979cbbdfeae51cc11e..8e05757db32997bedcb40de946bdbdda3ac8431f 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriter.cpp
@@ -62,7 +62,7 @@ namespace armarx
     bool AronTypeNavigatorWriter::writeStart##upperType##Type() \
     { \
         AronPath path = generateAronPath(); \
-        typenavigator::AronTypeNavigatorPtr type = typenavigator::AronTypeNavigatorPtr(new typenavigator::Aron##upperType##TypeNavigator(path)); \
+        typenavigator::Aron##upperType##TypeNavigatorPtr type = typenavigator::Aron##upperType##TypeNavigatorPtr(new typenavigator::Aron##upperType##TypeNavigator(path)); \
         AronTypeNavigatorWriterTokenPtr new_token = AronTypeNavigatorWriterTokenPtr(new AronTypeNavigatorWriterToken(type->getDescriptor(), type)); \
         stack.push(new_token); \
         return true; \
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriterToken.h
index 95a6a0b9044ac88a7bd3a3d87fdd0940ac0aed87..393f297ababa21a07b35350fc22407026fcb1a4b 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NavigatorWriter/AronTypeNavigatorWriterToken.h
@@ -41,7 +41,7 @@ namespace armarx
             typedef std::shared_ptr<AronTypeNavigatorWriterToken> AronTypeNavigatorWriterTokenPtr;
 
             class AronTypeNavigatorWriterToken :
-                virtual public AronTypeClassWriterToken<typenavigator::AronTypeNavigatorPtr>
+                virtual public AronTypeClassWriterToken<typenavigator::AronContainerTypeNavigatorPtr, typenavigator::AronTypeNavigatorPtr>
             {
             public:
                 using PointerType = AronTypeNavigatorWriterTokenPtr;
@@ -49,9 +49,9 @@ namespace armarx
             public:
                 // constructor
                 AronTypeNavigatorWriterToken() = delete;
-                AronTypeNavigatorWriterToken(const AronTypeDescriptor desc, const typenavigator::AronTypeNavigatorPtr& t) :
-                    AronWriterToken<AronTypeDescriptor, typenavigator::AronTypeNavigatorPtr>(desc, t),
-                    AronTypeClassWriterToken<typenavigator::AronTypeNavigatorPtr>(desc, t)
+                AronTypeNavigatorWriterToken(const AronTypeDescriptor desc, const typenavigator::AronContainerTypeNavigatorPtr& t) :
+                    AronWriterToken<AronTypeDescriptor, typenavigator::AronContainerTypeNavigatorPtr, typenavigator::AronTypeNavigatorPtr>(desc, t),
+                    AronTypeClassWriterToken<typenavigator::AronContainerTypeNavigatorPtr, typenavigator::AronTypeNavigatorPtr>(desc, t)
                 {
                 }
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.cpp
index f13315860bd3da3a3715218e4f35106a43f9eb41..84c0016cd11aa8ab8a63ac0a09818c063557eeeb 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.cpp
@@ -25,61 +25,61 @@
 
 namespace armarx
 {
-namespace aron
-{
-namespace io
-{
+    namespace aron
+    {
+        namespace io
+        {
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronTypeNlohmannJSONWriter::writeStart##upperType##Type() \
-{ \
-    nlohmann::jsonPtr data = nlohmann::jsonPtr(new nlohmann::json()); \
-    AronTypeNlohmannJSONWriterTokenPtr new_token = AronTypeNlohmannJSONWriterTokenPtr(new AronTypeNlohmannJSONWriterToken(AronTypeDescriptor::eAron##upperType##Type, data)); \
-    stack.push(new_token); \
-    return true; \
-} \
-    bool AronTypeNlohmannJSONWriter::writeEnd##upperType##Type() \
-{ \
-    lastRemovedToken = stack.top(); \
-    stack.pop(); \
-    \
-    if (stack.size() > 0) \
     { \
-        AronTypeNlohmannJSONWriterTokenPtr prevToken = stack.top(); \
-        prevToken->addElement(lastRemovedToken->getElement()); \
+        nlohmann::json data; \
+        AronTypeNlohmannJSONWriterTokenPtr new_token = AronTypeNlohmannJSONWriterTokenPtr(new AronTypeNlohmannJSONWriterToken(AronTypeDescriptor::eAron##upperType##Type, data)); \
+        stack.push(new_token); \
+        return true; \
     } \
-    return true; \
-}
+    bool AronTypeNlohmannJSONWriter::writeEnd##upperType##Type() \
+    { \
+        lastRemovedToken = stack.top(); \
+        stack.pop(); \
+        \
+        if (stack.size() > 0) \
+        { \
+            AronTypeNlohmannJSONWriterTokenPtr prevToken = stack.top(); \
+            prevToken->addElement(lastRemovedToken->getElement()); \
+        } \
+        return true; \
+    }
 
-HANDLE_CONTAINER_TYPES
+            HANDLE_CONTAINER_TYPES
 #undef RUN_ARON_MACRO
 
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronTypeNlohmannJSONWriter::write##upperType##Type(const std::vector<int>& dims, const std::string& t) \
-{ \
-    AronTypeNlohmannJSONWriterTokenPtr token = stack.top(); \
-    nlohmann::jsonPtr j = nlohmann::jsonPtr(new nlohmann::json()); \
-    (*j)["ARON_NDARRAY_DIMENSIONS"] = dims; \
-    (*j)["ARON_NDARRAY_TYPE"] = t; \
-    token->addElement(j); \
-    return true; \
-}
+    { \
+        AronTypeNlohmannJSONWriterTokenPtr token = stack.top(); \
+        nlohmann::json j; \
+        j["ARON_NDARRAY_DIMENSIONS"] = dims; \
+        j["ARON_NDARRAY_TYPE"] = t; \
+        token->addElement(j); \
+        return true; \
+    }
 
-HANDLE_COMPLEX_TYPES
+            HANDLE_COMPLEX_TYPES
 #undef RUN_ARON_MACRO
 
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronTypeNlohmannJSONWriter::write##upperType##Type() \
-{ \
-    AronTypeNlohmannJSONWriterTokenPtr token = stack.top(); \
-    lowerType x = {}; \
-    nlohmann::jsonPtr j = nlohmann::jsonPtr(new nlohmann::json(x)); \
-    token->addElement(j); \
-    return true; \
-}
+    { \
+        AronTypeNlohmannJSONWriterTokenPtr token = stack.top(); \
+        lowerType x = {}; \
+        nlohmann::json j(x); \
+        token->addElement(j); \
+        return true; \
+    }
 
-HANDLE_PRIMITIVE_TYPES
+            HANDLE_PRIMITIVE_TYPES
 #undef RUN_ARON_MACRO
 
-}
-}
+        }
+    }
 }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.h
index 237b6fecf1aaafdb04b661e36155d3347199c2e3..8241d0c31ffc4145d7af67c7bd225fd294cef7d5 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriter.h
@@ -39,7 +39,7 @@ namespace armarx
         namespace io
         {
             class AronTypeNlohmannJSONWriter :
-                virtual public AronTypeClassWriter<nlohmann::jsonPtr, AronTypeNlohmannJSONWriterTokenPtr>
+                virtual public AronTypeClassWriter<nlohmann::json, AronTypeNlohmannJSONWriterTokenPtr>
             {
             public:
                 AronTypeNlohmannJSONWriter() = default;
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriterToken.h
index 88379e5f255086d24abd5b42586fed6f8992e33b..71c8d03a8d214fb899932efae0e9adbc9eb0ef4e 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/NlohmannJSONWriter/AronTypeNlohmannJSONWriterToken.h
@@ -33,12 +33,6 @@
 // ArmarX
 #include <RobotAPI/libraries/aron/aroncore/AronConfig.h>
 
-
-namespace nlohmann
-{
-    typedef std::shared_ptr<nlohmann::json> jsonPtr;
-}
-
 namespace armarx
 {
     namespace aron
@@ -49,7 +43,7 @@ namespace armarx
             typedef std::shared_ptr<AronTypeNlohmannJSONWriterToken> AronTypeNlohmannJSONWriterTokenPtr;
 
             class AronTypeNlohmannJSONWriterToken :
-                virtual public AronTypeClassWriterToken<nlohmann::jsonPtr>
+                virtual public AronTypeClassWriterToken<nlohmann::json, nlohmann::json>
             {
             public:
                 using PointerType = AronTypeNlohmannJSONWriterTokenPtr;
@@ -57,36 +51,36 @@ namespace armarx
             public:
                 // constructor
                 AronTypeNlohmannJSONWriterToken() = delete;
-                AronTypeNlohmannJSONWriterToken(const AronTypeDescriptor desc, const nlohmann::jsonPtr& t) :
-                    AronWriterToken<AronTypeDescriptor, nlohmann::jsonPtr>(desc, t),
-                    AronTypeClassWriterToken<nlohmann::jsonPtr>(desc, t)
+                AronTypeNlohmannJSONWriterToken(const AronTypeDescriptor desc, const nlohmann::json& t) :
+                    AronWriterToken<AronTypeDescriptor, nlohmann::json, nlohmann::json>(desc, t),
+                    AronTypeClassWriterToken<nlohmann::json, nlohmann::json>(desc, t)
                 {
                 }
 
                 // virtual member functions
-                virtual void addElement(const nlohmann::jsonPtr& n) override
+                virtual void addElement(const nlohmann::json& n) override
                 {
                     auto desc = getDescriptor();
                     switch (desc)
                     {
                         case eAronDictType:
                         {
-                            (*getElement())["ARON_DICT_ACCEPTED_TYPE"] = (*n);
+                            element["ARON_DICT_ACCEPTED_TYPE"] = n;
                             break;
                         }
                         case eAronListType:
                         {
-                            (*getElement())["ARON_LIST_ACCEPTED_TYPE"] = (*n);
+                            element["ARON_LIST_ACCEPTED_TYPE"] = n;
                             break;
                         }
                         case eAronObjectType:
                         {
-                            (*getElement())[currentKey] = (*n);
+                            element[currentKey] = n;
                             break;
                         }
                         case eAronTupleType:
                         {
-                            (*getElement()).push_back(*n);
+                            element.push_back(n);
                             break;
                         }
                         default:
@@ -103,7 +97,7 @@ namespace armarx
                     {
                         throw exception::AronTypeDescriptorNotValidException("AronTypeNlohmannJSONWriterToken", "setName", "Cant set the name of a non-object token.", desc);
                     }
-                    (*getElement())["OBJECT_NAME"] = n;
+                    element["OBJECT_NAME"] = n;
                 }
             };
         }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriter.cpp b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriter.cpp
index adf1101bf6e922dc40ac4641f7b4e45ff9ed91f0..4dcf9562ff8cfbe2bc7778b452cc7e7259c08992 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriter.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriter.cpp
@@ -25,47 +25,47 @@
 
 namespace armarx
 {
-namespace aron
-{
-namespace io
-{
-bool AronTypeXMLWriter::writeStartDictType()
-{
-    return true;
-}
+    namespace aron
+    {
+        namespace io
+        {
+            bool AronTypeXMLWriter::writeStartDictType()
+            {
+                return true;
+            }
 
-bool AronTypeXMLWriter::writeEndDictType()
-{
-    return true;
-}
+            bool AronTypeXMLWriter::writeEndDictType()
+            {
+                return true;
+            }
 
-bool AronTypeXMLWriter::writeStartListType()
-{
-    return true;
-}
+            bool AronTypeXMLWriter::writeStartListType()
+            {
+                return true;
+            }
 
-bool AronTypeXMLWriter::writeEndListType()
-{
-    return true;
-}
+            bool AronTypeXMLWriter::writeEndListType()
+            {
+                return true;
+            }
 
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronTypeXMLWriter::write##upperType##Type(const std::vector<int>& d, const std::string& t) \
-{ \
-    return true; \
-}
+    { \
+        return true; \
+    }
 
-HANDLE_COMPLEX_TYPES
+            HANDLE_COMPLEX_TYPES
 #undef RUN_ARON_MACRO
 
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     bool AronTypeXMLWriter::write##upperType##Type() \
-{ \
-    return true; \
-}
+    { \
+        return true; \
+    }
 
-HANDLE_PRIMITIVE_TYPES
+            HANDLE_PRIMITIVE_TYPES
 #undef RUN_ARON_MACRO
-}
-}
+        }
+    }
 }
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriterToken.h
index fd2b4398febb8ffd948b6cd563ad8063933dd5f7..c33498ad83af9d074ea62235ab5a974c09be354e 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronTypeIO/classWriters/RapidXMLWriter/AronTypeRapidXMLWriterToken.h
@@ -36,8 +36,6 @@
 
 namespace armarx
 {
-    typedef std::shared_ptr<RapidXmlReaderNode> RapidXmlReaderNodePtr;
-
     namespace aron
     {
         namespace io
@@ -46,7 +44,7 @@ namespace armarx
             typedef std::shared_ptr<AronTypeRapidXMLWriterToken> AronTypeRapidXMLWriterTokenPtr;
 
             class AronTypeRapidXMLWriterToken :
-                virtual public AronTypeClassWriterToken<RapidXmlReaderNodePtr>
+                virtual public AronTypeClassWriterToken<RapidXmlReaderNode, RapidXmlReaderNode>
             {
             public:
                 using PointerType = AronTypeRapidXMLWriterTokenPtr;
@@ -54,14 +52,14 @@ namespace armarx
             public:
                 // constructor
                 AronTypeRapidXMLWriterToken() = delete;
-                AronTypeRapidXMLWriterToken(const AronTypeDescriptor desc, const RapidXmlReaderNodePtr& t) :
-                    AronWriterToken<AronTypeDescriptor, RapidXmlReaderNodePtr>(desc, t),
-                    AronTypeClassWriterToken<RapidXmlReaderNodePtr>(desc, t)
+                AronTypeRapidXMLWriterToken(const AronTypeDescriptor desc, const RapidXmlReaderNode& t) :
+                    AronWriterToken<AronTypeDescriptor, RapidXmlReaderNode, RapidXmlReaderNode>(desc, t),
+                    AronTypeClassWriterToken<RapidXmlReaderNode, RapidXmlReaderNode>(desc, t)
                 {
                 }
 
                 // virtual member functions
-                virtual void addElement(const RapidXmlReaderNodePtr& n) override
+                virtual void addElement(const RapidXmlReaderNode& n) override
                 {
                     auto desc = getDescriptor();
                     switch (desc)
@@ -88,7 +86,7 @@ namespace armarx
                 }
 
                 // name functions
-                void setName(const std::string& n)
+                void setName(const std::string& n) override
                 {
                     auto desc = getDescriptor();
                     if (desc != AronTypeDescriptor::eAronObjectType)
diff --git a/source/RobotAPI/libraries/aron/aroncore/io/AronWriterToken.h b/source/RobotAPI/libraries/aron/aroncore/io/AronWriterToken.h
index 9bda546c9a5010e0b93848ebf7827d2a5339429b..f2621ee034d22975644a4f29341da3e39d60c81e 100644
--- a/source/RobotAPI/libraries/aron/aroncore/io/AronWriterToken.h
+++ b/source/RobotAPI/libraries/aron/aroncore/io/AronWriterToken.h
@@ -38,13 +38,13 @@ namespace armarx
     {
         namespace io
         {
-            template<typename AronDescriptorTypename, typename ElementTypename>
+            template<typename AronDescriptorTypename, typename ElementTypename, typename ElementAddTypename>
             class AronWriterToken;
 
-            template<typename AronDescriptorTypename, typename ElementTypename>
-            using AronWriterTokenPtr = std::shared_ptr<AronWriterToken<AronDescriptorTypename, ElementTypename>> ;
+            template<typename AronDescriptorTypename, typename ElementTypename, typename ElementAddTypename>
+            using AronWriterTokenPtr = std::shared_ptr<AronWriterToken<AronDescriptorTypename, ElementTypename, ElementAddTypename>> ;
 
-            template<typename AronDescriptorTypename, typename ElementTypename>
+            template<typename AronDescriptorTypename, typename ElementTypename, typename ElementAddTypename>
             class AronWriterToken
             {
             public:
@@ -55,7 +55,7 @@ namespace armarx
                 };
 
                 // virtual definitions
-                virtual void addElement(const ElementTypename&) = 0;
+                virtual void addElement(const ElementAddTypename&) = 0;
                 virtual std::string toElementAccessor() const = 0;
 
                 void setCurrentKey(const std::string& s)
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDataNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDataNavigator.h
index 08f001f7e926ef8d84f73d73780ea084528f44b1..93347c1e85ef993d7da83a23194e6c89d79efce4 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDataNavigator.h
@@ -96,12 +96,10 @@ namespace armarx
             public:
                 using PointerType = AronContainerDataNavigatorPtr;
 
-                // constructors
-                AronContainerDataNavigator() =  delete;
-                AronContainerDataNavigator(const AronDataDescriptor& d, const AronPath& p = AronPath()) :
-                    AronDataNavigator(d, p)
+                static AronContainerDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n)
                 {
-
+                    AronContainerDataNavigatorPtr casted = std::dynamic_pointer_cast<AronContainerDataNavigator>(n);
+                    return casted;
                 }
             };
 
@@ -114,12 +112,10 @@ namespace armarx
             public:
                 using PointerType = AronComplexDataNavigatorPtr;
 
-                // constructors
-                AronComplexDataNavigator() =  delete;
-                AronComplexDataNavigator(const AronDataDescriptor& d, const AronPath& p = AronPath()) :
-                    AronDataNavigator(d, p)
+                static AronComplexDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n)
                 {
-
+                    AronComplexDataNavigatorPtr casted = std::dynamic_pointer_cast<AronComplexDataNavigator>(n);
+                    return casted;
                 }
             };
 
@@ -132,12 +128,10 @@ namespace armarx
             public:
                 using PointerType = AronPrimitiveDataNavigatorPtr;
 
-                // constructors
-                AronPrimitiveDataNavigator() =  delete;
-                AronPrimitiveDataNavigator(const AronDataDescriptor& d, const AronPath& p = AronPath()) :
-                    AronDataNavigator(d, p)
+                static AronPrimitiveDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n)
                 {
-
+                    AronPrimitiveDataNavigatorPtr casted = std::dynamic_pointer_cast<AronPrimitiveDataNavigator>(n);
+                    return casted;
                 }
             };
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.cpp
index 43f756bba635144c03c498149c9a07d383ffa257..9648916b27196a4a931acc188bda0d25ebce3c34 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.cpp
@@ -37,10 +37,16 @@ namespace armarx
         {
 
             // constructors
+            AronDictDataNavigator::AronDictDataNavigator(const AronPath& path) :
+                AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronDict, path),
+                AronDataNavigator(AronDataDescriptor::eAronDict, path),
+                aron(new data::AronDict())
+            {
+            }
+
             AronDictDataNavigator::AronDictDataNavigator(const data::AronDictPtr& o, const AronPath& path) :
                 AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronDict, path),
                 AronDataNavigator(AronDataDescriptor::eAronDict, path),
-                AronContainerDataNavigator(AronDataDescriptor::eAronDict, path),
                 aron(o)
             {
                 CheckAronPtrForNull("AronDictDataNavigator", "AronDictDataNavigator", getPath(), aron);
@@ -51,14 +57,20 @@ namespace armarx
                 }
             }
 
-            AronDictDataNavigator::AronDictDataNavigator(const AronPath& path) :
-                AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronDict, path),
-                AronDataNavigator(AronDataDescriptor::eAronDict, path),
-                AronContainerDataNavigator(AronDataDescriptor::eAronDict, path),
-                aron(new data::AronDict())
+            AronDictDataNavigator::AronDictDataNavigator(const data::AronDataDict& d, const AronPath& path) :
+                AronDictDataNavigator(data::AronDictPtr(new data::AronDict(d)), path)
             {
             }
 
+            AronDictDataNavigator::AronDictDataNavigator(const std::map<std::string, AronDataNavigatorPtr>& m, const AronPath& path) :
+                AronDictDataNavigator(path)
+            {
+                for (const auto& [key, dataPtr] : m)
+                {
+                    addElement(key, dataPtr);
+                }
+            }
+
             // static methods
             AronDictDataNavigatorPtr AronDictDataNavigator::DynamicCast(const AronDataNavigatorPtr& n)
             {
@@ -66,6 +78,21 @@ namespace armarx
                 return casted;
             }
 
+            AronDictDataNavigatorPtr AronDictDataNavigator::FromAronDict(const data::AronDictPtr& aron)
+            {
+                return std::make_shared<AronDictDataNavigator>(aron);
+            }
+
+            data::AronDictPtr AronDictDataNavigator::ToAronDict(const AronDictDataNavigatorPtr& navigator)
+            {
+                return navigator ? navigator->toAronDict() : nullptr;
+            }
+
+            data::AronDictPtr AronDictDataNavigator::toAronDict() const
+            {
+                return aron;
+            }
+
             // public member functions
             std::vector<std::string> AronDictDataNavigator::getAllKeys() const
             {
@@ -98,10 +125,26 @@ namespace armarx
                 return it->second;
             }
 
+            std::map<std::string, AronDataNavigatorPtr> AronDictDataNavigator::getElements() const
+            {
+                return childrenNavigators;
+            }
+
+            void AronDictDataNavigator::clear()
+            {
+                childrenNavigators.clear();
+                aron->elements.clear();
+            }
+
+            data::AronDictPtr AronDictDataNavigator::getCastedResult() const
+            {
+                return aron;
+            }
+
             // virtual implementations
             data::AronDataPtr AronDictDataNavigator::getResult() const
             {
-                return aron;
+                return getCastedResult();
             }
 
             std::string AronDictDataNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h
index 287718efe7b433f5c653ea83e34483d08d073762..8cbcdb81bac7c35c57e3db9a0947c5c44a0eccf6 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronDictDataNavigator.h
@@ -51,14 +51,24 @@ namespace armarx
                 // constructors
                 AronDictDataNavigator(const AronPath& path = AronPath());
                 AronDictDataNavigator(const data::AronDictPtr&, const AronPath& path = AronPath());
+                AronDictDataNavigator(const data::AronDataDict&, const AronPath& path = AronPath());
+                AronDictDataNavigator(const std::map<std::string, AronDataNavigatorPtr>&, const AronPath& path = AronPath());
 
                 // static methods
                 static AronDictDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n);
+                static AronDictDataNavigatorPtr FromAronDict(const data::AronDictPtr& aron);
+                static data::AronDictPtr ToAronDict(const AronDictDataNavigatorPtr& navigator);
 
                 // public member functions
+                /// Get the derived aron type (in contrast to getResult()).
+                data::AronDictPtr toAronDict() const;
                 std::vector<std::string> getAllKeys() const;
                 void addElement(const std::string& key, const AronDataNavigatorPtr&);
                 AronDataNavigatorPtr getElement(const std::string&) const;
+                std::map<std::string, AronDataNavigatorPtr> getElements() const;
+                void clear();
+
+                data::AronDictPtr getCastedResult() const;
 
                 // virtual implementations
                 virtual data::AronDataPtr getResult() const override;
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.cpp
index 63e0840087beb63471f2ad60478c0b9b75b0aa04..4db8327fe8b380027cb85739de83f1c6554f6e87 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.cpp
@@ -36,10 +36,16 @@ namespace armarx
         namespace datanavigator
         {
             // constructors
+            AronListDataNavigator::AronListDataNavigator(const AronPath& path) :
+                AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronList, path),
+                AronDataNavigator(AronDataDescriptor::eAronList, path),
+                aron(new data::AronList())
+            {
+            }
+
             AronListDataNavigator::AronListDataNavigator(const data::AronListPtr& l, const AronPath& path) :
                 AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronList, path),
                 AronDataNavigator(AronDataDescriptor::eAronList, path),
-                AronContainerDataNavigator(AronDataDescriptor::eAronList, path),
                 aron(l)
             {
                 CheckAronPtrForNull("AronListDataNavigator", "AronListDataNavigator", getPath(), aron);
@@ -51,12 +57,18 @@ namespace armarx
                 }
             }
 
-            AronListDataNavigator::AronListDataNavigator(const AronPath& path) :
-                AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronList, path),
-                AronDataNavigator(AronDataDescriptor::eAronList, path),
-                AronContainerDataNavigator(AronDataDescriptor::eAronList, path),
-                aron(new data::AronList())
+            AronListDataNavigator::AronListDataNavigator(const data::AronDataList& d, const AronPath& path) :
+                AronListDataNavigator(data::AronListPtr(new data::AronList(d)), path)
+            {
+            }
+
+            AronListDataNavigator::AronListDataNavigator(const std::vector<AronDataNavigatorPtr>& n, const AronPath& path) :
+                AronListDataNavigator(path)
             {
+                for (const auto& dataPtr : n)
+                {
+                    addElement(dataPtr);
+                }
             }
 
             // static methods
@@ -82,10 +94,26 @@ namespace armarx
                 return childrenNavigators[i];
             }
 
+            std::vector<AronDataNavigatorPtr> AronListDataNavigator::getElements() const
+            {
+                return childrenNavigators;
+            }
+
+            void AronListDataNavigator::clear()
+            {
+                childrenNavigators.clear();
+                aron->elements.clear();
+            }
+
+            data::AronListPtr AronListDataNavigator::getCastedResult() const
+            {
+                return aron;
+            }
+
             // virtual implementations
             data::AronDataPtr AronListDataNavigator::getResult() const
             {
-                return aron;
+                return getCastedResult();
             }
 
             std::string AronListDataNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.h
index 2a7ea718f405223b7f6d11b3d53f6289f9c62a0f..03151c2cfe616f239253d6864619596c3d82b0bf 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronListDataNavigator.h
@@ -50,6 +50,8 @@ namespace armarx
                 // constructors
                 AronListDataNavigator(const AronPath& path = AronPath());
                 AronListDataNavigator(const data::AronListPtr&, const AronPath& path = AronPath());
+                AronListDataNavigator(const data::AronDataList&, const AronPath& path = AronPath());
+                AronListDataNavigator(const std::vector<AronDataNavigatorPtr>&, const AronPath& path = AronPath());
 
                 // static methods
                 static AronListDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n);
@@ -57,6 +59,10 @@ namespace armarx
                 // public member functions
                 void addElement(const AronDataNavigatorPtr&);
                 AronDataNavigatorPtr getElement(unsigned int) const;
+                std::vector<AronDataNavigatorPtr> getElements() const;
+                void clear();
+
+                data::AronListPtr getCastedResult() const;
 
                 // virtual implementations
                 virtual data::AronDataPtr getResult() const override;
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.cpp
index 5c748218f11e8262bee7d7fbb424966e0830372a..13ef492d76a54535c6f3a0d7982b6c457e606284 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.cpp
@@ -39,22 +39,27 @@ namespace armarx
         namespace datanavigator
         {
             // constructors
-            AronNDArrayDataNavigator::AronNDArrayDataNavigator(const data::AronNDArrayPtr& o, const AronPath& path) :
+            AronNDArrayDataNavigator::AronNDArrayDataNavigator(const AronPath& path) :
                 AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronNDArray, path),
                 AronDataNavigator(AronDataDescriptor::eAronNDArray, path),
-                AronComplexDataNavigator(AronDataDescriptor::eAronNDArray, path),
-                aron(o)
+                aron(new data::AronNDArray())
             {
-                CheckAronPtrForNull("AronNDArrayDataNavigator", "AronNDArrayDataNavigator", getPath(), aron);
+
             }
 
-            AronNDArrayDataNavigator::AronNDArrayDataNavigator(const AronPath& path) :
+            AronNDArrayDataNavigator::AronNDArrayDataNavigator(const data::AronNDArrayPtr& o, const AronPath& path) :
                 AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAronNDArray, path),
                 AronDataNavigator(AronDataDescriptor::eAronNDArray, path),
-                AronComplexDataNavigator(AronDataDescriptor::eAronNDArray, path),
-                aron(new data::AronNDArray())
+                aron(o)
             {
+                CheckAronPtrForNull("AronNDArrayDataNavigator", "AronNDArrayDataNavigator", getPath(), aron);
+            }
+
 
+
+            AronNDArrayDataNavigator::AronNDArrayDataNavigator(const std::vector<int>& dim, const std::string& t, const std::vector<unsigned char>& data, const AronPath& path) :
+                AronNDArrayDataNavigator(data::AronNDArrayPtr(new data::AronNDArray(dim, t, data)), path)
+            {
             }
 
             // static methods
@@ -106,10 +111,15 @@ namespace armarx
                 aron->type = t;
             }
 
+            data::AronNDArrayPtr AronNDArrayDataNavigator::getCastedResult() const
+            {
+                return aron;
+            }
+
             // virtual implementations
             data::AronDataPtr AronNDArrayDataNavigator::getResult() const
             {
-                return aron;
+                return getCastedResult();
             }
 
             std::string AronNDArrayDataNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.h
index 3a32dd987cca25d01015ee5e15be396be9209c59..b5182d9d3f06933486a99b3a851ab84436cfcf31 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronNDArrayDataNavigator.h
@@ -50,6 +50,7 @@ namespace armarx
                 // constructors
                 AronNDArrayDataNavigator(const AronPath& path = AronPath());
                 AronNDArrayDataNavigator(const data::AronNDArrayPtr&, const AronPath& path = AronPath());
+                AronNDArrayDataNavigator(const std::vector<int>&, const std::string&, const std::vector<unsigned char>&, const AronPath& path = AronPath());
 
                 // static methods
                 static AronNDArrayDataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n);
@@ -65,6 +66,8 @@ namespace armarx
                 std::string getType() const;
                 void setType(const std::string&);
 
+                data::AronNDArrayPtr getCastedResult() const;
+
                 // virtual implementations
                 virtual data::AronDataPtr getResult() const override;
                 virtual std::string getName() const override;
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.cpp
index a1c14461afe71328a4d352e74840386aab42e56f..7a2e1f755c2e4cceeba8bd60901b04a4b9f38d5d 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.cpp
@@ -35,22 +35,25 @@ namespace armarx
         {
 #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
     /* constructors */ \
-    Aron##upperType##DataNavigator::Aron##upperType##DataNavigator(const AronPath& path) : \
+    Aron##upperType##DataNavigator::Aron##upperType##DataNavigator(const data::Aron##upperType##Ptr& o, const AronPath& path) : \
         AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAron##upperType, path), \
         AronDataNavigator(AronDataDescriptor::eAron##upperType, path), \
-        AronPrimitiveDataNavigator(AronDataDescriptor::eAron##upperType, path), \
-        aron(new data::Aron##upperType()) \
+        aron(o) \
     { \
-        aron->value = {}; \
     } \
-    Aron##upperType##DataNavigator::Aron##upperType##DataNavigator(const data::Aron##upperType##Ptr& o, const AronPath& path) : \
+    \
+    Aron##upperType##DataNavigator::Aron##upperType##DataNavigator(const AronPath& path) : \
         AronNavigator<AronDataDescriptor, data::AronData>::AronNavigator(AronDataDescriptor::eAron##upperType, path), \
         AronDataNavigator(AronDataDescriptor::eAron##upperType, path), \
-        AronPrimitiveDataNavigator(AronDataDescriptor::eAron##upperType, path), \
-        aron(o) \
+        aron(new data::Aron##upperType()) \
     { \
+        aron->value = {}; \
     } \
     \
+    Aron##upperType##DataNavigator::Aron##upperType##DataNavigator(const lowerType& d, const AronPath& path) : \
+        Aron##upperType##DataNavigator(data::Aron##upperType##Ptr(new data::Aron##upperType(d)), path) \
+    { \
+    } \
     /* static methods */ \
     Aron##upperType##DataNavigatorPtr Aron##upperType##DataNavigator::DynamicCast(const AronDataNavigatorPtr& n) \
     { \
@@ -69,10 +72,15 @@ namespace armarx
         return aron->value; \
     } \
     \
+    data::Aron##upperType##Ptr Aron##upperType##DataNavigator::getCastedResult() const \
+    { \
+        return aron; \
+    } \
+    \
     /* virtual implementations */ \
     data::AronDataPtr Aron##upperType##DataNavigator::getResult() const \
     { \
-        return aron; \
+        return getCastedResult(); \
     } \
     \
     std::string Aron##upperType##DataNavigator::getName() const \
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.h
index 2dc5f7a58e903b361ff9aab5a35a8207f9849519..8a0ee59dab2078c15a152e3ed5d94ae1135b5754 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronPrimitiveDataNavigator.h
@@ -50,6 +50,7 @@ namespace armarx
         /* constructors */ \
         Aron##upperType##DataNavigator(const AronPath& = AronPath()); \
         Aron##upperType##DataNavigator(const data::Aron##upperType##Ptr&, const AronPath& = AronPath()); \
+        Aron##upperType##DataNavigator(const lowerType&, const AronPath& = AronPath()); \
         \
         /* static methods */ \
         static Aron##upperType##DataNavigatorPtr DynamicCast(const AronDataNavigatorPtr& n); \
@@ -58,6 +59,8 @@ namespace armarx
         void setValue(const lowerType& x); \
         lowerType getValue() const; \
         \
+        data::Aron##upperType##Ptr getCastedResult() const; \
+        \
         /* virtual implementations */ \
         virtual data::AronDataPtr getResult() const override; \
         virtual std::string getName() const override; \
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp
index 3b8fa846988f3321b8c59803d9d7a41e1c59b28a..9d1ba674f32d4043274efe63ac7b94092ab8610c 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp
@@ -35,7 +35,6 @@ namespace armarx
             AronDictTypeNavigator::AronDictTypeNavigator(const AronPath& path) :
                 AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronDictType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronDictType, path),
-                AronDictSerializerTypeNavigator(AronTypeDescriptor::eAronDictType, path),
                 type(new type::AronDictType())
             {
 
@@ -44,7 +43,6 @@ namespace armarx
             AronDictTypeNavigator::AronDictTypeNavigator(const type::AronDictTypePtr& o, const AronPath& path) :
                 AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronDictType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronDictType, path),
-                AronDictSerializerTypeNavigator(AronTypeDescriptor::eAronDictType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronDictTypeNavigator", "AronDictTypeNavigator", getPath(), o);
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.h
index 1b73f153717626480ac8e4a2af00ce0ccabe760f..b633eab3b6d41fe9a56042cd7d5de3aba22780d6 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.h
@@ -51,6 +51,8 @@ namespace armarx
                 AronDictTypeNavigator(const AronPath& path);
                 AronDictTypeNavigator(const type::AronDictTypePtr&, const AronPath& path);
 
+                type::AronDictTypePtr getCastedResult() const;
+
                 // static methods
                 static AronDictTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.cpp
index 4aa1aa03a804a8ad4bef1122e0f245528e19a7f6..d26786cd29041438eb29c649efe0678bcdbb2fac 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.cpp
@@ -45,7 +45,6 @@ namespace armarx
             AronEigenMatrixTypeNavigator::AronEigenMatrixTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(new type::AronEigenMatrixType({1, 1}, ""))
             {
             }
@@ -53,7 +52,6 @@ namespace armarx
             AronEigenMatrixTypeNavigator::AronEigenMatrixTypeNavigator(const type::AronEigenMatrixTypePtr& o, const AronPath& path) :
                 AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronDictType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronEigenMatrixTypeNavigator", "AronEigenMatrixTypeNavigator", getPath(), o);
@@ -91,6 +89,26 @@ namespace armarx
                 type->dimensions[1] = h;
             }
 
+            type::AronEigenMatrixTypePtr AronEigenMatrixTypeNavigator::getCastedResult() const
+            {
+                if (type->dimensions.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronEigenMatrixTypeNavigator", "getCastedResult", "The dimension eigen matrix is empty", getPath());
+                }
+                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
+            {
+                return i < -1;
+            }))
+                {
+                    throw exception::AronExceptionWithPathInfo("AronEigenMatrixTypeNavigator", "getCastedResult", "The dimension size is wrong. At least one empty is <-1", getPath());
+                }
+                if (type->typeName.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronEigenMatrixTypeNavigator", "getCastedResult", "The useType of eigen matrix is empty.", getPath());
+                }
+                return type;
+            }
+
             std::string AronEigenMatrixTypeNavigator::getUsedType() const
             {
                 return type->typeName;
@@ -147,22 +165,7 @@ namespace armarx
             // virtual implementations
             type::AronTypePtr AronEigenMatrixTypeNavigator::getResult() const
             {
-                if (type->dimensions.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronEigenMatrixTypeNavigator", "getResult", "The dimension eigen matrix is empty", getPath());
-                }
-                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
-            {
-                return i < -1;
-            }))
-                {
-                    throw exception::AronExceptionWithPathInfo("AronEigenMatrixTypeNavigator", "getResult", "The dimension size is wrong. At least one empty is <-1", getPath());
-                }
-                if (type->typeName.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronEigenMatrixTypeNavigator", "getResult", "The useType of eigen matrix is empty.", getPath());
-                }
-                return type;
+                return getCastedResult();
             }
 
             std::string AronEigenMatrixTypeNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.h
index f5d2761b1eec5203c82fcff35087c88c91fd962c..14a24b99881938dd05af0893415091c1d308d4b2 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronEigenMatrixTypeNavigator.h
@@ -58,6 +58,8 @@ namespace armarx
                 void setRows(const unsigned int&);
                 void setCols(const unsigned int&);
 
+                type::AronEigenMatrixTypePtr getCastedResult() const;
+
                 // static methods
                 static AronEigenMatrixTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.cpp
index b7b867ebed8f0bd6c99fc1e06275524f6d2581cc..49670a0c2a29e30382a7582a138efa924e649051 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.cpp
@@ -41,7 +41,6 @@ namespace armarx
             AronIVTCByteImageTypeNavigator::AronIVTCByteImageTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronIVTCByteImageType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronIVTCByteImageType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(new type::AronIVTCByteImageType({0, 0}, ""))
             {
             }
@@ -49,7 +48,6 @@ namespace armarx
             AronIVTCByteImageTypeNavigator::AronIVTCByteImageTypeNavigator(const type::AronIVTCByteImageTypePtr& o, const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronIVTCByteImageType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronIVTCByteImageType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronIVTCByteImageTypeNavigator", "AronIVTCByteImageTypeNavigator", getPath(), o);
@@ -59,6 +57,26 @@ namespace armarx
                 }
             }
 
+            type::AronIVTCByteImageTypePtr AronIVTCByteImageTypeNavigator::getCastedResult() const
+            {
+                if (type->dimensions.size() != 2)
+                {
+                    throw exception::AronExceptionWithPathInfo("AronIVTCByteImageTypeNavigator", "getCastedResult", "The dimension size is wrong. Got size: " + std::to_string(type->dimensions.size()), getPath());
+                }
+                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
+            {
+                return i <= 0;
+            }))
+                {
+                    throw exception::AronExceptionWithPathInfo("AronIVTCByteImageTypeNavigator", "getCastedResult", "The dimension size is wrong. At least one empty is <=0", getPath());
+                }
+                if (type->typeName.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronIVTCByteImageTypeNavigator", "getCastedResult", "The useType of image is empty.", getPath());
+                }
+                return type;
+            }
+
             std::vector<int> AronIVTCByteImageTypeNavigator::getDimensions() const
             {
                 return type->dimensions;
@@ -137,22 +155,7 @@ namespace armarx
             // virtual implementations
             type::AronTypePtr AronIVTCByteImageTypeNavigator::getResult() const
             {
-                if (type->dimensions.size() != 2)
-                {
-                    throw exception::AronExceptionWithPathInfo("AronIVTCByteImageTypeNavigator", "getResult", "The dimension size is wrong. Got size: " + std::to_string(type->dimensions.size()), getPath());
-                }
-                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
-            {
-                return i <= 0;
-            }))
-                {
-                    throw exception::AronExceptionWithPathInfo("AronIVTCByteImageTypeNavigator", "getResult", "The dimension size is wrong. At least one empty is <=0", getPath());
-                }
-                if (type->typeName.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronIVTCByteImageTypeNavigator", "getResult", "The useType of image is empty.", getPath());
-                }
-                return type;
+                return getCastedResult();
             }
 
             std::string AronIVTCByteImageTypeNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.h
index 6a61e9f05bc20bdc9cde83c67ac9eac6305faa52..9d1ba206d4f615d76ad797f2a4147e8b030c7d9a 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronIVTCByteImageTypeNavigator.h
@@ -58,6 +58,8 @@ namespace armarx
                 void setWidth(const unsigned int&);
                 void setHeight(const unsigned int&);
 
+                type::AronIVTCByteImageTypePtr getCastedResult() const;
+
                 // static methods
                 static AronIVTCByteImageTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp
index 2a1c787a487b54e285893e64c7ef70a6832feff4..34a31e4352a5820c8502e98ad7d0141462e88eb8 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp
@@ -34,7 +34,6 @@ namespace armarx
             AronListTypeNavigator::AronListTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronListType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronListType, path),
-                AronListSerializerTypeNavigator(AronTypeDescriptor::eAronListType, path),
                 type(new type::AronListType())
             {
             }
@@ -42,12 +41,37 @@ namespace armarx
             AronListTypeNavigator::AronListTypeNavigator(const type::AronListTypePtr& o, const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronListType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronListType, path),
-                AronListSerializerTypeNavigator(AronTypeDescriptor::eAronListType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronListTypeNavigator", "AronListTypeNavigator", getPath(), o);
             }
 
+            // static methods
+            AronListTypeNavigatorPtr AronListTypeNavigator::DynamicCast(const AronTypeNavigatorPtr& n)
+            {
+                CheckTypeNavigatorPtrForNull("AronListTypeNavigator", "DynamicCast[Before]", n);
+                AronListTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronListTypeNavigator>(n);
+                CheckTypeNavigatorPtrForNull("AronListTypeNavigator", "DynamicCast[After]", casted);
+                return casted;
+            }
+
+            type::AronListTypePtr AronListTypeNavigator::getCastedResult() const
+            {
+                CheckAronPtrForNull("AronListTypeNavigator", "getCastedResult", getPath(), type->acceptedType);
+                return type;
+            }
+
+            // virtual implementations
+            type::AronTypePtr AronListTypeNavigator::getResult() const
+            {
+                return getCastedResult();
+            }
+
+            std::string AronListTypeNavigator::getName() const
+            {
+                return "AronListType<" + acceptedTypeNavigator->getName() + ">";
+            }
+
             void AronListTypeNavigator::addAcceptedType(const AronTypeNavigatorPtr&)
             {
                 throw exception::AronExceptionWithPathInfo("AronListTypeNavigator", "addAcceptedType", "Called invalid function!", getPath());
@@ -69,27 +93,6 @@ namespace armarx
                 type->acceptedType = a->getResult();
                 acceptedTypeNavigator = a;
             }
-
-            // static methods
-            AronListTypeNavigatorPtr AronListTypeNavigator::DynamicCast(const AronTypeNavigatorPtr& n)
-            {
-                CheckTypeNavigatorPtrForNull("AronListTypeNavigator", "DynamicCast[Before]", n);
-                AronListTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronListTypeNavigator>(n);
-                CheckTypeNavigatorPtrForNull("AronListTypeNavigator", "DynamicCast[After]", casted);
-                return casted;
-            }
-
-            // virtual implementations
-            type::AronTypePtr AronListTypeNavigator::getResult() const
-            {
-                CheckAronPtrForNull("AronListTypeNavigator", "getResult", getPath(), type->acceptedType);
-                return type;
-            }
-
-            std::string AronListTypeNavigator::getName() const
-            {
-                return "AronListType<" + acceptedTypeNavigator->getName() + ">";
-            }
         }
     }
 }
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.h
index 405107ff1155597f318e8c05870bcdeaf35a5647..3982291ffef4373e68fe061d144291e3bceba44b 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.h
@@ -51,6 +51,8 @@ namespace armarx
                 AronListTypeNavigator(const AronPath& path);
                 AronListTypeNavigator(const type::AronListTypePtr&, const AronPath& path);
 
+                type::AronListTypePtr getCastedResult() const;
+
                 // static methods
                 static AronListTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp
index 13903d9e2cfaad75481d3314e96ab9f598982424..01b87e9f7bb0adfe51a69fcbb62c4db0634e0a85 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp
@@ -36,7 +36,6 @@ namespace armarx
             AronObjectTypeNavigator::AronObjectTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronObjectType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronObjectType, path),
-                AronDictSerializerTypeNavigator(AronTypeDescriptor::eAronObjectType, path),
                 type((new type::AronObjectType()))
             {
             }
@@ -44,7 +43,6 @@ namespace armarx
             AronObjectTypeNavigator::AronObjectTypeNavigator(const type::AronObjectTypePtr& o, const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronObjectType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronObjectType, path),
-                AronDictSerializerTypeNavigator(AronTypeDescriptor::eAronObjectType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronObjectTypeNavigator", "AronObjectTypeNavigator", getPath(), o);
@@ -55,7 +53,7 @@ namespace armarx
             {
                 CheckTypeNavigatorPtrForNull("AronObjectTypeNavigator", "DynamicCast[Before]", n);
                 AronObjectTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronObjectTypeNavigator>(n);
-                CheckTypeNavigatorPtrForNull("AronObjectTypeNavigator", "DynamicCast[After]", casted);
+                CheckTypeNavigatorPtrForNull("AronObjectTypeNavigator", "DynamicCast[After]", n->getPath(), casted);
                 return casted;
             }
 
@@ -110,8 +108,7 @@ namespace armarx
                 return extends;
             }
 
-            // virtual implementations
-            type::AronTypePtr AronObjectTypeNavigator::getResult() const
+            type::AronObjectTypePtr AronObjectTypeNavigator::getCastedResult() const
             {
                 if (type->objectName.empty())
                 {
@@ -124,6 +121,12 @@ namespace armarx
                 return type;
             }
 
+            // virtual implementations
+            type::AronTypePtr AronObjectTypeNavigator::getResult() const
+            {
+                return getCastedResult();
+            }
+
             std::string AronObjectTypeNavigator::getName() const
             {
                 return "AronObjectType<" + type->objectName + ">";
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h
index 01cedad2f5fe50f950096abed03a1add3d726aa8..e1fdc5ed2a86b47e4157271c14aebbcc02ab0d37 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h
@@ -60,6 +60,8 @@ namespace armarx
                 void setObjectName(const std::string&);
                 void setExtends(const AronObjectTypeNavigatorPtr&);
 
+                type::AronObjectTypePtr getCastedResult() const;
+
                 // static methods
                 static AronObjectTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.cpp
index 75a0e9670c90ce9301799cde3e4996dfe3e9635c..c165e101a6aead04881b1a3d74c2c0f7b50f28b3 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.cpp
@@ -45,7 +45,6 @@ namespace armarx
             AronOpenCVMatTypeNavigator::AronOpenCVMatTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronOpenCVMatType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronOpenCVMatType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(new type::AronOpenCVMatType({}, ""))
             {
             }
@@ -53,12 +52,31 @@ namespace armarx
             AronOpenCVMatTypeNavigator::AronOpenCVMatTypeNavigator(const type::AronOpenCVMatTypePtr& o, const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronOpenCVMatType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronOpenCVMatType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronOpenCVMatTypeNavigator", "AronOpenCVMatTypeNavigator", getPath(), o);
             }
 
+            type::AronOpenCVMatTypePtr AronOpenCVMatTypeNavigator::getCastedResult() const
+            {
+                if (type->dimensions.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronOpenCVMatTypeNavigator", "getCastedResult", "The dimension size is empty", getPath());
+                }
+                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
+            {
+                return i < -1;
+            }))
+                {
+                    throw exception::AronExceptionWithPathInfo("AronOpenCVMatTypeNavigator", "getCastedResult", "The dimension size is wrong. At least one empty is <-1", getPath());
+                }
+                if (type->typeName.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronOpenCVMatTypeNavigator", "getCastedResult", "The useType of image is empty.", getPath());
+                }
+                return type;
+            }
+
             std::string AronOpenCVMatTypeNavigator::getUsedType() const
             {
                 return type->typeName;
@@ -116,22 +134,7 @@ namespace armarx
             // virtual implementations
             type::AronTypePtr AronOpenCVMatTypeNavigator::getResult() const
             {
-                if (type->dimensions.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronOpenCVMatTypeNavigator", "getResult", "The dimension size is empty", getPath());
-                }
-                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
-            {
-                return i < -1;
-            }))
-                {
-                    throw exception::AronExceptionWithPathInfo("AronOpenCVMatTypeNavigator", "getResult", "The dimension size is wrong. At least one empty is <-1", getPath());
-                }
-                if (type->typeName.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronOpenCVMatTypeNavigator", "getResult", "The useType of image is empty.", getPath());
-                }
-                return type;
+                return getCastedResult();
             }
 
             std::string AronOpenCVMatTypeNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.h
index e7972a2028331076feb5a7ac1a4f692f9bdec6c7..2af188f766ff93c78060ec63841210542fe4ed1c 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronOpenCVMatTypeNavigator.h
@@ -52,6 +52,8 @@ namespace armarx
                 AronOpenCVMatTypeNavigator(const AronPath& path);
                 AronOpenCVMatTypeNavigator(const type::AronOpenCVMatTypePtr&, const AronPath& path);
 
+                type::AronOpenCVMatTypePtr getCastedResult() const;
+
                 // static methods
                 static AronOpenCVMatTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.cpp
index 21461a55957f59ee0650f63d962e37e2490a1cac..a1bf3e4e17eecbd8599d61c02cdfc8f3dff2e37a 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.cpp
@@ -45,7 +45,6 @@ namespace armarx
             AronPCLPointCloudTypeNavigator::AronPCLPointCloudTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronPCLPointCloudType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronPCLPointCloudType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(new type::AronPCLPointCloudType({0, 0}, ""))
             {
             }
@@ -53,12 +52,31 @@ namespace armarx
             AronPCLPointCloudTypeNavigator::AronPCLPointCloudTypeNavigator(const type::AronPCLPointCloudTypePtr& o, const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronPCLPointCloudType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronPCLPointCloudType, path),
-                AronNDArraySerializerTypeNavigator(AronTypeDescriptor::eAronEigenMatrixType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronPCLPointCloudTypeNavigator", "AronPCLPointCloudTypeNavigator", getPath(), o);
             }
 
+            type::AronPCLPointCloudTypePtr AronPCLPointCloudTypeNavigator::getCastedResult() const
+            {
+                if (type->dimensions.size() != 2)
+                {
+                    throw exception::AronExceptionWithPathInfo("AronPCLPointCloudTypeNavigator", "getCastedResult", "The dimension size is wrong. Got size: " + std::to_string(type->dimensions.size()), getPath());
+                }
+                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
+            {
+                return i <= 0;
+            }))
+                {
+                    throw exception::AronExceptionWithPathInfo("AronPCLPointCloudTypeNavigator", "getCastedResult", "The dimension size is wrong. At least one empty is <=0", getPath());
+                }
+                if (type->typeName.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronPCLPointCloudTypeNavigator", "getCastedResult", "The useType of image is empty.", getPath());
+                }
+                return type;
+            }
+
             std::vector<int> AronPCLPointCloudTypeNavigator::getDimensions() const
             {
                 return type->dimensions;
@@ -137,22 +155,7 @@ namespace armarx
             // virtual implementations
             type::AronTypePtr AronPCLPointCloudTypeNavigator::getResult() const
             {
-                if (type->dimensions.size() != 2)
-                {
-                    throw exception::AronExceptionWithPathInfo("AronPCLPointCloudTypeNavigator", "getResult", "The dimension size is wrong. Got size: " + std::to_string(type->dimensions.size()), getPath());
-                }
-                if (std::any_of(type->dimensions.begin(), type->dimensions.end(), [](int i)
-            {
-                return i <= 0;
-            }))
-                {
-                    throw exception::AronExceptionWithPathInfo("AronPCLPointCloudTypeNavigator", "getResult", "The dimension size is wrong. At least one empty is <=0", getPath());
-                }
-                if (type->typeName.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronPCLPointCloudTypeNavigator", "getResult", "The useType of image is empty.", getPath());
-                }
-                return type;
+                return getCastedResult();
             }
 
             std::string AronPCLPointCloudTypeNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.h
index 22643da1e0da05b7afc9d189492fb92344d6f753..ae22bfcae6d91ce75c9a193e1ce6b581fd65d470 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPCLPointCloudTypeNavigator.h
@@ -58,6 +58,8 @@ namespace armarx
                 void setWidth(const unsigned int&);
                 void setHeight(const unsigned int&);
 
+                type::AronPCLPointCloudTypePtr getCastedResult() const;
+
                 // static methods
                 static AronPCLPointCloudTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.cpp
index b88cd7cff81cde8eaa3ed9119eb3d2852cb7f2d1..d3eb95da63885fc84b253cd57f83afaae74f7867 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.cpp
@@ -55,6 +55,11 @@ namespace armarx
         CheckAronPtrForNull("Aron" + std::string(#upperType) + "TypeNavigator", "Aron" + std::string(#upperType) + "TypeNavigator", getPath(), o); \
     } \
     \
+    type::Aron##upperType##TypePtr Aron##upperType##TypeNavigator::getCastedResult() const \
+    { \
+        return type; \
+    } \
+    \
     /* static methods */ \
     Aron##upperType##TypeNavigatorPtr Aron##upperType##TypeNavigator::DynamicCast(const AronTypeNavigatorPtr& n) \
     {\
@@ -67,7 +72,7 @@ namespace armarx
     /* virtual implementations */\
     type::AronTypePtr Aron##upperType##TypeNavigator::getResult() const\
     {\
-        return type; \
+        return getCastedResult(); \
     }\
     \
     std::string Aron##upperType##TypeNavigator::getName() const \
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.h
index df375192079db8539b8db54c57ea4a7af75a86b9..deaebb49c9bd8865bb8aa314eff721f9cec15927 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronPrimitiveTypeNavigator.h
@@ -44,7 +44,7 @@ namespace armarx
     typedef std::shared_ptr<Aron##upperType##TypeNavigator> Aron##upperType##TypeNavigatorPtr; \
     \
     class Aron##upperType##TypeNavigator : \
-        virtual public AronTypeNavigator \
+        virtual public AronPrimitiveTypeNavigator \
     { \
     public: \
         using PointerType = Aron##upperType##TypeNavigatorPtr; \
@@ -54,6 +54,8 @@ namespace armarx
         Aron##upperType##TypeNavigator(const AronPath&); \
         Aron##upperType##TypeNavigator(const type::Aron##upperType##TypePtr&, const AronPath&); \
         \
+        type::Aron##upperType##TypePtr getCastedResult() const; \
+        \
         /* static methods */ \
         static Aron##upperType##TypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr&); \
         \
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp
index cf6e74db39742e2efe10717d1995bc457c6bcfcf..b0b463456fff30d59b692099bb291ed389f734d3 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp
@@ -35,7 +35,6 @@ namespace armarx
             AronTupleTypeNavigator::AronTupleTypeNavigator(const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronTupleType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronTupleType, path),
-                AronListSerializerTypeNavigator(AronTypeDescriptor::eAronTupleType, path),
                 type(new type::AronTupleType())
             {
             }
@@ -43,12 +42,20 @@ namespace armarx
             AronTupleTypeNavigator::AronTupleTypeNavigator(const type::AronTupleTypePtr& o, const AronPath& path) :
                 navigator::AronNavigator<AronTypeDescriptor, type::AronType>::AronNavigator(AronTypeDescriptor::eAronTupleType, path),
                 AronTypeNavigator(AronTypeDescriptor::eAronTupleType, path),
-                AronListSerializerTypeNavigator(AronTypeDescriptor::eAronTupleType, path),
                 type(o)
             {
                 CheckAronPtrForNull("AronTupleTypeNavigator", "AronTupleTypeNavigator", getPath(), o);
             }
 
+            type::AronTupleTypePtr AronTupleTypeNavigator::getCastedResult() const
+            {
+                if (acceptedTypeNavigators.empty())
+                {
+                    throw exception::AronExceptionWithPathInfo("AronTupleTypeNavigator", "getCastedResult", "No accepted types set", getPath());
+                }
+                return type;
+            }
+
             // static methods
             AronTupleTypeNavigatorPtr AronTupleTypeNavigator::DynamicCast(const AronTypeNavigatorPtr& n)
             {
@@ -84,11 +91,7 @@ namespace armarx
             // virtual implementations
             type::AronTypePtr AronTupleTypeNavigator::getResult() const
             {
-                if (acceptedTypeNavigators.empty())
-                {
-                    throw exception::AronExceptionWithPathInfo("AronTupleTypeNavigator", "getResult", "No accepted types set", getPath());
-                }
-                return type;
+                return getCastedResult();
             }
 
             std::string AronTupleTypeNavigator::getName() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.h
index eadb6317803d7019e4d4a73068fa0ec7cdef55cd..0435f61c89cf8c60ecf7135a7d85cbc6f4dfe402 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.h
@@ -52,6 +52,8 @@ namespace armarx
                 AronTupleTypeNavigator(const AronPath& path);
                 AronTupleTypeNavigator(const type::AronTupleTypePtr&, const AronPath& path);
 
+                type::AronTupleTypePtr getCastedResult() const;
+
                 // static methods
                 static AronTupleTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n);
 
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTypeNavigator.h b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTypeNavigator.h
index a584dfd5cfd505c1d73954d0623a1acf1becafb2..6de37ba39979f6cb80485766a2946c5a3556c200 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTypeNavigator.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTypeNavigator.h
@@ -85,23 +85,34 @@ namespace armarx
             };
 
 
+            class AronContainerTypeNavigator;
+            typedef std::shared_ptr<AronContainerTypeNavigator> AronContainerTypeNavigatorPtr;
+
+            class AronContainerTypeNavigator :
+                virtual public AronTypeNavigator
+            {
+            public:
+                using PointerType = AronContainerTypeNavigatorPtr;
+
+                // static methods
+                static AronContainerTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n)
+                {
+                    AronContainerTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronContainerTypeNavigator>(n);
+                    return casted;
+                }
+            };
 
 
             class AronDictSerializerTypeNavigator;
             typedef std::shared_ptr<AronDictSerializerTypeNavigator> AronDictSerializerTypeNavigatorPtr;
 
             class AronDictSerializerTypeNavigator :
-                virtual public AronTypeNavigator
+                virtual public AronContainerTypeNavigator
             {
             public:
                 using PointerType = AronDictSerializerTypeNavigatorPtr;
 
             public:
-                AronDictSerializerTypeNavigator() = delete;
-                AronDictSerializerTypeNavigator(const AronTypeDescriptor& n, const AronPath& path) :
-                    AronTypeNavigator(n, path)
-                {}
-
                 // virtual methods
                 virtual void setAcceptedType(const AronTypeNavigatorPtr&) = 0;
                 virtual void addAcceptedType(const std::string&, const AronTypeNavigatorPtr&) = 0;
@@ -112,9 +123,7 @@ namespace armarx
                 // static methods
                 static AronDictSerializerTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n)
                 {
-                    CheckTypeNavigatorPtrForNull("AronDictSerializerTypeNavigator", "DynamicCast[Before]", n);
                     AronDictSerializerTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronDictSerializerTypeNavigator>(n);
-                    CheckTypeNavigatorPtrForNull("AronDictSerializerTypeNavigator", "DynamicCast[After]", casted);
                     return casted;
                 }
             };
@@ -125,17 +134,12 @@ namespace armarx
             typedef std::shared_ptr<AronListSerializerTypeNavigator> AronListSerializerTypeNavigatorPtr;
 
             class AronListSerializerTypeNavigator :
-                virtual public AronTypeNavigator
+                virtual public AronContainerTypeNavigator
             {
             public:
                 using PointerType = AronListSerializerTypeNavigatorPtr;
 
             public:
-                AronListSerializerTypeNavigator() = delete;
-                AronListSerializerTypeNavigator(const AronTypeDescriptor& n, const AronPath& path) :
-                    AronTypeNavigator(n, path)
-                {}
-
                 // virtual methods
                 virtual void setAcceptedType(const AronTypeNavigatorPtr&) = 0;
                 virtual void addAcceptedType(const AronTypeNavigatorPtr&) = 0;
@@ -146,30 +150,40 @@ namespace armarx
                 // static methods
                 static AronListSerializerTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n)
                 {
-                    CheckTypeNavigatorPtrForNull("AronListSerializerTypeNavigator", "DynamicCast[Before]", n);
                     AronListSerializerTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronListSerializerTypeNavigator>(n);
-                    CheckTypeNavigatorPtrForNull("AronListSerializerTypeNavigator", "DynamicCast[After]", casted);
                     return casted;
                 }
             };
 
 
+            class AronComplexTypeNavigator;
+            typedef std::shared_ptr<AronComplexTypeNavigator> AronComplexTypeNavigatorPtr;
+
+            class AronComplexTypeNavigator :
+                virtual public AronTypeNavigator
+            {
+            public:
+                using PointerType = AronComplexTypeNavigatorPtr;
+
+                // static methods
+                static AronComplexTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n)
+                {
+                    AronComplexTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronComplexTypeNavigator>(n);
+                    return casted;
+                }
+            };
+
 
             class AronNDArraySerializerTypeNavigator;
             typedef std::shared_ptr<AronNDArraySerializerTypeNavigator> AronNDArraySerializerTypeNavigatorPtr;
 
             class AronNDArraySerializerTypeNavigator :
-                virtual public AronTypeNavigator
+                virtual public AronComplexTypeNavigator
             {
             public:
                 using PointerType = AronNDArraySerializerTypeNavigatorPtr;
 
             public:
-                AronNDArraySerializerTypeNavigator() = delete;
-                AronNDArraySerializerTypeNavigator(const AronTypeDescriptor& n, const AronPath& path) :
-                    AronTypeNavigator(n, path)
-                {}
-
                 // virtual methods
                 virtual void setDimensions(const std::vector<int>&) = 0;
                 virtual void setUsedType(const std::string&) = 0;
@@ -180,9 +194,25 @@ namespace armarx
                 // static methods
                 static AronNDArraySerializerTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n)
                 {
-                    CheckTypeNavigatorPtrForNull("AronNDArraySerializerTypeNavigator", "DynamicCast[Before]", n);
                     AronNDArraySerializerTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronNDArraySerializerTypeNavigator>(n);
-                    CheckTypeNavigatorPtrForNull("AronNDArraySerializerTypeNavigator", "DynamicCast[After]", casted);
+                    return casted;
+                }
+            };
+
+
+            class AronPrimitiveTypeNavigator;
+            typedef std::shared_ptr<AronPrimitiveTypeNavigator> AronPrimitiveTypeNavigatorPtr;
+
+            class AronPrimitiveTypeNavigator :
+                virtual public AronTypeNavigator
+            {
+            public:
+                using PointerType = AronPrimitiveTypeNavigatorPtr;
+
+                // static methods
+                static AronPrimitiveTypeNavigatorPtr DynamicCast(const AronTypeNavigatorPtr& n)
+                {
+                    AronPrimitiveTypeNavigatorPtr casted = std::dynamic_pointer_cast<AronPrimitiveTypeNavigator>(n);
                     return casted;
                 }
             };
diff --git a/source/RobotAPI/libraries/aron/aroncore/test/aronDataWithoutCodeGeneration.h b/source/RobotAPI/libraries/aron/aroncore/test/aronDataWithoutCodeGeneration.h
index ce409513fde1421437cd165b09c9db6d4b9e7209..67fd71120c0946be7d21c4df0229b78ec90eff6f 100644
--- a/source/RobotAPI/libraries/aron/aroncore/test/aronDataWithoutCodeGeneration.h
+++ b/source/RobotAPI/libraries/aron/aroncore/test/aronDataWithoutCodeGeneration.h
@@ -59,24 +59,24 @@ namespace armarx
             w.readEndDict();
         }
 
-        armarx::aron::datanavigator::AronDataNavigatorPtr toAron() const
+        armarx::aron::datanavigator::AronDictDataNavigatorPtr toAron() const
         {
             armarx::aron::io::AronDataNavigatorWriter writer;
             this->write(writer);
-            return writer.getResult();
+            return armarx::aron::datanavigator::AronDictDataNavigator::DynamicCast(writer.getResult());
         }
 
-        void fromAron(const armarx::aron::datanavigator::AronDataNavigatorPtr& input)
+        void fromAron(const armarx::aron::datanavigator::AronDictDataNavigatorPtr& input)
         {
             armarx::aron::io::AronDataNavigatorReader reader(input);
             //this->read(reader);
         }
 
-        armarx::aron::typenavigator::AronTypeNavigatorPtr toInitialAronType() const
+        armarx::aron::typenavigator::AronObjectTypeNavigatorPtr toInitialAronType() const
         {
             armarx::aron::io::AronTypeNavigatorWriter writer;
             this->writeType(writer);
-            return writer.getResult();
+            return armarx::aron::typenavigator::AronObjectTypeNavigator::DynamicCast(writer.getResult());
         }
     };
 
@@ -144,24 +144,24 @@ namespace armarx
         }
 
 
-        armarx::aron::datanavigator::AronDataNavigatorPtr toAron() const
+        armarx::aron::datanavigator::AronDictDataNavigatorPtr toAron() const
         {
             armarx::aron::io::AronDataNavigatorWriter writer;
             this->write(writer);
-            return writer.getResult();
+            return armarx::aron::datanavigator::AronDictDataNavigator::DynamicCast(writer.getResult());
         }
 
-        void fromAron(const armarx::aron::datanavigator::AronDataNavigatorPtr& input)
+        void fromAron(const armarx::aron::datanavigator::AronDictDataNavigatorPtr& input)
         {
             //armarx::aron::io::AronDataNavigatorReader reader(input);
             //this->read(reader);
         }
 
-        armarx::aron::typenavigator::AronTypeNavigatorPtr toInitialAronType() const
+        armarx::aron::typenavigator::AronObjectTypeNavigatorPtr toInitialAronType() const
         {
             armarx::aron::io::AronTypeNavigatorWriter writer;
             this->writeType(writer);
-            return writer.getResult();
+            return armarx::aron::typenavigator::AronObjectTypeNavigator::DynamicCast(writer.getResult());
         }
     };
 
@@ -267,24 +267,24 @@ namespace armarx
             w.readEndDict();
         }
 
-        armarx::aron::datanavigator::AronDataNavigatorPtr toAron() const
+        armarx::aron::datanavigator::AronDictDataNavigatorPtr toAron() const
         {
             armarx::aron::io::AronDataNavigatorWriter writer;
             this->write(writer);
-            return writer.getResult();
+            return armarx::aron::datanavigator::AronDictDataNavigator::DynamicCast(writer.getResult());
         }
 
-        void fromAron(const armarx::aron::datanavigator::AronDataNavigatorPtr& input)
+        void fromAron(const armarx::aron::datanavigator::AronDictDataNavigatorPtr& input)
         {
             armarx::aron::io::AronDataNavigatorReader reader(input);
             //this->read(reader);
         }
 
-        armarx::aron::typenavigator::AronTypeNavigatorPtr toInitialAronType() const
+        armarx::aron::typenavigator::AronObjectTypeNavigatorPtr toInitialAronType() const
         {
             armarx::aron::io::AronTypeNavigatorWriter writer;
             this->writeType(writer);
-            return writer.getResult();
+            return armarx::aron::typenavigator::AronObjectTypeNavigator::DynamicCast(writer.getResult());
         }
 
     };
diff --git a/source/RobotAPI/libraries/aron/aroncore/test/aronTest.cpp b/source/RobotAPI/libraries/aron/aroncore/test/aronTest.cpp
index d3f1fd199144d11539d294b73cd8d1ea656a70b4..14bd92cebc7f196c966c4d6c70643c5fae12896f 100644
--- a/source/RobotAPI/libraries/aron/aroncore/test/aronTest.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/test/aronTest.cpp
@@ -28,6 +28,7 @@
 #include <iostream>
 #include <cstdlib>
 #include <ctime>
+#include <numeric>
 
 // IVT
 #include <Image/ByteImage.h>
@@ -115,310 +116,281 @@ std::vector<unsigned char> generateRandomBlob(unsigned int size)
     return new_blob;
 }
 
-data::AronDataPtr generateAronDataFromType(const type::AronTypePtr& type)
+datanavigator::AronDataNavigatorPtr generateAronDataFromType(const typenavigator::AronTypeNavigatorPtr& type)
 {
+    BOOST_CHECK_NE(type, nullptr);
     BOOST_CHECK_NE(type.get(), nullptr);
 
+    const AronTypeDescriptor desc = type->getDescriptor();
+    switch (desc)
     {
-        type::AronObjectTypePtr t = type::AronObjectTypePtr::dynamicCast(type);
-        if (t)
-        {
-            data::AronDictPtr d = data::AronDictPtr(new data::AronDict());
-            for (const auto& [k, tt] : t->elementTypes)
-            {
-                d->elements[k] = generateAronDataFromType(tt);
-            }
-            return d;
-        }
-    }
+    case eAronObjectType:
     {
-        type::AronDictTypePtr t = type::AronDictTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronDict();
-        }
-    }
-    {
-        type::AronListTypePtr t = type::AronListTypePtr::dynamicCast(type);
-        if (t)
+        typenavigator::AronObjectTypeNavigatorPtr t = typenavigator::AronObjectTypeNavigator::DynamicCast(type);
+        BOOST_CHECK_NE(t, nullptr);
+        BOOST_CHECK_NE(t.get(), nullptr);
+
+        datanavigator::AronDictDataNavigatorPtr d = datanavigator::AronDictDataNavigatorPtr(new datanavigator::AronDictDataNavigator());
+        for (const auto& [k, tt] : t->getAcceptedTypes())
         {
-            return new data::AronList();
+            d->addElement(k, generateAronDataFromType(tt));
         }
+        return d;
     }
+    case eAronDictType:
     {
-        type::AronTupleTypePtr t = type::AronTupleTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronList();
-        }
+        typenavigator::AronDictTypeNavigatorPtr t = typenavigator::AronDictTypeNavigator::DynamicCast(type);
+        BOOST_CHECK_NE(t, nullptr);
+        BOOST_CHECK_NE(t.get(), nullptr);
+
+        return datanavigator::AronDataNavigatorPtr(new datanavigator::AronDictDataNavigator());
     }
-    {
-        type::AronEigenMatrixTypePtr t = type::AronEigenMatrixTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronNDArray();
-        }
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+    case eAron##upperType##Type: \
+    { \
+        typenavigator::Aron##upperType##TypeNavigatorPtr t = typenavigator::Aron##upperType##TypeNavigator::DynamicCast(type); \
+        BOOST_CHECK_NE(t, nullptr); \
+        BOOST_CHECK_NE(t.get(), nullptr); \
+        \
+        return datanavigator::AronDataNavigatorPtr(new datanavigator::AronListDataNavigator()); \
     }
-    {
-        type::AronIntTypePtr t = type::AronIntTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronInt();
-        }
-    }
-    {
-        type::AronLongTypePtr t = type::AronLongTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronLong();
-        }
+
+            HANDLE_LIST_SERIALIZER_TYPES
+#undef RUN_ARON_MACRO
+
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+    case eAron##upperType##Type: \
+    { \
+        typenavigator::Aron##upperType##TypeNavigatorPtr t = typenavigator::Aron##upperType##TypeNavigator::DynamicCast(type); \
+        BOOST_CHECK_NE(t, nullptr); \
+        BOOST_CHECK_NE(t.get(), nullptr); \
+        \
+        return datanavigator::AronDataNavigatorPtr(new datanavigator::AronNDArrayDataNavigator()); \
     }
-    {
-        type::AronFloatTypePtr t = type::AronFloatTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronFloat();
-        }
+
+            HANDLE_COMPLEX_TYPES
+#undef RUN_ARON_MACRO
+
+#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
+    case eAron##upperType##Type: \
+    { \
+        typenavigator::Aron##upperType##TypeNavigatorPtr t = typenavigator::Aron##upperType##TypeNavigator::DynamicCast(type); \
+        BOOST_CHECK_NE(t, nullptr); \
+        BOOST_CHECK_NE(t.get(), nullptr); \
+        \
+        return datanavigator::AronDataNavigatorPtr(new datanavigator::Aron##upperType##DataNavigator()); \
     }
+
+            HANDLE_PRIMITIVE_TYPES
+#undef RUN_ARON_MACRO
+    default:
     {
-        type::AronDoubleTypePtr t = type::AronDoubleTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronDouble();
-        }
+        throw armarx::aron::exception::AronTypeDescriptorNotValidException("AronTest", "generateAronDataFromType", "No valid type found!", desc);
     }
-    {
-        type::AronStringTypePtr t = type::AronStringTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronString();
-        }
     }
-    {
-        type::AronBoolTypePtr t = type::AronBoolTypePtr::dynamicCast(type);
-        if (t)
-        {
-            return new data::AronBool();
-        }
-    }
-    throw armarx::aron::exception::AronTypeNotValidException("AronTest", "generateAronDataFromType", "No valid type found!", type);
 }
 
-void initializeRandomly(data::AronDataPtr& data, const type::AronTypePtr& type)
+// Prototype recursion
+void initializeRandomly(datanavigator::AronDataNavigatorPtr& data, const typenavigator::AronTypeNavigatorPtr& type);
+
+void initializeRandomly(datanavigator::AronDictDataNavigatorPtr& data, const typenavigator::AronObjectTypeNavigatorPtr& type)
 {
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
     BOOST_CHECK_NE(data.get(), nullptr);
     BOOST_CHECK_NE(type.get(), nullptr);
 
-    // Containers
+    for (auto& [key, nextData] : data->getElements())
     {
-        type::AronObjectTypePtr objectType = type::AronObjectTypePtr::dynamicCast(type);
-        if (objectType)
-        {
-            data::AronDictPtr dict = data::AronDictPtr::dynamicCast(data);
-            BOOST_CHECK_NE(dict.get(), nullptr);
-            for (auto& [key, nextData] : dict->elements)
-            {
-                initializeRandomly(nextData, objectType->elementTypes[key]);
-            }
-            return;
-        }
+        initializeRandomly(nextData, type->getAcceptedTypes()[key]);
     }
+}
 
+void initializeRandomly(datanavigator::AronDictDataNavigatorPtr& data, const typenavigator::AronDictTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
+
+    data->clear();
+    int numElements = generateRandom(5, 1);
+    std::set<std::string> usedKeys;
+    for (int i = 0; i < numElements; ++i)
     {
-        type::AronListTypePtr listType = type::AronListTypePtr::dynamicCast(type);
-        if (listType)
+        std::string key = generateRandomWord();
+        while (usedKeys.count(key) > 0)
         {
-            data::AronListPtr list = data::AronListPtr::dynamicCast(data);
-            BOOST_CHECK_NE(list.get(), nullptr);
-
-            list->elements.clear();
-            int numElements = generateRandom(5, 1);
-            for (int i = 0; i < numElements; ++i)
-            {
-                data::AronDataPtr newData = generateAronDataFromType(listType->acceptedType);
-                initializeRandomly(newData, listType->acceptedType);
-                list->elements.push_back(newData);
-            }
-            return;
+            key = generateRandomWord();
         }
+        usedKeys.insert(key);
+        datanavigator::AronDataNavigatorPtr newData = generateAronDataFromType(type->getAcceptedType());
+        initializeRandomly(newData, type->getAcceptedType());
+        data->addElement(key, newData);
     }
+}
+
+void initializeRandomly(datanavigator::AronListDataNavigatorPtr& data, const typenavigator::AronTupleTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
+    unsigned int i = 0;
+    for (auto& nextData : data->getElements())
     {
-        type::AronDictTypePtr dictType = type::AronDictTypePtr::dynamicCast(type);
-        if (dictType)
-        {
-            data::AronDictPtr dict = data::AronDictPtr::dynamicCast(data);
-            BOOST_CHECK_NE(dict.get(), nullptr);
-
-            dict->elements.clear();
-            int numElements = generateRandom(5, 1);
-            std::cout << "Generating " << numElements << " elements" << std::endl;
-            std::set<std::string> usedKeys;
-            for (int i = 0; i < numElements; ++i)
-            {
-                std::string key = generateRandomWord();
-                while (usedKeys.count(key) > 0)
-                {
-                    key = generateRandomWord();
-                }
-                usedKeys.insert(key);
-                data::AronDataPtr newData = generateAronDataFromType(dictType->acceptedType);
-                initializeRandomly(newData, dictType->acceptedType);
-                dict->elements[key] = newData;
-            }
-            return;
-        }
+        initializeRandomly(nextData, type->getAcceptedTypes()[i++]);
     }
+}
+
+void initializeRandomly(datanavigator::AronListDataNavigatorPtr& data, const typenavigator::AronListTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
+    data->clear();
+    int numElements = generateRandom(5, 1);
+    for (int i = 0; i < numElements; ++i)
     {
-        type::AronTupleTypePtr tupleType = type::AronTupleTypePtr::dynamicCast(type);
-        if (tupleType)
-        {
-            data::AronListPtr list = data::AronListPtr::dynamicCast(data);
-            BOOST_CHECK_NE(list.get(), nullptr);
-            BOOST_CHECK_EQUAL(list->elements.size(), tupleType->elementTypes.size());
-
-            int i = 0;
-            for (auto& nextData : list->elements)
-            {
-                initializeRandomly(nextData, tupleType->elementTypes[i++]);
-            }
-            return;
-        }
+        datanavigator::AronDataNavigatorPtr newData = generateAronDataFromType(type->getAcceptedType());
+        initializeRandomly(newData, type->getAcceptedType());
+        data->addElement(newData);
     }
+}
 
-    // Complex
-    {
-        type::AronEigenMatrixTypePtr eigenMatrixType = type::AronEigenMatrixTypePtr::dynamicCast(type);
-        if (eigenMatrixType)
-        {
-            data::AronNDArrayPtr blob = data::AronNDArrayPtr::dynamicCast(data);
-            BOOST_CHECK_NE(blob.get(), nullptr);
-            return;
-        }
+void initializeRandomly(datanavigator::AronNDArrayDataNavigatorPtr& data, const typenavigator::AronNDArraySerializerTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
-        type::AronIVTCByteImageTypePtr ivtCByteImageType = type::AronIVTCByteImageTypePtr::dynamicCast(type);
-        if (ivtCByteImageType)
-        {
-            data::AronNDArrayPtr blob = data::AronNDArrayPtr::dynamicCast(data);
-            BOOST_CHECK_NE(blob.get(), nullptr);
+    std::vector<int> dims = data->getDimensions();
+    int bytes = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+    std::vector<unsigned char> blob = generateRandomBlob(bytes);
+    data->setData(bytes, blob.data());
+}
 
-            blob->data = generateRandomBlob(blob->data.size());
-            return;
-        }
+void initializeRandomly(datanavigator::AronIntDataNavigatorPtr& data, const typenavigator::AronPrimitiveTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
-        type::AronOpenCVMatTypePtr openCVMatType = type::AronOpenCVMatTypePtr::dynamicCast(type);
-        if (openCVMatType)
-        {
-            data::AronNDArrayPtr blob = data::AronNDArrayPtr::dynamicCast(data);
-            BOOST_CHECK_NE(blob.get(), nullptr);
+    data->setValue(generateRandom(32, -32));
+}
 
-            blob->data = generateRandomBlob(blob->data.size());
-            return;
-        }
+void initializeRandomly(datanavigator::AronLongDataNavigatorPtr& data, const typenavigator::AronPrimitiveTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
-        type::AronPCLPointCloudTypePtr pclPointCloudType = type::AronPCLPointCloudTypePtr::dynamicCast(type);
-        if (pclPointCloudType)
-        {
-            data::AronNDArrayPtr blob = data::AronNDArrayPtr::dynamicCast(data);
-            BOOST_CHECK_NE(blob.get(), nullptr);
+    data->setValue(generateRandom(32, -32));
+}
 
-            blob->data = generateRandomBlob(blob->data.size());
-            return;
-        }
-    }
+void initializeRandomly(datanavigator::AronFloatDataNavigatorPtr& data, const typenavigator::AronPrimitiveTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
+    data->setValue(generateRandom(32, -32));
+}
 
-    // Primitives
-    {
-        type::AronIntTypePtr primitiveType = type::AronIntTypePtr::dynamicCast(type);
-        if (primitiveType)
-        {
-            data::AronIntPtr primitiveData = data::AronIntPtr::dynamicCast(data);
-            BOOST_CHECK_NE(primitiveData.get(), nullptr);
+void initializeRandomly(datanavigator::AronDoubleDataNavigatorPtr& data, const typenavigator::AronPrimitiveTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
-            primitiveData->value = generateRandom(32, -32);
-            return;
-        }
-    }
-    {
-        type::AronLongTypePtr primitiveType = type::AronLongTypePtr::dynamicCast(type);
-        if (primitiveType)
-        {
-            data::AronLongPtr primitiveData = data::AronLongPtr::dynamicCast(data);
-            BOOST_CHECK_NE(primitiveData.get(), nullptr);
+    data->setValue(generateRandom(32, -32));
+}
 
-            primitiveData->value = generateRandom(32, -32);
-            return;
-        }
-    }
-    {
-        type::AronFloatTypePtr primitiveType = type::AronFloatTypePtr::dynamicCast(type);
-        if (primitiveType)
-        {
-            data::AronFloatPtr primitiveData = data::AronFloatPtr::dynamicCast(data);
-            BOOST_CHECK_NE(primitiveData.get(), nullptr);
+void initializeRandomly(datanavigator::AronBoolDataNavigatorPtr& data, const typenavigator::AronPrimitiveTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
 
-            primitiveData->value = generateRandom(32, -32);
-            return;
-        }
-    }
-    {
-        type::AronDoubleTypePtr primitiveType = type::AronDoubleTypePtr::dynamicCast(type);
-        if (primitiveType)
-        {
-            data::AronDoublePtr primitiveData = data::AronDoublePtr::dynamicCast(data);
-            BOOST_CHECK_NE(primitiveData.get(), nullptr);
+    data->setValue(generateRandom(1, 0));
+}
 
-            primitiveData->value = generateRandom(32, -32);
-            return;
-        }
-    }
+void initializeRandomly(datanavigator::AronStringDataNavigatorPtr& data, const typenavigator::AronPrimitiveTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
+
+    data->setValue(generateRandomWord());
+}
+
+void initializeRandomly(datanavigator::AronDataNavigatorPtr& data, const typenavigator::AronTypeNavigatorPtr& type)
+{
+    BOOST_CHECK_NE(data, nullptr);
+    BOOST_CHECK_NE(type, nullptr);
+    BOOST_CHECK_NE(data.get(), nullptr);
+    BOOST_CHECK_NE(type.get(), nullptr);
+
+    // Containers
+    AronTypeDescriptor desc = type->getDescriptor();
+    switch (desc)
     {
-        type::AronStringTypePtr primitiveType = type::AronStringTypePtr::dynamicCast(type);
-        if (primitiveType)
-        {
-            data::AronStringPtr primitiveData = data::AronStringPtr::dynamicCast(data);
-            BOOST_CHECK_NE(primitiveData.get(), nullptr);
 
-            primitiveData->value = generateRandomWord();
-            return;
-        }
+#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
+    case eAron##upperType##Type: \
+    { \
+        typenavigator::Aron##upperType##TypeNavigatorPtr t = typenavigator::Aron##upperType##TypeNavigator::DynamicCast(type); \
+        BOOST_CHECK_NE(t, nullptr); \
+        BOOST_CHECK_NE(t.get(), nullptr); \
+        \
+        datanavigator::Aron##upperData##DataNavigatorPtr d = datanavigator::Aron##upperData##DataNavigator::DynamicCast(data); \
+        BOOST_CHECK_NE(d, nullptr); \
+        BOOST_CHECK_NE(d.get(), nullptr); \
+        \
+        initializeRandomly(d, t); \
+        break; \
     }
-    {
-        type::AronBoolTypePtr primitiveType = type::AronBoolTypePtr::dynamicCast(type);
-        if (primitiveType)
-        {
-            data::AronBoolPtr primitiveData = data::AronBoolPtr::dynamicCast(data);
-            BOOST_CHECK_NE(primitiveData.get(), nullptr);
 
-            primitiveData->value = generateRandom(1, 0);
-            return;
-        }
+            HANDLE_ALL_CORRESPONDING
+#undef RUN_ARON_MACRO
+
+    default:
+    {
+        throw armarx::aron::exception::AronTypeDescriptorNotValidException("AronTest", "initializeRandomly", "No valid type found!", desc);
+    }
     }
-    throw armarx::aron::exception::AronTypeNotValidException("AronTest", "initializeRandomly", "No valid type found!", type);
 }
 
 template <typename T>
 void runTestWithInstances(T& k1, T& k2)
 {
     std::cout << "\t getting type 1" << std::endl;
-    typenavigator::AronTypeNavigatorPtr k1_type_nav = k1.toInitialAronType();
-    type::AronTypePtr k1_type = k1_type_nav->getResult();
+    typenavigator::AronObjectTypeNavigatorPtr k1_type_nav = k1.toInitialAronType();
+    type::AronObjectTypePtr k1_type = k1_type_nav->getCastedResult();
     BOOST_CHECK_NE(k1_type.get(), nullptr);
 
     std::cout << "\t getting type 2" << std::endl;
-    typenavigator::AronTypeNavigatorPtr k2_type_nav = k2.toInitialAronType();
-    type::AronTypePtr k2_type = k2_type_nav->getResult();
+    typenavigator::AronObjectTypeNavigatorPtr k2_type_nav = k2.toInitialAronType();
+    type::AronObjectTypePtr k2_type = k2_type_nav->getCastedResult();
     BOOST_CHECK_NE(k2_type.get(), nullptr);
 
     std::cout << "\t getting aron 1" << std::endl;
-    datanavigator::AronDataNavigatorPtr k1_aron_nav = k1.toAron();
-    data::AronDataPtr k1_aron = k1_aron_nav->getResult();
+    datanavigator::AronDictDataNavigatorPtr k1_aron_nav = k1.toAron();
+    data::AronDictPtr k1_aron = k1_aron_nav->getCastedResult();
     BOOST_CHECK_NE(k1_aron.get(), nullptr);
 
     std::cout << "\t initialize aron 1 randomly" << std::endl;
-    initializeRandomly(k1_aron, k1_type);
+    initializeRandomly(k1_aron_nav, k1_type_nav);
 
     //std::cout << "K Aron:" << std::endl;
     //std::cout << AronDebug::AronDataPtrToString(k_aron) << std::endl;
@@ -427,8 +399,8 @@ void runTestWithInstances(T& k1, T& k2)
     k2.fromAron(k1_aron);
 
     std::cout << "\t getting aron 2" << std::endl;
-    datanavigator::AronDataNavigatorPtr k2_aron_nav = k2.toAron();
-    data::AronDataPtr k2_aron = k2_aron_nav->getResult();
+    datanavigator::AronDictDataNavigatorPtr k2_aron_nav = k2.toAron();
+    data::AronDictPtr k2_aron = k2_aron_nav->getCastedResult();
     BOOST_CHECK_NE(k2_aron.get(), nullptr);
 
     //std::cout << "K2 Aron:" << std::endl;
@@ -449,7 +421,7 @@ void runTestWithInstances(T& k1, T& k2)
     //type::AronTypePtr k2_current_type = k2_current_type_nav->getResult();
     //BOOST_CHECK_NE(k2_current_type.get(), nullptr);
 
-    initializeRandomly(k1_aron, k1_type);
+    initializeRandomly(k1_aron_nav, k1_type_nav);
     k1.fromAron(k1_aron);
 
     std::cout << "\t check JSON export of k and k2 for equality" << std::endl;
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 7cc1ec24e04d0eee40c77fb5dc705b3a86187531..0bca83408ff4831cd028c664aed9c20f7e056c1f 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -346,7 +346,7 @@ namespace armarx
 
 void armarx::fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector)
 {
-    vector = fromIce(ice );
+    vector = fromIce(ice);
 }
 
 void armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion)