diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
index 73de8c3a8ec6487035985e022f5277463353c7c7..e0c3e4db489cdbe6a6434616348229b68c552abc 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
@@ -4,8 +4,9 @@
 
 #include <RobotAPI/interface/ArViz/Elements.h>
 
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <Inventor/nodes/SoComplexity.h>
+#include <Inventor/nodes/SoScale.h>
+#include <Inventor/nodes/SoSphere.h>
 
 namespace armarx::viz::coin
 {
@@ -13,43 +14,33 @@ namespace armarx::viz::coin
     {
         using ElementType = data::ElementEllipsoid;
 
+        VisualizationEllipsoid()
+        {
+            complexity = new SoComplexity();
+            complexity->type.setValue(SoComplexity::OBJECT_SPACE);
+            complexity->value.setValue(1.0f);
+
+            scale = new SoScale;
+
+            sphere = new SoSphere();
+            // We create a unit sphere and create an ellipsoid through scaling
+            sphere->radius.setValue(1.0f);
+
+            node->addChild(complexity);
+            node->addChild(scale);
+            node->addChild(sphere);
+        }
+
         bool update(ElementType const& element)
         {
-            auto color = element.color;
-            constexpr float conv = 1.0f / 255.0f;
-            const float r = color.r * conv;
-            const float g = color.g * conv;
-            const float b = color.b * conv;
-            const float a = color.a * conv;
-
-            VirtualRobot::VisualizationNodePtr ellipsoid_node;
-            {
-                // Params.
-                SoMaterial* mat = new SoMaterial;
-                mat->diffuseColor.setValue(r, g, b);
-                mat->ambientColor.setValue(r, g, b);
-                mat->transparency.setValue(1. - a);
-                const bool show_axes = false;  // Do not show axes.  If needed, draw a Pose instead.
-
-                SoSeparator* res = new SoSeparator();
-                res->ref();
-                SoUnits* u = new SoUnits();
-                u->units = SoUnits::MILLIMETERS;
-                res->addChild(u);
-                res->addChild(VirtualRobot::CoinVisualizationFactory::CreateEllipse(
-                                  element.axisLengths.e0, element.axisLengths.e1,
-                                  element.axisLengths.e2, mat, show_axes));
-                ellipsoid_node.reset(new VirtualRobot::CoinVisualizationNode(res));
-                res->unref();
-            }
-
-            SoNode* ellipsoid = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(
-                                    *ellipsoid_node).getCoinVisualization();
-
-            node->removeAllChildren();
-            node->addChild(ellipsoid);
+            scale->scaleFactor.setValue(element.axisLengths.e0, element.axisLengths.e1, element.axisLengths.e2);
 
             return true;
         }
+
+        SoMaterial* material = nullptr;
+        SoComplexity* complexity = nullptr;
+        SoScale* scale = nullptr;
+        SoSphere* sphere = nullptr;
     };
 }
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
index 28eed664669be393980f966559587cc2648dc725..1c2a29de35247e78dd48bbc9861b1738976093d1 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
@@ -23,48 +23,49 @@ namespace armarx::viz::coin
             pclMatBind->value = SoMaterialBinding::PER_PART;
 
             pclCoords = new SoCoordinate3;
-            pclStye = new SoDrawStyle;
+            pclStyle = new SoDrawStyle;
 
             node->addChild(pclMat);
             node->addChild(pclMatBind);
             node->addChild(pclCoords);
-            node->addChild(pclStye);
+            node->addChild(pclStyle);
             node->addChild(new SoPointSet);
         }
 
         bool update(ElementType const& element)
         {
-            auto& pcl = element.points;
+            data::ColoredPointList const& pcl = element.points;
 
-            colors.clear();
-            colors.reserve(pcl.size());
-            coords.clear();
-            coords.reserve(pcl.size());
+            int pclSize = (int)pcl.size();
+            colors.resize(pclSize);
+            coords.resize(pclSize);
 
             const float conv = 1.0f / 255.0f;
-            for (auto& point : pcl)
+            SbColor* colorsData = colors.data();
+            SbVec3f* coordsData = coords.data();
+            for (int i = 0; i < pclSize; ++i)
             {
+                data::ColoredPoint point = pcl[i];
                 float r = point.color.r * conv;
                 float g = point.color.g * conv;
                 float b = point.color.b * conv;
-                colors.emplace_back(r, g, b);
-                coords.emplace_back(point.x, point.y, point.z);
+                colorsData[i].setValue(r, g, b);
+                coordsData[i].setValue(point.x, point.y, point.z);
             }
-            pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
-            pclMat->ambientColor.setValues(0, colors.size(), colors.data());
+            pclMat->diffuseColor.setValuesPointer(pclSize, colors.data());
+            pclMat->ambientColor.setValuesPointer(pclSize, colors.data());
             pclMat->transparency = element.transparency;
 
-            pclCoords->point.setValues(0, coords.size(), coords.data());
-            pclCoords->point.setNum(coords.size());
+            pclCoords->point.setValuesPointer(pclSize, coords.data());
 
-            pclStye->pointSize = element.pointSizeInPixels;
+            pclStyle->pointSize = element.pointSizeInPixels;
 
             return true;
         }
 
         SoMaterial* pclMat;
         SoCoordinate3* pclCoords;
-        SoDrawStyle* pclStye;
+        SoDrawStyle* pclStyle;
 
         std::vector<SbColor> colors;
         std::vector<SbVec3f> coords;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 60a8a46f1311cecefd00f887831bec5a6ffeb4e8..35d7a4357f23dd886e08dc871621b265c9d24b65 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -78,37 +78,108 @@ namespace armarx::viz::coin
             return result;
         }
 
-        static std::vector<LoadedRobot> robotcache;
+        struct RobotInstancePool
+        {
+            std::string project;
+            std::string filename;
+            std::size_t usedInstances = 0;
+            std::vector<VirtualRobot::RobotPtr> robots;
+        };
+
+        static std::vector<RobotInstancePool> robotCache;
 
         LoadedRobot getRobotFromCache(std::string const& project, std::string const& filename)
         {
             // We can use a global variable, since this code is only executed in the GUI thread
 
             LoadedRobot result;
+            result.project = project;
+            result.filename = filename;
 
-            for (LoadedRobot const& loaded : robotcache)
+            for (RobotInstancePool& instancePool : robotCache)
             {
-                if (loaded.project == project && loaded.filename == filename)
+                if (instancePool.project == project && instancePool.filename == filename)
                 {
-                    ARMARX_DEBUG << "loading robot from chace " << VAROUT(project) << ", " << VAROUT(filename);
-                    result = loaded;
-                    //do not scale the robot and do not clone meshes if scaling = 1
-                    result.robot = loaded.robot->clone(nullptr, 1, true);
+                    // There are two possibilities here:
+                    if (instancePool.usedInstances < instancePool.robots.size())
+                    {
+                        // 1) We have still unused instances in the pool ==> Just return one
+                        ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project) << ", " << VAROUT(filename);
+                        result.robot = instancePool.robots[instancePool.usedInstances];
+                        instancePool.usedInstances += 1;
+                    }
+
+                    else
+                    {
+                        // 2) We do not have unused instances in the pool ==> Clone one
+                        ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", " << VAROUT(filename);
+
+                        if (instancePool.robots.size() > 0)
+                        {
+                            VirtualRobot::RobotPtr const& robotToClone = instancePool.robots.front();
+                            float scaling = 1.0f;
+                            bool preventCloningMeshesIfScalingIs1 = true;
+                            result.robot = robotToClone->clone(nullptr, scaling, preventCloningMeshesIfScalingIs1);
+
+                            // Insert the cloned robot into the instance pool
+                            instancePool.robots.push_back(result.robot);
+                            instancePool.usedInstances += 1;
+                        }
+                        else
+                        {
+                            ARMARX_WARNING << "Encountered empty robot instance pool while trying to clone new instance"
+                                           << "\nRobot: " << VAROUT(project) << ", " << VAROUT(filename)
+                                           << "\nUsed instances: " << instancePool.usedInstances
+                                           << "\nRobots: " << instancePool.robots.size();
+                        }
+                    }
                     return result;
                 }
             }
 
-            ARMARX_DEBUG << "loading robot from file  " << VAROUT(project) << ", " << VAROUT(filename);
-            result.project = project;
-            result.filename = filename;
+            ARMARX_DEBUG << "Loading robot from file  " << VAROUT(project) << ", " << VAROUT(filename);
             result.robot = loadRobot(project, filename);
 
-            robotcache.push_back(result);
+            RobotInstancePool& instancePool = robotCache.emplace_back();
+            instancePool.project = project;
+            instancePool.filename = filename;
+            instancePool.robots.push_back(result.robot);
+            instancePool.usedInstances = 1;
 
             return result;
         }
     }
 
