diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index 0afaed33a07d95b74c2325e6fd7dfa64e018938b..08b7d20ff5440d018e2c39604ab00dd7b4918f23 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -28,8 +28,6 @@
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 
-#include <boost/circular_buffer.hpp>
-
 #include <mutex>
 #include <atomic>
 #include <condition_variable>
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index d9beef41b45235611beec0b6d02bc66fadcc4178..f11c7aa9de6672cafcda2bbcfdb2e5114eb2be69 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -1,13 +1,12 @@
 #include "Visualizer.h"
 
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
 
 #include <Inventor/sensors/SoTimerSensor.h>
 #include <Inventor/nodes/SoUnits.h>
 #include <thread>
 
-#include <boost/core/demangle.hpp>
-
 #include "VisualizationRobot.h"
 
 namespace armarx::viz
@@ -168,7 +167,7 @@ namespace armarx::viz
             {
                 ARMARX_WARNING << deactivateSpam(1)
                                << "No visualizer for element type found: "
-                               << boost::core::demangle(elementType.name());
+                               << armarx::GetTypeString(elementType);
                 continue;
             }
             coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
@@ -216,7 +215,7 @@ namespace armarx::viz
             }
             else
             {
-                std::string typeName = boost::core::demangle(elementType.name());
+                std::string typeName = armarx::GetTypeString(elementType);
                 ARMARX_WARNING << deactivateSpam(typeName, 1)
                                << "CoinElementVisualizer returned null for type: " << typeName << "\n"
                                << "You need to register a visualizer for each type in ArViz/Coin/Visualizer.cpp";
diff --git a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
index a68eb49d875c6d8eece619f72e2b6f64bd82a08a..96b0f863c87245ddf16d84c983e08407f39e3331 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
@@ -56,7 +56,7 @@ namespace armarx
 
     void CyberGloveObserver::reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         IceUtil::Time now = TimeUtil::GetTime();
 
@@ -107,7 +107,7 @@ namespace armarx
 
     CyberGloveValues armarx::CyberGloveObserver::getLatestValues(const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         return latestValues;
     }
 }
diff --git a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
index 8eb91e4654620598605f4b04fc1f4c8629ef0048..45eb2f52a2c86c47c8c0272a6f2c55ce9c39b865 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
@@ -23,13 +23,12 @@
 #pragma once
 
 
-//#include <ArmarXCore/core/Component.h>
-//#include <RobotAPI/interface/units/CyberGloveInterface.h>
 #include <RobotAPI/interface/units/CyberGloveObserverInterface.h>
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -103,7 +102,7 @@ namespace armarx
 
     private:
 
-        Mutex dataMutex;
+        std::mutex dataMutex;
         CyberGloveValues latestValues;
         IceUtil::Time lastUpdate;
 
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index eacd69f2afe16f464dc4486665e7c4e6674a7147..b59531d4c840f9d7bbfb773f2a5da361bedbcf39 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -56,7 +56,7 @@
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 #include <boost/algorithm/string/predicate.hpp>
-#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string/replace.hpp>
 
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/XML/RobotIO.h>
@@ -66,13 +66,13 @@
 
 using namespace VirtualRobot;
 
-#define SELECTION_NAME_PREFIX   ("selection_" + boost::lexical_cast<std::string>(this))
+#define SELECTION_NAME_PREFIX   ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
 #define SELECTION_NAME_SPLITTER "____"
 #define SELECTION_NAME(layerName, elementName)   boost::replace_all_copy(std::string(SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName), " ", "_").c_str()
 
 namespace armarx
 {
-    boost::recursive_mutex DebugDrawerComponent::selectionMutex;
+    std::recursive_mutex DebugDrawerComponent::selectionMutex;
 
     void selection_callback(void* userdata, SoPath* path)
     {
@@ -124,7 +124,7 @@ namespace armarx
 
     void DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         ARMARX_INFO << "Enabling selections for layer " << layerName;
         selectableLayers.insert(layerName);
@@ -132,7 +132,7 @@ namespace armarx
 
     void DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         ARMARX_INFO << "Disabling selections for layer " << layerName;
         if (layerName == "")
@@ -147,8 +147,8 @@ namespace armarx
 
     void DebugDrawerComponent::clearSelections(const std::string& layerName, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
-        ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+        auto l = getScopedVisuLock();
+        std::unique_lock l2(selectionMutex);
 
         removeSelectionCallbacks();
 
@@ -174,8 +174,8 @@ namespace armarx
 
     void DebugDrawerComponent::select(const std::string& layerName, const std::string& elementName, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
-        ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+        auto l = getScopedVisuLock();
+        std::unique_lock l2(selectionMutex);
 
         removeSelectionCallbacks();
 
@@ -199,8 +199,8 @@ namespace armarx
 
     void DebugDrawerComponent::deselect(const std::string& layerName, const std::string& elementName, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
-        ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+        auto l = getScopedVisuLock();
+        std::unique_lock l2(selectionMutex);
 
         removeSelectionCallbacks();
 
@@ -224,34 +224,34 @@ namespace armarx
 
     void DebugDrawerComponent::selectionCallback()
     {
-        ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*topicMutex));
+        std::unique_lock l(*topicMutex);
         listenerPrx->reportSelectionChanged(getSelections());
     }
 
     void DebugDrawerComponent::deselectionCallback()
     {
-        ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*topicMutex));
+        std::unique_lock l(*topicMutex);
         listenerPrx->reportSelectionChanged(getSelections());
     }
 
     void DebugDrawerComponent::installSelectionCallbacks()
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         selectionNode->addSelectionCallback(selection_callback, this);
         selectionNode->addDeselectionCallback(deselection_callback, this);
     }
 
     void DebugDrawerComponent::removeSelectionCallbacks()
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         selectionNode->removeSelectionCallback(selection_callback, this);
         selectionNode->removeDeselectionCallback(deselection_callback, this);
     }
 
     void DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
-        ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+        auto l = getScopedVisuLock();
+        std::unique_lock l2(selectionMutex);
 
         removeSelectionCallbacks();
         selectionNode->deselectAll();
@@ -273,7 +273,7 @@ namespace armarx
 
     DebugDrawerSelectionList DebugDrawerComponent::getSelections(const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         DebugDrawerSelectionList selectedElements;
 
@@ -344,7 +344,7 @@ namespace armarx
     DebugDrawerComponent::~DebugDrawerComponent()
     {
         /*{
-                ScopedRecursiveLockPtr l = getScopedLock();
+                auto l = getScopedLock();
                 layerMainNode->unref();
                 coinVisu->unref();
             }*/
@@ -387,7 +387,7 @@ namespace armarx
             return;
         }
 
-        ScopedLock lock(drawer->timerSensorMutex);
+        std::unique_lock lock(drawer->timerSensorMutex);
         drawer->updateVisualization();
     }
 
@@ -412,7 +412,7 @@ namespace armarx
                 counter++; // sec counter
             }
             {
-                ScopedRecursiveLockPtr l = getScopedLock();
+                auto l = getScopedLock();
                 coinVisu->removeAllChildren();
             }*/
     }
@@ -425,7 +425,7 @@ namespace armarx
         //ARMARX_DEBUG << "onExitComponent";
         if (timerSensor)
         {
-            ScopedLock lock(timerSensorMutex);
+            std::unique_lock lock(timerSensorMutex);
             SoSensorManager* sensor_mgr = SoDB::getSensorManager();
             sensor_mgr->removeTimerSensor(timerSensor);
         }
