diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
index 2f2bd887a7dfe5776b66892198bc719725bec3c0..68512914578343ac958a7ed4377b427a04357603 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
@@ -23,7 +23,8 @@
 #pragma once
 
 #include "ControlTargetBase.h"
-#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
 
 namespace armarx
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index 011b0c4283ae180a49cdc8ce3a80c43829eb067a..d997104446ffea9aec162cd559541acf78e8def8 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -22,20 +22,23 @@
 
 #pragma once
 
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
+#include <RobotAPI/components/units/RobotUnit//util/HeterogenousContinuousContainerMacros.h>
+#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
+
+//#include <ArmarXCore/interface/observers/VariantBase.h>
+
+#include <Ice/Handle.h>
+
 #include <memory>
 #include <string>
 #include <map>
 
-#include <ArmarXCore/observers/variant/TimedVariant.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-
-#include "../util.h"
-#include "../Constants.h"
-#include "../ControlModes.h"
-#include "../util/HeterogenousContinuousContainerMacros.h"
 
 namespace armarx
 {
+    typedef ::IceInternal::Handle<class VariantBase> VariantBasePtr;
+
     /**
     * @class ControlTargetBase
     * @ingroup Library-RobotUnit
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
index 04b537b8545c57b162ff67f99735d4c21fa095b4..cad312b44637034c4c254a8b300e00c42cbb09fb 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
@@ -23,7 +23,8 @@
 #pragma once
 
 #include "ControlTargetBase.h"
-#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
 
 namespace armarx
 {
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
index b4e2a00f3ca0439e70b6a9606f0a3ef3527922ac..869ad5163e781a218795dcedf119f1b096be9ccb 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
@@ -22,17 +22,24 @@
 
 #pragma once
 
-#include "../ControlTargets/ControlTargetBase.h"
-#include "../util.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
+
+#include <Ice/ProxyHandle.h>
 
 #include <memory>
 #include <atomic>
 
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+namespace IceProxy::armarx
+{
+    class DebugDrawerInterface;
+    class DebugObserverInterface;
+}
 
 namespace armarx
 {
     class ControlDevice;
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
+    typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
 
     /**
      * @brief The JointController class represents one joint in one control mode.
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 44c1df4514e42b5f62c8548843971e0b9b7b4ce5..17bdc45e13085d67407d061256319f526e4355c1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -24,4 +24,10 @@
 
 namespace armarx
 {
+
+RobotUnit::~RobotUnit()
+{
+
+}
+
 }
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 6024c0dbbf0a48509afc455922a1dab9ec8103d0..c2b35013a66e792a6fef2a2e02f6beef429e1683 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -187,6 +187,8 @@ namespace armarx
         virtual public RobotUnitModule::ControlThreadDataBuffer
     {
     public:
+        ~RobotUnit();
+
         static RobotUnit& Instance()
         {
             return ModuleBase::Instance<RobotUnit>();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 871d8427eb59e673ec1070c7aafda9fdcad2c2b9..1f6215664d073ca1c329599b8ff8ecb58f2d5272 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -771,6 +771,11 @@ namespace armarx::RobotUnitModule
         nJointControllers.clear();
     }
 
+    ControllerManagement::~ControllerManagement()
+    {
+
+    }
+
     void ControllerManagement::_preOnInitRobotUnit()
     {
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
index 17b5a00e9b28a8e8d2dec03eea1c6aec9b70cdd9..3317540bc5e9adf326ba8410d06df2576d907ec3 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -48,6 +48,9 @@ namespace armarx::RobotUnitModule
         {
             return ModuleBase::Instance<ControllerManagement>();
         }
+
+        ~ControllerManagement();
+
         // //////////////////////////////////////////////////////////////////////////////////////// //
         // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
         // //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index eff567f2748ab7c333deb43537f37a22e961de79..b9649773942d2c539cad4f1b38b03dc4a78d3f40 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -21,10 +21,11 @@
  */
 #pragma once
 
-#include <ArmarXCore/core/util/algorithm.h>
-
 #include "ClassMemberInfoEntry.h"
 
+#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
+
+
 namespace armarx::introspection
 {
     /**
@@ -168,7 +169,14 @@ namespace armarx::introspection
         std::map<std::string, VariantBasePtr> result;
         for (const auto& e : GetEntries().values())
         {
-            mergeMaps(result, e.toVariants(timestamp, ptr), MergeMapsMode::OverrideNoValues);
+            for (auto& elem : e.toVariants(timestamp, ptr))
+            {
+                if (result.count(elem.first))
+                {
+                    throw std::invalid_argument {"mergeMaps: newMap would override values from oldMap"};
+                }
+            }
+            //mergeMaps(result, , MergeMapsMode::OverrideNoValues);
         }
         return result;
     }
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 6188648d33e64d86dbac8a168446bbe5127e6339..1ad242babb61a5f2970b641d0ac3bbb09c338ba1 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -22,6 +22,7 @@
 #pragma once
 
 #include "DataFieldsInfo.h"
+
 namespace armarx::introspection
 {