+    VisualizationRobot::~VisualizationRobot()
+    {
+        for (RobotInstancePool& instancePool : robotCache)
+        {
+            if (instancePool.project == loaded.project && instancePool.filename == loaded.filename)
+            {
+                ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename);
+                std::vector<VirtualRobot::RobotPtr>& robots = instancePool.robots;
+                auto robotIter = std::find(robots.begin(), robots.end(), loaded.robot);
+                if (robotIter != robots.end())
+                {
+                    // Do not erase the robot, but rather move it to the back
+                    // We can later reuse the unused instances at the back of the vector
+                    std::swap(*robotIter, robots.back());
+                    if (instancePool.usedInstances > 0)
+                    {
+                        instancePool.usedInstances -= 1;
+                    }
+                    else
+                    {
+                        ARMARX_WARNING << "Expected there to be at least one used instance "
+                                       << "while trying to put robot instance back into the pool"
+                                       << "\nRobot: " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename)
+                                       << "\nUsed instances: " << instancePool.usedInstances;
+                    }
+                }
+            }
+        }
+    }
+
     bool VisualizationRobot::update(ElementType const& element)
     {
         IceUtil::Time time_start = IceUtil::Time::now();
@@ -141,7 +212,7 @@ namespace armarx::viz::coin
 
         // Set pose, configuration and so on
 
-        auto& robot = *loaded.robot;
+        VirtualRobot::Robot& robot = *loaded.robot;
 
         // We do not need to update the global pose in the robot
         // Since we only visualize the results, we can simply use the
@@ -150,7 +221,24 @@ namespace armarx::viz::coin
         // Eigen::Matrix4f pose = defrost(element.pose);
         // robot.setGlobalPose(pose, false);
 
-        robot.setJointValues(element.jointValues);
+        // Check joint values for changes
+        bool jointValuesChanged = false;
+        for (auto& pair : element.jointValues)
+        {
+            std::string const& nodeName = pair.first;
+            float newJointValue = pair.second;
+            VirtualRobot::RobotNodePtr robotNode = robot.getRobotNode(nodeName);
+            float oldJointValue = robotNode->getJointValue();
+            float diff = std::abs(newJointValue - oldJointValue);
+            jointValuesChanged = diff > 0.001f;
+            if (jointValuesChanged)
+            {
+                // Only set the joint values if they changed
+                // This call causes internal updates to the visualization even if joint angles are the same!
+                robot.setJointValues(element.jointValues);
+                break;
+            }
+        }
         //IceUtil::Time time_set = IceUtil::Time::now();
 
         if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
@@ -235,6 +323,6 @@ namespace armarx::viz::coin
 
     void clearRobotCache()
     {
-        robotcache.clear();
+        robotCache.clear();
     }
 }
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
index 63aa3f1f8cabf92029b1d1b763d808f9b2123d77..8bb77b7051bbb574eb4fc6f07d068eeebd33029c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
@@ -19,6 +19,8 @@ namespace armarx::viz::coin
     {
         using ElementType = data::ElementRobot;
 
+        ~VisualizationRobot();
+
         bool update(ElementType const& element);
 
         void recreateVisualizationNodes(int drawStyle);
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index ffc286590837b87defa10ac9c183092dd185f372..ff525ff98eda9eb834dfabb2f6bdfcd42ddb019b 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -7,6 +7,15 @@
 #include <Inventor/nodes/SoUnits.h>
 #include <thread>
 
+
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/actions/SoToVRML2Action.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
+#include <Inventor/nodes/SoRotation.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+
+
 #include "VisualizationRobot.h"
 
 namespace armarx::viz
@@ -416,4 +425,37 @@ namespace armarx::viz
             }
         }
     }
+
+    void CoinVisualizer::exportToVRML(const std::string& s)
+    {
+
+
+        SoOutput* so = new SoOutput();
+        if (!so->openFile(s.c_str()))
+        {
+            ARMARX_ERROR << "Could not open file " << s << " for writing." << std::endl;
+            return;
+        }
+
+        so->setHeaderString("#VRML V2.0 utf8");
+
+        SoGroup* n = new SoGroup;
+        n->ref();
+        n->addChild(root);
+        SoGroup* newVisu = VirtualRobot::CoinVisualizationFactory::convertSoFileChildren(n);
+        newVisu->ref();
+
+        SoToVRML2Action tovrml2;
+        tovrml2.apply(newVisu);
+        SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
+        newroot->ref();
+        SoWriteAction wra(so);
+        wra.apply(newroot);
+        newroot->unref();
+
+        so->closeFile();
+
+        newVisu->unref();
+        n->unref();
+    }
 }
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index f3a2248254821f96067f16a6306b02141a1438a6..e2d9a4634d5f1747174c125292a33233699dceda 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -129,6 +129,8 @@ namespace armarx::viz
 
         void update();
 
+        void exportToVRML(const std::string& s);
+
         template <typename ElementVisuT>
         void registerVisualizerFor()
         {
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index f22461ad04d036fac3623c499244b35000ac1696..87966a2f7502ab85b3984fbff927b3ed9e8a03f1 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -141,6 +141,27 @@ namespace armarx
 
     }
 
+    void fillRobotHandsLayer(viz::Layer& layer)
+    {
+        Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+
+        for (int i = 0; i < 10; ++i)
+        {
+            // Always generate a new name, so the robot needs to be cached through the instance pool
+            int randomIndex = std::rand();
+            std::string name = "Hand_" + std::to_string(randomIndex);
+
+            pos.x() = 1500.0f;
+            pos.y() = i * 200.0f;
+            viz::Robot robot = viz::Robot(name)
+                               .position(pos)
+                               .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
+                               .overrideColor(simox::Color::green(64 + i * 8));
+
+            layer.add(robot);
+        }
+    }
+
 
     void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
     {
@@ -449,6 +470,7 @@ namespace armarx
         viz::Layer pointsLayer = arviz.layer("Points");
         viz::Layer objectsLayer = arviz.layer("Objects");
         viz::Layer disAppearingLayer = arviz.layer("DisAppearing");
+        viz::Layer robotHandsLayer = arviz.layer("RobotHands");
 
 
         // These layers are not updated in the loop.
@@ -457,7 +479,8 @@ namespace armarx
             fillPermanentLayer(permanentLayer);
             arviz.commit(permanentLayer);
         }
-        if (getProperty<bool>("layers.ManyElements"))
+        bool manyElements = getProperty<bool>("layers.ManyElements");
+        if (manyElements)
         {
             viz::Layer manyElementsLayer = arviz.layer("ManyElements");
             fillManyElementsLayer(manyElementsLayer, 0);
@@ -486,7 +509,13 @@ namespace armarx
             disAppearingLayer.clear();
             fillDisAppearingLayer(disAppearingLayer, timeInSeconds);
 
-            arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer});
+            if (manyElements)
+            {
+                robotHandsLayer.clear();
+                fillRobotHandsLayer(robotHandsLayer);
+            }
+
+            arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer, robotHandsLayer});
 
             c.waitForCycleDuration();
         }
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 73a75e56fe8083732813634c63148ca54b1c3f1d..c2418e3d7e0106bc68eac983f1f35b5bc8c9b663 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -24,6 +24,8 @@
 
 #include "RobotStateComponent.h"
 
+#include <thread>
+
 #include <Ice/ObjectAdapter.h>
 
 #include <VirtualRobot/RobotNodeSet.h>
@@ -31,6 +33,7 @@
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/XML/RobotIO.h>
 
+#include <ArmarXCore/core/util/OnScopeExit.h>
 #include <ArmarXCore/core/ArmarXManager.h>
 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
 #include <ArmarXCore/core/application/Application.h>
@@ -45,23 +48,557 @@ using namespace Eigen;
 using namespace Ice;
 using namespace VirtualRobot;
 