@@ -441,7 +441,7 @@ namespace armarx
         }
 
         {
-            ScopedRecursiveLockPtr l = getScopedVisuLock();
+            auto l = getScopedVisuLock();
             coinVisu->removeAllChildren();
             layerMainNode->unref();
             selectionNode->unref();
@@ -451,7 +451,7 @@ namespace armarx
 
     void DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         ARMARX_INFO << "Exporting scene to '" << filename << "'";
 
@@ -472,7 +472,7 @@ namespace armarx
 
     void DebugDrawerComponent::exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
 
@@ -499,7 +499,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawCoordSystem(const CoordData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawing coord system";
 
         auto& layer = requestLayer(d.layerName);
@@ -531,7 +531,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawLine(const LineData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawLine1" << flush;
 
         auto& layer = requestLayer(d.layerName);
@@ -560,7 +560,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawLineSet";
 
         auto& layer = requestLayer(d.layerName);
@@ -656,7 +656,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawBox(const BoxData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawBox";
 
         auto& layer = requestLayer(d.layerName);
@@ -691,7 +691,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawText(const TextData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawText1";
 
         auto& layer = requestLayer(d.layerName);
@@ -737,7 +737,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawSphere(const SphereData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawSphere";
 
         auto& layer = requestLayer(d.layerName);
@@ -777,7 +777,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         ARMARX_DEBUG << "drawCylinder";
 
         auto& layer = requestLayer(d.layerName);
@@ -838,7 +838,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawCircle(const DebugDrawerComponent::CircleData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -878,7 +878,7 @@ namespace armarx
                     d.color.b,
                     d.color.transparency,
                     16, 30);
-        SoNode* circle = boost::dynamic_pointer_cast<CoinVisualizationNode>(node)->getCoinVisualization();
+        SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
         circle->setName(SELECTION_NAME(d.layerName, d.name));
         sep->addChild(circle);
 
@@ -888,7 +888,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -937,7 +937,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -964,7 +964,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -1018,7 +1018,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -1043,7 +1043,7 @@ namespace armarx
 
     void DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!d.active)
         {
@@ -1076,7 +1076,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -1231,7 +1231,7 @@ namespace armarx
 
     VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
         std::string entryName = boost::replace_all_copy(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
@@ -1315,7 +1315,7 @@ namespace armarx
             nodeMat->setOverride(false);
             nodeSep->addChild(nodeMat);
 
-            boost::shared_ptr<CoinVisualization> robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType);
+            CoinVisualizationPtr robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType);
 
             if (robNodeVisu)
             {
@@ -1340,7 +1340,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1360,7 +1360,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1380,7 +1380,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1400,7 +1400,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1420,7 +1420,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1440,7 +1440,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1460,7 +1460,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeCircle(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1480,7 +1480,7 @@ namespace armarx
 
     void DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1500,7 +1500,7 @@ namespace armarx
 
     void DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1520,7 +1520,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1540,7 +1540,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1560,7 +1560,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         // process active robots
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + name + "__"), " ", "_");
@@ -1614,7 +1614,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1634,7 +1634,7 @@ namespace armarx
 
     void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -1662,7 +1662,7 @@ namespace armarx
 
     void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (selectionNode->findChild(layerMainNode) >= 0)
         {
@@ -1672,7 +1672,7 @@ namespace armarx
 
     void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (selectionNode->findChild(layerMainNode) < 0)
         {
@@ -1685,7 +1685,7 @@ namespace armarx
         ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
         Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
             CoordData& d = accumulatedUpdateData.coord[entryName];
             d.globalPose = gp;
@@ -1714,7 +1714,7 @@ namespace armarx
     void DebugDrawerComponent::removePoseVisu(const std::string& layerName, const std::string& poseName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
             CoordData& d = accumulatedUpdateData.coord[entryName];
             d.layerName = layerName;
@@ -1734,7 +1734,7 @@ namespace armarx
         Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
         VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
             LineData& d = accumulatedUpdateData.line[entryName];
             d.p1 = p1;
@@ -1755,7 +1755,7 @@ namespace armarx
     void DebugDrawerComponent::removeLineVisu(const std::string& layerName, const std::string& lineName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
             LineData& d = accumulatedUpdateData.line[entryName];
             d.layerName = layerName;
@@ -1773,7 +1773,7 @@ namespace armarx
     {
         ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
             LineSetData& d = accumulatedUpdateData.lineSet[entryName];
             d.lineSet = lineSet;
@@ -1791,7 +1791,7 @@ namespace armarx
     void DebugDrawerComponent::removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
             LineSetData& d = accumulatedUpdateData.lineSet[entryName];
             d.layerName = layerName;
@@ -1810,7 +1810,7 @@ namespace armarx
         Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
         VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
             BoxData& d = accumulatedUpdateData.box[entryName];
             d.width = dimensions->x;
@@ -1832,7 +1832,7 @@ namespace armarx
     void DebugDrawerComponent::removeBoxVisu(const std::string& layerName, const std::string& boxName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
             BoxData& d = accumulatedUpdateData.box[entryName];
             d.layerName = layerName;
@@ -1850,7 +1850,7 @@ namespace armarx
     {
         ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
             TextData& d = accumulatedUpdateData.text[entryName];
             d.text = text;
@@ -1871,7 +1871,7 @@ namespace armarx
     void DebugDrawerComponent::removeTextVisu(const std::string& layerName, const std::string& textName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
             TextData& d = accumulatedUpdateData.text[entryName];
             d.layerName = layerName;
@@ -1889,7 +1889,7 @@ namespace armarx
     {
         ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
             SphereData& d = accumulatedUpdateData.sphere[entryName];
             d.radius = radius;
@@ -1909,7 +1909,7 @@ namespace armarx
     void DebugDrawerComponent::removeSphereVisu(const std::string& layerName, const std::string& sphereName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
             SphereData& d = accumulatedUpdateData.sphere[entryName];
             d.layerName = layerName;
@@ -1927,7 +1927,7 @@ namespace armarx
     {
         ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
             CylinderData& d = accumulatedUpdateData.cylinder[entryName];
             d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
@@ -1949,7 +1949,7 @@ namespace armarx
     void DebugDrawerComponent::removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
             CylinderData& d = accumulatedUpdateData.cylinder[entryName];
             d.layerName = layerName;
@@ -1967,7 +1967,7 @@ namespace armarx
     {
         ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
             PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
             d.pointCloud = pointCloud;
@@ -1985,7 +1985,7 @@ namespace armarx
     void DebugDrawerComponent::removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
             PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
             d.layerName = layerName;
@@ -2002,7 +2002,7 @@ namespace armarx
     void DebugDrawerComponent::setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
             PolygonData& d = accumulatedUpdateData.polygons[entryName];
 
@@ -2033,7 +2033,7 @@ namespace armarx
     void DebugDrawerComponent::removePolygonVisu(const std::string& layerName, const std::string& polygonName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
             PolygonData& d = accumulatedUpdateData.polygons[entryName];
             d.layerName = layerName;
@@ -2050,7 +2050,7 @@ namespace armarx
     void DebugDrawerComponent::setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
             TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
             d.triMesh = triMesh;
@@ -2068,7 +2068,7 @@ namespace armarx
     void DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
             TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
             d.layerName = layerName;
@@ -2085,7 +2085,7 @@ namespace armarx
     void DebugDrawerComponent::setArrowVisu(const std::string& layerName, const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
             ArrowData& d = accumulatedUpdateData.arrows[entryName];
             d.position = Vector3Ptr::dynamicCast(position)->toEigen();
@@ -2107,7 +2107,7 @@ namespace armarx
     void DebugDrawerComponent::removeArrowVisu(const std::string& layerName, const std::string& arrowName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
             ArrowData& d = accumulatedUpdateData.arrows[entryName];
             d.layerName = layerName;
@@ -2123,7 +2123,7 @@ namespace armarx
 
     void DebugDrawerComponent::setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
         ARMARX_DEBUG << "setting robot visualization for " << entryName;
         RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2139,7 +2139,7 @@ namespace armarx
 
     void DebugDrawerComponent::updateRobotPose(const std::string& layerName, const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
         ARMARX_DEBUG << "updating robot pose for " << entryName;
         RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2156,7 +2156,7 @@ namespace armarx
 
     void DebugDrawerComponent::updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map<std::string, float>& configuration, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
         ARMARX_DEBUG << "updating robot config for " << entryName;
         RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2177,7 +2177,7 @@ namespace armarx
 
     void DebugDrawerComponent::updateRobotColor(const std::string& layerName, const std::string& robotName, const DrawColor& c, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
         ARMARX_DEBUG << "updating robot color for " << entryName;
         RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2202,7 +2202,7 @@ namespace armarx
 
     void DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const DrawColor& c, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
         ARMARX_DEBUG << "updating robot color for " << entryName;
         RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2228,7 +2228,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeRobotVisu(const std::string& layerName, const std::string& robotName, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
         ARMARX_DEBUG << "removing robot visu for " << entryName;
         RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2248,7 +2248,7 @@ namespace armarx
 
     void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
     {
-        ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+        auto lockData = getScopedAccumulatedDataLock();
         //updates should only contain elements to add for each layer after the last clear for this layer was executed
         //this prevents a sequence add,clear,add to result in an empty layer
         //=>remove all objects pending for this layer
@@ -2259,8 +2259,8 @@ namespace armarx
 
     void DebugDrawerComponent::clearLayerQt(const std::string& layerName)
     {
-        ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
-        ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+        auto lockData = getScopedAccumulatedDataLock();
+        auto lockVisu = getScopedVisuLock();
 
         ARMARX_DEBUG << "clearing layer " << layerName;
 
@@ -2369,20 +2369,19 @@ namespace armarx
         clearLayer(DEBUG_LAYER_NAME);
     }
 
-    void DebugDrawerComponent::setMutex(boost::shared_ptr<boost::recursive_mutex> m)
+    void DebugDrawerComponent::setMutex(RecursiveMutexPtr const& m)
     {
-        //ARMARX_IMPORTANT << "set mutex3d:" << m.get();
         mutex = m;
     }
 
-    ScopedRecursiveLockPtr DebugDrawerComponent::getScopedVisuLock()
+    auto DebugDrawerComponent::getScopedVisuLock() -> RecursiveMutexLockPtr
     {
-        ScopedRecursiveLockPtr l;
+        RecursiveMutexLockPtr l;
 
         if (mutex)
         {
             //ARMARX_IMPORTANT << mutex.get();
-            l.reset(new ScopedRecursiveLock(*mutex));
+            l.reset(new RecursiveMutexLock(*mutex));
         }
         else
         {
@@ -2392,9 +2391,9 @@ namespace armarx
         return l;
     }
 
-    ScopedRecursiveLockPtr DebugDrawerComponent::getScopedAccumulatedDataLock()
+    auto DebugDrawerComponent::getScopedAccumulatedDataLock() -> RecursiveMutexLockPtr
     {
-        ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*dataUpdateMutex));
+        RecursiveMutexLockPtr l(new RecursiveMutexLock(*dataUpdateMutex));
         return l;
     }
 
@@ -2405,7 +2404,7 @@ namespace armarx
 
     DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (hasLayer(layerName))
         {
@@ -2428,8 +2427,8 @@ namespace armarx
 
     void DebugDrawerComponent::updateVisualization()
     {
-        ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
-        ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+        auto lockData = getScopedAccumulatedDataLock();
+        auto lockVisu = getScopedVisuLock();
         // check for clear&remove
         //(updates only contain elements to add for each layer after the last clear for this layer was executed)
         //this prevents a sequence add,clear,add to result in an empty layer
@@ -2545,13 +2544,13 @@ namespace armarx
 
     bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         return layers.find(layerName) != layers.end();
     }
 
     void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+        auto lockData = getScopedAccumulatedDataLock();
         //updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
         //this prevents a sequence add,clear,add to result in an empty layer
         //=>remove all objects pending for this layer
@@ -2561,8 +2560,8 @@ namespace armarx
 
     void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
     {
-        ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
-        ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+        auto lockData = getScopedAccumulatedDataLock();
+        auto lockVisu = getScopedVisuLock();
         if (!hasLayer(layerName))
         {
             if (verbose)
@@ -2603,7 +2602,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
     {
-        ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+        auto lockData = getScopedAccumulatedDataLock();
 
         removeUpdateDataFromMap(layerName, accumulatedUpdateData.coord);
         removeUpdateDataFromMap(layerName, accumulatedUpdateData.line);
@@ -2636,7 +2635,7 @@ namespace armarx
 
     Ice::StringSeq DebugDrawerComponent::layerNames(const ::Ice::Current&)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
         Ice::StringSeq seq {};
 
         for (const auto& layer : layers)
@@ -2650,7 +2649,7 @@ namespace armarx
     ::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
     {
         ::armarx::LayerInformationSequence seq {};
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         for (const auto& layer : layers)
         {
@@ -2679,7 +2678,7 @@ namespace armarx
 
     void DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -2739,7 +2738,7 @@ namespace armarx
 
     void DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -2760,7 +2759,7 @@ namespace armarx
     void DebugDrawerComponent::setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
             ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
             d.pointCloud = pointCloud;
@@ -2778,7 +2777,7 @@ namespace armarx
     void DebugDrawerComponent::removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
             ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
             d.layerName = layerName;
@@ -2794,7 +2793,7 @@ namespace armarx
 
     void DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         auto& layer = requestLayer(d.layerName);
 
@@ -2855,7 +2854,7 @@ namespace armarx
 
     void DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName, const std::string& name)
     {
-        ScopedRecursiveLockPtr l = getScopedVisuLock();
+        auto l = getScopedVisuLock();
 
         if (!hasLayer(layerName))
         {
@@ -2876,7 +2875,7 @@ namespace armarx
     void DebugDrawerComponent::set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
             Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
             d.pointCloud = pointCloud;
@@ -2894,7 +2893,7 @@ namespace armarx
     void DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
     {
         {
-            ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+            auto l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
             Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
             d.layerName = layerName;
@@ -2911,7 +2910,7 @@ namespace armarx
     void DebugDrawerComponent::setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
     {
 
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
         CircleData& d = accumulatedUpdateData.circle[entryName];
         d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
@@ -2934,7 +2933,7 @@ namespace armarx
     void DebugDrawerComponent::removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current&)
     {
 
-        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        auto l = getScopedAccumulatedDataLock();
         std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
         CircleData& d = accumulatedUpdateData.circle[entryName];
         d.layerName = layerName;
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index c91d4aca1747454bd6533595ef0bda8359bae6cc..9b7c3709bc92be7256deae774a11b748be5ee14a 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -37,7 +37,6 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
@@ -47,6 +46,7 @@
 
 //std
 #include <memory>
+#include <mutex>
 
 namespace armarx
 {
@@ -274,11 +274,17 @@ namespace armarx
 
         void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const ::Ice::Current& = Ice::emptyCurrent) override;
 
+        // This is all total madness. Why do we put mutexes and locks into shared pointers?
+        using RecursiveMutex = std::recursive_mutex;
+        using RecursiveMutexPtr = std::shared_ptr<RecursiveMutex>;
+        using RecursiveMutexLock = std::unique_lock<RecursiveMutex>;
+        using RecursiveMutexLockPtr = std::shared_ptr<RecursiveMutexLock>;
+
         /*!
          * \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
          * \return The lock that is automatically destructed when leaving the current scope.
          */
-        ScopedRecursiveLockPtr getScopedVisuLock();
+        RecursiveMutexLockPtr getScopedVisuLock();
 
         /*!
          * \brief getVisualization Ensure that all access to this node is protected with the scoped lock mutex.
@@ -286,7 +292,7 @@ namespace armarx
          */
         SoSeparator* getVisualization();
 
-        void setMutex(boost::shared_ptr<boost::recursive_mutex> m);
+        void setMutex(RecursiveMutexPtr const& m);
 
         ~DebugDrawerComponent() override;
 
@@ -590,24 +596,24 @@ namespace armarx
         std::map<const std::string, Layer> layers;
 
         //! The coin mutex
-        boost::shared_ptr<boost::recursive_mutex> mutex;
+        RecursiveMutexPtr mutex;
 
         //! The data update mutex
-        std::shared_ptr<boost::recursive_mutex> dataUpdateMutex;
+        RecursiveMutexPtr dataUpdateMutex;
 
-        std::shared_ptr<boost::recursive_mutex> topicMutex;
+        RecursiveMutexPtr topicMutex;
 
-        static boost::recursive_mutex selectionMutex;
+        static std::recursive_mutex selectionMutex;
         std::set<std::string> selectableLayers;
 
-        ScopedRecursiveLockPtr getScopedAccumulatedDataLock();
+        RecursiveMutexLockPtr getScopedAccumulatedDataLock();
 
         DebugDrawerListenerPrx listenerPrx;
 
         float cycleTimeMS;
         //PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
         SoTimerSensor* timerSensor;
-        Mutex timerSensorMutex;
+        std::mutex timerSensorMutex;
         void removeAccumulatedData(const std::string& layerName);
 
         std::map<std::string, VirtualRobot::RobotPtr> loadedRobots;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 1365ca76a761f5956aaebc5d96ef1adcca8034f5..934b88073535870c139d3677e45d8dcdc19bbf57 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -187,7 +187,7 @@ namespace armarx
                 return entries.at(i);
             }
         }
-        ScopedLock lock(mutex);
+        std::unique_lock lock(mutex);
         ARMARX_INFO << "registering '" << name << "'";
         entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
         validEntries++;
@@ -219,7 +219,7 @@ namespace armarx
         e.isRunning = true;
         e.enabled = true;
         {
-            ScopedLock lock(e.messageMutex);
+            std::unique_lock lock(e.messageMutex);
             e.message = args.message;
         }
     }
@@ -285,7 +285,7 @@ namespace armarx
             }
             ARMARX_TRACE;
 
-            ScopedLock lock(e.messageMutex);
+            std::unique_lock lock(e.messageMutex);
             if (e.message.size())
             {
                 ss << " - " << e.message;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 97852dab631cc1e6e1df0c4f738585f43395b0f4..49ca51167f87d7c83636c7d7d8f744797dd2d5e2 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -31,10 +31,10 @@
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 #include <ArmarXCore/interface/components/EmergencyStopInterface.h>
 
 #include <atomic>
+#include <mutex>
 
 namespace armarx
 {
@@ -111,7 +111,7 @@ namespace armarx
             bool isRunning = false;
             bool required = false;
             bool enabled = true;
-            Mutex messageMutex;
+            std::mutex messageMutex;
             std::vector<long> history;
             size_t historyIndex = 0;
         };
@@ -145,7 +145,7 @@ namespace armarx
         void monitorHealthTaskClb();
         Entry& findOrCreateEntry(const std::string& name);
 
-        Mutex mutex;
+        std::mutex mutex;
         std::deque<Entry> entries;
         std::atomic_size_t validEntries {0};
         PeriodicTask<RobotHealth>::pointer_type monitorHealthTask;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index e3c8d00b914d2431ac9a746158d1d3e7af155026..473ec9a16d21f415925efedbfa8c5150ae2f55ea 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -23,8 +23,6 @@
  */
 
 #include "RobotStateComponent.h"
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
 
 #include <Ice/ObjectAdapter.h>
 
@@ -323,7 +321,7 @@ namespace armarx
         }
 
         {
-            boost::unique_lock<SharedMutex> lock(jointHistoryMutex);
+            std::unique_lock lock(jointHistoryMutex);
             jointHistory.emplace_hint(jointHistory.end(), time, jointAngles);
 
             if (jointHistory.size() > jointHistoryLength)
@@ -512,7 +510,7 @@ namespace armarx
         IceUtil::Time duration;
         {
             IceUtil::Time start = IceUtil::Time::now();
-            boost::unique_lock<SharedMutex> lock(poseHistoryMutex);
+            std::unique_lock lock(poseHistoryMutex);
             duration = IceUtil::Time::now() - start;
 
             poseHistory.emplace_hint(poseHistory.end(),
@@ -553,7 +551,7 @@ namespace armarx
 
     auto RobotStateComponent::interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
     {
-        boost::shared_lock<SharedMutex> lock(jointHistoryMutex);
+        std::shared_lock lock(jointHistoryMutex);
         if (jointHistory.empty())
         {
             ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
@@ -628,7 +626,7 @@ namespace armarx
 
     auto RobotStateComponent::interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
     {
-        boost::shared_lock<SharedMutex> lock(poseHistoryMutex);
+        std::shared_lock lock(poseHistoryMutex);
 
         if (poseHistory.empty())
         {
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 27751d203b73c11313caa6a87bbaeabe3cf675ff..2a1da853b696150112fc5ee982cf3a7734c9a4d2 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -37,6 +37,9 @@
 
 #include "SharedRobotServants.h"
 
+#include <shared_mutex>
+#include <mutex>
+
 
 namespace armarx
 {
@@ -216,11 +219,11 @@ namespace armarx
         std::string robotFile;
         std::string relativeRobotFile;
 
-        mutable SharedMutex jointHistoryMutex;
+        mutable std::shared_mutex jointHistoryMutex;
         std::map<IceUtil::Time, NameValueMap> jointHistory;
         size_t jointHistoryLength;
 
-        mutable SharedMutex poseHistoryMutex;
+        mutable std::shared_mutex poseHistoryMutex;
         std::map<IceUtil::Time, FramedPosePtr> poseHistory;
         size_t poseHistoryLength;
 
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
index e49aac6a5abb3f596201cdc23f823805eba10654..cfc854b4c19309b39c56591fda88ca4b54300086 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
@@ -25,8 +25,6 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 
-#include <boost/foreach.hpp>
-
 #include <Eigen/Geometry>
 
 #include <VirtualRobot/Nodes/RobotNode.h>
@@ -68,7 +66,7 @@ namespace armarx
 
     void SharedObjectBase::ref(const Current& current)
     {
-        boost::lock_guard<boost::mutex> lock(this->_counterMutex);
+        std::unique_lock lock(this->_counterMutex);
 
         _referenceCount++;
 
@@ -79,7 +77,7 @@ namespace armarx
 
     void SharedObjectBase::unref(const Current& current)
     {
-        boost::lock_guard<boost::mutex> lock(this->_counterMutex);
+        std::unique_lock lock(this->_counterMutex);
 
 #ifdef VERBOSE
         ARMARX_LOG_S << "unref: " <<   _referenceCount << " " << this << flush;
@@ -184,7 +182,7 @@ namespace armarx
     Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const
     {
         ReadLockPtr lock = _node->getRobot()->getReadLock();
-        boost::shared_ptr<RobotNodePrismatic> prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(_node);
+        RobotNodePrismatic* prismatic = dynamic_cast<RobotNodePrismatic*>(_node.get());
 
         if (prismatic)
         {
@@ -199,7 +197,7 @@ namespace armarx
     Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current& current) const
     {
         ReadLockPtr lock = _node->getRobot()->getReadLock();
-        boost::shared_ptr<RobotNodeRevolute> revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(_node);
+        RobotNodeRevolute* revolute = dynamic_cast<RobotNodeRevolute*>(_node.get());
 
         if (revolute)
         {
@@ -237,7 +235,7 @@ namespace armarx
         ReadLockPtr lock = _node->getRobot()->getReadLock();
         std::vector<SceneObjectPtr> children = _node->getChildren();
         NameList names;
-        BOOST_FOREACH(SceneObjectPtr node, children)
+        for (SceneObjectPtr const& node : children)
         {
             names.push_back(node->getName());
         }
@@ -249,7 +247,7 @@ namespace armarx
         ReadLockPtr lock = _node->getRobot()->getReadLock();
         std::vector<RobotNodePtr> parents = _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
         NameList names;
-        BOOST_FOREACH(RobotNodePtr node, parents)
+        for (RobotNodePtr const& node : parents)
         {
             names.push_back(node->getName());
         }
@@ -320,7 +318,7 @@ namespace armarx
 #ifdef VERBOSE
         ARMARX_WARNING_S << "destruct " << this << flush;
 #endif
-        boost::recursive_mutex::scoped_lock cloneLock(m);
+        std::unique_lock cloneLock(m);
 
         for (auto value : this->_cachedNodes)
         {
@@ -341,7 +339,7 @@ namespace armarx
     {
         //    ARMARX_LOG_S << "Looking for node: " << name << flush;
         assert(_robot);
-        boost::recursive_mutex::scoped_lock cloneLock(m);
+        std::unique_lock cloneLock(m);
         SharedRobotNodeInterfacePrx prx;
 
         if (this->_cachedNodes.find(name) == this->_cachedNodes.end())
@@ -369,7 +367,7 @@ namespace armarx
     SharedRobotNodeInterfacePrx SharedRobotServant::getRootNode(const Current& current)
     {
         assert(_robot);
-        boost::recursive_mutex::scoped_lock cloneLock(m);
+        std::unique_lock cloneLock(m);
         std::string name = _robot->getRootNode()/*,current*/->getName();
         return this->getRobotNode(name, current);
     }
@@ -384,7 +382,7 @@ namespace armarx
     {
         std::vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
         NameList names;
-        BOOST_FOREACH(RobotNodePtr node, robotNodes)
+        for (RobotNodePtr const& node : robotNodes)
         {
             names.push_back(node->getName());
         }
@@ -402,7 +400,7 @@ namespace armarx
 
         std::vector<RobotNodePtr> robotNodes = robotNodeSet->getAllRobotNodes();
         NameList names;
-        BOOST_FOREACH(RobotNodePtr node, robotNodes)
+        for (RobotNodePtr const& node : robotNodes)
         {
             names.push_back(node->getName());
         }
@@ -421,7 +419,7 @@ namespace armarx
     {
         std::vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
         NameList names;
-        BOOST_FOREACH(RobotNodeSetPtr set, robotNodeSets)
+        for (RobotNodeSetPtr const& set : robotNodeSets)
         {
             names.push_back(set->getName());
         }
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index 922588ddbdbf15100fa4a3e358a5abd232de3546..5dff6c3891075d67a6a3e0ff9fa1f3a9a5d615f3 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -26,19 +26,11 @@
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/system/ImportExportComponent.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
-
-
 #include <RobotAPI/interface/core/RobotState.h>
 
-#include <boost/thread.hpp>
+#include <VirtualRobot/VirtualRobot.h>
 
-namespace VirtualRobot
-{
-    class Robot;
-    using RobotPtr = boost::shared_ptr<Robot>;
-    class RobotNode;
-    using RobotNodePtr = boost::shared_ptr<RobotNode>;
-}
+#include <mutex>
 
 namespace armarx
 {
@@ -59,7 +51,7 @@ namespace armarx
         SharedObjectBase();
     private:
         unsigned int _referenceCount;
-        boost::mutex _counterMutex;
+        std::mutex _counterMutex;
     };
 
     /**
@@ -149,7 +141,7 @@ namespace armarx
         RobotStateComponentInterfacePrx getRobotStateComponent(const Ice::Current&) const override;
     protected:
         VirtualRobot::RobotPtr _robot;
-        boost::recursive_mutex m;
+        std::recursive_mutex m;
         std::map<std::string, SharedRobotNodeInterfacePrx> _cachedNodes;
         IceUtil::Time updateTimestamp;
         RobotStateComponentInterfacePrx robotStateComponent;
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index 3cb860a4dbc16e48e0c5d60ba0b6985f0d99990c..a44d697794ceaf47d46266b6e6b515dec9acf84d 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
@@ -31,6 +31,8 @@
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 #include <RobotAPI/libraries/core/math/ColorUtils.h>
 
+#include <thread>
+
 namespace armarx
 {
     void ViewSelection::onInitComponent()
@@ -142,7 +144,7 @@ namespace armarx
 
     void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
     {
-        boost::mutex::scoped_lock lock(syncMutex);
+        std::unique_lock lock(syncMutex);
 
         IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
         for (const auto& p : saliencyMaps)
@@ -180,7 +182,7 @@ namespace armarx
         int maxIndex = -1;
         float maxSaliency = std::numeric_limits<float>::min();
 
-        boost::mutex::scoped_lock lock(syncMutex);
+        std::unique_lock lock(syncMutex);
 
         hasNewSaliencyMap = false;
 
@@ -284,7 +286,7 @@ namespace armarx
 
         if (!manualViewTargets.empty())
         {
-            ScopedLock lock(manualViewTargetsMutex);
+            std::unique_lock lock(manualViewTargetsMutex);
 
             ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
 
@@ -332,11 +334,11 @@ namespace armarx
 
             viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
 
-            boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
+            std::this_thread::sleep_for(std::chrono::milliseconds(duration));
         }
         else
         {
-            boost::this_thread::sleep(boost::posix_time::milliseconds(5));
+            std::this_thread::sleep_for(std::chrono::milliseconds(5));
         }
     }
 
@@ -344,7 +346,7 @@ namespace armarx
 
     void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
     {
-        boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+        std::unique_lock lock(manualViewTargetsMutex);
 
         //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
 
@@ -356,14 +358,14 @@ namespace armarx
 
         manualViewTargets.push(viewTarget);
 
-        boost::mutex::scoped_lock lock2(syncMutex);
+        std::unique_lock lock2(syncMutex);
 
         condition.notify_all();
     }
 
     void ViewSelection::clearManualViewTargets(const Ice::Current& c)
     {
-        boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+        std::unique_lock lock(manualViewTargetsMutex);
 
         while (!manualViewTargets.empty())
         {
@@ -376,7 +378,7 @@ namespace armarx
 
     ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
     {
-        boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+        std::unique_lock lock(manualViewTargetsMutex);
 
         ViewTargetList result;
 
@@ -396,7 +398,7 @@ namespace armarx
 
     void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
     {
-        boost::mutex::scoped_lock lock(syncMutex);
+        std::unique_lock lock(syncMutex);
 
         hasNewSaliencyMap = true;
 
@@ -439,7 +441,7 @@ namespace armarx
         }
 
 
-        boost::mutex::scoped_lock lock(syncMutex);
+        std::unique_lock lock(syncMutex);
 
         TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
         DebugDrawer24BitColoredPointCloud cloud;
@@ -492,7 +494,7 @@ namespace armarx
 
     void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
     {
-        boost::mutex::scoped_lock lock(syncMutex);
+        std::unique_lock lock(syncMutex);
 
         if (saliencyMaps.count(name))
         {
@@ -506,7 +508,7 @@ namespace armarx
     {
         std::vector<std::string> names;
 
-        boost::mutex::scoped_lock lock(syncMutex);
+        std::unique_lock lock(syncMutex);
 
         for (const auto& p : saliencyMaps)
         {
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
index f8e6cbdc93a26500fef455bc989b2149bd1554b4..98beeedd5e6d9a176d9b76e83b4b9659d99ef8d5 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.h
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h
@@ -46,6 +46,8 @@
 #include <Eigen/Geometry>
 
 
+#include <condition_variable>
+#include <mutex>
 #include <queue>
 
 namespace armarx
@@ -151,7 +153,7 @@ namespace armarx
 
         void activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
         {
-            boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+            std::unique_lock lock(manualViewTargetsMutex);
 
             ARMARX_INFO << "activating automatic view selection";
 
@@ -161,7 +163,7 @@ namespace armarx
 
         void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
         {
-            boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+            std::unique_lock lock(manualViewTargetsMutex);
 
             ARMARX_INFO << "deactivating automatic view selection";
 
@@ -171,7 +173,7 @@ namespace armarx
 
         bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
         {
-            boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+            std::unique_lock lock(manualViewTargetsMutex);
 
             return doAutomaticViewSelection;
         }
@@ -214,16 +216,16 @@ namespace armarx
 
         bool drawViewDirection;
 
-        armarx::Mutex manualViewTargetsMutex;
+        std::mutex manualViewTargetsMutex;
         std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets;
 
         bool doAutomaticViewSelection;
 
         Eigen::Vector3f offsetToHeadCenter;
 
-        boost::condition_variable condition;
+        std::condition_variable condition;
         bool hasNewSaliencyMap;
-        boost::mutex syncMutex;
+        std::mutex syncMutex;
 
         std::map<std::string, SaliencyMapBasePtr> saliencyMaps;
 
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index a365530d8022e8ac1f7e51f7246c44f28903c5ef..a2f4135e4f835166d911f5fc373919cc1885ffe8 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -36,6 +36,8 @@
 
 #include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
 
+#include <boost/algorithm/string/trim.hpp>
+
 #define RAWFORCE "_rawforcevectors"
 #define OFFSETFORCE "_forceswithoffsetvectors"
 #define FILTEREDFORCE "_filteredforcesvectors"
@@ -123,7 +125,7 @@ namespace armarx
         auto channels = getAvailableChannels(false);
         auto batchPrx = debugDrawer->ice_batchOneway();
         {
-            ScopedLock lock(dataMutex);
+            std::unique_lock lock(dataMutex);
 
             std::set<std::string> frameAlreadyReported;
             for (auto& channel : channels)
@@ -242,7 +244,7 @@ namespace armarx
 
     void ForceTorqueObserver::updateRobot()
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         RemoteRobot::synchronizeLocalClone(localRobot, robot);
     }
 
@@ -304,7 +306,7 @@ namespace armarx
 
     void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& c)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         auto offerForceAndTorqueValue = [ = ](std::string const & sensorNodeName, std::string const & frame, std::string channelName, const FramedDirectionBasePtr & forces, const FramedDirectionBasePtr & torques)
         {
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 24c031200b51613c9b3409129df79b2804d7a48b..0f17d8a498377664b98882e26dff3c9b8df1bb50 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -27,6 +27,8 @@
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
+#include <mutex>
+
 namespace armarx
 {
     /**
@@ -92,7 +94,7 @@ namespace armarx
     private:
         void updateRobot();
 
-        armarx::Mutex dataMutex;
+        std::mutex dataMutex;
         std::string topicName;
         RobotStateComponentInterfacePrx robot;
         VirtualRobot::RobotPtr localRobot;
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.cpp b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
index e6e5226324c46df8d35f6307660b9e016b71fd02..b1848c5325d931a623c828e1dd58973121beedb5 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.cpp
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
@@ -69,7 +69,7 @@ namespace armarx
 
     void GamepadUnitObserver::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.h b/source/RobotAPI/components/units/GamepadUnitObserver.h
index fcccf65fc017cda9f3cc69e89a2eebb747df61cc..2f4d59c2a7a44ede66adaec1663be2263b6d25de 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.h
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.h
@@ -29,6 +29,7 @@
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -79,7 +80,7 @@ namespace armarx
 
 
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
         DebugDrawerInterfacePrx debugDrawerPrx;
 
 
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 3a0cac2ec7835df8e0362b59b70d9d48ef36708e..527216c96a63d769c625cc5deeae30fab64fef47 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -122,21 +122,21 @@ void GraspCandidateObserver::handleProviderUpdate(const std::string& providerNam
 
 void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     this->candidates[providerName] = candidates;
     handleProviderUpdate(providerName, candidates.size());
 }
 
 void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     this->bimanualCandidates[providerName] = candidates;
     handleProviderUpdate(providerName, candidates.size());
 }
 
 void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     providers[providerName] = info;
     if (updateCounters.count(providerName) == 0)
     {
@@ -153,13 +153,13 @@ void GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
 
 InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     return providers;
 }
 
 StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     return getAvailableProviderNames();
 }
 
@@ -167,14 +167,14 @@ StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
 
 ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     checkHasProvider(providerName);
     return providers[providerName];
 }
 
 bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     return hasProvider(providerName);
 }
 
@@ -182,7 +182,7 @@ bool GraspCandidateObserver::hasProvider(const std::string& providerName, const
 
 GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     GraspCandidateSeq all;
     for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
     {
@@ -200,7 +200,7 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::str
 
 GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     GraspCandidateSeq matching;
     for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
     {
@@ -217,20 +217,20 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
 
 Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     checkHasProvider(providerName);
     return updateCounters[providerName];
 }
 
 IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     return updateCounters;
 }
 
 bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     if (providers.count(providerName) == 0)
     {
         return false;
@@ -241,19 +241,19 @@ bool GraspCandidateObserver::setProviderConfig(const std::string& providerName,
 
 void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
 {
-    ScopedLock lock(selectedCandidatesMutex);
+    std::unique_lock lock(selectedCandidatesMutex);
     selectedCandidates = candidates;
 }
 
 GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
 {
-    ScopedLock lock(selectedCandidatesMutex);
+    std::unique_lock lock(selectedCandidatesMutex);
     return selectedCandidates;
 }
 
 BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     BimanualGraspCandidateSeq all;
     for (const std::pair<std::string, grasping::BimanualGraspCandidateSeq>& pair : bimanualCandidates)
     {
@@ -264,13 +264,13 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
 
 void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
 {
-    ScopedLock lock(selectedCandidatesMutex);
+    std::unique_lock lock(selectedCandidatesMutex);
     selectedBimanualCandidates = candidates;
 }
 
 BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
 {
-    ScopedLock lock(selectedCandidatesMutex);
+    std::unique_lock lock(selectedCandidatesMutex);
     return selectedBimanualCandidates;
 }
 
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index a481ec2791d6c97a5c882361109d96d494e44e9e..beb95c3a2bde2c9bd3c568031dd4ce2db7939d07 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -26,6 +26,8 @@
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
 
+#include <mutex>
+
 namespace armarx
 {
     /**
@@ -106,7 +108,7 @@ namespace armarx
         bool hasProvider(const std::string& providerName);
         void checkHasProvider(const std::string& providerName);
         grasping::StringSeq getAvailableProviderNames();
-        Mutex dataMutex;
+        std::mutex dataMutex;
         std::map<std::string, grasping::GraspCandidateSeq> candidates;
         std::map<std::string, grasping::BimanualGraspCandidateSeq> bimanualCandidates;
         grasping::InfoMap providers;
@@ -114,7 +116,7 @@ namespace armarx
 
         grasping::GraspCandidateProviderInterfacePrx configTopic;
 
-        Mutex selectedCandidatesMutex;
+        std::mutex selectedCandidatesMutex;
         grasping::GraspCandidateSeq selectedCandidates;
 
         grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 6f7cc714ab242930d2a2a32d6022b77889a473bc..3816605266f1bc44dce40a752bfe94b14af8177b 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -32,15 +32,9 @@
 #include <ArmarXCore/core/system/ImportExportComponent.h>
 #include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
+#include <VirtualRobot/VirtualRobot.h>
 
 
-namespace VirtualRobot
-{
-    class Robot;
-    using RobotPtr = boost::shared_ptr<Robot>;
-    class EndEffector;
-    using EndEffectorPtr = boost::shared_ptr<EndEffector>;
-}
 namespace armarx
 {
     /**
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 759927bd8799c8d319ba69fe1e3e384402ac8d7a..0fe52df490098255ef86b56bd43025017a2997b5 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -76,7 +76,7 @@ namespace armarx
 
     void HapticObserver::reportSensorValues(const std::string& device, const std::string& name, const armarx::MatrixFloatBasePtr& values, const armarx::TimestampBasePtr& timestamp, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(values);
         if (matrix->cols == 0)
@@ -159,7 +159,7 @@ namespace armarx
 
     void HapticObserver::updateStatistics()
     {
-        /*ScopedLock lock(dataMutex);
+        /*std::unique_lock lock(dataMutex);
         //ARMARX_LOG << "updateStatistics";
         long now = TimestampVariant::nowLong();
         for (std::map<std::string, HapticSampleStatistics>::iterator it = statistics.begin(); it != statistics.end(); ++it)
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index 5e75db5b05f74cf23cecb6ce3908f6f9ca1fd2c9..cac9a85c903e4c83125489f97c8c65dd3b368ff5 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -30,6 +30,7 @@
 #include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -146,7 +147,7 @@ namespace armarx
          */
         PropertyDefinitionsPtr createPropertyDefinitions() override;
     private:
-        armarx::Mutex dataMutex;
+        std::mutex dataMutex;
         std::string topicName;
         PeriodicTask<HapticObserver>::pointer_type statisticsTask;
 
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index d240a3c60d2fa0a382ae4fe69cb33df69a643c9c..8e3037badb78a49b9cd6ef2847460c70995f448b 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -31,6 +31,13 @@
 #include <VirtualRobot/IK/GazeIK.h>
 #include <VirtualRobot/LinkedCoordinate.h>
 
+#include <boost/algorithm/string/trim.hpp>
+
+#include <boost/shared_ptr.hpp>
+#include <memory>
+
+using boost::dynamic_pointer_cast;
+using std::dynamic_pointer_cast;
 
 namespace armarx
 {
@@ -46,7 +53,7 @@ namespace armarx
 
     void HeadIKUnit::onInitComponent()
     {
-        ScopedLock lock(accessMutex);
+        std::unique_lock lock(accessMutex);
 
         usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
         usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
@@ -61,7 +68,7 @@ namespace armarx
 
     void HeadIKUnit::onConnectComponent()
     {
-        ScopedLock lock(accessMutex);
+        std::unique_lock lock(accessMutex);
 
         drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
@@ -99,7 +106,7 @@ namespace armarx
     {
         release();
 
-        //ScopedLock lock(accessMutex);
+        //std::unique_lock lock(accessMutex);
         if (drawer)
         {
             drawer->removeSphereDebugLayerVisu("HeadViewTarget");
@@ -122,7 +129,7 @@ namespace armarx
 
     void HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
     {
-        ScopedLock lock(accessMutex);
+        std::unique_lock lock(accessMutex);
 
         cycleTime = milliseconds;
 
@@ -135,7 +142,7 @@ namespace armarx
 
     void HeadIKUnit::setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c)
     {
-        ScopedLock lock(accessMutex);
+        std::unique_lock lock(accessMutex);
 
         this->robotNodeSetNames = armarx::Split(robotNodeSetName, ",");
         for (auto& setName : robotNodeSetNames)
@@ -204,7 +211,7 @@ namespace armarx
 
     void HeadIKUnit::request(const Ice::Current& c)
     {
-        ScopedLock lock(accessMutex);
+        std::unique_lock lock(accessMutex);
 
         requested = true;
         ARMARX_IMPORTANT << "Requesting HeadIKUnit";
@@ -224,7 +231,7 @@ namespace armarx
 
     void HeadIKUnit::release(const Ice::Current& c)
     {
-        ScopedLock lock(accessMutex);
+        std::unique_lock lock(accessMutex);
 
         ARMARX_INFO << "Releasing HeadIKUnit";
         requested = false;
@@ -256,9 +263,9 @@ namespace armarx
         bool doCalculation = false;
 
         {
-            ScopedTryLock lock(accessMutex);
+            std::unique_lock lock(accessMutex, std::defer_lock);
 
-            if (lock.owns_lock())
+            if (lock.try_lock())
             {
                 doCalculation = requested && newTargetSet;
                 newTargetSet = false;
@@ -272,7 +279,7 @@ namespace armarx
 
         if (doCalculation)
         {
-            ScopedLock lock(accessMutex);
+            std::unique_lock lock(accessMutex);
 
             VirtualRobot::RobotNodeSetPtr kinematicChain;
             bool foundSolution = false;
@@ -304,8 +311,7 @@ namespace armarx
                 auto tcpNode = kinematicChain->getTCP();
                 VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
 
-
-                virtualPrismaticJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
+                virtualPrismaticJoint = dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
                 if (!virtualPrismaticJoint)
                 {
                     ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set";
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index 251c88d4d5a1cfd0822f79a744b3757a23cd451d..b814b519aae4f377a55e0610759e52663007ad50 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -25,13 +25,14 @@
 
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 #include <RobotAPI/interface/units/HeadIKUnit.h>
 
+#include <mutex>
+
 
 namespace armarx
 {
@@ -98,7 +99,7 @@ namespace armarx
     private:
         void periodicExec();
 
-        Mutex accessMutex;
+        std::mutex accessMutex;
         bool requested;
         int cycleTime;
         PeriodicTask<HeadIKUnit>::pointer_type execTask;
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index 52e6f92ea93cb4c5f4f58611a56fb6f73533b3af..dc73454fe838fafeddebbbfca0915efaca5752f0 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -71,7 +71,7 @@ namespace armarx
         const std::string& device, const std::string& name, const IMUData& values,
         const TimestampBasePtr& timestamp, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 4591228d92d457967b85106554898345895b6ee2..728e2dc385906114fa09c2b7b044990196a94a2a 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -29,6 +29,7 @@
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -82,7 +83,7 @@ namespace armarx
 
 
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
         DebugDrawerInterfacePrx debugDrawerPrx;
 
 
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 0459725252d769f7c117bb477a4482a887cd5c5f..ccd47dcb5ff5de17eadd3653c749d981da1a8265 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -34,6 +34,9 @@
 #include <ArmarXCore/core/application/Application.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
 namespace armarx
 {
     void KinematicUnit::onInitComponent()
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 15712aa55724e6eb29ffe97e9fa9226a5c9c9872..c1796936e47e97e06b6a923a4ca6832bcdf81f40 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -38,6 +38,9 @@
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
 
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
 namespace armarx
 {
     // ********************************************************************
@@ -287,7 +290,7 @@ namespace armarx
         //    ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
         bool newChannel;
         {
-            ScopedLock lock(initializedChannelsMutex);
+            std::unique_lock lock(initializedChannelsMutex);
             newChannel = initializedChannels.count(channelName) == 0;
             initializedChannels.insert(channelName);
         }
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 8d726600692d58d32b35c22f11b43fa7f4e3b4dd..5b21e89d0cb300c331cd105b907439da70cc937a 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -32,6 +32,7 @@
 #include <ArmarXCore/observers/Observer.h>
 
 #include <string>
+#include <mutex>
 
 namespace armarx
 {
@@ -154,7 +155,7 @@ namespace armarx
     protected:
         void nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp, bool aValueChanged);
         std::set<std::string> initializedChannels;
-        Mutex initializedChannelsMutex;
+        std::mutex initializedChannelsMutex;
     private:
         std::string robotNodeSetName;
         std::set<std::string> robotNodes;
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 81f545e0d01e864ac1576718f841bb798987fa01..f830c604c404e232535cd1812e49154da09dc496 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -63,7 +63,7 @@ void KinematicUnitSimulation::onInitKinematicUnit()
     ARMARX_TRACE;
     {
         // duplicate the loop because of locking
-        boost::mutex::scoped_lock lock(jointStatesMutex);
+        std::unique_lock lock(jointStatesMutex);
 
         // initialize JoinStates
         for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
@@ -115,7 +115,7 @@ void KinematicUnitSimulation::simulationFunction()
     };
 
     {
-        boost::mutex::scoped_lock lock(jointStatesMutex);
+        std::unique_lock lock(jointStatesMutex);
 
         for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
         {
@@ -221,7 +221,7 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
     bool aValueChanged = false;
     NameControlModeMap changedControlModes;
     {
-        boost::mutex::scoped_lock lock(jointStatesMutex);
+        std::unique_lock lock(jointStatesMutex);
 
         for (NameControlModeMap::const_iterator it = targetJointModes.begin(); it != targetJointModes.end(); it++)
         {
@@ -256,7 +256,7 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
 void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
 {
 
-    boost::mutex::scoped_lock lock(jointStatesMutex);
+    std::unique_lock lock(jointStatesMutex);
     // name value maps for reporting
     NameValueMap actualJointAngles;
     bool aValueChanged = false;
@@ -321,7 +321,7 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
 
 void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
 {
-    boost::mutex::scoped_lock lock(jointStatesMutex);
+    std::unique_lock lock(jointStatesMutex);
     NameValueMap actualJointVelocities;
     bool aValueChanged = false;
 
@@ -354,7 +354,7 @@ void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJoint
 
 void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c)
 {
-    boost::mutex::scoped_lock lock(jointStatesMutex);
+    std::unique_lock lock(jointStatesMutex);
     NameValueMap actualJointTorques;
     bool aValueChanged = false;
 
@@ -408,7 +408,7 @@ void KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJo
 void KinematicUnitSimulation::stop(const Ice::Current& c)
 {
 
-    boost::mutex::scoped_lock lock(jointStatesMutex);
+    std::unique_lock lock(jointStatesMutex);
 
     for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
     {
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index cc3dae20ac7476572a4250c384a6395c7fd7d61b..f6512ecf5f261c3afa4047b6d87b5d6b3ff50811 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -29,10 +29,12 @@
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/system/ImportExportComponent.h>
 #include <RobotAPI/libraries/core/PIDController.h>
-#include <random>
 
 #include <IceUtil/Time.h>
 
+#include <random>
+#include <mutex>
+
 namespace armarx
 {
     /**
@@ -178,7 +180,7 @@ namespace armarx
         KinematicUnitSimulationJointStates jointStates;
         KinematicUnitSimulationJointStates previousJointStates;
         KinematicUnitSimulationJointInfos jointInfos;
-        boost::mutex jointStatesMutex;
+        std::mutex jointStatesMutex;
         IceUtil::Time lastExecutionTime;
         float noise;
         int intervalMs;
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
index 0944336bfaf52400a6685ce58d4b1c409a3e86c3..04bd47d5a9cd4931ad9783391e346c88d29aa4ce 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -64,7 +64,7 @@ namespace armarx
 
     void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         if (!existsChannel(device))
         {
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.h b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
index 4b093eb49cc12f3d07b457f744f3fda78e8872c0..05aa07cb9c36668d72d4fa8046c855c28e0ab735 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.h
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
@@ -29,6 +29,7 @@
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -78,7 +79,7 @@ namespace armarx
                                 const TimestampBasePtr& timestamp, const Ice::Current& c) override;
 
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
 
     };
 }
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
index b6885037df8700f4f6c09b01d0483a3c277210b2..95937330857902c4127d0ffe3b35eebff3e8d86d 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
@@ -64,7 +64,7 @@ namespace armarx
 
     void armarx::MetaWearIMUObserver::reportIMUValues(const std::string& name, const MetaWearIMUData& data, const TimestampBasePtr& timestamp, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         if (!existsChannel(name))
         {
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.h b/source/RobotAPI/components/units/MetaWearIMUObserver.h
index 6dba1f74653563fc132706e39d23036b2595ae4a..87eb6b49fbd390a66aef98c4f0a22d174115d659 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.h
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.h
@@ -29,6 +29,7 @@
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -74,7 +75,7 @@ namespace armarx
 
 
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
         DebugDrawerInterfacePrx debugDrawerPrx;
 
         void offerVector3(const std::string& channelName, const std::string& dfName, const std::vector<float>& data);
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
index ce09bacd4543fb2ac779229d5c02de89bf3d6e05..f87a5b56edb87cc841b71da64a5018d42ace1274 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -65,7 +65,7 @@ namespace armarx
 
     void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
         if (!existsChannel(name))
@@ -86,7 +86,7 @@ namespace armarx
 
     /*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.h b/source/RobotAPI/components/units/OptoForceUnitObserver.h
index 3bdc63e1a6b874cb9005260801c8c11869b009c7..0d415cb2bc89396f9fc0d81669009f034b17c646 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.h
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.h
@@ -29,6 +29,7 @@
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -81,7 +82,7 @@ namespace armarx
 
 
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
         DebugDrawerInterfacePrx debugDrawerPrx;
 
 
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 514a1767b5c95ad06228d87f4c09df94ff87f833..75640877635972e5cbf11f078580487834f64d4d 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -59,7 +59,7 @@ namespace armarx
 
     void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
         std::stringstream ss;
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
index 033f9cd08bd4f82bf399eb33df20eb2ac5a4956c..70916163f96bfa87944269e5bcf3f85a88bb5278 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
@@ -29,6 +29,7 @@
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+#include <mutex>
 
 namespace armarx
 {
@@ -80,7 +81,7 @@ namespace armarx
 
 
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
         DebugDrawerInterfacePrx debugDrawerPrx;
     };
 }
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index b4b4879b4ac4c0fd3246565c06eef9f83d50b35c..93b61e1d4f7338e888e55e0427e203a89843f731 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -94,7 +94,7 @@ namespace armarx
         lastExecutionTime = now;
         std::vector<float> vel(3, 0.0f);
         {
-            ScopedLock lock(currentPoseMutex);
+            std::unique_lock lock(currentPoseMutex);
             switch (platformMode)
             {
                 case ePositionControl:
@@ -177,7 +177,7 @@ namespace armarx
     {
         ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
         {
-            ScopedLock lock(currentPoseMutex);
+            std::unique_lock lock(currentPoseMutex);
             platformMode = ePositionControl;
             targetPositionX = targetPlatformPositionX;
             targetPositionY = targetPlatformPositionY;
@@ -199,7 +199,7 @@ namespace armarx
     void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
     {
         ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
-        ScopedLock lock(currentPoseMutex);
+        std::unique_lock lock(currentPoseMutex);
         platformMode = eVelocityControl;
         linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
         linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
@@ -211,7 +211,7 @@ namespace armarx
     {
         float targetPositionX, targetPositionY, targetRotation;
         {
-            ScopedLock lock(currentPoseMutex);
+            std::unique_lock lock(currentPoseMutex);
             targetPositionX = currentPositionX + targetPlatformOffsetX;
             targetPositionY = currentPositionY + targetPlatformOffsetY;
             targetRotation = currentRotation + targetPlatformOffsetRotation;
@@ -221,7 +221,7 @@ namespace armarx
 
     void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
     {
-        ScopedLock lock(currentPoseMutex);
+        std::unique_lock lock(currentPoseMutex);
         maxLinearVelocity = positionalVelocity;
         maxAngularVelocity = orientaionalVelocity;
     }
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index 8c7743a7f940c74860597157a46beb16b4de6794..c6d9851b1345a8aad3bd296b99f7c06ea774fec6 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -99,7 +99,7 @@ namespace armarx
         PropertyDefinitionsPtr createPropertyDefinitions() override;
 
     protected:
-        boost::mutex currentPoseMutex;
+        std::mutex currentPoseMutex;
         IceUtil::Time lastExecutionTime;
         int intervalMs;
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index d5f45c229d9595643638eb7965354c2591afe952..28bc1e4819946d6734110fa3831e33f4bc1009b0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -7,6 +7,8 @@
 
 #include <VirtualRobot/math/Helpers.h>
 
+#include <iomanip>
+
 #include "NJointCartesianNaturalPositionController.h"
 
 namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index b8b5c3c93a55497807556278ac357a2c2c5f7115..6c1b1930b58993ae49a58d487cc540503abbc0d5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -7,6 +7,8 @@
 
 #include "NJointCartesianWaypointController.h"
 
+#include <iomanip>
+
 namespace armarx
 {
     std::ostream& operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 7f73e77b62de92a209368a3f379e0c9e1d3da81e..e42b68f1d78db459b423fd93e2c6723fa1cdd39e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -80,7 +80,7 @@ namespace armarx
         return d;
     }
 
-    boost::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
+    std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
     {
         ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
         auto result = used;
@@ -90,13 +90,13 @@ namespace armarx
             {
                 if (used.at(i))
                 {
-                    return boost::optional<std::vector<char>>();
+                    return std::nullopt;
                 }
                 result.at(i) = true;
             }
 
         }
-        return {true, std::move(result)};
+        return std::move(result);
     }
 
     NJointControllerStatus NJointControllerBase::getControllerStatus(const Ice::Current&) const
@@ -282,7 +282,7 @@ namespace armarx
         onExitNJointController();
         // make sure the destructor of the handles does not throw an exception and triggers a termination of the process
         ARMARX_DEBUG << "Deleting thread handles";
-        ScopedLock lock(threadHandlesMutex);
+        std::unique_lock lock(threadHandlesMutex);
         for (auto& pair : threadHandles)
         {
             try
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 1f6e157696ee50f33a216cf6665d271b11be7939..e20d9525d3c36676e47203a25a11d4e26eaf2968 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -28,14 +28,11 @@
 #include <functional>
 #include <unordered_set>
 
-#include <boost/optional.hpp>
-
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/util/Registrar.h>
 #include <ArmarXCore/core/util/TripleBuffer.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/services/tasks/ThreadPool.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 
 #include <ArmarXGui/interface/WidgetDescription.h>
 
@@ -53,13 +50,8 @@
 #include "../util/JointAndNJointControllers.h"
 #include "../util/ControlThreadOutputBuffer.h"
 
-namespace VirtualRobot
-{
-    class Robot;
-    class RobotNode;
-    using RobotNodePtr = boost::shared_ptr<RobotNode>;
-    using RobotPtr = boost::shared_ptr<Robot>;
-}
+#include <VirtualRobot/VirtualRobot.h>
+#include <optional>
 
 namespace armarx::RobotUnitModule
 {
@@ -707,7 +699,7 @@ namespace armarx
         template < typename Task >
         void runTask(const std::string& taskName, Task&& task)
         {
-            ScopedLock lock(threadHandlesMutex);
+            std::unique_lock lock(threadHandlesMutex);
             ARMARX_CHECK_EXPRESSION(!taskName.empty());
             ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
             ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
@@ -717,7 +709,7 @@ namespace armarx
             threadHandles[taskName] = handlePtr;
         }
         std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
-        Mutex threadHandlesMutex;
+        std::mutex threadHandlesMutex;
 
         // //////////////////////////////////////////////////////////////////////////////////////// //
         // ///////////////////////////////////// ice interface //////////////////////////////////// //
@@ -877,11 +869,11 @@ namespace armarx
         const std::map<std::string, const JointController*>& getControlDevicesUsedJointController();
 
         //check for conflict
-        boost::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
-        boost::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
+        std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
+        std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
 
         template<class ItT>
-        static boost::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last);
+        static std::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last);
         // //////////////////////////////////////////////////////////////////////////////////////// //
         // ////////////////////////////////////// publishing ////////////////////////////////////// //
         // //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index bcefd5a83350721c67aff181034f5f0ef80fe8d6..0b89eaf5f18d12223a623d3b1eba13d672ddfd3b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -149,7 +149,7 @@ namespace armarx
         return controlDeviceUsedJointController;
     }
 
-    inline boost::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
+    inline std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
     {
         return isNotInConflictWith(other->getControlDeviceUsedBitmap());
     }
@@ -273,11 +273,11 @@ namespace armarx
 namespace armarx
 {
     template<class ItT>
-    inline boost::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
+    inline std::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
     {
         if (first == last)
         {
-            return {true, std::vector<char>{}};
+            return std::vector<char>{};
         }
         std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
         std::vector<char> inuse(n, false);
@@ -288,10 +288,10 @@ namespace armarx
             {
                 return r;
             }
-            inuse = std::move(r.get());
+            inuse = std::move(*r);
             ++first;
         }
-        return {true, std::move(inuse)};
+        return std::move(inuse);
     }
 }
 namespace armarx::detail
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index cb80274fe3e8e0fb67715fcc887576afb9fba8a2..bc0c3d694843f9c207a38ce2b30c5f5d8ff75a52 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -22,6 +22,8 @@
 
 #include "NJointTaskSpaceImpedanceController.h"
 
+#include <VirtualRobot/MathTools.h>
+
 
 using namespace armarx;
 
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 1cb5f37569f80c4d8ada490a901c6633c61877e1..8f5960857d371d51d9053113a3b032732f3bb905 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -364,7 +364,7 @@ namespace armarx::RobotUnitModule
                 ARMARX_ERROR << ss.str();
                 throw InvalidArgumentException {ss.str()};
             }
-            inuse = std::move(r.get());
+            inuse = std::move(*r);
         }
         ARMARX_DEBUG << "all requested controllers are conflict free" << std::flush;
         auto printInUse = ARMARX_STREAM_PRINTER
@@ -393,7 +393,7 @@ namespace armarx::RobotUnitModule
                     ARMARX_DEBUG << "keeping  already requested NJointControllerBase '"
                                  << nJointCtrl->getInstanceName() << "' in   list of requested controllers";
                     ctrlsToAct.insert(nJointCtrl);
-                    inuse = std::move(r.get());
+                    inuse = std::move(*r);
                 }
                 else
                 {
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 526db6aa7e1771f8dc234a8bc890be2f3003e6be..219ba7815eba0cb2b03c4a5c93ccbe0e0973d175 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -24,6 +24,8 @@
 #include "RobotUnitModuleControlThreadDataBuffer.h"
 #include "RobotUnitModuleRobotData.h"
 
+#include <ArmarXCore/core/util/StringHelpers.h>
+
 namespace armarx::RobotUnitModule
 {
     const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
@@ -679,17 +681,14 @@ namespace armarx::RobotUnitModule
                                        controlDeviceHardwareControlModeGroupsStr.end(), ';'
                                    ) + 1;
             ctrlModeGroups.groups.reserve(numGroups);
-            std::vector<std::string> strGroups;
-            strGroups.reserve(numGroups);
-            boost::algorithm::split(strGroups, controlDeviceHardwareControlModeGroupsStr, boost::is_any_of(";"));
+            std::vector<std::string> strGroups = Split(controlDeviceHardwareControlModeGroupsStr, ";");
             for (const auto& gstr : strGroups)
             {
-                std::vector<std::string> strElems;
-                boost::algorithm::split(strElems, gstr, boost::is_any_of(","));
+                bool trimDeviceNames = true;
+                std::vector<std::string> strElems = Split(gstr, ",", trimDeviceNames);
                 std::set<std::string> group;
                 for (auto& device : strElems)
                 {
-                    boost::algorithm::trim(device);
                     ARMARX_CHECK_EXPRESSION_W_HINT(
                         !device.empty(),
                         "The ControlDeviceHardwareControlModeGroups property contains empty device names"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index f5d357ce82651f0f5aa52c94bf8015a158f85f75..70da5df0a915823ed517022cd287ce16b5eb057b 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -30,6 +30,8 @@
 
 #include "../util/ControlThreadOutputBuffer.h"
 
+#include <boost/algorithm/string/predicate.hpp>
+
 namespace armarx::RobotUnitModule
 {
     void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index a75e7810451595119ef11af4d711c931e27f4b79..a1c4fb5941d1108f8d5626f7995d690b6733f8e1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -25,6 +25,8 @@
 #include "../NJointControllers/NJointController.h"
 #include "../Units/RobotUnitSubUnit.h"
 
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 namespace armarx::RobotUnitModule
 {
     /**
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 12acad70bbf3e49c8d2aaa4ff6e69c299570fd8c..541147c28728402614fb24648376efd893615746 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -30,6 +30,10 @@
 
 #include "RobotUnitModuleRobotData.h"
 
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/trim.hpp>
+
 namespace armarx::RobotUnitModule
 {
     const std::string& RobotData::getRobotPlatformName() const
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index aeea0cfd4ac9bbd1f6c899c1d887d8749b1f92d8..1e0ce16e1da5103672893f829883630ec186eb6d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -29,6 +29,9 @@
 
 #include "../NJointControllers/NJointController.h"
 
+#include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
 #define FLOOR_OBJ_STR "FLOOR"
 
 namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 65e8263cc44f4669dd43f0c4b084840f76b2b847..7913d2dd9a4a27c7b90ee4b6d4e5a7b9d4d13736 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -24,8 +24,6 @@
 
 #include <mutex>
 
-#include <boost/algorithm/string.hpp>
-
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 #include <RobotAPI/components/units/KinematicUnit.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index f67bb14d7a0df2743ee3d65410f58f768937f1fb..99ec57ddd276fe00bbc9ca9c76967d8e1d56ac11 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -21,6 +21,8 @@
  */
 #include "PlatformSubUnit.h"
 
+#include <boost/algorithm/clamp.hpp>
+
 void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
 {
     if (!getProxy())
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 7420f33e06d32adbbcc66d1b0ed09dc66effdfd5..7fb01dd2a3d2f49059a5906c7ea998ed8c95a4d9 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -24,8 +24,6 @@
 
 #include <mutex>
 
-#include <boost/algorithm/clamp.hpp>
-
 #include <Eigen/Core>
 
 #include <VirtualRobot/MathTools.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index f978876e538d9b8420af57d8c35b8f8f8063a9bd..1c98f6b653589f3a6f6fd611d4695e1933111458 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -58,7 +58,7 @@ void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
 
 void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
     std::string tcp;
     if (tcpName.empty())
@@ -185,7 +185,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
 
 bool TCPControllerSubUnit::isRequested(const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     for (auto& pair : tcpControllerNameMap)
     {
         auto ctrl = robotUnit->getNJointController(pair.second);
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index b834d2b8adb16a0405bd3c60834693a3f7df4f57..cb7f6603057bdf2219d3f2aa92506bc6d76d2d53 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -27,6 +27,9 @@
 #include "RobotUnitSubUnit.h"
 #include "../NJointControllers/NJointTCPController.h"
 #include "../util.h"
+
+#include <mutex>
+
 namespace armarx
 {
     TYPEDEF_PTRS_HANDLE(RobotUnit);
@@ -57,7 +60,7 @@ namespace armarx
         // RobotUnitSubUnit interface
         void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
     private:
-        mutable Mutex dataMutex;
+        mutable std::mutex dataMutex;
         RobotUnit* robotUnit = nullptr;
         VirtualRobot::RobotPtr coordinateTransformationRobot;
         std::map<std::string, std::string> tcpControllerNameMap;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index 0a3669091152c2de2ec2caad4865a0e3d6b6c08d..4af60478ff5da6a4a57d055dd919473a64b1d1d5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -158,7 +158,7 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
 {
     ARMARX_DEBUG << "Loading Trajectory ...";
 
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
 
     this->jointTraj = TrajectoryPtr::dynamicCast(jointTraj);
 
@@ -215,7 +215,7 @@ void TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& base
 
 void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
 
     if (!controllerExists())
     {
@@ -237,7 +237,7 @@ double TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
 
 double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
 
     if (!controllerExists())
     {
@@ -251,7 +251,7 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
 {
     ARMARX_DEBUG << "Setting end-time ...";
 
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
 
     if (!jointTraj)
     {
@@ -306,7 +306,7 @@ bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, b
 {
     ARMARX_DEBUG << "Setting joints in use ...";
 
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
 
     if (controllerExists())
     {
@@ -366,7 +366,7 @@ void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::
 
 NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist)
 {
-    ScopedRecursiveLock lock(controllerMutex);
+    std::unique_lock lock(controllerMutex);
 
     if (controllerExists() && deleteIfAlreadyExist)
     {
@@ -413,7 +413,7 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
 
 void TrajectoryControllerSubUnit::assureTrajectoryController()
 {
-    ScopedRecursiveLock lock(controllerMutex);
+    std::unique_lock lock(controllerMutex);
 
     if (!controllerExists())
     {
@@ -424,7 +424,7 @@ void TrajectoryControllerSubUnit::assureTrajectoryController()
 
 bool TrajectoryControllerSubUnit::controllerExists()
 {
-    ScopedRecursiveLock lock(controllerMutex);
+    std::unique_lock lock(controllerMutex);
 
     auto allNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
     NJointTrajectoryControllerPtr trajController;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index d50323bf97082b1fb89df256ddf63784c0b7c277..b0b65db9dad4ee9cd94742d27b542cb03719492a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -30,6 +30,8 @@
 
 #include "KinematicSubUnit.h"
 
+#include <mutex>
+
 namespace armarx
 {
     class TrajectoryControllerSubUnitPropertyDefinitions:
@@ -152,8 +154,8 @@ namespace armarx
         DebugDrawerInterfacePrx debugDrawer;
         PeriodicTask<TrajectoryControllerSubUnit>::pointer_type previewTask;
 
-        Mutex dataMutex;
-        RecursiveMutex controllerMutex;
+        std::mutex dataMutex;
+        std::recursive_mutex controllerMutex;
         bool recreateController = true;
 
         // Component interface
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index bef26a5e7c92e97d3ab2889e4081b59caee0daec..2c18c79e6bd840ed60f5660a858e104faecc3de6 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -27,7 +27,7 @@
 #include <ArmarXCore/core/exceptions/Exception.h>
 #include <RobotAPI/interface/units/UnitInterface.h>
 
-#include <boost/thread/mutex.hpp>
+#include <mutex>
 
 namespace armarx
 {
@@ -125,7 +125,7 @@ namespace armarx
         */
         virtual void onStop() {};
 
-        boost::mutex unitMutex;
+        std::mutex unitMutex;
         Ice::Identity ownerId;
         UnitExecutionState  executionState;
     };
diff --git a/source/RobotAPI/components/units/SpeechObserver.cpp b/source/RobotAPI/components/units/SpeechObserver.cpp
index bdc7f41b93b19e0491444127c29ec89e4a5cd31b..29c03c5eefe9ff3089c662adf61bee28cd8817e7 100644
--- a/source/RobotAPI/components/units/SpeechObserver.cpp
+++ b/source/RobotAPI/components/units/SpeechObserver.cpp
@@ -74,7 +74,7 @@ std::string SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
 
 void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     reportStateCounter++;
     setDataField("TextToSpeech", "State", Variant(SpeechStateToString(state)));
     setDataField("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter));
@@ -83,7 +83,7 @@ void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current
 
 void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     reportTextCounter++;
     setDataField("TextToSpeech", "Text", Variant(text));
     setDataField("TextToSpeech", "TextChangeCounter", Variant(reportTextCounter));
@@ -92,7 +92,7 @@ void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
 
 void SpeechObserver::reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     ARMARX_WARNING << "reportTextWithParams is not implemented";
 }
 
@@ -105,7 +105,7 @@ SpeechListenerImpl::SpeechListenerImpl(SpeechObserver* obs) :
 
 void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     JSONObject json;
     json.fromString(text);
     obs->setDataField("SpeechToText", "RecognizedText", Variant(json.getString("text")));
@@ -113,6 +113,6 @@ void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::
 
 void armarx::SpeechListenerImpl::reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&)
 {
-    ScopedLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     ARMARX_WARNING << "reportTextWithParams is not implemented";
 }
diff --git a/source/RobotAPI/components/units/SpeechObserver.h b/source/RobotAPI/components/units/SpeechObserver.h
index 43e3bbc6fc37511b052d590fde29a9e8a09fabd9..2cf428ace331d0afc54e5e711dcd60304b542172 100644
--- a/source/RobotAPI/components/units/SpeechObserver.h
+++ b/source/RobotAPI/components/units/SpeechObserver.h
@@ -26,6 +26,8 @@
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/observers/SpeechObserverInterface.h>
 
+#include <mutex>
+
 namespace armarx
 {
     class SpeechObserverPropertyDefinitions:
@@ -46,7 +48,7 @@ namespace armarx
         SpeechListenerImpl(SpeechObserver* obs);
     protected:
         SpeechObserver* obs;
-        Mutex dataMutex;
+        std::mutex dataMutex;
         // TextListenerInterface interface
     public:
         void reportText(const std::string&, const Ice::Current&) override;
@@ -78,7 +80,7 @@ namespace armarx
 
         static std::string SpeechStateToString(TextToSpeechStateType state);
     private:
-        Mutex dataMutex;
+        std::mutex dataMutex;
         int reportTextCounter = 0;
         int reportStateCounter = 0;
     };
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 8f4bd0e84990ce14bdc686609e3fdbb317321247..37e1df461dc90453d02567bc65feb6deeb744daf 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -23,18 +23,21 @@
 #include "TCPControlUnit.h"
 #include <RobotAPI/libraries/core/LinkedPose.h>
 
-#include <boost/unordered_map.hpp>
-
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <VirtualRobot/RobotConfig.h>
 #include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/MathTools.h>
 
 #include <Eigen/Core>
 
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
 #include <memory>
+#include <cfloat>
 
 using namespace VirtualRobot;
 using namespace Eigen;
@@ -66,7 +69,7 @@ namespace armarx
 
     void TCPControlUnit::onConnectComponent()
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         debugObs  = getTopic<DebugObserverInterfacePrx>("DebugObserver");
         kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
@@ -170,7 +173,7 @@ namespace armarx
 
     void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
     {
-        ScopedLock lock(taskMutex);
+        std::unique_lock lock(taskMutex);
         cycleTime = milliseconds;
 
         if (execTask)
@@ -187,7 +190,7 @@ namespace armarx
             request();
         }
 
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
 
 
@@ -266,7 +269,7 @@ namespace armarx
         }
 
         ARMARX_IMPORTANT << "Requesting TCPControlUnit";
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         requested = true;
 
         execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
@@ -278,7 +281,7 @@ namespace armarx
     void TCPControlUnit::release(const Ice::Current& c)
     {
         ARMARX_IMPORTANT << "Releasing TCPControlUnit";
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
 
         //        while (calculationRunning)
         //        {
@@ -302,9 +305,9 @@ namespace armarx
 
     void TCPControlUnit::periodicExec()
     {
-        ScopedTryLock lock(dataMutex);
+        std::unique_lock lock(dataMutex, std::defer_lock);
 
-        if (lock.owns_lock())
+        if (lock.try_lock())
         {
 
             {
@@ -358,7 +361,7 @@ namespace armarx
                 localDIKMap[data.nodeSetName] = dIk;
             }
 
-            EDifferentialIKPtr dIk = boost::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
+            auto dIk = std::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
             dIk->clearGoals();
         }
 
@@ -476,7 +479,7 @@ namespace armarx
         {
             RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
 
-            EDifferentialIKPtr dIK = boost::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
+            auto dIK = std::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
             //            ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
             //            dIK->setVerbose(true);
             Eigen::VectorXf jointDelta;
@@ -656,7 +659,7 @@ namespace armarx
             }
             else
             {
-                VR_ERROR << "Internal error?!" << endl;    // Error
+                VR_ERROR << "Internal error?!" << std::endl;    // Error
             }
 
 
@@ -752,9 +755,9 @@ namespace armarx
                 ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
                 jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
 
-                if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
+                if (std::isnan(jv[i]) || std::isinf(jv[i]))
                 {
-                    ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << endl;
+                    ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << std::endl;
                     dofWeights = tempDOFWeights;
                     return false;
                 }
@@ -776,7 +779,7 @@ namespace armarx
             {
                 if (verbose)
                 {
-                    ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << endl;
+                    ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << std::endl;
                 }
 
                 break;
@@ -789,14 +792,14 @@ namespace armarx
             if (dTheta.norm() < mininumChange)
             {
                 //                if (verbose)
-                ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
+                ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << std::endl;
                 break;
             }
 
             if (checkImprovement && posDist > lastDist)
             {
                 //                if (verbose)
-                ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << endl;
+                ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << std::endl;
                 robot->setJointValues(rns, jvBest);
                 break;
             }
@@ -835,9 +838,9 @@ namespace armarx
 
         if (step >=  maxNStep && verbose)
         {
-            ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << endl;
-            ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << endl;
-            ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << endl;
+            ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << std::endl;
+            ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << std::endl;
+            ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << std::endl;
             return false;
         }
 
@@ -901,7 +904,7 @@ namespace armarx
             }
             else
             {
-                VR_ERROR << "Internal error?!" << endl;    // Error
+                VR_ERROR << "Internal error?!" << std::endl;    // Error
             }
 
 
@@ -1051,7 +1054,7 @@ namespace armarx
             }
             else
             {
-                VR_ERROR << "Internal error?!" << endl;    // Error
+                VR_ERROR << "Internal error?!" << std::endl;    // Error
             }
 
 
@@ -1221,7 +1224,7 @@ namespace armarx
         }
 
         Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1);
-        //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << endl;
+        //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << std::endl;
         //cout << boost::format("Error Position < %1% >: %2%") % tcp->getName() % position.norm()  << std::endl;
         float result = 0.0f;
         Eigen::VectorXf tcpWeight(3);
@@ -1297,7 +1300,7 @@ void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&,
 
 void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&)
 {
-    ScopedLock lock(reportMutex);
+    std::unique_lock lock(reportMutex);
     FramedPoseBaseMap tcpPoses;
     std::string rootFrame =  localReportRobot->getRootNode()->getName();
     auto it = jointAngles.find("timestamp");
@@ -1330,7 +1333,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
     }
 
     lastTopicReportTime = TimeUtil::GetTime();
-    ScopedLock lock(reportMutex);
+    std::unique_lock lock(reportMutex);
 
     if (!localVelReportRobot)
     {
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index cc984d4358f9e0b1dbec9455f6cb26283aa876bf..8b373d10178d0975bbf78831b08859c15781f9b6 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -25,13 +25,14 @@
 #include <RobotAPI/interface/units/TCPControlUnit.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 #include <ArmarXCore/core/Component.h>
 
 #include <VirtualRobot/IK/DifferentialIK.h>
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 #include <ArmarXCore/interface/observers/ObserverInterface.h>
 
+#include <memory>
+
 namespace armarx
 {
     /**
@@ -211,9 +212,9 @@ namespace armarx
         FramedPoseBaseMap lastTCPPoses;
         IceUtil::Time lastTopicReportTime;
 
-        Mutex dataMutex;
-        Mutex taskMutex;
-        Mutex reportMutex;
+        std::mutex dataMutex;
+        std::mutex taskMutex;
+        std::mutex reportMutex;
         bool calculationRunning;
         double syncTimeDelta;
 
@@ -276,7 +277,7 @@ namespace armarx
         std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf> tcpWeights;
         Eigen::VectorXf tcpWeightVec;
     };
-    using EDifferentialIKPtr = boost::shared_ptr<EDifferentialIK>;
+    using EDifferentialIKPtr = std::shared_ptr<EDifferentialIK>;
 
 }
 
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
index 2c34c81588359653d6c2592568a0c822a8f7a1b1..8fda50da01097a9f7eb8ba77d056dfb19dda12e1 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
@@ -74,7 +74,7 @@ namespace armarx
 
     void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         //        ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
         FramedPoseBaseMap::const_iterator it = poseMap.begin();
 
@@ -126,7 +126,7 @@ namespace armarx
 
     void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current&)
     {
-        ScopedLock lock(dataMutex);
+        std::unique_lock lock(dataMutex);
         FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
 
         for (; it != tcpTranslationVelocities.end(); it++)
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index 88cdf43c30f840a00c3b82857bf51b059aba8ad0..92859f6e597b85231d02a6fd1b00b92970f642fa 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -24,6 +24,8 @@
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/units/TCPControlUnit.h>
 
+#include <mutex>
+
 namespace armarx
 {
     /**
@@ -76,7 +78,7 @@ namespace armarx
         void reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
 
-        Mutex dataMutex;
+        std::mutex dataMutex;
     };
 
 }
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 38e01c30f7cb4cfdd13ae802c6e3c7dc31e714b2..31ed73e85ab61b61ed48ede725bc03447114e05f 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
@@ -44,7 +44,7 @@ void GamepadUnit::onConnectComponent()
     sendTask = new SimplePeriodicTask<>([&]
     {
         ARMARX_TRACE;
-        ScopedLock lock(mutex);
+        std::unique_lock lock(mutex);
         if (!js.opened())
         {
             return;
@@ -114,7 +114,7 @@ void GamepadUnit::run()
             }
         }
         ARMARX_TRACE;
-        ScopedLock lock(mutex);
+        std::unique_lock lock(mutex);
         IceUtil::Time now = IceUtil::Time::now();
         dataTimestamp = new TimestampVariant(now);
 
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index 85ac99dc262ec38a2b1e4e3b1464e97e4866f22e..109ce81a17a98c5154b396e09064d9943be6ff0a 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -31,7 +31,6 @@
 
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 #include <ArmarXCore/core/services/tasks/TaskUtil.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 
 #include <RobotAPI/interface/units/GamepadUnit.h>
 
@@ -115,7 +114,7 @@ namespace armarx
         SimplePeriodicTask<>::pointer_type sendTask;
 
         void run();
-        Mutex mutex;
+        std::mutex mutex;
         std::string deviceName;
         Joystick js;
         GamepadData data;
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 9f1508cce0928aa91169c61cd36f5389ef6d75aa..af108e0e0839fe4b8d1d6c912163e67e0ffeaec9 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -24,9 +24,7 @@
 
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
 
-#include <boost/algorithm/string/split.hpp>
 #include <boost/algorithm/string/replace.hpp>
-#include <boost/algorithm/string/classification.hpp>
 
 #include <HokuyoLaserScannerDriver/urg_utils.h>
 
@@ -45,14 +43,12 @@ void HokuyoLaserUnit::onInitComponent()
     angleOffset = getProperty<float>("AngleOffset").getValue();
 
     std::string deviceStrings = getProperty<std::string>("Devices").getValue();
-    std::vector<std::string> splitDeviceStrings;
-    boost::split(splitDeviceStrings, deviceStrings, boost::is_any_of(";"));
+    std::vector<std::string> splitDeviceStrings = Split(deviceStrings, ";");
     devices.clear();
     devices.reserve(splitDeviceStrings.size());
     for (std::string const& deviceString : splitDeviceStrings)
     {
-        std::vector<std::string> deviceInfo;
-        boost::split(deviceInfo, deviceString, boost::is_any_of(","));
+        std::vector<std::string> deviceInfo = Split(deviceString, ",");
         if (deviceInfo.size() != 3)
         {
             ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 0856bed5a856ca6114a3fa3c2c6ba13a833c0fae..61a75d997f2ef1871b12cffc78cde76cb797b3ca 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -5,6 +5,7 @@
 #include <fcntl.h>
 #include <math.h>
 #include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
@@ -316,8 +317,7 @@ void OrientedTactileSensorUnit::run()
 OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::string line)
 {
     SensorData data;
-    std::vector<std::string> splitValues;
-    boost::split(splitValues, line, boost::is_any_of(" "));
+    std::vector<std::string> splitValues = Split(line, " ");
     data.id = stoi(splitValues.at(0));
     data.pressure = std::stof(splitValues.at(1));
     data.qw = std::stof(splitValues.at(2));
@@ -358,8 +358,7 @@ bool OrientedTactileSensorUnit::loadCalibration()
 
 bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
 {
-    std::vector<std::string> splitValues;
-    boost::split(splitValues, line, boost::is_any_of(" "));
+    std::vector<std::string> splitValues = Split(line, " ");
     calibration.accel_offset_x = stoi(splitValues.at(0));
     calibration.accel_offset_y = stoi(splitValues.at(1));
     calibration.accel_offset_z = stoi(splitValues.at(2));
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index d8a1c3496194a4b1fa3850a67e9b53159a683bc2..7e584629a24c0673cd3fb145a28ea3b771358974 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -9,7 +9,6 @@
 #include <iostream>
 #include <fstream>
 #include <stdio.h>
-#include <boost/date_time/posix_time/posix_time.hpp>
 #include <Eigen/Dense>
 #include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
index 8559064a772d5b9dc299df845b56481c7f031b68..b0a2f0b3705d9c00c88fff8e0470651f8f3f2f43 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
@@ -35,7 +35,6 @@
 #include <sys/stat.h>
 #include <sys/ioctl.h>
 #include <iostream>
-#include <boost/lexical_cast.hpp>
 #include <boost/format.hpp>
 
 static inline tcflag_t __bitrate_to_flag(unsigned int bitrate)
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 3f13ff1bcc6aa970425931d46afe02cf12657c1c..899c86434cea6757fb0cbb899203131452dea07d 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -123,7 +123,7 @@ std::string WeissHapticSensor::getDeviceName()
 
 void WeissHapticSensor::scheduleSetDeviceTag(std::string tag)
 {
-    boost::mutex::scoped_lock lock(mutex);
+    std::unique_lock lock(mutex);
     setDeviceTagValue = tag;
     setDeviceTagScheduled = true;
 }
@@ -206,7 +206,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
 
         if (setDeviceTagScheduled)
         {
-            boost::mutex::scoped_lock lock(mutex);
+            std::unique_lock lock(mutex);
             setDeviceTagScheduled = false;
 
             ARMARX_INFO << "[" << device << "] Stopping periodic frame aquisition to set new device tag";
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
index 9e006ae3d9f367d041d7c501eb62142b02f62d1c..25191aab19608735c499bacfe3f9d7ae0fb1e401 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -34,7 +34,8 @@
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
 #include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
 #include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-#include <boost/thread/mutex.hpp>
+
+#include <mutex>
 
 namespace armarx
 {
@@ -73,7 +74,7 @@ namespace armarx
         bool setDeviceTagScheduled;
         std::string setDeviceTagValue;
 
-        boost::mutex mutex;
+        std::mutex mutex;
         int minimumReportIntervalMs;
     };
 }
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
index fa70bd0987bcf9b62ba26634bc863ec89b57d3f5..f4a9f5673d6d782a774fe27333f449fee5cb57b6 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -25,6 +25,8 @@
 
 #include <QTimer>
 
+#include <boost/algorithm/string/join.hpp>
+
 #include <string>
 
 
@@ -85,7 +87,7 @@ namespace armarx
         m->addObject(debugDrawer, false);
 
         {
-            boost::recursive_mutex::scoped_lock lock(*mutex3D);
+            std::unique_lock lock(*mutex3D);
             rootVisu->addChild(debugDrawer->getVisualization());
         }
         enableMainWidgetAsync(true);
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 4ace604723047d0cb2688feaeedf286a3b944815..5c86b2dda49248a4148b5f3c83785272a94789f9 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -25,6 +25,7 @@
 #include "HandUnitConfigDialog.h"
 #include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
 // Qt headers
 #include <Qt>
@@ -35,7 +36,7 @@
 #include <QMessageBox>
 #include <QTimer>
 
-#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
+#include <cmath>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
index 43b5ca8e42613ea6b6a2e9110329bf482aa87ef6..78b41fd88ccc8fbe9a397be585af03ce28f1d483 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
@@ -22,6 +22,8 @@
 
 #include "HomogeneousMatrixCalculatorWidgetController.h"
 
+#include <Eigen/Dense>
+
 #include <string>
 
 
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 76b9edea96974cce02ccb474e0a0e8452a5fe122..17ce02826bd5eb2ab427cfc88310292b82d2d6f3 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -131,7 +131,7 @@ void KinematicUnitWidgetController::onInitComponent()
 
 
     {
-        boost::recursive_mutex::scoped_lock lock(*mutex3D);
+        std::unique_lock lock(*mutex3D);
         debugLayerVisu = new SoSeparator();
         debugLayerVisu->ref();
         debugLayerVisu->addChild(debugDrawer->getVisualization());
@@ -182,13 +182,9 @@ void KinematicUnitWidgetController::onConnectComponent()
             }
 
             CMakePackageFinder project(projectName);
-            Ice::StringSeq projectIncludePaths;
             auto pathsString = project.getDataDir();
             ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
-            boost::split(projectIncludePaths,
-                         pathsString,
-                         boost::is_any_of(";,"),
-                         boost::token_compress_on);
+            Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
             ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
             includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
         }
@@ -262,14 +258,14 @@ void KinematicUnitWidgetController::onDisconnectComponent()
     enableMainWidgetAsync(false);
 
     {
-        boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+        std::unique_lock lock(mutexNodeSet);
         robot.reset();
         robotNodeSet.reset();
         currentNode.reset();
     }
 
     {
-        boost::recursive_mutex::scoped_lock lock(*mutex3D);
+        std::unique_lock lock(*mutex3D);
         robotVisu->removeAllChildren();
         debugLayerVisu->removeAllChildren();
     }
@@ -280,7 +276,7 @@ void KinematicUnitWidgetController::onExitComponent()
     enableMainWidgetAsync(false);
 
     {
-        boost::recursive_mutex::scoped_lock lock(*mutex3D);
+        std::unique_lock lock(*mutex3D);
 
         if (robotVisu)
         {
@@ -376,7 +372,7 @@ void KinematicUnitWidgetController::copyToClipboard()
 {
     NameValueMap values;
     {
-        boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+        std::unique_lock lock(mutexNodeSet);
 
         if (selectedControlMode == ePositionControl)
         {
@@ -470,7 +466,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
     NameValueMap jointAngles;
     NameControlModeMap jointModes;
@@ -668,7 +664,7 @@ VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string
     return robot;
 }
 
-CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
+VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
 {
     CoinVisualizationPtr coinVisualization;
 
@@ -806,7 +802,7 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
 
 void KinematicUnitWidgetController::selectJoint(int i)
 {
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
 
     if (!robotNodeSet || i < 0 || i >= (int)robotNodeSet->getSize())
     {
@@ -846,7 +842,7 @@ void KinematicUnitWidgetController::selectJointFromTableWidget(int row, int colu
 
 void KinematicUnitWidgetController::sliderValueChanged(int pos)
 {
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
 
     if (!currentNode)
     {
@@ -939,7 +935,7 @@ void KinematicUnitWidgetController::updateControlModesTable()
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
 
     for (unsigned int i = 0; i < rn.size(); i++)
@@ -1011,7 +1007,7 @@ void KinematicUnitWidgetController::updateJointStatusesTable()
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
 
     for (unsigned int i = 0; i < rn.size(); i++)
@@ -1085,7 +1081,7 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
 
     float dirty_squaresum = 0;
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     if (jointAnglesUpdateFrequency && jointAnglesUpdateFrequency->getValue())
     {
 
@@ -1145,7 +1141,7 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable()
     }
 
     float dirty_squaresum = 0;
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     if (!robotNodeSet)
     {
         return;
@@ -1185,7 +1181,7 @@ void KinematicUnitWidgetController::updateJointTorquesTable()
 {
 
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     if (!getWidget() || !robotNodeSet)
     {
         return;
@@ -1213,7 +1209,7 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
 {
 
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
     {
         return;
@@ -1244,7 +1240,7 @@ void KinematicUnitWidgetController::updateMotorTemperaturesTable()
 {
 
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
     {
         return;
@@ -1278,7 +1274,7 @@ void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointA
     }
 
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     jointAnglesUpdateFrequency->update(timestamp, new Variant((double)(timestamp - lastJointAngleUpdateTimestamp) * 1e-6));
     lastJointAngleUpdateTimestamp = timestamp;
     mergeMaps(reportedJointAngles, jointAngles);
@@ -1300,7 +1296,7 @@ void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jo
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     mergeMaps(reportedJointVelocities, jointVelocities);
     if (skipper.checkFrequency(c))
     {
@@ -1315,7 +1311,7 @@ void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& joint
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     reportedJointTorques = jointTorques;
     if (skipper.checkFrequency(c))
     {
@@ -1332,7 +1328,7 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
 {
     //    if(!aValueChanged && reportedJointControlModes.size() > 0)
     //        return;
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     mergeMaps(reportedJointControlModes, jointModes);
     if (skipper.checkFrequency(c))
     {
@@ -1342,7 +1338,7 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
 
 void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
 {
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
 
     if (aValueChanged && jointCurrents.size() > 0)
     {
@@ -1371,7 +1367,7 @@ void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValue
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     mergeMaps(this->reportedJointTemperatures, jointMotorTemperatures);
     if (skipper.checkFrequency(c))
     {
@@ -1386,7 +1382,7 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& joi
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     mergeMaps(reportedJointStatuses, jointStatuses);
     if (skipper.checkFrequency(c))
     {
@@ -1398,7 +1394,7 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& joi
 void KinematicUnitWidgetController::updateModel()
 {
     //    ARMARX_INFO << "updateModel()" << flush;
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
     if (!robotNodeSet)
     {
         return;
@@ -1413,7 +1409,7 @@ void KinematicUnitWidgetController::highlightCriticalValues()
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+    std::unique_lock lock(mutexNodeSet);
 
     std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
 
@@ -1480,10 +1476,8 @@ void KinematicUnitWidgetController::highlightCriticalValues()
     }
 }
 
-void KinematicUnitWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D)
+void KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
 {
-    //ARMARX_IMPORTANT << "KinematicUnitWidgetController controller " << getInstanceName() << ": set mutex " << mutex3D.get();
-
     this->mutex3D = mutex3D;
 
     if (debugDrawer)
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 047655ab2555949865fa9cc0b13b9dd4cda48642..e0d9428388657a0bc07dc871d8ca6a126af26d95 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -54,13 +54,12 @@
 #include <QStyledItemDelegate>
 #include <ArmarXCore/core/util/IceReportSkipper.h>
 
-#include <boost/shared_ptr.hpp>
-#include <boost/cstdint.hpp>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <mutex>
 
 namespace armarx
 {
-    using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>;
-
     class KinematicUnitConfigDialog;
 
     /*!
@@ -176,7 +175,7 @@ namespace armarx
         void updateGuiElements();
 
         // overwrite setMutex, so that we can inform the debugdrawer
-        void setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D) override;
+        void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
 
         QPointer<QWidget> getCustomTitlebarWidget(QWidget* parent) override;
 
@@ -245,7 +244,7 @@ namespace armarx
         VirtualRobot::RobotNodeSetPtr robotNodeSet;
         VirtualRobot::RobotNodePtr currentNode;
 
-        CoinVisualizationPtr kinematicUnitVisualization;
+        VirtualRobot::CoinVisualizationPtr kinematicUnitVisualization;
         SoNode* kinematicUnitNode;
         SoSeparator* rootVisu;
         SoSeparator* robotVisu;
@@ -273,10 +272,10 @@ namespace armarx
         void on_pushButtonFromJson_clicked();
     private:
 
-        boost::recursive_mutex mutexNodeSet;
+        std::recursive_mutex mutexNodeSet;
         // init stuff
         VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
-        CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
+        VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
         VirtualRobot::RobotNodeSetPtr getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName);
         bool initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet);
         bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet);
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 4dc0dee2b359dc32e42785050c979031d0ed5063..68a21112e18da9b40ea282a899b5802d003279eb 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -36,6 +36,7 @@
 
 //std
 #include <memory>
+#include <cmath>
 
 using namespace armarx;
 
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 1c1c83237601ca2dda19101551d3c93295336c10..7df85b9141364c93721785486e19de74da364655 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -48,7 +48,7 @@ namespace armarx
             path p = path {homeDir} / ".cmake" / "packages";
             if (is_directory(p))
             {
-                for (const path& entry : boost::make_iterator_range(directory_iterator(p), {}))
+                for (const path& entry : directory_iterator(p))
                 {
                     const std::string pkg = entry.filename().string();
                     if (CMakePackageFinder {pkg, "", true} .packageFound())
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
index 3f2d74a09c04af7cdef406f37afd5f30feca3647..9141512a800d382cbe917659b1fd236b7598bae8 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
@@ -23,6 +23,8 @@
 #include <ArmarXCore/core/exceptions/Exception.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <thread>
+
 namespace armarx
 {
     RobotUnitWidgetBase::RobotUnitWidgetBase(QString name, QWidget* parent) :
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index 76479d44965d055fd707ea9e13eaa49ecf41e4e1..d4d399ecf19b96604c17a822f122ce8a4bf99870 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -31,7 +31,10 @@
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 #include <ArmarXCore/core/application/Application.h>
 
+#include <RobotAPI/libraries/core/FramedPose.h>
+
 #include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 
 // Qt headers
 #include <Qt>
@@ -45,6 +48,7 @@
 #include <Inventor/Qt/SoQt.h>
 #include <ArmarXGui/libraries/StructuralJson/JsonWriter.h>
 #include <ArmarXCore/util/json/JSONObject.h>
+
 // System
 #include <stdio.h>
 #include <string>
@@ -139,7 +143,7 @@ void RobotViewerWidgetController::onInitComponent()
 
 
         {
-            boost::recursive_mutex::scoped_lock lock(*mutex3D);
+            std::unique_lock lock(*mutex3D);
             debugLayerVisu = new SoSeparator();
             debugLayerVisu->ref();
             debugLayerVisu->addChild(debugDrawer->getVisualization());
@@ -180,13 +184,9 @@ void RobotViewerWidgetController::onConnectComponent()
             }
 
             CMakePackageFinder project(projectName);
-            Ice::StringSeq projectIncludePaths;
             auto pathsString = project.getDataDir();
             ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
-            boost::split(projectIncludePaths,
-                         pathsString,
-                         boost::is_any_of(";,"),
-                         boost::token_compress_on);
+            Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
             ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
             includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
 
@@ -282,7 +282,7 @@ void RobotViewerWidgetController::onDisconnectComponent()
     robotStateComponentPrx = nullptr;
 
     {
-        boost::recursive_mutex::scoped_lock lock(*mutex3D);
+        std::unique_lock lock(*mutex3D);
 
         if (robotVisu)
         {
@@ -301,7 +301,7 @@ void RobotViewerWidgetController::onExitComponent()
     enableMainWidgetAsync(false);
 
     {
-        boost::recursive_mutex::scoped_lock lock(*mutex3D);
+        std::unique_lock lock(*mutex3D);
 
         if (debugLayerVisu)
         {
@@ -380,7 +380,7 @@ void RobotViewerWidgetController::setRobotVisu(bool colModel)
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(*mutex3D);
+    std::unique_lock lock(*mutex3D);
 
     VirtualRobot::SceneObject::VisualizationType v = VirtualRobot::SceneObject::Full;
 
@@ -444,7 +444,7 @@ void RobotViewerWidgetController::showRobot(bool show)
         return;
     }
 
-    boost::recursive_mutex::scoped_lock lock(*mutex3D);
+    std::unique_lock lock(*mutex3D);
 
     if (show && rootVisu->findChild(robotVisu) < 0)
     {
@@ -679,7 +679,7 @@ void RobotViewerWidgetController::onConfigurationChanged()
 
 void RobotViewerWidgetController::updateRobotVisu()
 {
-    boost::recursive_mutex::scoped_lock lock(*mutex3D);
+    std::unique_lock lock(*mutex3D);
 
     if (!robotStateComponentPrx || !robot)
     {
@@ -718,7 +718,7 @@ void RobotViewerWidgetController::updateRobotVisu()
 
 void RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
 {
-    boost::recursive_mutex::scoped_lock lock(*mutex3D);
+    std::unique_lock lock(*mutex3D);
 
     for (auto& model : robot->getCollisionModels())
     {
@@ -728,7 +728,7 @@ void RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
     setRobotVisu(true);
 }
 
-void RobotViewerWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D)
+void RobotViewerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
 {
     //ARMARX_IMPORTANT << "set mutex3d :" << mutex3D.get();
     this->mutex3D = mutex3D;
@@ -741,7 +741,7 @@ void RobotViewerWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_
 
 void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long timestamp, bool poseChanged, const Ice::Current&)
 {
-    boost::recursive_mutex::scoped_lock lock(*mutex3D);
+    std::unique_lock lock(*mutex3D);
     if (!robotStateComponentPrx || !robot)
     {
         return;
@@ -756,7 +756,7 @@ void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const Framed
 
 void armarx::RobotViewerWidgetController::reportJointValues(const NameValueMap& jointAngles, Ice::Long, bool aValueChanged, const Ice::Current&)
 {
-    boost::recursive_mutex::scoped_lock lock(*mutex3D);
+    std::unique_lock lock(*mutex3D);
 
     if (!robotStateComponentPrx || !robot || !aValueChanged)
     {
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index 9292a3666978a4741ef8f6a75dd6da4021d8ef8d..48a161aa9eed86e54d45d7cf58a753b258880005 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -44,14 +44,10 @@
 #include <VirtualRobot/Visualization/VisualizationFactory.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
 
-#include <boost/shared_ptr.hpp>
-
 #include <Inventor/sensors/SoTimerSensor.h>
 
 namespace armarx
 {
-    using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>;
-
     class RobotViewerConfigDialog;
 
     /**
@@ -117,7 +113,7 @@ namespace armarx
         SoNode* getScene() override;
 
         // overwrite setMutex, so that we can inform the debugdrawer
-        void setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D) override;
+        void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
 
     signals:
         void robotStatusUpdated();
@@ -171,7 +167,7 @@ namespace armarx
         // init stuff
         VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
 
-        CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
+        VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
 
         QPointer<QWidget> __widget;
         QPointer<SimpleConfigDialog> dialog;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
index ab2ad3915589358848b643d624dae4270eb035a8..0bbbf7011e67bcb59c3e1dacbc0e5fe5541f1221 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
@@ -2,7 +2,6 @@
 
 #include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
 #include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <boost/smart_ptr/shared_ptr.hpp>
 #include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
 #include <RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h>
 
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index 41701aaf3f7953445affcfd0292a581120637905..ad1a33863adfa1148400a2166182790a64c6a53d 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -7,6 +7,8 @@
 #include <ctime>
 #include <chrono>
 #include <thread>
+#include <iomanip>
+
 #include <ArmarXCore/core/logging/Logging.h>
 //#include <Armar6RT/units/Armar6Unit/FunctionalDevices/Joint.h>
 //#include "Armar6RT/units/Armar6Unit/BusSlaves/SensorBoard.h"
@@ -1387,7 +1389,7 @@ bool EtherCAT::generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex,
         ARMARX_WARNING << "Bus is not running no write";
         return false;
     }
-    ScopedLock lock(etherCatMutex);
+    std::unique_lock lock(etherCatMutex);
     IceUtil::Time start = IceUtil::Time::now();
     int workCount = ec_SDOwrite(slave, index, subindex, (boolean) ca, buflen, buf, SDO_WRITE_TIMEOUT);
     IceUtil::Time end = IceUtil::Time::now();
@@ -1431,7 +1433,7 @@ bool EtherCAT::generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex,
     {
         return false;
     }
-    ScopedLock lock(etherCatMutex);
+    std::unique_lock lock(etherCatMutex);
     IceUtil::Time start = IceUtil::Time::now();
     int workCount = ec_SDOread(slave, index, subindex, (boolean) ca, &buflen, buf, SDO_READ_TIMEOUT);
     IceUtil::Time end = IceUtil::Time::now();
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
index e9bc549d71890e6345cd2ef8706b0d338baafcc6..f103df995c10cb2210071b2b58c8a368231d06d9 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
@@ -3,8 +3,6 @@
 
 #include "AbstractFunctionalDevice.h"
 
-#include <ArmarXCore/core/system/Synchronization.h>
-
 #include <IceUtil/Time.h>
 
 #include <iostream>
@@ -13,6 +11,7 @@
 #include <memory>
 #include <sstream>
 #include <atomic>
+#include <mutex>
 
 
 /**
@@ -236,7 +235,7 @@ namespace armarx
         RobotUnit* mainUnitPtr;
 
         std::string ifname;
-        mutable Mutex etherCatMutex;
+        mutable std::mutex etherCatMutex;
         bool emergencyStopWasActivated = false;
         IceUtil::Time busUpdateLastUpdateTime;
         IceUtil::Time ermergencyStopRecoverStartpoint;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index c4dd0f79ab5d8b0b953f7933c2d8e9fcceddd89f..0fa7cc0b918228cc21398ca5ccd4825b2c78cfdb 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -40,6 +40,7 @@
 #include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
 #include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 #include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
 #include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
@@ -50,8 +51,9 @@
 #include <VirtualRobot/RobotNodeSet.h>
 #include <sched.h>
 #include <sys/mman.h>
-#include <syscall.h>
 #include <sys/stat.h>
+#include <fcntl.h>
+#include <syscall.h>
 #include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index a040355c292794e446ed5ff6b7c8adf1228f57ba..9cb91dae9d98f8ee798851c37c0d60282a09414c 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -28,6 +28,7 @@
 
 
 #include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/MathTools.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <RobotAPI/libraries/core/Trajectory.h>
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
index 109cf497d8f2becb93953215df58f4070bb38131..87973287030bf4dac0f45a0a0af25c8fb930afe7 100644
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
@@ -21,7 +21,9 @@
  */
 
 #include "DSJointCarryController.h"
+
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 using namespace armarx;
 
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
index 3cd1070441917ed0fc0fbbed77aecd9f63b5df3b..b27e2d95809ae52f73c5c6e341f123bef6e6b941 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
@@ -21,7 +21,9 @@
  */
 
 #include "DSRTBimanualController.h"
+
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
index 42f3555d878961c5e5082d8e98ec5f21f18c2a32..53d89208654b540280009cc687fcc47194e2ee71 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
@@ -21,7 +21,9 @@
  */
 
 #include "DSRTController.h"
+
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
index bee6f061d2051bc0307b172ee57d01fc8c209aef..4130b956b90dbae919c281d7ec9eacea13a3880c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
@@ -1,6 +1,7 @@
 #include "NJointBimanualForceController.h"
 
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 02e92853f94cd09f89d443c72438f2b81cf55d25..7f5f67758fe368038d697df0beda4f96b22af98a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -1,6 +1,7 @@
 #include "NJointBimanualObjLevelController.h"
 
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
index 19b87eb3d8c2402ab6aacb1f48026e565e2ba918..630e11b7630c28fa15807a364a30e867fdf881f2 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
@@ -1,5 +1,7 @@
 #include "NJointAdaptiveWipingController.h"
 
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointAdaptiveWipingController> registrationControllerNJointAdaptiveWipingController("NJointAdaptiveWipingController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
index 730062df308da449c88d308edfc8547fbbf142aa..843a8704145eef3c80b133bd2f1e225e012f5fd9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -1,5 +1,7 @@
 #include "NJointAnomalyDetectionAdaptiveWipingController.h"
 
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController> registrationControllerNJointAnomalyDetectionAdaptiveWipingController("NJointAnomalyDetectionAdaptiveWipingController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
index cfd77287685f0719fe865819dc5a94eadcb760df..f62e71798edaaac3e00ddd5e9b243f362415cbd6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
@@ -1,6 +1,7 @@
 #include "NJointBimanualCCDMPVelocityController.h"
 
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 
 namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index 3e57047ea1d09a8b5d469e1f067489ddb5dea3e5..11d0637a53ea10892ddf40f82b3f3cb64b6d61fd 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -1,6 +1,8 @@
 #include "NJointJSDMPController.h"
 
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/archive/text_iarchive.hpp>
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 2742cd9944d5b5bc74cbc6d1a76b344aae51d182..b8e560bb9dd0f341cde6a8ae57bf65ae58ff196a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -1,5 +1,7 @@
 #include "NJointPeriodicTSDMPCompliantController.h"
 
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointPeriodicTSDMPCompliantController> registrationControllerNJointPeriodicTSDMPCompliantController("NJointPeriodicTSDMPCompliantController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
index 8d892d2c7ddf40dbef0b3bace0835d6890b848b7..24b98a2ea15d0919be10196f834482d385983dcb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
@@ -1,5 +1,7 @@
 #include "NJointPeriodicTSDMPForwardVelController.h"
 
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointPeriodicTSDMPForwardVelController> registrationControllerNJointPeriodicTSDMPForwardVelController("NJointPeriodicTSDMPForwardVelController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index b05288e5c1d00dd8d5de429e7b5a3ed2ef5d8dcc..b63554a7dc4ae326a6d1e80af6210581590c6594 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -1,4 +1,7 @@
 #include "NJointTSDMPController.h"
+
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/archive/text_iarchive.hpp>
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 724ad162a736bf3b1e55990381e3ff6fe2fb307c..e284a6c0866fa35d34baa580ede3976b0049afef 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -1,5 +1,7 @@
 #include "NJointTaskSpaceImpedanceDMPController.h"
 
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController");
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 3cfdc7cac8d3b756479dd3166b0caf93bb318b94..becf5e2ec7f6d035fb87b59b6615d04b04ed16f2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -28,6 +28,7 @@
 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
 #include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index b4d16c5e2e3bff8fdbcf2d32469076825306bfff..10cb284d673b1eb358a289a043044271a99552b2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -22,7 +22,11 @@
  */
 
 #include "RobotNameHelper.h"
+
 #include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
+
+#include <Eigen/Dense>
 
 namespace armarx
 {
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index bd95565df108c2b30086bb83aae8a2588cb9cc86..eca096cc5e62727236ad8c04e482d7ab4f5cdfec 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -25,15 +25,12 @@
 
 #include <RobotAPI/interface/core/RobotState.h>
 
-#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
-
-#include <boost/optional.hpp>
-
 #include <VirtualRobot/Robot.h>
 
 namespace armarx
 {
     using RobotNameHelperPtr = std::shared_ptr<class RobotNameHelper>;
+    using StatechartProfilePtr = std::shared_ptr<class StatechartProfile>;
 
     class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
     {
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index c759e1f768e1b766ee5ae77bcd670ab3ff15f448..b1f6850fbf3cd8067a0903c8a6973561f2418c9d 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -21,7 +21,6 @@
  *             GNU General Public License
  */
 
-#include <VirtualRobot/math/Helpers.h>
 
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -30,6 +29,9 @@
 
 #include "CartesianNaturalPositionController.h"
 
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
+
 namespace armarx
 {
     CartesianNaturalPositionController::CartesianNaturalPositionController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& elbow,
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index af8d731a99eb24bc7082a7c3d7cb9f08278492cb..122d5b4d82d99194b3ac451be8e24343373d8742 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -21,7 +21,6 @@
  *             GNU General Public License
  */
 
-#include <VirtualRobot/math/Helpers.h>
 
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -30,6 +29,10 @@
 
 #include "CartesianWaypointController.h"
 
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
+#include <cfloat>
+
 namespace armarx
 {
     CartesianWaypointController::CartesianWaypointController(
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index e7a796a6eb65e02cb59ac06751278d73912ca8cc..3e987d97dacfa905903e8ac6f86cbf3c8afb87fb 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -33,20 +33,14 @@
 #include <ArmarXCore/observers/AbstractObjectSerializer.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/LinkedCoordinate.h>
+
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <boost/shared_ptr.hpp>
-
 #include <sstream>
 
-namespace VirtualRobot
-{
-    class Robot;
-    using RobotPtr = boost::shared_ptr<Robot>;
-    class LinkedCoordinate;
-}
-
 namespace armarx::VariantType
 {
     // variant types
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 56ff6d28cd17522453014f9b880d8e88294ea3dc..f25048c8fec61c0923c8da06532f1a05570fba87 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -28,10 +28,11 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 
 #include <Eigen/Core>
 
+#include <mutex>
+
 namespace armarx
 {
     template <int dimensions = Eigen::Dynamic>
@@ -218,7 +219,7 @@ namespace armarx
         double maxControlValue;
         double maxDerivation;
         bool firstRun;
-        mutable  RecursiveMutex mutex;
+        mutable  std::recursive_mutex mutex;
         bool threadSafe = true;
         std::vector<bool> limitless;
     private:
@@ -231,6 +232,8 @@ namespace armarx
             PIDVectorX zeroVec;
         } stackAllocations;
 
+        using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
+        using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
         ScopedRecursiveLockPtr getLock() const
         {
             if (threadSafe)
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 34c5e6c30469a5ef7d38d19a90a0bf5d67f57b5f..02f198137de13f4687bb212753b204b9ea49aaca 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -68,11 +68,11 @@ namespace armarx
         }
     }
 
-    ScopedRecursiveLockPtr PIDController::getLock() const
+    auto PIDController::getLock() const -> ScopedRecursiveLockPtr
     {
         if (threadSafe)
         {
-            return std::make_shared<ScopedRecursiveLock>(mutex);
+            return std::make_unique<ScopedRecursiveLock>(mutex);
         }
         else
         {
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index b8631b35b9154e28962149e3c1ca5688447d59ac..320f2e6146f018b9803d34dac4a4418dd22aebcc 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -28,10 +28,12 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
-#include <ArmarXCore/core/system/Synchronization.h>
 
 #include <Eigen/Core>
 
+#include <memory>
+#include <mutex>
+
 namespace armarx
 {
 
@@ -73,8 +75,10 @@ namespace armarx
         rtfilters::RTFilterBasePtr differentialFilter;
         rtfilters::RTFilterBasePtr pdOutputFilter;
     private:
+        using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
+        using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
         ScopedRecursiveLockPtr getLock() const;
-        mutable RecursiveMutex mutex;
+        mutable std::recursive_mutex mutex;
     };
     using PIDControllerPtr = std::shared_ptr<PIDController>;
 
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
index 154cfb2890c9fb2f0f5e6a8b2867bf82ea68d775..2b9444a4d594a6483c435638fad3385f9b544f57 100644
--- a/source/RobotAPI/libraries/core/RobotPool.cpp
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -43,7 +43,7 @@ namespace armarx
 
     VirtualRobot::RobotPtr RobotPool::getRobot(int inflation)
     {
-        ScopedLock lock(mutex);
+        std::scoped_lock lock(mutex);
         for (auto& r : robots[inflation])
         {
             if (r.use_count() == 1)
@@ -61,7 +61,7 @@ namespace armarx
 
     size_t RobotPool::getPoolSize() const
     {
-        ScopedLock lock(mutex);
+        std::scoped_lock lock(mutex);
         size_t size = 0;
         for (auto& pair : robots)
         {
@@ -72,7 +72,7 @@ namespace armarx
 
     size_t RobotPool::getRobotsInUseCount() const
     {
-        ScopedLock lock(mutex);
+        std::scoped_lock lock(mutex);
         size_t count = 0;
         for (auto& pair : robots)
         {
@@ -89,7 +89,7 @@ namespace armarx
 
     size_t RobotPool::clean()
     {
-        ScopedLock lock(mutex);
+        std::scoped_lock lock(mutex);
         size_t count = 0;
         for (auto& pair : robots)
         {
diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h
index 6aa0ccb24c917a6e76c2d2a0e1d06f9cc8ae16be..4d6db6fc1ed2ba14a1f3c9bffe2b7557deaf0508 100644
--- a/source/RobotAPI/libraries/core/RobotPool.h
+++ b/source/RobotAPI/libraries/core/RobotPool.h
@@ -22,8 +22,11 @@
  *             GNU General Public License
  */
 #pragma once
+
 #include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/system/Synchronization.h>
+
+#include <mutex>
+
 namespace armarx
 {
 
@@ -53,7 +56,7 @@ namespace armarx
     private:
         std::map<int, std::vector<VirtualRobot::RobotPtr>> robots;
         VirtualRobot::RobotPtr baseRobot;
-        mutable Mutex mutex;
+        mutable std::mutex mutex;
     };
     using RobotPoolPtr = std::shared_ptr<RobotPool>;
 }
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
index 6e4988445a86ee0a2e42efc1ddcc4da2600d3609..cb3a549b7d45fdc48d727da8a46db0586512917e 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
@@ -28,6 +28,10 @@
 #include <ArmarXCore/core/system/ImportExportComponent.h>
 #include <ArmarXCore/statechart/Statechart.h>
 
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/trim.hpp>
+
 namespace armarx
 {
     // ****************************************************************
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index cdb7046dbf34e051339eabf8bfe4342c4c5d00f8..2e330626166b264497319869534e99a2f093896e 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -42,7 +42,7 @@ namespace armarx::filters
 
         VariantBasePtr calculate(const Ice::Current&) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
@@ -73,7 +73,7 @@ namespace armarx::filters
 
         VariantBasePtr calculate(const Ice::Current&) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
@@ -104,7 +104,7 @@ namespace armarx::filters
 
         VariantBasePtr calculate(const Ice::Current&) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
@@ -140,7 +140,7 @@ namespace armarx::filters
 
         VariantBasePtr calculate(const Ice::Current&) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
@@ -199,7 +199,7 @@ namespace armarx::filters
 
         VariantBasePtr calculate(const Ice::Current&) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
@@ -248,7 +248,7 @@ namespace armarx::filters
         }
         VariantBasePtr calculate(const Ice::Current&) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
index 42c89f07eadf0ec85339654db7f5baeee4a6743b..eaf21147840e8ec1c2e95fa7eedbb218f5518ed0 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
@@ -39,7 +39,7 @@ armarx::filters::MedianDerivativeFilterV3::MedianDerivativeFilterV3(std::size_t
 
 armarx::VariantBasePtr armarx::filters::MedianDerivativeFilterV3::calculate(const Ice::Current& c) const
 {
-    ScopedLock lock(historyMutex);
+    std::unique_lock lock(historyMutex);
 
     if (dataHistory.size() == 0)
     {
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index d161d4ca39078718e8b37653e60e536dcbf0d0f3..68b2cc80905a7747ab73c04f9d19c873b4e3fb77 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -55,7 +55,7 @@ namespace armarx::filters
         VariantBasePtr calculate(const Ice::Current& c = Ice::emptyCurrent) const override
         {
 
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             VariantPtr newVariant;
 
@@ -135,7 +135,7 @@ namespace armarx::filters
 
             if (firstRun)
             {
-                ScopedLock lock(historyMutex);
+                std::unique_lock lock(historyMutex);
 
                 if (dataHistory.size() == 0)
                 {
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
index 5b0cf4dbe22bedcaee456006c02c83c08566631d..aa3b14347927cbbcd068f0164f2c92b86012fabd 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
@@ -28,7 +28,7 @@ namespace armarx::filters
 
     VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
     {
-        ScopedLock lock(historyMutex);
+        std::unique_lock lock(historyMutex);
 
         if (dataHistory.size() == 0)
         {
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 8b43dd5487849e96fb9bdc34de95c5ce96a039c7..33bbed17812e9c8d06336da6ce37ca9b7e73703e 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -50,7 +50,7 @@ namespace armarx::filters
     public:
         VariantBasePtr calculate(const Ice::Current& c) const override
         {
-            ScopedLock lock(historyMutex);
+            std::unique_lock lock(historyMutex);
 
             if (dataHistory.size() == 0)
             {
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
index 42bdc1c19577e18c7f157885549a39c7055dbceb..e4d4a1444065ad83bbc681fa83832bca5ccabb29 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
@@ -37,7 +37,7 @@ armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize)
 
 armarx::VariantBasePtr armarx::filters::PoseMedianOffsetFilter::calculate(const Ice::Current& c) const
 {
-    ScopedLock lock(historyMutex);
+    std::unique_lock lock(historyMutex);
 
     if (dataHistory.size() == 0)
     {
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index dfc84e32aa84f0bba6e323322cd78cab998473be..99495ecee5a9b0ce1402f6fb182eaa63341f80a4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -22,23 +22,30 @@
  *             GNU General Public License
  */
 #include "RemoteRobot.h"
+
 #include <boost/foreach.hpp>
 #include <boost/format.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/RobotFactory.h>
 #include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
 #include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
 #include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 
 
-
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-#include <Eigen/Geometry>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/core/logging/Logging.h>
 
-//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj << endl;
+#include <Eigen/Geometry>
+
+//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj;
 #define DMES(Obj) ;
 
 using namespace VirtualRobot;
@@ -47,7 +54,7 @@ using namespace Eigen;
 namespace armarx
 {
 
-    boost::recursive_mutex RemoteRobot::m;
+    std::recursive_mutex RemoteRobot::m;
 
     RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
         Robot(),
@@ -199,12 +206,12 @@ namespace armarx
 
     VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, RobotPtr robo)
     {
-        boost::recursive_mutex::scoped_lock cloneLock(m);
+        std::scoped_lock cloneLock(m);
         static int nonameCounter = 0;
 
         if (!remoteNode || !robo)
         {
-            ARMARX_ERROR_S << " NULL data " << endl;
+            ARMARX_ERROR_S << " NULL data ";
             return VirtualRobot::RobotNodePtr();
         }
 
@@ -217,7 +224,7 @@ namespace armarx
 
         if (name.empty())
         {
-            ARMARX_LOG_S << "Node without name!!!" << endl;
+            ARMARX_LOG_S << "Node without name!!!";
             std::stringstream ss;
             ss << "robot_node_" << nonameCounter;
             nonameCounter++;
@@ -285,7 +292,7 @@ namespace armarx
             break;
 
             default:
-                ARMARX_ERROR_S << "JointType nyi..." << endl;
+                ARMARX_ERROR_S << "JointType nyi...";
                 return VirtualRobot::RobotNodePtr();
                 break;
         }
@@ -308,14 +315,14 @@ namespace armarx
                 /* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase);
                 if (!rnRemote)
                 {
-                    ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i] << endl;
+                    ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i];
                     continue;
                 }*/
 
 
                 if (!localNode)
                 {
-                    ARMARX_ERROR_S << "Could not create local node: " << children[i] << endl;
+                    ARMARX_ERROR_S << "Could not create local node: " << children[i];
                     continue;
                 }
 
@@ -329,8 +336,7 @@ namespace armarx
 
     VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
     {
-        //ARMARX_IMPORTANT_S << "RemoteRobot local clone" << flush;
-        boost::recursive_mutex::scoped_lock cloneLock(m);
+        std::scoped_lock cloneLock(m);
         std::string robotType = getName();
         std::string robotName = getName();
         VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName, robotType));
@@ -347,7 +353,7 @@ namespace armarx
 
         if (!res)
         {
-            ARMARX_ERROR_S << "Failed to initialize local robot..." << endl;
+            ARMARX_ERROR_S << "Failed to initialize local robot...";
             return VirtualRobot::RobotPtr();
         }
 
@@ -375,24 +381,24 @@ namespace armarx
     {
         RobotPtr result;
 
-        boost::recursive_mutex::scoped_lock cloneLock(m);
-        ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
+        std::scoped_lock cloneLock(m);
+        ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")";
 
 
         if (!sharedRobotPrx)
         {
-            ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..." << endl;
+            ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting...";
             return result;
         }
 
         if (filename.empty())
         {
-            RemoteRobotPtr rob(new RemoteRobot(sharedRobotPrx));
+            std::shared_ptr<RemoteRobot> rob(new RemoteRobot(sharedRobotPrx));
             result = rob->createLocalClone();
 
             if (!result)
             {
-                ARMARX_ERROR_S << "Could not create local clone. Aborting..." << endl;
+                ARMARX_ERROR_S << "Could not create local clone. Aborting...";
                 return result;
             }
         }
@@ -429,7 +435,7 @@ namespace armarx
 
             if (!result)
             {
-                ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
+                ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting...";
                 return result;
             }
         }
@@ -460,12 +466,12 @@ namespace armarx
     {
         if (!robot)
         {
-            ARMARX_ERROR << "Robot is NULL! Aborting..." << endl;
+            ARMARX_ERROR << "Robot is NULL! Aborting...";
             return false;
         }
         if (!sharedRobotPrx)
         {
-            ARMARX_ERROR_S << "shared robot prx is NULL! Aborting..." << endl;
+            ARMARX_ERROR_S << "shared robot prx is NULL! Aborting...";
             return false;
         }
         PoseBasePtr globalPose;
@@ -480,7 +486,7 @@ namespace armarx
 
             if (!c->setConfig(jointName, jointAngle))
             {
-                ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+                ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
             }
         }
 
@@ -518,7 +524,7 @@ namespace armarx
 
             if (!c->setConfig(jointName, jointAngle))
             {
-                ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+                ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
             }
         }
 
@@ -559,5 +565,10 @@ namespace armarx
         }
     }
 
+    CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker()
+    {
+        return VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
+    }
+
 
 }
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 815d81841a7b09abb3650da0ce98fcd8b357a700..42970b7e78c7f62fbc7d5ab42ec4a5107a6e6487 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -22,32 +22,18 @@
  *             GNU General Public License
  */
 #pragma once
-#ifndef _ARMARX_REMOTE_ROBOT_H__
-#define _ARMARX_REMOTE_ROBOT_H__
-
 
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/RobotNodeSet.h>
 #include <VirtualRobot/Nodes/RobotNode.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/interface/core/RobotState.h>
 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
 #include <VirtualRobot/Nodes/RobotNodePrismatic.h>
 #include <VirtualRobot/Nodes/RobotNodeFixed.h>
 #include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-// boost
-#include <boost/thread/mutex.hpp>
-#include <boost/utility/enable_if.hpp>
-#include <boost/type_traits/is_base_of.hpp>
-
-//namespace VirtualRobot
-//{
-//    class RobotNodeRevolute;
-//    class RobotNodePrismatic;
-//    class RobotNodeFixed;
-//}
+
+#include <mutex>
 
 namespace armarx
 {
@@ -73,6 +59,8 @@ namespace armarx
     template<>
     void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode);
 
+    VirtualRobot::CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker();
+
 
     /** @brief Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
      * @tparam VirtualRobotNodeType Must be a descendant of VirtualRobot::RobotNode
@@ -94,7 +82,7 @@ namespace armarx
             _node->getJointValueOffest();
             setJointLimits(_node->getJointLimitLow(), _node->getJointLimitHigh());
 
-            this->collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
+            this->collisionChecker = RemoteRobotNode_getGlobalCollisionChecker();
 
             RemoteRobotNodeInitializer<VirtualRobotNodeType>::initialize(this);
         }
@@ -150,13 +138,10 @@ namespace armarx
      */
     class RemoteRobot : public VirtualRobot::Robot
     {
-        template<class T>
-        friend void boost::checked_delete(T*);
-    protected:
-        ~RemoteRobot() override;
     public:
         RemoteRobot(SharedRobotInterfacePrx robot);
 
+        ~RemoteRobot() override;
 
         VirtualRobot::RobotNodePtr getRootNode() const override;
 
@@ -270,12 +255,11 @@ namespace armarx
         mutable std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
         mutable VirtualRobot::RobotNodePtr _root;
 
-        static boost::recursive_mutex m;
+        static std::recursive_mutex m;
 
         static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
     };
 
-    using RemoteRobotPtr = boost::shared_ptr<RemoteRobot>;
+    using RemoteRobotPtr = std::shared_ptr<RemoteRobot>;
 }
 
-#endif
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index b230a3bb08cc49535081c714b7926ba817ada50d..2c259eafc983fc5b78cdffb42c5471578f2ac3c8 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -26,6 +26,8 @@
 
 #include <VirtualRobot/VirtualRobot.h>
 
+#include <RobotAPI/libraries/core/FramedPose.h>
+
 #include <ArmarXCore/interface/core/BasicTypes.h>
 
 namespace armarx
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 9a2dbb98e6026f602741b6020b50e0d1fd1a9ad4..08fbb612d15a9734c86d1043aee74088a330171a 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -29,6 +29,7 @@
 #include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
 
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
 
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/RobotNodeSet.h>
@@ -39,6 +40,8 @@
 #include <VirtualRobot/IK/DifferentialIK.h>
 
 #include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
 
 #define TCP_POSE_CHANNEL "TCPPose"
 #define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
@@ -92,7 +95,7 @@ void RobotStateObserver::updatePoses()
         return;
     }
 
-    ScopedRecursiveLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     ReadLockPtr lock2 = robot->getReadLock();
     FramedPoseBaseMap tcpPoses;
     std::string rootFrame =  robot->getRootNode()->getName();
@@ -181,7 +184,7 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
         return;
     }
 
-    ScopedRecursiveLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
 
     if (!robot)
     {
@@ -316,7 +319,7 @@ PropertyDefinitionsPtr RobotStateObserver::createPropertyDefinitions()
 
 void RobotStateObserver::setRobot(RobotPtr robot)
 {
-    ScopedRecursiveLock lock(dataMutex);
+    std::unique_lock lock(dataMutex);
     this->robot = robot;
 
     std::vector< VirtualRobot::RobotNodeSetPtr > robotNodes;
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index e8c1146821c153ab5767805431e2a8c8a9d53163..e3acfd611dc6487744decc1c4d3183e33f74c383 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -31,15 +31,9 @@
 #include <ArmarXCore/observers/ConditionCheck.h>
 #include <ArmarXCore/observers/Observer.h>
 
-#include <string>
+#include <VirtualRobot/VirtualRobot.h>
 
-namespace VirtualRobot
-{
-    class Robot;
-    using RobotPtr = boost::shared_ptr<Robot>;
-    class RobotNode;
-    using RobotNodePtr = boost::shared_ptr<RobotNode>;
-}
+#include <string>
 
 namespace armarx
 {
@@ -100,7 +94,7 @@ namespace armarx
         RobotStateComponentInterfacePrx server;
         VirtualRobot::RobotPtr  robot, velocityReportRobot;
         std::vector<std::pair<VirtualRobot::RobotNodePtr, std::string> > nodesToReport;
-        RecursiveMutex dataMutex;
+        std::recursive_mutex dataMutex;
         IceUtil::Time lastVelocityUpdate;
 
         // RobotStateObserverInterface interface
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index f9a6e2a1022e46cae08520a4313a503fc52e022c..0f98f30dc1b8fa0b6408abe5786f6f7ee3647db2 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -27,6 +27,8 @@
 #include <RobotAPI/libraries/core/CartesianPositionController.h>
 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
+#include <cfloat>
+
 namespace armarx
 {
     Eigen::VectorXf NaturalDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
index f4c9a5af180f2ee62426584c3cbd4e0e06befc05..0bdafb506e4cf6a22203b9b88cde2eccacba32d6 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -23,8 +23,6 @@
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
-
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/RobotNodeSet.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index 6d0021aec8bab8f99e5fe64ddf86f969b4bdcf80..c1a91dacdec922aac74d8160fff0f8c03256353f 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -26,6 +26,8 @@
 #include <RobotAPI/libraries/core/CartesianPositionController.h>
 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
+#include <cfloat>
+
 namespace armarx
 {
     SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index 4ae4e22fa9ddf740455e9eec2cb561f365a7ca53..ffd371a6fe8d8ea8cd080e97fc1658c632b46868 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -23,14 +23,14 @@
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
-
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/RobotNodeSet.h>
 
+#include <memory>
+
 namespace armarx
 {
-    typedef boost::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
+    typedef std::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
 
     class SimpleDiffIK
     {
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index ea98170c468c82a599e5dd1f22a73f2a76ffaaa5..e6987e894a504547847e1afd546dbb3af8daa254 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -22,12 +22,14 @@
  */
 
 #include "CartesianNaturalPositionControllerProxy.h"
-#include <VirtualRobot/math/Helpers.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <RobotAPI/libraries/core/CartesianPositionController.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <RobotAPI/libraries/aron/AronNavigator.h>
 
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
+
 using namespace armarx;
 
 
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index d87c65c4bf5136cf2ea3812059242ed6710c87b5..eb75ba24394fb6deb1ae836b195e788d82957624 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -25,11 +25,12 @@
 
 #include "NaturalIK.h"
 
-#include <boost/shared_ptr.hpp>
 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 #include <RobotAPI/interface/aron.h>
 
+#include <memory>
+
 namespace armarx
 {
     typedef std::shared_ptr<class CartesianNaturalPositionControllerProxy> CartesianNaturalPositionControllerProxyPtr;