-
-namespace armarx
+namespace armarx::detail
 {
-    RobotStateComponent::~RobotStateComponent()
+    class RobotStateComponentWorker
     {
-        try
+    public:
+        struct Job
         {
-            if (_synchronizedPrx)
+            enum class JobType
             {
-                _synchronizedPrx->unref();
+                none,
+                getSynchronizedRobot,
+                getRobotSnapshot,
+                getRobotSnapshotAtTimestamp,
+                getJointConfigAtTimestamp,
+                getRobotStateAtTimestamp,
+
+                reportGlobalRobotPose,
+                reportPlatformPose,
+                reportJointAngles,
+                reportJointVelocities,
+                simulatorWasReset
+            };
+            AMD_RobotStateComponentInterface_getSynchronizedRobotPtr getSynchronizedRobot;
+            AMD_RobotStateComponentInterface_getRobotSnapshotPtr getRobotSnapshot;
+            AMD_RobotStateComponentInterface_getRobotSnapshotAtTimestampPtr getRobotSnapshotAtTimestamp;
+            AMD_RobotStateComponentInterface_getJointConfigAtTimestampPtr getJointConfigAtTimestamp;
+            AMD_RobotStateComponentInterface_getRobotStateAtTimestampPtr getRobotStateAtTimestamp;
+
+            JobType type = JobType::none;
+            PoseStamped currentPose;
+            std::string robotName;
+            Eigen::Matrix4f pose;
+            long timestamp;
+            double time;
+            NameValueMap perJointValues;
+            bool aValueChanged;
+        };
+        template <class ValueT>
+        struct Timestamped
+        {
+            IceUtil::Time timestamp;
+            ValueT value;
+        };
+
+        VirtualRobot::RobotPtr synchronized;
+
+        std::thread      worker;
+        std::atomic_bool stopWrk{false};
+
+        std::map<IceUtil::Time, NameValueMap> jointHistory;
+        std::map<IceUtil::Time, FramedPosePtr> poseHistory;
+
+        size_t jointHistoryLength;
+        size_t poseHistoryLength;
+        RobotStateObserverPtr robotStateObs;
+        SharedRobotInterfacePrx synchronizedPrx;
+        SharedRobotServantPtr sharedRobotServant;
+        RobotStateListenerInterfacePrx robotStateListenerPrx;
+        RobotStateComponentInterfacePrx parent;
+        Ice::ObjectAdapterPtr objectAdapter;
+
+        std::mutex      jobsMutex;
+        std::deque<Job> jobs;
+
+    public:
+        RobotStateComponentWorker()
+        {
+            try
+            {
+                if (synchronizedPrx)
+                {
+                    synchronizedPrx->unref();
+                }
             }
+            catch (...)
+            {}
         }
-        catch (...)
-        {}
-    }
+        ~RobotStateComponentWorker()
+        {
+            stopWorker();
+        }
+
+        void startWorker()
+        {
+            stopWrk = false;
+            ARMARX_CHECK_EXPRESSION(!worker.joinable());
+            ARMARX_INFO << "starting worker";
+            worker = std::thread{[&]{work();}};
+        }
+        void stopWorker()
+        {
+            stopWrk = true;
+            if (worker.joinable())
+            {
+                ARMARX_INFO << "stopping worker";
+                worker.join();
+            }
+        }
+
+        void addJob(Job j)
+        {
+            std::lock_guard g{jobsMutex};
+            jobs.emplace_back(std::move(j));
+            ARMARX_DEBUG <<::deactivateSpam(1) << "adding job -> queue size: " << jobs.size();
+        }
+    private:
+        void work()
+        {
+            ARMARX_INFO << "started worker";
+            ARMARX_ON_SCOPE_EXIT
+            {
+                ARMARX_INFO << "stopped worker";
+            };
+            while (!stopWrk)
+            {
+                std::deque<Job> jobs;
+                {
+                    std::lock_guard g{jobsMutex};
+                    std::swap(jobs, this->jobs);
+                }
+                if (jobs.empty())
+                {
+                    std::this_thread::sleep_for(std::chrono::milliseconds{2});
+                    ARMARX_DEBUG_S << ::deactivateSpam(1) << "no jobs";
+                    continue;
+                }
+                using clock_t = std::chrono::high_resolution_clock;
+                const auto start = clock_t::now();
+                for (auto& j : jobs)
+                {
+                    run(j);
+                }
+                const auto msecs = std::chrono::duration_cast<std::chrono::nanoseconds>(clock_t::now() - start).count() * 1e-6;
+                ARMARX_DEBUG_S << ::deactivateSpam(1) << jobs.size() << " jobs took " << msecs << " ms";
+            }
+        }
+
+        void run(Job& j)
+        {
+            switch (j.type)
+            {
+                case Job::JobType::getSynchronizedRobot:
+                    getSynchronizedRobot(j);
+                    break;
+                case Job::JobType::getRobotSnapshot:
+                    getRobotSnapshot(j);
+                    break;
+                case Job::JobType::getRobotSnapshotAtTimestamp:
+                    getRobotSnapshotAtTimestamp(j);
+                    break;
+                case Job::JobType::getJointConfigAtTimestamp:
+                    getJointConfigAtTimestamp(j);
+                    break;
+                case Job::JobType::getRobotStateAtTimestamp:
+                    getRobotStateAtTimestamp(j);
+                    break;
+                case Job::JobType::reportGlobalRobotPose:
+                    reportGlobalRobotPose(j);
+                    break;
+                case Job::JobType::reportPlatformPose:
+                    reportPlatformPose(j);
+                    break;
+                case Job::JobType::reportJointAngles:
+                    reportJointAngles(j);
+                    break;
+                case Job::JobType::reportJointVelocities:
+                    reportJointVelocities(j);
+                    break;
+                case Job::JobType::simulatorWasReset:
+                    simulatorWasReset(j);
+                    break;
+                default:
+                    ARMARX_FATAL << "job type not set!";
+                    std::terminate();
+            }
+        }
+
+        void getSynchronizedRobot(Job& j)
+        {
+            if (!synchronizedPrx)
+            {
+                j.getSynchronizedRobot->ice_exception(
+                    NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot"));
+                return;
+            }
+            j.getSynchronizedRobot->ice_response(synchronizedPrx);
+        }
+        void getRobotSnapshot(Job& j)
+        {
+            if (!synchronized)
+            {
+                j.getRobotSnapshot->ice_exception(NotInitializedException("Shared Robot is NULL", "getRobotSnapshot"));
+                return;
+            }
+
+            auto clone = synchronized->clone(synchronized->getName());
+
+            SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(parent), nullptr);
+            p->setTimestamp(IceUtil::Time::microSecondsDouble(sharedRobotServant->getTimestamp()->timestamp));
+            auto result = objectAdapter->addWithUUID(p);
+            // virtal robot clone is buggy -> set global pose here
+            p->setGlobalPose(new Pose(synchronized->getGlobalPose()));
+            j.getRobotSnapshot->ice_response(SharedRobotInterfacePrx::uncheckedCast(result));
+        }
+        void getRobotSnapshotAtTimestamp(Job& j)
+        {
+            const IceUtil::Time time = IceUtil::Time::microSecondsDouble(j.time);
+
+            if (!synchronized)
+            {
+                j.getRobotSnapshotAtTimestamp->ice_exception(
+                    NotInitializedException("Shared Robot is NULL", "getRobotSnapshot"));
+                return;
+            }
+
+            VirtualRobot::RobotPtr clone = synchronized->clone(synchronized->getName());
+            auto config = interpolate(time);
+            if (!config)
+            {
+                ARMARX_WARNING << "Could not find or interpolate a robot state for time " << time.toDateTime();
+                j.getRobotSnapshotAtTimestamp->ice_response(nullptr);
+                return;
+            }
+
+            clone->setJointValues(config->jointMap);
+            SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(parent), nullptr);
+            p->setTimestamp(time);
+            auto result = objectAdapter->addWithUUID(p);
+            // Virtal robot clone is buggy -> set global pose here.
+            p->setGlobalPose(config->globalPose);
+
+            j.getRobotSnapshotAtTimestamp->ice_response(SharedRobotInterfacePrx::uncheckedCast(result));
+        }
+        void getJointConfigAtTimestamp(Job& j)
+        {
+            auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(j.time));
+            j.getJointConfigAtTimestamp->ice_response(jointAngles ? jointAngles->value : NameValueMap());
+        }
+        void getRobotStateAtTimestamp(Job& j)
+        {
+            auto config = interpolate(IceUtil::Time::microSecondsDouble(j.time));
+            j.getRobotStateAtTimestamp->ice_response(config ? *config : RobotStateConfig());
+        }
+        void reportPlatformPose(Job& j)
+        {
+            const float z = 0;
+            const Eigen::Vector3f position(j.currentPose.x, j.currentPose.y, z);
+            const Eigen::Matrix3f orientation =
+                Eigen::AngleAxisf(j.currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+            const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
+
+            IceUtil::Time time = IceUtil::Time::microSeconds(j.currentPose.timestampInMicroSeconds);
+            insertPose(time, globalPose);
+
+            if (sharedRobotServant)
+            {
+                sharedRobotServant->setTimestamp(time);
+            }
+        }
+        void reportGlobalRobotPose(Job& j)
+        {
+            if (synchronized)
+            {
+                std::string localRobotName = synchronized->getName();
+                ARMARX_DEBUG << "Comparing " << localRobotName
+                             << " and " << j.robotName << ".";
+                if (localRobotName == j.robotName)
+                {
+                    const IceUtil::Time time = IceUtil::Time::microSeconds(j.timestamp);
+
+                    insertPose(time, j.pose);
+                    synchronized->setGlobalPose(j.pose);
+
+                    if (sharedRobotServant)
+                    {
+                        sharedRobotServant->setTimestamp(time);
+                    }
+                }
+            }
+            else
+            {
+                throw NotInitializedException("Robot Ptr is NULL", "reportGlobalRobotPose");
+            }
+        }
+        void reportJointAngles(Job& j)
+        {
+            if (j.timestamp <= 0)
+            {
+                j.timestamp = IceUtil::Time::now().toMicroSeconds();
+            }
+
+            IceUtil::Time time = IceUtil::Time::microSeconds(j.timestamp);
+
+            ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << j.perJointValues
+                         << " from timestamp " << time.toDateTime()
+                         << " a value changed: " << j.aValueChanged;
+
+            if (j.aValueChanged)
+            {
+                {
+                    WriteLockPtr lock = synchronized->getWriteLock();
+
+                    synchronized->setJointValues(j.perJointValues);
+                }
+
+                if (robotStateObs)
+                {
+                    robotStateObs->updatePoses();
+                }
+            }
+            if (sharedRobotServant)
+            {
+                sharedRobotServant->setTimestamp(time);
+            }
+
+            {
+                jointHistory.emplace_hint(jointHistory.end(), time, j.perJointValues);
+
+                if (jointHistory.size() > jointHistoryLength)
+                {
+                    jointHistory.erase(jointHistory.begin());
+                }
+            }
+
+            robotStateListenerPrx->reportJointValues(j.perJointValues, j.timestamp, j.aValueChanged);
+        }
+        void reportJointVelocities(Job& j)
+        {
+            if (robotStateObs)
+            {
+                robotStateObs->updateNodeVelocities(j.perJointValues, j.timestamp);
+            }
+        }
+        void simulatorWasReset(Job& j)
+        {
+            poseHistory.clear();
+            jointHistory.clear();
+        }
+
+    private:
+        void
+        insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
+        {
+            IceUtil::Time duration;
+            {
+                IceUtil::Time start = IceUtil::Time::now();
+                duration = IceUtil::Time::now() - start;
+
+                poseHistory.emplace_hint(poseHistory.end(),
+                                         timestamp, new FramedPose(globalPose, GlobalFrame, ""));
+
+                if (poseHistory.size() > poseHistoryLength)
+                {
+                    poseHistory.erase(poseHistory.begin());
+                }
+            }
+
+            if (robotStateObs)
+            {
+                robotStateObs->updatePoses();
+            }
+        }
+        std::optional<RobotStateConfig>
+        interpolate(IceUtil::Time time) const
+        {
+            auto joints = interpolateJoints(time);
+            auto globalPose = interpolatePose(time);
+
+            if (joints && globalPose)
+            {
+                RobotStateConfig config;
+                // Use time stamp from interpolation.
+                // config.timestamp = time.toMicroSeconds();
+                config.timestamp = std::min(joints->timestamp, globalPose->timestamp).toMicroSeconds();
+                config.jointMap = joints->value;
+                config.globalPose = globalPose->value;
+                return config;
+            }
+            else
+            {
+                return std::nullopt;
+            }
+        }
+        auto
+        interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
+        {
+            if (jointHistory.empty())
+            {
+                ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
+                return std::nullopt;
+            }
+
+            const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
+            if (time > newestTimeInHistory)
+            {
+                IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+                if (time <= newestTimeInHistory + maxOffset)
+                {
+                    ARMARX_INFO << deactivateSpam(5)
+                                << "Requested joint timestamp is newer than newest available timestamp!"
+                                << "\n- requested timestamp: \t" << time.toDateTime()
+                                << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
+                                << "\n- difference:          \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+                }
+                else
+                {
+                    ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
+                                   << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+                                   << "\n- requested timestamp: \t" << time.toDateTime()
+                                   << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
+                    return std::nullopt;
+                }
+
+                return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
+            }
+
+            // Get the oldest entry whose time >= time.
+            auto nextIt = jointHistory.lower_bound(time);
+            if (nextIt == jointHistory.end())
+            {
+                ARMARX_WARNING << deactivateSpam(1)
+                               << "Could not find or interpolate a robot joint angles for time " << time.toDateTime()
+                               << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+                               << "\n- newest available value: " << newestTimeInHistory.toDateTime();
+                return std::nullopt;
+            }
+
+            if (nextIt == jointHistory.begin())
+            {
+                // Next was oldest entry.
+                return Timestamped<NameValueMap> {nextIt->first, nextIt->second};
+            }
+
+            const NameValueMap& next = nextIt->second;
+            auto prevIt = std::prev(nextIt);
 
 
+            // Interpolate.
+
+            IceUtil::Time prevTime = prevIt->first;
+            IceUtil::Time nextTime = nextIt->first;
+
+            float t = static_cast<float>((time - prevTime).toSecondsDouble()
+                                         / (nextTime - prevTime).toSecondsDouble());
+
+            NameValueMap jointAngles;
+            auto prevJointIt = prevIt->second.begin();
+            for (const auto& [name, nextAngle] : next)
+            {
+                float value = (1 - t) * prevJointIt->second + t * nextAngle;
+                prevJointIt++;
+
+                jointAngles.emplace(name, value);
+            }
+
+            return Timestamped<NameValueMap> {time, std::move(jointAngles)};
+        }
+        auto
+        interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
+        {
+            if (poseHistory.empty())
+            {
+                ARMARX_INFO << deactivateSpam(10)
+                            << "Pose history is empty. This can happen if there is no platform unit.";
+
+                ReadLockPtr readLock = synchronized->getReadLock();
+
+                FramedPosePtr pose = new FramedPose(synchronized->getGlobalPose(), armarx::GlobalFrame, "");
+                return Timestamped<FramedPosePtr> {IceUtil::Time::seconds(0), pose};
+            }
+
+
+            const IceUtil::Time newestTimeInHistory = poseHistory.rbegin()->first;
+            if (time > newestTimeInHistory)
+            {
+                IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+                if (time <= newestTimeInHistory + maxOffset)
+                {
+                    ARMARX_INFO << deactivateSpam(5)
+                                << "Requested pose timestamp is newer than newest available timestamp!"
+                                << "\n- requested timestamp: \t" << time.toDateTime()
+                                << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
+                                << "\n- difference:          \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+                }
+                else
+                {
+                    ARMARX_WARNING << deactivateSpam(1) << "Requested pose timestamp is substantially newer (>"
+                                   << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+                                   << "\n- requested timestamp: \t" << time.toDateTime()
+                                   << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
+                    return std::nullopt;
+                }
+                return Timestamped<FramedPosePtr> {poseHistory.rbegin()->first, poseHistory.rbegin()->second};
+            }
+
+
+            auto nextIt = poseHistory.lower_bound(time);
+            if (nextIt == poseHistory.end())
+            {
+                ARMARX_WARNING << deactivateSpam(1)
+                               << "Could not find or interpolate platform pose for time " << time.toDateTime()
+                               << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+                               << "\n- newest available value: " << newestTimeInHistory.toDateTime();
+                return std::nullopt;
+            }
+
+
+            if (nextIt == poseHistory.begin())
+            {
+                // Next was oldest entry.
+                return Timestamped<FramedPosePtr> {nextIt->first, nextIt->second};
+            }
+
+            auto prevIt = std::prev(nextIt);
+            ARMARX_CHECK_EXPRESSION(prevIt->second);
+
+            const FramedPosePtr& next = nextIt->second;
+            const FramedPosePtr& prev = prevIt->second;
+
+
+            // Interpolate.
+
+            IceUtil::Time prevTime = prevIt->first;
+            IceUtil::Time nextTime = nextIt->first;
+
+            float t = static_cast<float>((time - prevTime).toSecondsDouble()
+                                         / (nextTime - prevTime).toSecondsDouble());
+
+            Eigen::Matrix4f globalPoseNext = next->toEigen();
+            Eigen::Matrix4f globalPosePrev = prev->toEigen();
+
+
+            Eigen::Matrix4f globalPose;
+
+            math::Helpers::Position(globalPose) =
+                (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
+
+            Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
+            Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
+            Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
+
+            math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
+
+            return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
+        }
+    };
+}
+
+
+namespace armarx
+{
     RobotStatePropertyDefinitions::RobotStatePropertyDefinitions(std::string prefix) :
         ComponentPropertyDefinitions(prefix)
     {
@@ -77,6 +614,11 @@ namespace armarx
     }
 
 
+    RobotStateComponent::RobotStateComponent()
+    {
+        _worker = std::make_unique<detail::RobotStateComponentWorker>();
+    }
+
     std::string RobotStateComponent::getDefaultName() const
     {
         return "RobotStateComponent";
@@ -85,15 +627,9 @@ namespace armarx
 
     void RobotStateComponent::onInitComponent()
     {
+        ARMARX_CHECK_NOT_NULL(_worker);
         robotStateTopicName = getProperty<std::string>("RobotStateReportingTopic").getValue();
         offeringTopic(getProperty<std::string>("RobotStateReportingTopic"));
-        const std::size_t historyLength = static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
-
-        jointHistory.clear();
-        jointHistoryLength = historyLength;
-
-        poseHistory.clear();
-        poseHistoryLength = historyLength;
 
         relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
 
@@ -102,21 +638,34 @@ namespace armarx
             throw UserException("Could not find robot file " + robotFile);
         }
 
-        this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
-        _synchronized->setName(getProperty<std::string>("AgentName").getValue());
+        //setup worker
+        {
+            const std::size_t historyLength = static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
+            _worker->jointHistoryLength = historyLength;
+            _worker->poseHistoryLength = historyLength;
+            _worker->objectAdapter         = getObjectAdapter();
+            _worker->synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+            _worker->synchronized->setName(getProperty<std::string>("AgentName").getValue());
+        }
+
+        robotName = _worker->synchronized->getName();
 
         robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
         ARMARX_INFO << "scale factor: " << robotModelScaling;
         if (robotModelScaling != 1.0f)
         {
             ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
-            _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
+            _worker->synchronized =
+                _worker->synchronized->clone(_worker->synchronized->getName(),
+                                             _worker->synchronized->getCollisionChecker(),
+                                             robotModelScaling);
         }
 
-        if (this->_synchronized)
+        if (_worker->synchronized)
         {
-            ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
-            this->_synchronized->setPropagatingJointValuesEnabled(false);
+            ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: "
+                           << _worker->synchronized->getName();
+            _worker->synchronized->setPropagatingJointValuesEnabled(false);
         }
         else
         {
@@ -131,7 +680,7 @@ namespace armarx
             throw UserException("RobotNodeSet not defined");
         }
 
-        VirtualRobot::RobotNodeSetPtr rns =  this->_synchronized->getRobotNodeSet(robotNodeSetName);
+        VirtualRobot::RobotNodeSetPtr rns =  _worker->synchronized->getRobotNodeSet(robotNodeSetName);
 
         if (!rns)
         {
@@ -190,96 +739,89 @@ namespace armarx
 
     void RobotStateComponent::onConnectComponent()
     {
-        robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(getProperty<std::string>("RobotStateReportingTopic"));
-        _sharedRobotServant =  new SharedRobotServant(this->_synchronized, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), robotStateListenerPrx);
-        _sharedRobotServant->ref();
-
-        _sharedRobotServant->setRobotStateComponent(RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
-        this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_sharedRobotServant));
-        ARMARX_INFO << "Started RobotStateComponent" << flush;
-        if (robotStateObs)
         {
-            robotStateObs->setRobot(_synchronized);
+            getProxy(_worker->parent, -1);
+            _worker->robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(getProperty<std::string>("RobotStateReportingTopic"));
+
+            _worker->sharedRobotServant    = new SharedRobotServant(_worker->synchronized,
+                    RobotStateComponentInterfacePrx::uncheckedCast(getProxy()),
+                    _worker->robotStateListenerPrx);
+            _worker->sharedRobotServant->ref();
+            _worker->sharedRobotServant->setRobotStateComponent(RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
+            _worker->synchronizedPrx       = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_worker->sharedRobotServant));
+
+            if (_worker->robotStateObs)
+            {
+                _worker->robotStateObs->setRobot(_worker->synchronized);
+            }
+
+            _worker->startWorker();
         }
+        ARMARX_INFO << "Started RobotStateComponent" << flush;
     }
-
     void RobotStateComponent::onDisconnectComponent()
     {
+        _worker->stopWorker();
     }
 
-
-    SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current&) const
+    void RobotStateComponent::getSynchronizedRobot_async(
+        const AMD_RobotStateComponentInterface_getSynchronizedRobotPtr& amd,
+        const Ice::Current&) const
     {
-        if (!this->_synchronizedPrx)
-        {
-            throw NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot");
-        }
-
-        return this->_synchronizedPrx;
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::getSynchronizedRobot;
+        j.getSynchronizedRobot = amd;
+        _worker->addJob(std::move(j));
     }
 
-
-    SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const std::string& deprecated, const Current&)
+    void RobotStateComponent::getRobotSnapshot_async(
+        const AMD_RobotStateComponentInterface_getRobotSnapshotPtr& amd,
+        const std::string& deprecated,
+        const Ice::Current&)
     {
         (void) deprecated;
-
-        if (!_synchronized)
-        {
-            throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
-        }
-
-        auto clone = this->_synchronized->clone(_synchronized->getName());
-
-        SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
-        p->setTimestamp(IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
-        auto result = getObjectAdapter()->addWithUUID(p);
-        // virtal robot clone is buggy -> set global pose here
-        p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
-        return SharedRobotInterfacePrx::uncheckedCast(result);
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::getRobotSnapshot;
+        j.getRobotSnapshot = amd;
+        _worker->addJob(std::move(j));
     }
 
-    SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
+    void RobotStateComponent::getRobotSnapshotAtTimestamp_async(
+        const AMD_RobotStateComponentInterface_getRobotSnapshotAtTimestampPtr& amd,
+        double time,
+        const Ice::Current&)
     {
-        const IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
-
-        if (!_synchronized)
-        {
-            throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
-        }
-
-        VirtualRobot::RobotPtr clone = this->_synchronized->clone(_synchronized->getName());
-        auto config = interpolate(time);
-        if (!config)
-        {
-            ARMARX_WARNING << "Could not find or interpolate a robot state for time " << time.toDateTime();
-            return nullptr;
-        }
-
-        clone->setJointValues(config->jointMap);
-        SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
-        p->setTimestamp(time);
-        auto result = getObjectAdapter()->addWithUUID(p);
-        // Virtal robot clone is buggy -> set global pose here.
-        p->setGlobalPose(config->globalPose);
-
-        return SharedRobotInterfacePrx::uncheckedCast(result);
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::getRobotSnapshotAtTimestamp;
+        j.time = time;
+        j.getRobotSnapshotAtTimestamp = amd;
+        _worker->addJob(std::move(j));
     }
 
-
-    NameValueMap RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
+    void RobotStateComponent::getJointConfigAtTimestamp_async(
+        const AMD_RobotStateComponentInterface_getJointConfigAtTimestampPtr& amd,
+        double time,
+        const Ice::Current&) const
     {
-        auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(timestamp));
-        return jointAngles ? jointAngles->value : NameValueMap();
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::getJointConfigAtTimestamp;
+        j.time = time;
+        j.getJointConfigAtTimestamp = amd;
+        _worker->addJob(std::move(j));
     }
 
-
-    RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
+    void RobotStateComponent::getRobotStateAtTimestamp_async(
+        const AMD_RobotStateComponentInterface_getRobotStateAtTimestampPtr& amd,
+        double time,
+        const Ice::Current&) const
     {
-        auto config = interpolate(IceUtil::Time::microSecondsDouble(timestamp));
-        return config ? *config : RobotStateConfig();
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::getRobotStateAtTimestamp;
+        j.time = time;
+        j.getRobotStateAtTimestamp = amd;
+        _worker->addJob(std::move(j));
     }
 
-
     std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
     {
         return relativeRobotFile;
@@ -295,53 +837,17 @@ namespace armarx
         return robotInfo;
     }
 
-
     void RobotStateComponent::reportJointAngles(
         const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current&)
     {
-        if (timestamp <= 0)
-        {
-            timestamp = IceUtil::Time::now().toMicroSeconds();
-        }
-
-        IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
-
-        ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles
-                     << " from timestamp " << time.toDateTime()
-                     << " a value changed: " << aValueChanged;
-
-        if (aValueChanged)
-        {
-            {
-                WriteLockPtr lock = _synchronized->getWriteLock();
-
-                _synchronized->setJointValues(jointAngles);
-            }
-
-            if (robotStateObs)
-            {
-                robotStateObs->updatePoses();
-            }
-        }
-        if (_sharedRobotServant)
-        {
-            _sharedRobotServant->setTimestamp(time);
-        }
-
-        {
-            std::unique_lock lock(jointHistoryMutex);
-            jointHistory.emplace_hint(jointHistory.end(), time, jointAngles);
-
-            if (jointHistory.size() > jointHistoryLength)
-            {
-                jointHistory.erase(jointHistory.begin());
-            }
-        }
-
-        robotStateListenerPrx->reportJointValues(jointAngles, timestamp, aValueChanged);
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::reportJointAngles;
+        j.perJointValues = jointAngles;
+        j.timestamp = timestamp;
+        j.aValueChanged = aValueChanged;
+        _worker->addJob(std::move(j));
     }
 
-
     void
     RobotStateComponent::reportGlobalRobotPose(
         const std::string& robotName,
@@ -349,45 +855,30 @@ namespace armarx
         const long timestamp,
         const Ice::Current&)
     {
-        if (_synchronized)
-        {
-            std::string localRobotName = _synchronized->getName();
-            ARMARX_DEBUG << "Comparing " << localRobotName << " and " << robotName << ".";
-            if (localRobotName == robotName)
-            {
-                const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
-
-                insertPose(time, pose);
-                _synchronized->setGlobalPose(pose);
-
-                if (_sharedRobotServant)
-                {
-                    _sharedRobotServant->setTimestamp(time);
-                }
-            }
-        }
-        else
-        {
-            throw NotInitializedException("Robot Ptr is NULL", "reportGlobalRobotPose");
-        }
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::reportGlobalRobotPose;
+        j.robotName = robotName;
+        j.pose = pose;
+        j.timestamp = timestamp;
+        _worker->addJob(std::move(j));
     }
 
 
     void RobotStateComponent::reportPlatformPose(const PoseStamped& platformPose, const Current&)
     {
-        const IceUtil::Time time = IceUtil::Time::microSeconds(platformPose.header.timestampInMicroSeconds);
-        insertPose(time, platformPose.pose);
-
-        if (_sharedRobotServant)
-        {
-            _sharedRobotServant->setTimestamp(time);
-        }
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::reportPlatformPose;
+        j.currentPose = currentPose;
+        _worker->addJob(std::move(j));
     }
 
 
     void RobotStateComponent::reportNewTargetPose(const PoseStamped&, const Current&)
+    void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
     {
         // Unused.
+        ARMARX_CHECK_EXPRESSION(_worker);
+        _worker->robotStateObs = observer;
     }
 
 
@@ -422,57 +913,22 @@ namespace armarx
         return result;
     }
 
-    void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Current&)
-    {
-        // Unused.
-        (void) jointModes, (void) timestamp, (void) aValueChanged;
-    }
     void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Current&)
     {
         // Unused.
-        (void) aValueChanged;
-        if (robotStateObs)
-        {
-            robotStateObs->updateNodeVelocities(jointVelocities, timestamp);
-        }
-    }
-    void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Current&)
-    {
-        // Unused.
-        (void) jointTorques, (void) timestamp, (void) aValueChanged;
-    }
-
-    void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Current&)
-    {
-        // Unused.
-        (void) jointAccelerations, (void) timestamp, (void) aValueChanged;
-    }
-    void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Current&)
-    {
-        // Unused.
-        (void) jointCurrents, (void) timestamp, (void) aValueChanged;
-    }
-    void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Current&)
-    {
-        // Unused.
-        (void) jointMotorTemperatures, (void) timestamp, (void) aValueChanged;
-    }
-    void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Current&)
-    {
-        // Unused.
-        (void) jointStatuses, (void) timestamp, (void) aValueChanged;
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::reportJointVelocities;
+        j.perJointValues = jointVelocities;
+        j.timestamp = timestamp;
+        j.aValueChanged = aValueChanged;
+        _worker->addJob(std::move(j));
     }
 
     void RobotStateComponent::simulatorWasReset(const Current&)
     {
-        {
-            std::lock_guard lock(poseHistoryMutex);
-            poseHistory.clear();
-        }
-        {
-            std::lock_guard lock(jointHistoryMutex);
-            jointHistory.clear();
-        }
+        detail::RobotStateComponentWorker::Job j;
+        j.type = detail::RobotStateComponentWorker::Job::JobType::simulatorWasReset;
+        _worker->addJob(std::move(j));
     }
 
     PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
@@ -481,22 +937,16 @@ namespace armarx
                                           getConfigIdentifier()));
     }
 
-    void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
-    {
-        robotStateObs = observer;
-    }
-
     std::string RobotStateComponent::getRobotName(const Current&) const
     {
-        if (_synchronized)
+        if (!robotName.empty())
         {
-            return _synchronized->getName();  // _synchronizedPrx->getName();
+            return robotName;
         }
         else
         {
             throw NotInitializedException("Robot Ptr is NULL", "getName");
         }
-
     }
 
     std::string RobotStateComponent::getRobotNodeSetName(const Current&) const
@@ -512,217 +962,4 @@ namespace armarx
     {
         return robotStateTopicName;
     }
-
-
-    void RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
-    {
-        IceUtil::Time duration;
-        {
-            IceUtil::Time start = IceUtil::Time::now();
-            std::unique_lock lock(poseHistoryMutex);
-            duration = IceUtil::Time::now() - start;
-
-            poseHistory.emplace_hint(poseHistory.end(),
-                                     timestamp, new FramedPose(globalPose, GlobalFrame, ""));
-
-            if (poseHistory.size() > poseHistoryLength)
-            {
-                poseHistory.erase(poseHistory.begin());
-            }
-        }
-
-        if (robotStateObs)
-        {
-            robotStateObs->updatePoses();
-        }
-    }
-
-    std::optional<RobotStateConfig> RobotStateComponent::interpolate(IceUtil::Time time) const
-    {
-        auto joints = interpolateJoints(time);
-        auto globalPose = interpolatePose(time);
-
-        if (joints && globalPose)
-        {
-            RobotStateConfig config;
-            // Use time stamp from interpolation.
-            // config.timestamp = time.toMicroSeconds();
-            config.timestamp = std::min(joints->timestamp, globalPose->timestamp).toMicroSeconds();
-            config.jointMap = joints->value;
-            config.globalPose = globalPose->value;
-            return config;
-        }
-        else
-        {
-            return std::nullopt;
-        }
-    }
-
-    auto RobotStateComponent::interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
-    {
-        std::shared_lock lock(jointHistoryMutex);
-        if (jointHistory.empty())
-        {
-            ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
-            return std::nullopt;
-        }
-
-        const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
-        if (time > newestTimeInHistory)
-        {
-            IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
-            if (time <= newestTimeInHistory + maxOffset)
-            {
-                ARMARX_INFO << deactivateSpam(5)
-                            << "Requested joint timestamp is newer than newest available timestamp!"
-                            << "\n- requested timestamp: \t" << time.toDateTime()
-                            << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
-                            << "\n- difference:          \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
-            }
-            else
-            {
-                ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
-                               << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
-                               << "\n- requested timestamp: \t" << time.toDateTime()
-                               << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
-                return std::nullopt;
-            }
-
-            return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
-        }
-
-        // Get the oldest entry whose time >= time.
-        auto nextIt = jointHistory.lower_bound(time);
-        if (nextIt == jointHistory.end())
-        {
-            ARMARX_WARNING << deactivateSpam(1)
-                           << "Could not find or interpolate a robot joint angles for time " << time.toDateTime()
-                           << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
-                           << "\n- newest available value: " << newestTimeInHistory.toDateTime();
-            return std::nullopt;
-        }
-
-        if (nextIt == jointHistory.begin())
-        {
-            // Next was oldest entry.
-            return Timestamped<NameValueMap> {nextIt->first, nextIt->second};
-        }
-
-        const NameValueMap& next = nextIt->second;
-        auto prevIt = std::prev(nextIt);
-
-
-        // Interpolate.
-
-        IceUtil::Time prevTime = prevIt->first;
-        IceUtil::Time nextTime = nextIt->first;
-
-        float t = static_cast<float>((time - prevTime).toSecondsDouble()
-                                     / (nextTime - prevTime).toSecondsDouble());
-
-        NameValueMap jointAngles;
-        auto prevJointIt = prevIt->second.begin();
-        for (const auto& [name, nextAngle] : next)
-        {
-            float value = (1 - t) * prevJointIt->second + t * nextAngle;
-            prevJointIt++;
-
-            jointAngles.emplace(name, value);
-        }
-
-        return Timestamped<NameValueMap> {time, std::move(jointAngles)};
-    }
-
-    auto RobotStateComponent::interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
-    {
-        std::shared_lock lock(poseHistoryMutex);
-
-        if (poseHistory.empty())
-        {
-            ARMARX_INFO << deactivateSpam(10)
-                        << "Pose history is empty. This can happen if there is no platform unit.";
-
-            ReadLockPtr readLock = _synchronized->getReadLock();
-
-            FramedPosePtr pose = new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
-            return Timestamped<FramedPosePtr> {IceUtil::Time::seconds(0), pose};
-        }
-
-
-        const IceUtil::Time newestTimeInHistory = poseHistory.rbegin()->first;
-        if (time > newestTimeInHistory)
-        {
-            IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
-            if (time <= newestTimeInHistory + maxOffset)
-            {
-                ARMARX_INFO << deactivateSpam(5)
-                            << "Requested pose timestamp is newer than newest available timestamp!"
-                            << "\n- requested timestamp: \t" << time.toDateTime()
-                            << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime()
-                            << "\n- difference:          \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
-            }
-            else
-            {
-                ARMARX_WARNING << deactivateSpam(1) << "Requested pose timestamp is substantially newer (>"
-                               << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
-                               << "\n- requested timestamp: \t" << time.toDateTime()
-                               << "\n- newest timestamp:    \t" << newestTimeInHistory.toDateTime();
-                return std::nullopt;
-            }
-            return Timestamped<FramedPosePtr> {poseHistory.rbegin()->first, poseHistory.rbegin()->second};
-        }
-
-
-        auto nextIt = poseHistory.lower_bound(time);
-        if (nextIt == poseHistory.end())
-        {
-            ARMARX_WARNING << deactivateSpam(1)
-                           << "Could not find or interpolate platform pose for time " << time.toDateTime()
-                           << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
-                           << "\n- newest available value: " << newestTimeInHistory.toDateTime();
-            return std::nullopt;
-        }
-
-
-        if (nextIt == poseHistory.begin())
-        {
-            // Next was oldest entry.
-            return Timestamped<FramedPosePtr> {nextIt->first, nextIt->second};
-        }
-
-        auto prevIt = std::prev(nextIt);
-        ARMARX_CHECK_EXPRESSION(prevIt->second);
-
-        const FramedPosePtr& next = nextIt->second;
-        const FramedPosePtr& prev = prevIt->second;
-
-
-        // Interpolate.
-
-        IceUtil::Time prevTime = prevIt->first;
-        IceUtil::Time nextTime = nextIt->first;
-
-        float t = static_cast<float>((time - prevTime).toSecondsDouble()
-                                     / (nextTime - prevTime).toSecondsDouble());
-
-        Eigen::Matrix4f globalPoseNext = next->toEigen();
-        Eigen::Matrix4f globalPosePrev = prev->toEigen();
-
-
-        Eigen::Matrix4f globalPose;
-
-        math::Helpers::Position(globalPose) =
-            (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
-
-        Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
-        Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
-        Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
-
-        math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
-
-        return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
-    }
-
-
-
 }
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index aba4d1e828a5b55b9165fb6681ca291f4d2bb5fb..f36f95e2fd954e40bf53cb6abf7dfbffe5cae896 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -41,6 +41,10 @@
 #include <shared_mutex>
 #include <mutex>
 
+namespace armarx::detail
+{
+    using RobotStateComponentWorkerPtr = std::unique_ptr<class RobotStateComponentWorker>;
+}
 
 namespace armarx
 {
@@ -82,6 +86,7 @@ namespace armarx
         virtual public RobotStateComponentInterface
     {
     public:
+        RobotStateComponent();
 
         std::string getDefaultName() const override;
 
@@ -89,7 +94,8 @@ namespace armarx
         // RobotStateComponentInterface interface
 
         /// \return SharedRobotInterface proxy to the internal synchronized robot.
-        SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&) const override;
+        void getSynchronizedRobot_async(const AMD_RobotStateComponentInterface_getSynchronizedRobotPtr& amd,
+                                        const Ice::Current& = Ice::emptyCurrent) const override;
 
         /**
          * Creates a snapshot of the robot state at this moment in time.
@@ -97,11 +103,18 @@ namespace armarx
          * \return Clone of the internal synchronized robot fixed to the configuration from the time of calling this function.
          */
         // [[deprecated]]
-        SharedRobotInterfacePrx getRobotSnapshot(const std::string& deprecated, const Ice::Current&) override;
-
-        SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time, const Ice::Current& current) override;
-        NameValueMap getJointConfigAtTimestamp(double timestamp, const Ice::Current&) const override;
-        RobotStateConfig getRobotStateAtTimestamp(double timestamp, const Ice::Current&) const override;
+        void getRobotSnapshot_async(const AMD_RobotStateComponentInterface_getRobotSnapshotPtr& amd,
+                                    const std::string& deprecated,
+                                    const Ice::Current& = Ice::emptyCurrent) override;
+        void getRobotSnapshotAtTimestamp_async(const AMD_RobotStateComponentInterface_getRobotSnapshotAtTimestampPtr& amd,
+                                               double time,
+                                               const Ice::Current& = Ice::emptyCurrent) override;
+        void getJointConfigAtTimestamp_async(const AMD_RobotStateComponentInterface_getJointConfigAtTimestampPtr& amd,
+                                             double time,
+                                             const Ice::Current& = Ice::emptyCurrent) const override;
+        void getRobotStateAtTimestamp_async(const AMD_RobotStateComponentInterface_getRobotStateAtTimestampPtr& amd,
+                                            double time,
+                                            const Ice::Current& = Ice::emptyCurrent) const override;
 
         /// \return the robot xml filename as specified in the configuration
         std::string getRobotFilename(const Ice::Current&) const override;
@@ -135,11 +148,8 @@ namespace armarx
         /// Does nothing.
         void reportPlatformOdometryPose(const PoseStamped& odometryPose, const Ice::Current&) override;
 
-
-        // Own interface.
+        //called from application
         void setRobotStateObserver(RobotStateObserverPtr observer);
-
-
     protected:
 
         // Component interface.
@@ -154,9 +164,6 @@ namespace armarx
         void onConnectComponent() override;
         void onDisconnectComponent() override;
 
-        /// Calls unref() on RobotStateComponent::_synchronizedPrx.
-        ~RobotStateComponent() override;
-
         /// Create an instance of RobotStatePropertyDefinitions.
         PropertyDefinitionsPtr createPropertyDefinitions() override;
 
@@ -164,77 +171,40 @@ namespace armarx
         // Inherited from KinematicUnitInterface
 
         /// Does nothing.
-        void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
         /// Stores the reported joint angles in the joint history and publishes the new joint angles.
         void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
         /// Sends the joint velocities to the robot state observer.
         void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
         /// Does nothing.
-        void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportJointTorques(const NameValueMap&, Ice::Long,  bool, const Ice::Current&)           override {}
         /// Does nothing.
-        void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+        void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&)      override {}
         /// Does nothing.
-        void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportJointCurrents(const NameValueMap&, Ice::Long,  bool, const Ice::Current&)          override {}
         /// Does nothing.
-        void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportJointMotorTemperatures(const NameValueMap&, Ice::Long,  bool, const Ice::Current&) override {}
         /// Does nothing.
-        void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportJointStatuses(const NameStatusMap&, Ice::Long,  bool, const Ice::Current&)         override {}
 
         void simulatorWasReset(const Ice::Current& = Ice::emptyCurrent) override;
     private:
 
         void readRobotInfo(const std::string& robotFile);
         RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
-
-        void insertPose(IceUtil::Time timestamp, const Eigen::Matrix4f& globalPose);
-
-
-        template <class ValueT>
-        struct Timestamped
-        {
-            IceUtil::Time timestamp;
-            ValueT value;
-        };
-
-        /// Interpolate the robot state from histories and store it in `config`.
-        std::optional<RobotStateConfig> interpolate(IceUtil::Time time) const;
-        /// Interpolate the joint angles from history and store it in `jointAngles`.
-        std::optional<Timestamped<NameValueMap>> interpolateJoints(IceUtil::Time time) const;
-        /// Interpolate the robot pose from history and store it in `pose`.
-        std::optional<Timestamped<FramedPosePtr>> interpolatePose(IceUtil::Time time) const;
-
-
     private:
 
-        /// Local robot model.
-        VirtualRobot::RobotPtr _synchronized;
-        /// Local shared robot.
-        SharedRobotServantPtr _sharedRobotServant;
-        /// Ice proxy to `_sharedRobotServant`.
-        SharedRobotInterfacePrx _synchronizedPrx;
-
-        RobotStateListenerInterfacePrx robotStateListenerPrx;
-        RobotStateObserverPtr robotStateObs;
-
         std::string robotStateTopicName;
         std::string robotFile;
         std::string relativeRobotFile;
-
-        mutable std::shared_mutex jointHistoryMutex;
-        std::map<IceUtil::Time, NameValueMap> jointHistory;
-        size_t jointHistoryLength;
-
-        mutable std::shared_mutex poseHistoryMutex;
-        std::map<IceUtil::Time, FramedPosePtr> poseHistory;
-        size_t poseHistoryLength;
-
+        std::string robotName;
         std::string robotNodeSetName;
 
         float robotModelScaling;
 
         RobotInfoNodePtr robotInfo;
 
+        detail::RobotStateComponentWorkerPtr _worker;
     };
-
 }
 
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
index b5dc5e5dd125a004758a383ad4c431517bd3f1d2..f5bb28f2f794200a6af845cfb107587149929d35 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
@@ -99,6 +99,30 @@
            </column>
           </widget>
          </item>
+         <item>
+          <layout class="QHBoxLayout" name="horizontalLayout_12">
+           <item>
+            <spacer name="horizontalSpacer_7">
+             <property name="orientation">
+              <enum>Qt::Horizontal</enum>
+             </property>
+             <property name="sizeHint" stdset="0">
+              <size>
+               <width>40</width>
+               <height>20</height>
+              </size>
+             </property>
+            </spacer>
+           </item>
+           <item>
+            <widget class="QPushButton" name="exportToVRMLButton">
+             <property name="text">
+              <string>Export To VRML</string>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </item>
         </layout>
        </item>
        <item>
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index fbb179979412d63867db7498608d9186234e1e16..cf42b9ff2bb4d42f04a6067f08803ea6ac69f873 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -21,6 +21,8 @@
  */
 
 #include "ArVizWidgetController.h"
+#include <RobotAPI/components/ArViz/Coin/VisualizationObject.h>
+#include <RobotAPI/components/ArViz/Coin/VisualizationRobot.h>
 
 #include <string>
 
@@ -32,6 +34,10 @@
 
 #include <ArmarXCore/observers/variant/Variant.h>
 
+
+#include <QFileDialog>
+#include  <boost/algorithm/string/predicate.hpp>
+
 #define ENABLE_INTROSPECTION 1
 
 
@@ -87,6 +93,8 @@ namespace armarx
         connect(widget.replayStopButton, &QPushButton::clicked, this, &This::onReplayStop);
         connect(widget.replayTimedButton, &QPushButton::toggled, this, &This::onReplayTimedStart);
 
+        connect(widget.exportToVRMLButton, &QPushButton::clicked, this, &This::exportToVRML);
+
         connect(this, &This::connectGui, this, &This::onConnectGui, Qt::QueuedConnection);
         connect(this, &This::disconnectGui, this, &This::onDisconnectGui, Qt::QueuedConnection);
 
@@ -143,6 +151,8 @@ namespace armarx
 
     void ArVizWidgetController::onExitComponent()
     {
+        armarx::viz::coin::clearObjectCache();
+        armarx::viz::coin::clearRobotCache();
     }
 
     void ArVizWidgetController::onConnectComponent()
@@ -985,4 +995,23 @@ namespace armarx
             debugObserverName = configDialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
         }
     }
+
+
+    void ArVizWidgetController::exportToVRML()
+    {
+
+        QString fi = QFileDialog::getSaveFileName(Q_NULLPTR, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
+        std::string s = std::string(fi.toLatin1());
+
+        if (s.empty())
+        {
+            return;
+        }
+        if (!boost::algorithm::ends_with(s, ".wrl"))
+        {
+            s += ".wrl";
+        }
+
+        visualizer.exportToVRML(s);
+    }
 }
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 4e9c887def80f19350373070799b456dca75d2db..e59b685797f5e8ee4b56220c9c5070109e50a140 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -162,6 +162,7 @@ namespace armarx
         long getRevisionForTimestamp(long timestamp);
         void onReplayTimedStart(bool checked);
         void onReplayTimerTick();
+        void exportToVRML();
 
         void changeMode(ArVizWidgetMode newMode);
         void enableWidgetAccordingToMode();
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 332959df89b7c0c4fe55ce2442f240d29e45213e..1beb3e37d11fd1fe1f4478e8c657766048928b28 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -198,28 +198,28 @@ module armarx
         /**
          * @return proxy to the shared robot which constantly updates all joint values
          */
-        ["cpp:const"]
+        ["cpp:const", "amd"]
         idempotent
         SharedRobotInterface* getSynchronizedRobot() throws NotInitializedException;
 
         /**
          * @return proxy to a copy of the shared robot with non updating joint values
          */
-        SharedRobotInterface* getRobotSnapshot(string deprecated) throws NotInitializedException;
+        ["amd"] SharedRobotInterface* getRobotSnapshot(string deprecated) throws NotInitializedException;
 
         /**
           * Create robot snapshot proxy from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
           * @return Snapshot
           *
           * */
-        SharedRobotInterface* getRobotSnapshotAtTimestamp(double time) throws NotInitializedException;
+        ["amd"] SharedRobotInterface* getRobotSnapshotAtTimestamp(double time) throws NotInitializedException;
 
         /**
           * Return the joint values from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
           * @return Jointname-jointvalue map
           *
           * */
-        ["cpp:const"]
+        ["cpp:const", "amd"]
         idempotent
         NameValueMap getJointConfigAtTimestamp(double time) throws NotInitializedException;
 
@@ -228,7 +228,7 @@ module armarx
           * @return Robot configuration containing jointvalue map and globalpose
           *
           * */
-        ["cpp:const"]
+        ["cpp:const", "amd"] 
         idempotent
         RobotStateConfig getRobotStateAtTimestamp(double time) throws NotInitializedException;
 
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index b52165269f99a97667f4a67ab3f5293cf58836bb..f9ef01e542e65c421fb5a80e82a8f4d23815c81d 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -109,6 +109,7 @@ module armarx
         void setLinearVelocityKp(float kp);
         void setAngularVelocityKd(float kd);
         void setAngularVelocityKp(float kp);
+
     };
 
     class NJointCCDMPControllerConfig extends NJointControllerConfig
@@ -511,6 +512,11 @@ module armarx
         float adaptForceCoeff;
         float changePositionTolerance;
         float changeTimerThreshold;
+
+        bool isManipulability = false;
+        Ice::DoubleSeq kmani;
+        Ice::DoubleSeq positionManipulability;
+
     };
 
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 1a173c43b48400f9796f92c437ff1a1ec934918e..ccd553f342786bc813a913b80e9b95b0feb4cf46 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -127,6 +127,28 @@ namespace armarx
         adaptK = kpos;
         lastDiff = 0;
         changeTimer = 0;
+
+
+        isManipulability = cfg->isManipulability;
+        auto manipulability = std::make_shared<VirtualRobot::SingleRobotNodeSetManipulability>(rns, rns->getTCP(), VirtualRobot::AbstractManipulability::Mode::Position, VirtualRobot::AbstractManipulability::Type::Velocity, rns->getRobot()->getRootNode());
+        manipulabilityTracker.reset(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+
+        ARMARX_CHECK_EQUAL(cfg->positionManipulability.size(), 9);
+        targetManipulability.setZero(3, 3);
+        targetManipulability << cfg->positionManipulability[0],  cfg->positionManipulability[1], cfg->positionManipulability[2],
+                             cfg->positionManipulability[3],  cfg->positionManipulability[4], cfg->positionManipulability[5],
+                             cfg->positionManipulability[6],  cfg->positionManipulability[7], cfg->positionManipulability[8];
+
+
+        Eigen::VectorXd kmaniVec;
+        kmaniVec.setZero(cfg->kmani.size());
+        for (size_t i = 0; i < cfg->kmani.size(); ++i)
+        {
+            kmaniVec[i] = cfg->kmani[i];
+        }
+
+        kmani = kmaniVec.asDiagonal();
+
     }
 
     void NJointPeriodicTSDMPCompliantController::onInitNJointController()
@@ -435,7 +457,6 @@ namespace armarx
         debugRT.getWriteBuffer().targetVel = targetVel;
         debugRT.getWriteBuffer().adaptK = adaptK;
         debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
-        debugRT.commitWrite();
 
         rt2CtrlData.getWriteBuffer().currentPose = currentPose;
         rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
@@ -490,7 +511,19 @@ namespace armarx
 
 
         Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-        Eigen::VectorXf nullspaceTorque = knull * (nullSpaceJointsVec - qpos) - dnull * qvel;
+        Eigen::VectorXf nullspaceTorque;
+
+        float manidist = 0;
+        if (isManipulability)
+        {
+            nullspaceTorque = manipulabilityTracker->calculateVelocity(targetManipulability, kmani);
+            manidist = manipulabilityTracker->computeDistance(targetManipulability);
+        }
+        else
+        {
+            nullspaceTorque = knull * (nullSpaceJointsVec - qpos) - dnull * qvel;
+        }
+
         Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
         Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
 
@@ -512,6 +545,10 @@ namespace armarx
             }
         }
 
+        debugRT.getWriteBuffer().manidist = manidist;
+
+        debugRT.commitWrite();
+
 
     }
 
@@ -624,6 +661,7 @@ namespace armarx
         datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
 
         datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
+        datafields["manidist"] = new Variant(debugRT.getUpToDateReadBuffer().manidist);
 
 
         //        datafields["targetVel_rx"] = new Variant(targetVel(3));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index 053113604a566892ec15c998c9705a6bc0dc0c6d..ebb3b05a5241c338cbdfc73ebd30ef3226d7219d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -15,7 +15,8 @@
 
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
 #include <RobotAPI/libraries/core/Trajectory.h>
-
+#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
 namespace armarx
 {
 
@@ -99,6 +100,7 @@ namespace armarx
             Eigen::VectorXf targetVel;
             Eigen::Matrix4f currentPose;
             bool isPhaseStop;
+            float manidist;
         };
         TripleBuffer<DebugRTData> debugRT;
 
@@ -172,6 +174,13 @@ namespace armarx
         Eigen::Vector2f lastPosition;
         double changeTimer;
 
+
+
+        bool isManipulability = false;
+        VirtualRobot::SingleChainManipulabilityTrackingPtr manipulabilityTracker;
+        Eigen::MatrixXd targetManipulability;
+        Eigen::MatrixXd kmani;
+
     };
 
 } // namespace armarx