diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index d4d68c30e9a198e5a50ba7238774f2cfaf3cb9b5..88b1806f31933d8d4b188b9005968f3017be4327 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -25,6 +25,8 @@
 
 #include <VirtualRobot/math/Helpers.h>
 
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
 #include <RobotAPI/libraries/core/Pose.h>
 
 using namespace math;
@@ -38,6 +40,7 @@ DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerP
 {
 }
 
+//1st order
 void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
 {
     CHECK_AND_ADD(name, DrawElementType::Pose)
@@ -74,12 +77,6 @@ void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f&
     debugDrawerPrx->setLineVisu(layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
 }
 
-void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
-{
-    CHECK_AND_ADD(name, DrawElementType::Line)
-    drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
-}
-
 void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
 {
     CHECK_AND_ADD(name, DrawElementType::Text)
@@ -123,6 +120,62 @@ void DebugDrawerHelper::setRobotConfig(const std::string& name, const std::map<s
     debugDrawerPrx->updateRobotConfig(layerName, name, config);
 }
 
+//2nd order
+void DebugDrawerHelper::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
+{
+    for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+    {
+        drawPose(prefix + std::to_string(idx), pose);
+    }
+}
+void DebugDrawerHelper::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale)
+{
+    for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+    {
+        drawPose(prefix + std::to_string(idx), pose, scale);
+    }
+}
+
+void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
+{
+    drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
+}
+
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx), width, color);
+    }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx));
+    }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx),
+                 ps.at(idx - 1).topRightCorner<3, 1>(),
+                 ps.at(idx).topRightCorner<3, 1>(),
+                 width, color);
+    }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx),
+                 ps.at(idx - 1).topRightCorner<3, 1>(),
+                 ps.at(idx).topRightCorner<3, 1>());
+    }
+}
+
+//utility
 void DebugDrawerHelper::clearLayer()
 {
     debugDrawerPrx->clearLayer(layerName);
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 334fa002eaa8cc1c8be7023757a48edd9cf5e620..521ed9cf56d9fbabbc81be3bb41337912d628ce5 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -69,6 +69,7 @@ namespace armarx
 
         DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot);
 
+        //1st order draw functions (direct calls to proxy)
         void drawPose(const std::string& name, const Eigen::Matrix4f& pose);
         void drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale);
 
@@ -77,7 +78,6 @@ namespace armarx
         void drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color);
 
         void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color);
-        void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
 
         void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
 
@@ -90,6 +90,18 @@ namespace armarx
         void drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const DrawColor& color);
         void setRobotConfig(const std::string& name, const std::map<std::string, float>& config);
 
+        //2nd order draw functions (call 1st order)
+        void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses);
+        void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale);
+
+        void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
+
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color);
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps);
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color);
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps);
+
+        //utility
         void clearLayer();
         void cyclicCleanup();
 
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index de0dde42b8e8670e6c3953405ad59993eca07460..9030e9d45d3b3d208b34b4af4e19b1c103ebb45b 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -27,6 +27,7 @@
 #include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/math/Helpers.h>
 #include <boost/format.hpp>
 #include <boost/foreach.hpp>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -56,15 +57,37 @@ namespace armarx
     }
 
 
+    RobotStatePropertyDefinitions::RobotStatePropertyDefinitions(std::string prefix) :
+        ComponentPropertyDefinitions(prefix)
+    {
+        defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
+        defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
+        defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+        defineOptionalProperty<std::string>("RobotStateReportingTopic", "RobotStateUpdates", "Name of the topic on which updates of the robot state are reported.");
+        defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history").setMin(0);
+        defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
+        defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
+        defineOptionalProperty<std::string>("PlatformTopicName", "PlatformState", "Topic where platform state is published.");
+    }
+
+
+    std::string RobotStateComponent::getDefaultName() const
+    {
+        return "RobotStateComponent";
+    }
+
+
     void RobotStateComponent::onInitComponent()
     {
         robotStateTopicName = getProperty<std::string>("RobotStateReportingTopic").getValue();
         offeringTopic(getProperty<std::string>("RobotStateReportingTopic"));
-        historyLength = getProperty<int>("HistoryLength").getValue();
-        {
-            boost::unique_lock<SharedMutex> lock(historyMutex);
-            history.clear();
-        }
+        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();
@@ -97,7 +120,7 @@ namespace armarx
         robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
 
 
-        if (robotNodeSetName == "")
+        if (robotNodeSetName.empty())
         {
             throw UserException("RobotNodeSet not defined");
         }
@@ -112,12 +135,14 @@ namespace armarx
         std::vector<RobotNodePtr> nodes = rns->getAllRobotNodes();
 
         ARMARX_VERBOSE << "Using RobotNodeSet " << robotNodeSetName << " with " << nodes.size() << " robot nodes.";
-        BOOST_FOREACH(RobotNodePtr node, nodes)
+        for (const RobotNodePtr& node : nodes)
         {
-            ARMARX_VERBOSE << "Node: " << node->getName() << endl;
+            ARMARX_VERBOSE << "Node: " << node->getName();
         }
         usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
 
+        usingTopicFromProperty("PlatformTopicName");
+
         try
         {
             readRobotInfo(robotFile);
@@ -126,20 +151,6 @@ namespace armarx
         {
             ARMARX_WARNING << "Failed to read robot info from file: " << robotFile;
         }
-
-
-        /*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
-        if (pns)
-        {
-            usingTopic("PlatformState");
-            vector<RobotNodePtr> nodes = pns->getAllRobotNodes();
-
-            ARMARX_VERBOSE << "Using RobotNodeSet Platform with " << nodes.size() << " robot nodes.";
-            BOOST_FOREACH(RobotNodePtr node, nodes)
-            {
-                ARMARX_VERBOSE << "Node: " << node->getName() << endl;
-            }
-        }*/
     }
 
     void RobotStateComponent::readRobotInfo(const std::string& robotFile)
@@ -184,7 +195,7 @@ namespace armarx
     }
 
 
-    SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current& current) const
+    SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current&) const
     {
         if (!this->_synchronizedPrx)
         {
@@ -195,8 +206,10 @@ namespace armarx
     }
 
 
-    SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const std::string& time, const Current& current)
+    SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const std::string& deprecated, const Current&)
     {
+        (void) deprecated;
+
         if (!_synchronized)
         {
             throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
@@ -204,7 +217,7 @@ namespace armarx
 
         auto clone = this->_synchronized->clone(_synchronized->getName());
 
-        SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), NULL);
+        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
@@ -212,27 +225,29 @@ namespace armarx
         return SharedRobotInterfacePrx::uncheckedCast(result);
     }
 
-    SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double time, const Current& current)
+    SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
     {
+        IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
+
         if (!_synchronized)
         {
             throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
         }
 
         VirtualRobot::RobotPtr clone = this->_synchronized->clone(_synchronized->getName());
-        RobotStateConfig config;
-        if (!interpolate(time, config))
+        auto config = interpolate(time);
+        if (!config)
         {
-            ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(time).toDateTime();
-            return NULL;
+            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()), NULL);
-        p->setTimestamp(IceUtil::Time::microSecondsDouble(time));
+        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);
+        // Virtal robot clone is buggy -> set global pose here.
+        p->setGlobalPose(config->globalPose);
 
         return SharedRobotInterfacePrx::uncheckedCast(result);
     }
@@ -240,99 +255,48 @@ namespace armarx
 
     NameValueMap RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
     {
-        RobotStateConfig config;
-        if (!interpolate(timestamp, config))
-        {
-            return NameValueMap();
-        }
-        return config.jointMap;
+        auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(timestamp));
+        return jointAngles ? *jointAngles : NameValueMap();
     }
 
 
     RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
     {
-        RobotStateConfig config;
-        if (!interpolate(timestamp, config))
-        {
-            return RobotStateConfig();
-        }
-        return config;
+        auto config = interpolate(IceUtil::Time::microSecondsDouble(timestamp));
+        return config ? *config : RobotStateConfig();
     }
 
-    bool RobotStateComponent::interpolate(double time, RobotStateConfig& config) const
+
+    std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
     {
-        boost::shared_lock<SharedMutex> lock(historyMutex);
-        auto it = history.lower_bound(time);
-        if (history.size() == 0)
-        {
-            ARMARX_WARNING << deactivateSpam(1) << "History of robot state component is empty!";
-            return false;
-        }
-        if (time > history.rbegin()->first)
-        {
-            double maxOffset = 2000000;
-            if (time > history.rbegin()->first + maxOffset)
-            {
-                ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>" << maxOffset << " usec) than newest available timestamp!\nrequested timestamp: "
-                               << IceUtil::Time::microSecondsDouble(time).toDateTime() << " newest timestamp: " << IceUtil::Time::microSecondsDouble(history.rbegin()->first).toDateTime();
-                return false;
-            }
-            config = history.rbegin()->second;
-        }
-        else if (it == history.end())
-        {
-            ARMARX_WARNING << deactivateSpam(1) << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(time).toDateTime() << " - oldest available value: " << IceUtil::Time::microSeconds(history.begin()->first).toDateTime();
-            return false;
-        }
-        else
-        {
-            config = it->second;
-            if (it->first == time)
-            {
-                //                ARMARX_INFO_S << "No Interpolation needed: " << time;
-            }
-            else
-            {
-                // interpolate
-                auto prevIt = std::prev(it);
-                if (prevIt != history.end())
-                {
-                    double deltaT = it->first - prevIt->first;
-                    double influenceNext = 1.0f - (double)(it->first - time) / deltaT;
-                    double influencePrev = 1.0f - (double)(time - prevIt->first) / deltaT;
-                    //                    ARMARX_INFO_S << "Interpolating: " << time << " prev: " << time - prevIt->first << " next: " << it->first - time << VAROUT(influenceNext) << VAROUT(influencePrev) << " sum: " << (influenceNext + influencePrev);
-                    auto jointIt = prevIt->second.jointMap.begin();
-                    for (auto& joint : config.jointMap)
-                    {
-                        joint.second = joint.second * influenceNext + jointIt->second * influencePrev;
-                        jointIt++;
-                    }
-                    config.timestamp = time;
-                    ARMARX_CHECK_EXPRESSION(it->second.globalPose);
-                    ARMARX_CHECK_EXPRESSION(prevIt->second.globalPose);
-                    Eigen::Matrix4f globalPoseNext = FramedPosePtr::dynamicCast(it->second.globalPose)->toEigen();
-                    Eigen::Matrix4f globalPosePref = FramedPosePtr::dynamicCast(prevIt->second.globalPose)->toEigen();
-                    Eigen::Quaternionf rotNext(globalPoseNext.block<3, 3>(0, 0));
-                    Eigen::Quaternionf rotPrev(globalPosePref.block<3, 3>(0, 0));
-                    Eigen::Quaternionf rotNew = rotPrev.slerp(influenceNext, rotNext);
-                    Eigen::Matrix4f globalPose;
-                    globalPose.block<3, 3>(0, 0) = rotNew.toRotationMatrix();
-                    globalPose.block<3, 1>(0, 3) = globalPoseNext.block<3, 1>(0, 3) * influenceNext +
-                                                   globalPosePref.block<3, 1>(0, 3) * influencePrev;
-
-                }
-            }
-        }
-        return true;
+        return relativeRobotFile;
     }
 
-    void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current& c)
+    float RobotStateComponent::getScaling(const Ice::Current&) const
+    {
+        return robotModelScaling;
+    }
+
+    RobotInfoNodePtr RobotStateComponent::getRobotInfo(const Ice::Current&) const
+    {
+        return robotInfo;
+    }
+
+
+    void RobotStateComponent::reportJointAngles(
+        const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current&)
     {
         if (timestamp <= 0)
         {
             timestamp = IceUtil::Time::now().toMicroSeconds();
         }
-        ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles << " from timestamp " << IceUtil::Time::microSeconds(timestamp).toDateTime() << " a value changed: " << aValueChanged;
+
+        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)
         {
             {
@@ -346,38 +310,56 @@ namespace armarx
                 robotStateObs->updatePoses();
             }
         }
-        auto time = IceUtil::Time::microSeconds(timestamp);
         if (_sharedRobotServant)
         {
             _sharedRobotServant->setTimestamp(time);
         }
-        boost::unique_lock<SharedMutex> lock(historyMutex);
-        history.insert(history.end(), std::make_pair(time.toMicroSecondsDouble(), RobotStateConfig {time.toMicroSecondsDouble(),
-                       new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
-                       jointAngles
-                                                                                                   }));
 
-        if (history.size() > historyLength)
         {
-            history.erase(history.begin());
+            boost::unique_lock<SharedMutex> lock(jointHistoryMutex);
+            jointHistory.emplace_hint(jointHistory.end(), time, jointAngles);
+
+            if (jointHistory.size() > jointHistoryLength)
+            {
+                jointHistory.erase(jointHistory.begin());
+            }
         }
 
         robotStateListenerPrx->reportJointValues(jointAngles, timestamp, aValueChanged);
     }
 
-    std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
+
+    void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&)
     {
-        return relativeRobotFile;
+        const float z = 0;
+        const Eigen::Vector3f position(currentPose.x, currentPose.y, z);
+        const Eigen::Matrix3f orientation =
+            Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+        const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
+
+        insertPose(IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds), globalPose);
     }
 
-    float RobotStateComponent::getScaling(const Ice::Current&) const
+    void RobotStateComponent::reportNewTargetPose(
+        Float newPlatformPositionX, Float newPlatformPositionY, Float newPlatformRotation,
+        const Current&)
     {
-        return robotModelScaling;
+        // Unused.
+        (void) newPlatformPositionX, (void) newPlatformPositionY, (void) newPlatformRotation;
     }
 
-    RobotInfoNodePtr RobotStateComponent::getRobotInfo(const Ice::Current&) const
+    void RobotStateComponent::reportPlatformVelocity(
+        Float currentPlatformVelocityX, Float currentPlatformVelocityY, Float currentPlatformVelocityRotation,
+        const Current&)
     {
-        return robotInfo;
+        // Unused.
+        (void) currentPlatformVelocityX, (void) currentPlatformVelocityY, (void) currentPlatformVelocityRotation;
+    }
+
+    void RobotStateComponent::reportPlatformOdometryPose(Float x, Float y, Float angle, const Current&)
+    {
+        // Unused.
+        (void) x, (void) y, (void) angle;
     }
 
     std::vector<std::string> RobotStateComponent::getArmarXPackages(const Current&) const
@@ -399,23 +381,46 @@ namespace armarx
         return result;
     }
 
-    void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
-    void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Current& c)
+    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& c) {}
-
-    void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Current& c)
+    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;
     }
-    void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
-    void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
-    void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
 
     PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
     {
@@ -432,7 +437,7 @@ namespace armarx
     {
         if (_synchronized)
         {
-            return _synchronizedPrx->getName();
+            return _synchronized->getName();  // _synchronizedPrx->getName();
         }
         else
         {
@@ -454,4 +459,193 @@ namespace armarx
     {
         return robotStateTopicName;
     }
+
+
+    void RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
+    {
+        boost::unique_lock<SharedMutex> lock(poseHistoryMutex);
+        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;
+            config.timestamp = time.toMicroSeconds();
+            config.jointMap = *joints;
+            config.globalPose = *globalPose;
+            return std::move(config);
+        }
+        else
+        {
+            return std::nullopt;
+        }
+    }
+
+    std::optional<NameValueMap> RobotStateComponent::interpolateJoints(IceUtil::Time time) const
+    {
+        boost::shared_lock<SharedMutex> 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_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
+                               << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+                               << "\nrequested timestamp: " << time.toDateTime()
+                               << " newest timestamp: " << newestTimeInHistory.toDateTime();
+                return std::nullopt;
+            }
+            return jointHistory.rbegin()->second;
+        }
+
+        // Get the next newer entry than 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;
+        }
+
+
+        const NameValueMap& next = nextIt->second;
+        auto prevIt = std::prev(nextIt);
+
+        if (prevIt == jointHistory.end())
+        {
+            // Next was oldest entry.
+            return next;
+        }
+
+        // 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 std::move(jointAngles);
+    }
+
+    std::optional<FramedPosePtr> RobotStateComponent::interpolatePose(IceUtil::Time time) const
+    {
+        boost::shared_lock<SharedMutex> lock(poseHistoryMutex);
+        // Get the older newer entry of time.
+
+        if (poseHistory.empty())
+        {
+            ARMARX_INFO << deactivateSpam(10)
+                        << "Pose history is empty. This can happen if there is no platform unit.";
+
+            ReadLockPtr readLock = _synchronized->getReadLock();
+            return new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
+        }
+
+
+        const IceUtil::Time newestTimeInHistory = poseHistory.rbegin()->first;
+        if (time > newestTimeInHistory)
+        {
+            IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+            if (time > newestTimeInHistory + maxOffset)
+            {
+                ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
+                               << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+                               << "\nrequested timestamp: " << time.toDateTime()
+                               << " newest timestamp: " << newestTimeInHistory.toDateTime();
+                return std::nullopt;
+            }
+            return 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;
+        }
+
+
+        auto prevIt = std::prev(nextIt);
+        ARMARX_CHECK_EXPRESSION(prevIt->second);
+
+        const FramedPosePtr& next = nextIt->second;
+
+        if (prevIt == poseHistory.end())
+        {
+            // Next was oldest entry.
+            return next;
+        }
+
+        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 new FramedPose(globalPose, armarx::GlobalFrame, "");
+    }
+
+
+
 }
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 661b9d3375240cf2d83e51f1b57819315dd574f5..e0fd01378fadb30ea339b7d790f2d043e9310aca 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -24,19 +24,19 @@
 
 #pragma once
 
-#include "SharedRobotServants.h"
+#include <optional>
 
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/application/properties/Properties.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
 #include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/interface/core/RobotState.h>
-#include "SharedRobotServants.h"
 
 #include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-
+#include <RobotAPI/interface/core/RobotState.h>
 #include <RobotAPI/libraries/core/remoterobot/RobotStateObserver.h>
 
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include "SharedRobotServants.h"
+
 
 namespace armarx
 {
@@ -44,22 +44,11 @@ namespace armarx
      * \class RobotStatePropertyDefinition
      * \brief
      */
-    class RobotStatePropertyDefinitions:
+    class RobotStatePropertyDefinitions :
         public ComponentPropertyDefinitions
     {
     public:
-        RobotStatePropertyDefinitions(std::string prefix):
-            ComponentPropertyDefinitions(prefix)
-        {
-            defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
-            defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
-            defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
-            defineOptionalProperty<std::string>("RobotStateReportingTopic", "RobotStateUpdates", "Name of the topic on which updates of the robot state are reported.");
-            defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
-            defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
-            defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
-
-        }
+        RobotStatePropertyDefinitions(std::string prefix);
     };
 
 
@@ -90,9 +79,12 @@ namespace armarx
     {
     public:
 
-        /**
-         * \return SharedRobotInterface proxy to the internal synchronized robot.
-         */
+        std::string getDefaultName() const override;
+
+
+        // RobotStateComponentInterface interface
+
+        /// \return SharedRobotInterface proxy to the internal synchronized robot.
         SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&) const override;
 
         /**
@@ -100,103 +92,134 @@ namespace armarx
          * \note You need to call destroy() or ref()/unref() to trigger deletion of this object.
          * \return Clone of the internal synchronized robot fixed to the configuration from the time of calling this function.
          */
-        SharedRobotInterfacePrx getRobotSnapshot(const std::string& time, const Ice::Current&) override;
+        // [[deprecated]]
+        SharedRobotInterfacePrx getRobotSnapshot(const std::string& deprecated, const Ice::Current&) override;
 
         SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time, const Ice::Current& current) override;
-        NameValueMap getJointConfigAtTimestamp(double, const Ice::Current&) const override;
+        NameValueMap getJointConfigAtTimestamp(double timestamp, const Ice::Current&) const override;
         RobotStateConfig getRobotStateAtTimestamp(double timestamp, const Ice::Current&) const override;
 
-        /**
-         * \return the robot xml filename as specified in the configuration
-         */
+        /// \return the robot xml filename as specified in the configuration
         std::string getRobotFilename(const Ice::Current&) const override;
 
-        /*!
-         * \brief getArmarXPackages
-         * \return All dependent packages, which might contain a robot file.
-         */
-        std::vector< std::string > getArmarXPackages(const Ice::Current&) const override;
+        /// \return All dependent packages, which might contain a robot file.
+        std::vector<std::string> getArmarXPackages(const Ice::Current&) const override;
 
-        /**
-         *
-         * \return  The name of this robot instance.
-         */
+        /// \return  The name of this robot instance.
         std::string getRobotName(const Ice::Current&) const override;
 
         std::string getRobotStateTopicName(const Ice::Current&) const override;
 
-        /**
-         *
-         * \return  The name of this robot instance.
-         */
+        /// \return  The name of this robot instance.
         std::string getRobotNodeSetName(const Ice::Current&) const override;
 
-        /**
-         * Create an instance of RobotStatePropertyDefinitions.
-         */
-        PropertyDefinitionsPtr createPropertyDefinitions() override;
+        float getScaling(const Ice::Current&) const override;
 
-        std::string getDefaultName() const override
-        {
-            return "RobotStateComponent";
-        }
-        void setRobotStateObserver(RobotStateObserverPtr observer);
+        RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override;
 
-        bool interpolate(double time, RobotStateConfig& config) const;
 
-        float getScaling(const Ice::Current&) const override;
+        // PlatformUnitListener interface
+        /// Stores the platform pose in the pose history.
+        void reportPlatformPose(const PlatformPose& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
+        /// Does nothing.
+        void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& = Ice::emptyCurrent) override;
+        /// Does nothing.
+        void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& = Ice::emptyCurrent) override;
+        /// Does nothing.
+        void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current& = Ice::emptyCurrent) override;
+
+
+        // Own interface.
+        void setRobotStateObserver(RobotStateObserverPtr observer);
 
-        RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override;
 
     protected:
+
+        // Component interface.
+
         /**
          * Load and create a VirtualRobot::Robot instance from the RobotFileName
          * property. Additionally listen on the KinematicUnit topic for the
          * RobotNodeSet specified in the RobotNodeSetName property.
          */
         void onInitComponent() override;
-        /**
-         * Setup RobotStateObjectFactories needed for creating RemoteRobot instances.
-         */
+        /// Setup RobotStateObjectFactories needed for creating RemoteRobot instances.
         void onConnectComponent() override;
         void onDisconnectComponent() override;
-        /**
-         * Calls unref() on RobotStateComponent::_synchronizedPrx.
-         */
+
+        /// Calls unref() on RobotStateComponent::_synchronizedPrx.
         ~RobotStateComponent() override;
 
-        // inherited from KinematicUnitInterface
+        /// Create an instance of RobotStatePropertyDefinitions.
+        PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+        // Inherited from KinematicUnitInterface
+
+        /// Does nothing.
         void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) 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;
+        /// Does nothing.
         void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+        /// Does nothing.
         void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        /// Does nothing.
         void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+        /// Does nothing.
         void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp,  bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
 
+
     private:
+
         void readRobotInfo(const std::string& robotFile);
         RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
 
-        SharedRobotInterfacePrx _synchronizedPrx;
-        SharedRobotServantPtr _sharedRobotServant;
+        void insertPose(IceUtil::Time timestamp, const Eigen::Matrix4f& globalPose);
+
+        /// 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<NameValueMap> interpolateJoints(IceUtil::Time time) const;
+        /// Interpolate the robot pose from history and store it in `pose`.
+        std::optional<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;
-        RobotStateObserverPtr robotStateObs;
 
-        mutable SharedMutex historyMutex;
-        std::map<double, RobotStateConfig> history;
-        size_t historyLength;
+        mutable SharedMutex jointHistoryMutex;
+        std::map<IceUtil::Time, NameValueMap> jointHistory;
+        size_t jointHistoryLength;
+
+        mutable SharedMutex poseHistoryMutex;
+        std::map<IceUtil::Time, FramedPosePtr> poseHistory;
+        size_t poseHistoryLength;
 
         std::string robotNodeSetName;
 
         float robotModelScaling;
 
         RobotInfoNodePtr robotInfo;
+
     };
 
 }
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index 9194408d35ea5596d18cf49a5dbe0f0c0aa85ab7..73661c4ceaacef34826d3a6e6ad22697b7ed628f 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -86,9 +86,9 @@ void PlatformUnitObserver::onConnectObserver()
 
 }
 
-void PlatformUnitObserver::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
+void PlatformUnitObserver::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
 {
-    nameValueMapToDataFields("platformPose", currentPlatformPositionX, currentPlatformPositionY, currentPlatformRotation);
+    nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
     updateChannel("platformPose");
 }
 
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 412b9c57f942a48789c833773c01c5f9d6927331..a79ee07926621bf333aa9b4775fab3e1ae76d307 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -78,7 +78,7 @@ namespace armarx
         void onConnectObserver() override;
 
         // slice interface implementation
-        void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&) override;
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 75fcbca37ad130dd9e19c9455ad10dbe6d166581..b526cff934941abee0918d5797838bc60380df17 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -58,7 +58,13 @@ void PlatformUnitSimulation::onInitPlatformUnit()
 
 void PlatformUnitSimulation::onStartPlatformUnit()
 {
-    listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
+    PlatformPose currentPose;
+    // FIXME: Take the timestamp from simulation
+    currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+    currentPose.x = currentPositionX;
+    currentPose.y = currentPositionY;
+    currentPose.rotationAroundZ = currentRotation;
+    listenerPrx->reportPlatformPose(currentPose);
     simulationTask->start();
 }
 
@@ -157,7 +163,13 @@ void PlatformUnitSimulation::simulationFunction()
                 break;
         }
     }
-    listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
+    PlatformPose currentPose;
+    // FIXME: Take the timestamp from simulation
+    currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+    currentPose.x = currentPositionX;
+    currentPose.y = currentPositionY;
+    currentPose.rotationAroundZ = currentRotation;
+    listenerPrx->reportPlatformPose(currentPose);
     listenerPrx->reportPlatformVelocity(vel[0], vel[1], vel[2]);
 }
 
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index c0f082947b658e46d526df7f6edebb4bf11169cd..f7087a226d59906ad7794a123742953d68e4c589 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -8,6 +8,29 @@
 
 namespace armarx
 {
+    std::ostream& operator<<(std::ostream& out, const CartesianWaypointControllerConfigWithForceLimit& cfg)
+    {
+        out << "maxPositionAcceleration     " << cfg.wpCfg.maxPositionAcceleration << '\n'
+            << "maxOrientationAcceleration  " << cfg.wpCfg.maxOrientationAcceleration << '\n'
+            << "maxNullspaceAcceleration    " << cfg.wpCfg.maxNullspaceAcceleration << '\n'
+
+            << "kpJointLimitAvoidance       " << cfg.wpCfg.kpJointLimitAvoidance << '\n'
+            << "jointLimitAvoidanceScale    " << cfg.wpCfg.jointLimitAvoidanceScale << '\n'
+
+            << "thresholdPositionNear       " << cfg.wpCfg.thresholdPositionNear << '\n'
+            << "thresholdPositionReached    " << cfg.wpCfg.thresholdPositionReached << '\n'
+            << "maxPosVel                   " << cfg.wpCfg.maxPosVel << '\n'
+            << "kpPos                       " << cfg.wpCfg.kpPos << '\n'
+
+            << "thresholdOrientationNear    " << cfg.wpCfg.thresholdOrientationNear << '\n'
+            << "thresholdOrientationReached " << cfg.wpCfg.thresholdOrientationReached << '\n'
+            << "maxOriVel                   " << cfg.wpCfg.maxOriVel << '\n'
+            << "kpOri                       " << cfg.wpCfg.kpOri << '\n'
+
+            << "forceThreshold              " << cfg.forceThreshold << '\n';
+        return out;
+    }
+
 
     NJointControllerRegistration<NJointCartesianWaypointController> registrationControllerNJointCartesianWaypointController("NJointCartesianWaypointController");
 
@@ -36,17 +59,18 @@ namespace armarx
         }
         //ctrl
         {
-            auto rtRobot = useSynchronizedRtRobot();
-            ARMARX_CHECK_NOT_NULL(rtRobot);
+            _rtRobot = useSynchronizedRtRobot();
+            ARMARX_CHECK_NOT_NULL(_rtRobot);
 
-            auto rns = rtRobot->getRobotNodeSet(config->rns);
-            ARMARX_CHECK_NOT_NULL(rns);
-            ARMARX_CHECK_NOT_NULL(rns->getTCP());
+            _rtRns = _rtRobot->getRobotNodeSet(config->rns);
+            ARMARX_CHECK_NOT_NULL(_rtRns);
+            _rtTcp = _rtRns->getTCP();
+            ARMARX_CHECK_NOT_NULL(_rtTcp);
 
-            _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(rns->getSize()));
+            _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
 
             _rtWpController = std::make_unique<CartesianWaypointController>(
-                                  rns,
+                                  _rtRns,
                                   _rtJointVelocityFeedbackBuffer,
                                   config->runCfg.wpCfg.maxPositionAcceleration,
                                   config->runCfg.wpCfg.maxOrientationAcceleration,
@@ -55,9 +79,9 @@ namespace armarx
 
             _rtWpController->setConfig({});
 
-            for (size_t i = 0; i < rns->getSize(); ++i)
+            for (size_t i = 0; i < _rtRns->getSize(); ++i)
             {
-                std::string jointName = rns->getNode(i)->getName();
+                std::string jointName = _rtRns->getNode(i)->getName();
                 auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
                 auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
                 ARMARX_CHECK_NOT_NULL(ct);
@@ -96,6 +120,8 @@ namespace armarx
 
     void NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
+        auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
+
         if (_tripBufWpCtrl.updateReadBuffer())
         {
             ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
@@ -126,11 +152,9 @@ namespace armarx
 
         if (_rtForceSensor)
         {
-            auto& ft = _tripBufFT.getWriteBuffer();
-            ft.force = *_rtForceSensor;
-            ft.torque = *_rtTorqueSensor;
-            ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
-            _tripBufFT.commitWrite();
+            rt2nonrtBuf.ft.force = *_rtForceSensor;
+            rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
+            rt2nonrtBuf.ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
 
             if (_setFTOffset)
             {
@@ -147,6 +171,8 @@ namespace armarx
 
         _publishWpsCur = _rtHasWps ? _rtWpController->currentWaypointIndex : 0;
 
+        rt2nonrtBuf.rootPose = _rtRobot->getGlobalPose();
+        rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
         if (_rtHasWps)
         {
             const float errorPos = _rtWpController->getPositionError();
@@ -166,6 +192,11 @@ namespace armarx
                 reachedTarget = (errorPos < _rtWpController->thresholdPositionReached) &&
                                 (errorOri < _rtWpController->thresholdOrientationReached);
             }
+            rt2nonrtBuf.tcpTarg = _rtWpController->getCurrentTarget();
+        }
+        else
+        {
+            rt2nonrtBuf.tcpTarg = rt2nonrtBuf.tcp;
         }
         _rtStopConditionReached = _rtStopConditionReached || reachedFtLimit || reachedTarget;
         _publishIsAtTarget = reachedTarget;
@@ -183,6 +214,7 @@ namespace armarx
                 *ptr = goal(idx);
             }
         }
+        _tripRt2NonRt.commitWrite();
     }
 
     void NJointCartesianWaypointController::rtPostDeactivateController()
@@ -202,7 +234,7 @@ namespace armarx
         w.cfgUpdated = true;
         w.cfg = cfg;
         _tripBufWpCtrl.commitWrite();
-        ARMARX_IMPORTANT << "set new config";
+        ARMARX_IMPORTANT << "set new config\n" << cfg;
     }
     void NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current&)
     {
@@ -212,7 +244,7 @@ namespace armarx
         w.cfgUpdated = false;
         w.wps = wps;
         _tripBufWpCtrl.commitWrite();
-        ARMARX_IMPORTANT << "set " << wps.size() << " new waypoints";
+        ARMARX_IMPORTANT << "set new waypoints\n" << wps;
     }
     void NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
     {
@@ -223,7 +255,7 @@ namespace armarx
         w.wps.clear();
         w.wps.emplace_back(wp);
         _tripBufWpCtrl.commitWrite();
-        ARMARX_IMPORTANT << "set new waypoint";
+        ARMARX_IMPORTANT << "set new waypoint\n" << wp;
     }
     void NJointCartesianWaypointController::setConfigAndWaypoints(const CartesianWaypointControllerConfigWithForceLimit& cfg,
             const std::vector<Eigen::Matrix4f>& wps,
@@ -236,7 +268,7 @@ namespace armarx
         w.cfg = cfg;
         w.wps = wps;
         _tripBufWpCtrl.commitWrite();
-        ARMARX_IMPORTANT << "set new config and" << wps.size() << " new waypoints";
+        ARMARX_IMPORTANT << "set new config\n" << cfg << " and new waypoints\n" << wps;
     }
     void NJointCartesianWaypointController::setConfigAndWaypoint(const CartesianWaypointControllerConfigWithForceLimit& cfg,
             const Eigen::Matrix4f& wp,
@@ -250,7 +282,7 @@ namespace armarx
         w.wps.clear();
         w.wps.emplace_back(wp);
         _tripBufWpCtrl.commitWrite();
-        ARMARX_IMPORTANT << "set new config and a new waypoint";
+        ARMARX_IMPORTANT << "set new config\n" << cfg << "and a new waypoint\n" << wp;
     }
     void NJointCartesianWaypointController::setNullVelocity()
     {
@@ -268,8 +300,8 @@ namespace armarx
     FTSensorValue NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
     {
         ARMARX_CHECK_NOT_NULL(_rtForceSensor);
-        std::lock_guard g{_tripBufFTMut};
-        return _tripBufFT.getUpToDateReadBuffer();
+        std::lock_guard g{_tripRt2NonRtMutex};
+        return _tripRt2NonRt.getUpToDateReadBuffer().ft;
     }
     void NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
     {
@@ -289,7 +321,7 @@ namespace armarx
 
     void NJointCartesianWaypointController::onPublish(
         const SensorAndControl&,
-        const DebugDrawerInterfacePrx&,
+        const DebugDrawerInterfacePrx& drawer,
         const DebugObserverInterfacePrx& obs)
     {
         const std::size_t wpsNum = _publishWpsNum;
@@ -325,5 +357,28 @@ namespace armarx
                     << ", perror " << errorPos << " / " << errorPosMax
                     << ", oerror " << errorOri << " / " << errorOriMax
                     << ')';
+
+        {
+            std::lock_guard g{_tripRt2NonRtMutex};
+            const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
+            if (buf.tcp != buf.tcpTarg)
+            {
+                const Eigen::Matrix4f gtcp = buf.rootPose * buf.tcp;
+                const Eigen::Matrix4f gtcptarg = buf.rootPose * buf.tcpTarg;
+                drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
+                drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
+                drawer->setLineVisu(
+                    getName(), "tcp2targ",
+                    new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+                    new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+                    3, {0, 0, 1, 1});
+            }
+            else
+            {
+                drawer->removePoseVisu(getName(), "tcp");
+                drawer->removePoseVisu(getName(), "tcptarg");
+                drawer->removeLineVisu(getName(), "tcp2targ");
+            }
+        }
     }
 }
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 50b76c2430b8d36bb5979acbb0c1fe80a895316e..16ebac8c13fa0acba8a0e96ce4950a428461aa68 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -76,11 +76,23 @@ namespace armarx
             bool cfgUpdated = false;
         };
 
+        struct RtToNonRtData
+        {
+            FTSensorValue ft;
+            Eigen::Matrix4f rootPose = Eigen::Matrix4f::Identity();
+            Eigen::Matrix4f tcp = Eigen::Matrix4f::Identity();
+            Eigen::Matrix4f tcpTarg = Eigen::Matrix4f::Identity();
+        };
+
         //data
     private:
         void setNullVelocity();
 
         //rt data
+        VirtualRobot::RobotPtr _rtRobot;
+        VirtualRobot::RobotNodeSetPtr _rtRns;
+        VirtualRobot::RobotNodePtr _rtTcp;
+
         std::unique_ptr<CartesianWaypointController> _rtWpController;
         Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
 
@@ -104,8 +116,9 @@ namespace armarx
         //buffers
         mutable std::recursive_mutex _tripBufWpCtrlMut;
         TripleBuffer<CtrlData> _tripBufWpCtrl;
-        mutable std::recursive_mutex _tripBufFTMut;
-        TripleBuffer<FTSensorValue> _tripBufFT;
+
+        mutable std::recursive_mutex _tripRt2NonRtMutex;
+        TripleBuffer<RtToNonRtData> _tripRt2NonRt;
 
         //publish data
         std::atomic_size_t _publishWpsNum{0};
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 48258990833084dbd0fee2a25240a5b71f9cb61a..1201da179c7ce4770ae7afffd0230c180b466b6a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -50,8 +50,13 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
         {
             const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
             abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
-            const float globR = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
-            listenerPrx->reportPlatformPose(abs(0, 3), abs(1, 3), globR);
+
+            PlatformPose currentPose;
+            currentPose.timestampInMicroSeconds = sc.sensorValuesTimestamp.toMicroSeconds();
+            currentPose.x = abs(0, 3);
+            currentPose.y = abs(1, 3);
+            currentPose.rotationAroundZ = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
+            listenerPrx->reportPlatformPose(currentPose);
         }
         else
         {
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
index a4d546f478e0c1b002ca19e9e1257e2fd180263f..a23cb5332f54d5107e7aa15a38e1577ad374c36c 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
@@ -163,7 +163,135 @@
       <item row="0" column="0">
        <widget class="QWidget" name="widgetSettings" native="true">
         <layout class="QGridLayout" name="gridLayout_5">
+         <item row="2" column="4">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosReached">
+           <property name="maximum">
+            <double>500.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>5.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="6">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosMaxVel">
+           <property name="maximum">
+            <double>500.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>80.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="0">
+          <widget class="QLabel" name="label_8">
+           <property name="text">
+            <string>Position</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
+          <widget class="QLabel" name="label_15">
+           <property name="text">
+            <string>Orientation</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="5">
+          <widget class="QLabel" name="label_20">
+           <property name="text">
+            <string>Max vel</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="2">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriNear">
+           <property name="maximum">
+            <double>5.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+           <property name="value">
+            <double>0.100000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_16">
+           <property name="text">
+            <string>Near</string>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="3">
+          <widget class="QLabel" name="label_17">
+           <property name="text">
+            <string>Reached</string>
+           </property>
+          </widget>
+         </item>
          <item row="1" column="0">
+          <widget class="QLabel" name="label_6">
+           <property name="text">
+            <string>Joint limit avoidance</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="4">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriReached">
+           <property name="maximum">
+            <double>5.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+           <property name="value">
+            <double>0.050000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="1">
+          <widget class="QLabel" name="label_14">
+           <property name="text">
+            <string>Near</string>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="2">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosNear">
+           <property name="maximum">
+            <double>500.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>50.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="4">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccOri">
+           <property name="maximum">
+            <double>4.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="1">
+          <widget class="QLabel" name="label_9">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="text">
+            <string>Pos</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="0">
           <widget class="QLabel" name="label_4">
            <property name="sizePolicy">
             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
@@ -176,7 +304,131 @@
            </property>
           </widget>
          </item>
-         <item row="2" column="0" colspan="2">
+         <item row="0" column="6">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccNull">
+           <property name="maximum">
+            <double>5.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>2.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="6">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriMaxVel">
+           <property name="maximum">
+            <double>5.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="0">
+          <widget class="QLabel" name="label_5">
+           <property name="text">
+            <string>Max acc</string>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="8">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxPosKP">
+           <property name="maximum">
+            <double>10.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="4">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidScale">
+           <property name="maximum">
+            <double>10.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>2.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="1">
+          <widget class="QLabel" name="label_12">
+           <property name="text">
+            <string>KP</string>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="5">
+          <widget class="QLabel" name="label_11">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="text">
+            <string>Nullspace</string>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="3">
+          <widget class="QLabel" name="label_13">
+           <property name="text">
+            <string>Scale</string>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="3">
+          <widget class="QLabel" name="label_10">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="text">
+            <string>Ori</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="8">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxOriKP">
+           <property name="maximum">
+            <double>10.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="5">
+          <widget class="QLabel" name="label_19">
+           <property name="text">
+            <string>Max vel</string>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="2">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccPos">
+           <property name="maximum">
+            <double>2000.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>500.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="2">
+          <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidKP">
+           <property name="maximum">
+            <double>10.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="0" colspan="9">
           <layout class="QHBoxLayout" name="horizontalLayout_2">
            <item>
             <widget class="QPushButton" name="pushButtonZeroFT">
@@ -194,7 +446,34 @@
            </item>
           </layout>
          </item>
-         <item row="1" column="1">
+         <item row="3" column="3">
+          <widget class="QLabel" name="label_18">
+           <property name="text">
+            <string>Reached</string>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="7">
+          <widget class="QLabel" name="label_21">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <property name="text">
+            <string>Kp</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="7">
+          <widget class="QLabel" name="label_22">
+           <property name="text">
+            <string>Kp</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="2" colspan="7">
           <widget class="QDoubleSpinBox" name="doubleSpinBoxFTLimit">
            <property name="minimum">
             <double>-1.000000000000000</double>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index df1458f3f5589e42f9167af964d8f2f9dfb362a9..09683de6a49e6a0948fd79532b170518751ed671 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -188,7 +188,6 @@ namespace armarx
     {
         _lastParsedWPs.clear();
         _ui.labelParsingSuccess->setText("<pre parsing>");
-        ///TODO _lastParsedWPs labelParsingSuccess
         if (_ui.radioButtonWPJson->isChecked())
         {
             //parse json
@@ -220,8 +219,25 @@ namespace armarx
     CartesianWaypointControllerConfigWithForceLimit CartesianWaypointControlGuiWidgetController::readRunCfg() const
     {
         CartesianWaypointControllerConfigWithForceLimit cfg;
-        cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
-        ///TODO add more gui elemetns to change the execution parameters
+
+        cfg.wpCfg.maxPositionAcceleration     = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
+        cfg.wpCfg.maxOrientationAcceleration  = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
+        cfg.wpCfg.maxNullspaceAcceleration    = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
+
+        cfg.wpCfg.kpJointLimitAvoidance       = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
+        cfg.wpCfg.jointLimitAvoidanceScale    = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
+
+        cfg.wpCfg.thresholdOrientationNear    = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
+        cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
+        cfg.wpCfg.thresholdPositionNear       = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
+        cfg.wpCfg.thresholdPositionReached    = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
+
+        cfg.wpCfg.maxOriVel                   = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
+        cfg.wpCfg.maxPosVel                   = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
+        cfg.wpCfg.kpOri                       = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
+        cfg.wpCfg.kpPos                       = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
+
+        cfg.forceThreshold                    = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
         return cfg;
     }
 
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index ac8efd42e927e1fd2e6258ba4e745ea5904af194..724a8c1ad7d31f5aaeca57f7ff29168624ea9c25 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -197,11 +197,11 @@ void PlatformUnitWidget::stopControlTimer()
     rotaCtrl->setNibble({0, 0});
 }
 
-void PlatformUnitWidget::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
+void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
 {
     // moved to qt thread for thread safety
-    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPlatformPositionX), Q_ARG(float, currentPlatformPositionY), Q_ARG(float, currentPlatformRotation));
-    platformRotation = currentPlatformRotation;
+    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPose.x), Q_ARG(float, currentPose.y), Q_ARG(float, currentPose.rotationAroundZ));
+    platformRotation = currentPose.rotationAroundZ;
 }
 
 void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index 11a09c2d031eee13a279acc0bdee9404e4cc670f..aeb8e8ce8f414bac77ec83e9e9237679b76873c8 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -116,7 +116,7 @@ namespace armarx
         void onExitComponent() override;
 
         // slice interface implementation
-        void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 4ae1c01d6dee2d60b0c36eba031ac79a016ab947..3ce455baa876d9adf3ec630d774b8138a5b0e1bc 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -15,9 +15,9 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    
- * @author     
- * @date       
+ * @package
+ * @author
+ * @date
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
@@ -85,9 +85,9 @@ module armarx
         ["cpp:const"] idempotent
         string getName();
 
-		["cpp:const"] idempotent
+        ["cpp:const"] idempotent
         PoseBase getLocalTransformation();
-        
+
         ["cpp:const"] idempotent
         FramedPoseBase getGlobalPose();
 
@@ -182,7 +182,7 @@ module armarx
      * snapshots of a robot configuration or retrieving a proxy to a constantly
      * updating synchronized robot.
      */
-    interface RobotStateComponentInterface extends KinematicUnitListener
+    interface RobotStateComponentInterface extends KinematicUnitListener, PlatformUnitListener
     {
         /**
          * @return proxy to the shared robot which constantly updates all joint values
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index df76354fa64b37ddbbb6b966141192aab9d9ed15..bec45c18e866a544a7a419f912c3169773187200 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -72,6 +72,15 @@ module armarx
 		**/
         void stopPlatform();
     };
+
+    struct PlatformPose
+    {
+        long timestampInMicroSeconds = 0;
+        float x = 0.0f;
+        float y = 0.0f;
+        float rotationAroundZ = 0.0f;
+    };
+
 	/**
 	* Implements an interface to an PlatformUnitListener.
 	**/
@@ -79,11 +88,9 @@ module armarx
     {
 		/**
 		* reportPlatformPose reports current platform pose.
-		* @param currentPlatformPositionX Global x-coordinate of current platform position.
-		* @param currentPlatformPositionY Global y-coordinate of current platform position.
-		* @param currentPlatformRotation Current global orientation of platform.
+        * @param currentPose Current global platform pose.
 		**/
-        void reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation);
+        void reportPlatformPose(PlatformPose currentPose);
         /**
 		* reportNewTargetPose reports target platform pose.
 		* @param newPlatformPositionX Global x-coordinate of target platform position.
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
index 7be0e04be38888d8af0ec895ebb2a804eedffed7..f5fc0ff5453ededba9f8743c09db6bffc1e51ff4 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
@@ -30,134 +30,127 @@ namespace armarx::RemoteGui
     {
         return RemoteGui::makeGroupBox(name)
                .addChild(
-                   RemoteGui::makeSimpleGridLayout().cols(3)
+                   RemoteGui::makeSimpleGridLayout().cols(10)
+
                    .addChild(RemoteGui::makeTextLabel("Max accelerations:"))
+                   .addChild(RemoteGui::makeTextLabel("Pos:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_maxAcc_Pos")
+                       .min(0)
+                       .max(10000)
+                       .value(val.maxPositionAcceleration)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("Ori:"))
                    .addChild(
-                       RemoteGui::makeHBoxLayout()
-                       .addChild(RemoteGui::makeTextLabel("Pos:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_maxAcc_Pos")
-                           .min(0)
-                           .max(10000)
-                           .value(val.kpJointLimitAvoidance)
-                           .decimals(3)
-                       )
-                       .addChild(new RemoteGui::HSpacer)
-                       .addChild(RemoteGui::makeTextLabel("Ori:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_maxAcc_Ori")
-                           .min(0)
-                           .max(10)
-                           .value(val.jointLimitAvoidanceScale)
-                           .decimals(3)
-                       )
-                       .addChild(new RemoteGui::HSpacer)
-                       .addChild(RemoteGui::makeTextLabel("Nullspace:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_maxAcc_Null")
-                           .min(0)
-                           .max(10)
-                           .value(val.jointLimitAvoidanceScale)
-                           .decimals(3)
-                       )
+                       RemoteGui::makeFloatSpinBox(name + "_maxAcc_Ori")
+                       .min(0)
+                       .max(10)
+                       .value(val.maxOrientationAcceleration)
+                       .decimals(3)
                    )
+                   .addChild(RemoteGui::makeTextLabel("Nullspace:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_maxAcc_Null")
+                       .min(0)
+                       .max(10)
+                       .value(val.maxNullspaceAcceleration)
+                       .decimals(3)
+                   )
+                   .addChild(new RemoteGui::Widget())
+                   .addChild(new RemoteGui::Widget())
                    .addChild(new RemoteGui::HSpacer)
+
                    .addChild(RemoteGui::makeTextLabel("JointLimitAvoidance:"))
+                   .addChild(RemoteGui::makeTextLabel("KP:"))
                    .addChild(
-                       RemoteGui::makeHBoxLayout()
-                       .addChild(RemoteGui::makeTextLabel("KP:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_KP")
-                           .min(0)
-                           .max(10)
-                           .value(val.kpJointLimitAvoidance)
-                           .decimals(3)
-                       )
-                       .addChild(new RemoteGui::HSpacer)
-                       .addChild(RemoteGui::makeTextLabel("Scale:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_Scale")
-                           .min(0)
-                           .max(10)
-                           .value(val.jointLimitAvoidanceScale)
-                           .decimals(3)
-                       )
+                       RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_KP")
+                       .min(0)
+                       .max(10)
+                       .value(val.kpJointLimitAvoidance)
+                       .decimals(3)
                    )
+                   .addChild(RemoteGui::makeTextLabel("Scale:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_Scale")
+                       .min(0)
+                       .max(10)
+                       .value(val.jointLimitAvoidanceScale)
+                       .decimals(3)
+                   )
+                   .addChild(new RemoteGui::Widget())
+                   .addChild(new RemoteGui::Widget())
+                   .addChild(new RemoteGui::Widget())
+                   .addChild(new RemoteGui::Widget())
                    .addChild(new RemoteGui::HSpacer)
-                   .addChild(RemoteGui::makeTextLabel("Thresholds:"))
+
+                   .addChild(RemoteGui::makeTextLabel("Position:"))
+                   .addChild(RemoteGui::makeTextLabel("Near:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Near")
+                       .min(0)
+                       .max(1000)
+                       .value(val.thresholdPositionNear)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("Reached:"))
                    .addChild(
-                       RemoteGui::makeHBoxLayout()
-                       .addChild(RemoteGui::makeTextLabel("Ori near:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Near")
-                           .min(0)
-                           .max(3.14f)
-                           .value(val.thresholdOrientationNear)
-                           .decimals(3)
-                       )
-                       .addChild(RemoteGui::makeTextLabel("Ori reached:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Reached")
-                           .min(0)
-                           .max(3.14f)
-                           .value(val.thresholdOrientationReached)
-                           .decimals(3)
-                       )
-                       .addChild(new RemoteGui::HSpacer)
-                       .addChild(RemoteGui::makeTextLabel("Pos near:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Near")
-                           .min(0)
-                           .max(1000)
-                           .value(val.thresholdPositionNear)
-                           .decimals(3)
-                       )
-                       .addChild(RemoteGui::makeTextLabel("Pos reached:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Reached")
-                           .min(0)
-                           .max(1000)
-                           .value(val.thresholdPositionReached)
-                           .decimals(3)
-                       )
+                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Reached")
+                       .min(0)
+                       .max(1000)
+                       .value(val.thresholdPositionReached)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("Max vel:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_Max_vel_pos")
+                       .min(0)
+                       .max(1000)
+                       .value(val.maxPosVel)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("KP:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_KP_pos")
+                       .min(0)
+                       .max(10)
+                       .value(val.kpPos)
+                       .decimals(3)
                    )
                    .addChild(new RemoteGui::HSpacer)
-                   .addChild(RemoteGui::makeTextLabel(""))
+
+                   .addChild(RemoteGui::makeTextLabel("Orientation:"))
+                   .addChild(RemoteGui::makeTextLabel("Near:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Near")
+                       .min(0)
+                       .max(3.14f)
+                       .value(val.thresholdOrientationNear)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("Reached:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Reached")
+                       .min(0)
+                       .max(3.14f)
+                       .value(val.thresholdOrientationReached)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("Max vel:"))
+                   .addChild(
+                       RemoteGui::makeFloatSpinBox(name + "_Max_vel_ori")
+                       .min(0)
+                       .max(31.4f)
+                       .value(val.maxOriVel)
+                       .decimals(3)
+                   )
+                   .addChild(RemoteGui::makeTextLabel("KP:"))
                    .addChild(
-                       RemoteGui::makeHBoxLayout()
-                       .addChild(RemoteGui::makeTextLabel("Max vel ori:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_Max_vel_ori")
-                           .min(0)
-                           .max(31.4f)
-                           .value(val.maxOriVel)
-                           .decimals(3)
-                       )
-                       .addChild(RemoteGui::makeTextLabel("Ori KP:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_KP_ori")
-                           .min(0)
-                           .max(10)
-                           .value(val.kpOri)
-                           .decimals(3)
-                       )
-                       .addChild(new RemoteGui::HSpacer)
-                       .addChild(RemoteGui::makeTextLabel("Max vel pos:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_Max_vel_pos")
-                           .min(0)
-                           .max(400)
-                           .value(val.maxPosVel)
-                           .decimals(3)
-                       )
-                       .addChild(RemoteGui::makeTextLabel("Pos KP:"))
-                       .addChild(
-                           RemoteGui::makeFloatSpinBox(name + "_KP_pos")
-                           .min(0)
-                           .max(10)
-                           .value(val.kpPos)
-                           .decimals(3)
-                       )
+                       RemoteGui::makeFloatSpinBox(name + "_KP_ori")
+                       .min(0)
+                       .max(10)
+                       .value(val.kpOri)
+                       .decimals(3)
                    )
                    .addChild(new RemoteGui::HSpacer)
                );
@@ -182,8 +175,8 @@ namespace armarx::RemoteGui
         cfg.thresholdPositionReached    = getValue<float>(values, name + "_Thresholds_Pos_Reached");
 
         cfg.maxOriVel                   = getValue<float>(values, name + "_Max_vel_ori");
-        cfg.maxPosVel                   = getValue<float>(values, name + "_KP_ori");
-        cfg.kpOri                       = getValue<float>(values, name + "_Max_vel_pos");
+        cfg.maxPosVel                   = getValue<float>(values, name + "_Max_vel_pos");
+        cfg.kpOri                       = getValue<float>(values, name + "_KP_ori");
         cfg.kpPos                       = getValue<float>(values, name + "_KP_pos");
         return cfg;
     }
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 03d860c56f2757a1c47de71c9d07eee0e4030c2b..e00ebc3c177ac6012992722de81935ee12013023 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -488,16 +488,24 @@ namespace armarx
     bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
     {
         ARMARX_CHECK_EXPRESSION(robotStatePrx);
+
+        RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
+        RobotStateConfig state = robotStatePrx->getRobotStateAtTimestamp(timestamp);
+
+        return synchronizeLocalCloneToState(robot, state);
+    }
+
+    bool RemoteRobot::synchronizeLocalCloneToState(RobotPtr robot, const RobotStateConfig& state)
+    {
         ARMARX_CHECK_EXPRESSION(robot);
 
         RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
-        RobotStateConfig config = robotStatePrx->getRobotStateAtTimestamp(timestamp);
-        if (config.jointMap.empty())
+        if (state.jointMap.empty())
         {
             return false;
         }
 
-        for (NameValueMap::const_iterator it = config.jointMap.begin(); it != config.jointMap.end(); it++)
+        for (NameValueMap::const_iterator it = state.jointMap.begin(); it != state.jointMap.end(); it++)
         {
             // joint values
             const std::string& jointName = it->first;
@@ -510,8 +518,9 @@ namespace armarx
         }
 
         robot->setConfig(c);
-        auto pose = PosePtr::dynamicCast(config.globalPose);
+        auto pose = PosePtr::dynamicCast(state.globalPose);
         robot->setGlobalPose(pose->toEigen());
+
         return true;
     }
 
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 03f07a1f4cc4118d3083df4d62d9ae527d9972ba..f9f637f33612c60860dd34990a7a16bb23683528 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -227,6 +227,8 @@ namespace armarx
          */
         static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp);
 
+        static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot, RobotStateConfig const& state);
+
         // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
 
         //! Clones the structure of this remote robot to a local instance
diff --git a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
index b6a2310df6ec38de4a5bb4f413636765212b5c0e..a18af95c22d8b53f97a5ab87121e5e815213a9d4 100644
--- a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
+++ b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
@@ -59,31 +59,31 @@ struct PointCloud
 private:
     /// The point container type.
     using VectorT = std::vector<PointT>;
-    
+
 public:
-    
+
     PointCloud() {}
     PointCloud(const VectorT& points) : points(points) {}
-    
+
     // Container methods.
     std::size_t size() const { return points.size(); }
-    
+
     PointT& operator[](std::size_t i) { return points[i]; }
     const PointT& operator[](std::size_t i) const { return points[i]; }
-    
+
     // Iterators.
     typename VectorT::iterator begin() { return points.begin(); }
     typename VectorT::const_iterator begin() const { return points.begin(); }
     typename VectorT::iterator end() { return points.end(); }
     typename VectorT::const_iterator end() const { return points.end(); }
-    
-    
+
+
     /// The points.
     VectorT points;
 };
 
 
-/* These test do not actually check any behaviour, 
+/* These test do not actually check any behaviour,
  * but check whether this code compiles.
  */
 
@@ -93,12 +93,12 @@ struct Fixture
     Fixture()
     {
     }
-    
-    const DebugDrawerTopic::VisuID id {"name", "layer"};
+
+    const DebugDrawerTopic::VisuID id {"layer", "name"};
     const int pointSize = 10;
-    
+
     DebugDrawerTopic drawer;
-    
+
     PointCloud<PointT> pointCloudMutable;
     const PointCloud<PointT>& pointCloud = pointCloudMutable;
 };
@@ -107,11 +107,11 @@ struct Fixture
 BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZ, Fixture<PointXYZ>)
 {
     pointCloudMutable.points = { {1, 2, 3}, {2, 3, 4}, {3, 4, 5} };
-    
+
     drawer.drawPointCloud(id, pointCloud);
     drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-    
-    drawer.drawPointCloud(id, pointCloud, 
+
+    drawer.drawPointCloud(id, pointCloud,
                           [](const PointXYZ&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
 }
 
@@ -120,10 +120,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBA, Fixture<PointXYZRGBA>)
 {
     drawer.drawPointCloud(id, pointCloud);
     drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-    
-    drawer.drawPointCloud(id, pointCloud, 
+
+    drawer.drawPointCloud(id, pointCloud,
                           [](const PointXYZRGBA&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
-    
+
     drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
 }
 
@@ -132,10 +132,10 @@ BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBL, Fixture<PointXYZRGBL>)
 {
     drawer.drawPointCloud(id, pointCloud);
     drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-    
+
     drawer.drawPointCloud(id, pointCloud,
                           [](const PointXYZRGBL&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
-    
+
     drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
 }
 
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index ef4c6d5a64a8a0dcc49ba960babc9c0c74848348..ca6e0b43978efa60f7e82dd5e5d17e05b027f2bb 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -16,13 +16,21 @@ namespace armarx
     DebugDrawerTopic::VisuID::VisuID() : VisuID("", "")
     {}
 
-    DebugDrawerTopic::VisuID::VisuID(const std::string& name, const std::string& layer) :
-        name(name), layer(layer)
+    DebugDrawerTopic::VisuID::VisuID(const std::string& name) : VisuID("", name)
     {}
 
+    DebugDrawerTopic::VisuID::VisuID(const std::string& layer, const std::string& name) :
+        layer(layer), name(name)
+    {}
+
+    DebugDrawerTopic::VisuID DebugDrawerTopic::VisuID::withName(const std::string& newName) const
+    {
+        return VisuID(this->layer, newName);
+    }
+
     std::ostream& operator<<(std::ostream& os, const DebugDrawerTopic::VisuID& rhs)
     {
-        os << "Visu '" << rhs.name << "' at layer '" << rhs.layer << "'";
+        os << "Visu '" << rhs.name << "' on layer '" << rhs.layer << "'";
         return os;
     }
 
@@ -50,6 +58,16 @@ namespace armarx
         return topic;
     }
 
+    void DebugDrawerTopic::setEnabled(bool enabled)
+    {
+        this->_enabled = enabled;
+    }
+
+    bool DebugDrawerTopic::enabled() const
+    {
+        return _enabled && topic;
+    }
+
     void DebugDrawerTopic::offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride) const
     {
         component.offeringTopic(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride);
@@ -194,9 +212,135 @@ namespace armarx
                 boundingBox.getMax() - boundingBox.getMin(), color, ignoreLengthScale);
     }
 
-    void DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
-                                        const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float radius, float length,
-                                        const DrawColor& color, bool ignoreLengthScale)
+    void DebugDrawerTopic::removeBox(const DebugDrawerTopic::VisuID& id)
+    {
+        if (enabled())
+        {
+            topic->removeBoxVisu(layer(id), id.name);
+        }
+    }
+
+    void DebugDrawerTopic::drawBoxEdges(
+        const DebugDrawerTopic::VisuID& id,
+        const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+        const Eigen::Vector3f& extents,
+        float width, const DrawColor& color, bool ignoreLengthScale)
+    {
+        drawBoxEdges(id, math::Helpers::Pose(position, orientation), extents, width, color, ignoreLengthScale);
+    }
+
+    void DebugDrawerTopic::drawBoxEdges(
+        const DebugDrawerTopic::VisuID& id,
+        const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+        float width, const DrawColor& color, bool ignoreLengthScale)
+    {
+        if (!enabled())
+        {
+            return;
+        }
+
+        std::vector<Eigen::Vector3f> points;
+
+        Eigen::Matrix<float, 3, 2> bb;
+        bb.col(0) = -extents / 2;
+        bb.col(1) =  extents / 2;
+
+        auto addLine = [&](int x1, int y1, int z1, int x2, int y2, int z2)
+        {
+            Eigen::Vector3f start = { bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z() };
+            Eigen::Vector3f end = { bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z() };
+
+            start = math::Helpers::TransformPosition(pose, start);
+            end = math::Helpers::TransformPosition(pose, end);
+
+            points.push_back(start);
+            points.push_back(end);
+        };
+
+        /*   001 +-----+ 011
+         *      /| 111/|
+         * 101 +-----+ |
+         *     | +---|-+ 010
+         *     |/000 |/
+         * 100 +-----+ 110
+         */
+
+        // 000 -> 100, 010, 001
+        addLine(0, 0, 0, 1, 0, 0);
+        addLine(0, 0, 0, 0, 1, 0);
+        addLine(0, 0, 0, 0, 0, 1);
+
+        // 111 -> 011, 101, 110
+        addLine(1, 1, 1, 0, 1, 1);
+        addLine(1, 1, 1, 1, 0, 1);
+        addLine(1, 1, 1, 1, 1, 0);
+
+        // 100 -> 101, 110
+        addLine(1, 0, 0, 1, 0, 1);
+        addLine(1, 0, 0, 1, 1, 0);
+
+        // 010 -> 110, 011
+        addLine(0, 1, 0, 1, 1, 0);
+        addLine(0, 1, 0, 0, 1, 1);
+
+        // 001 -> 101, 011
+        addLine(0, 0, 1, 1, 0, 1);
+        addLine(0, 0, 1, 0, 1, 1);
+
+        drawLineSet(id, points, width, color, ignoreLengthScale);
+    }
+
+    void DebugDrawerTopic::drawBoxEdges(
+        const DebugDrawerTopic::VisuID& id, const VirtualRobot::BoundingBox& boundingBox,
+        float width, const DrawColor& color, bool ignoreLengthScale)
+    {
+        drawBoxEdges(id, boundingBox, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
+    }
+
+    void DebugDrawerTopic::drawBoxEdges(
+            const DebugDrawerTopic::VisuID& id, const Eigen::Matrix32f& aabb,
+            float width, const DrawColor& color, bool ignoreLengthScale)
+    {
+        drawBoxEdges(id, aabb, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
+    }
+
+    void DebugDrawerTopic::drawBoxEdges(
+        const DebugDrawerTopic::VisuID& id,
+        const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+        float width, const DrawColor& color, bool ignoreLengthScale)
+    {
+        const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
+
+        drawBoxEdges(id, math::Helpers::Pose(math::Helpers::TransformPosition(pose, center),
+                                             math::Helpers::Orientation(pose)),
+                     boundingBox.getMax() - boundingBox.getMin(),
+                     width, color, ignoreLengthScale);
+    }
+
+    void DebugDrawerTopic::drawBoxEdges(
+        const DebugDrawerTopic::VisuID& id,
+        const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
+        float width, const DrawColor& color, bool ignoreLengthScale)
+    {
+        const Eigen::Vector3f center = 0.5 * (aabb.col(0) + aabb.col(1));
+
+        drawBoxEdges(id, math::Helpers::Pose(math::Helpers::TransformPosition(pose, center),
+                                             math::Helpers::Orientation(pose)),
+                     aabb.col(1) - aabb.col(0),
+                     width, color, ignoreLengthScale);
+    }
+
+    void DebugDrawerTopic::removeboxEdges(const DebugDrawerTopic::VisuID& id)
+    {
+        removeLineSet(id);
+    }
+
+
+    void DebugDrawerTopic::drawCylinder(
+        const DebugDrawerTopic::VisuID& id,
+        const Eigen::Vector3f& center, const Eigen::Vector3f& direction,
+        float length, float radius,
+        const DrawColor& color, bool ignoreLengthScale)
     {
         if (enabled())
         {
@@ -208,7 +352,8 @@ namespace armarx
 
     void DebugDrawerTopic::drawCylinder(
         const DebugDrawerTopic::VisuID& id,
-        const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float radius, float length,
+        const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation,
+        float length, float radius,
         const DrawColor& color, bool ignoreLengthScale)
     {
         drawCylinder(id, center, orientation * Eigen::Vector3f::UnitY(), radius, length, color,
@@ -228,6 +373,16 @@ namespace armarx
         }
     }
 
+
+    void DebugDrawerTopic::removeCylinder(const DebugDrawerTopic::VisuID& id)
+    {
+        if (enabled())
+        {
+            topic->removeCylinderVisu(layer(id), id.name);
+        }
+    }
+
+
     void DebugDrawerTopic::drawSphere(
         const DebugDrawerTopic::VisuID& id,
         const Eigen::Vector3f& center, float radius,
@@ -242,6 +397,15 @@ namespace armarx
     }
 
 
+    void DebugDrawerTopic::removeSphere(const DebugDrawerTopic::VisuID& id)
+    {
+        if (enabled())
+        {
+            topic->removeSphereVisu(layer(id), id.name);
+        }
+    }
+
+
     void DebugDrawerTopic::drawArrow(
         const VisuID& id,
         const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
@@ -255,6 +419,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::drawArrowFromTo(
         const VisuID& id,
         const Eigen::Vector3f& from, const Eigen::Vector3f& to,
@@ -268,6 +433,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::removeArrow(const DebugDrawerTopic::VisuID& id)
     {
         if (enabled())
@@ -299,6 +465,16 @@ namespace armarx
         }
     }
 
+
+    void DebugDrawerTopic::removePolygon(const DebugDrawerTopic::VisuID& id)
+    {
+        if (enabled())
+        {
+            topic->removePolygonVisu(layer(id), id.name);
+        }
+    }
+
+
     void DebugDrawerTopic::drawLine(
         const VisuID& id, const Eigen::Vector3f& from, const Eigen::Vector3f& to,
         float width, const DrawColor& color, bool ignoreLengthScale)
@@ -312,6 +488,16 @@ namespace armarx
         }
     }
 
+
+    void DebugDrawerTopic::removeLine(const DebugDrawerTopic::VisuID& id)
+    {
+        if (enabled())
+        {
+            topic->removeLineVisu(layer(id), id.name);
+        }
+    }
+
+
     void DebugDrawerTopic::drawLineSet(
         const VisuID& id, const DebugDrawerLineSet& lineSet, bool ignoreLengthScale)
     {
@@ -337,6 +523,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::drawLineSet(
         const VisuID& id, const std::vector<Eigen::Vector3f>& points,
         float width, const DrawColor& color, bool ignoreLengthScale)
@@ -385,12 +572,22 @@ namespace armarx
     }
 
 
+    void DebugDrawerTopic::removeLineSet(const DebugDrawerTopic::VisuID& id)
+    {
+        if (enabled())
+        {
+            topic->removeLineSetVisu(layer(id), id.name);
+        }
+    }
+
+
     void DebugDrawerTopic::drawPose(
         const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale)
     {
         drawPose(id, pose, _poseScale, ignoreLengthScale);
     }
 
+
     void DebugDrawerTopic::drawPose(
         const VisuID& id,
         const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
@@ -399,6 +596,7 @@ namespace armarx
         drawPose(id, math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
     }
 
+
     void DebugDrawerTopic::drawPose(
         const VisuID& id, const Eigen::Matrix4f& pose, float scale,
         bool ignoreLengthScale)
@@ -419,6 +617,7 @@ namespace armarx
 
     }
 
+
     void DebugDrawerTopic::drawPose(
         const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
         float scale, bool ignoreLengthScale)
@@ -426,6 +625,7 @@ namespace armarx
         drawPose(id, math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
     }
 
+
     void DebugDrawerTopic::removePose(const DebugDrawerTopic::VisuID& id)
     {
         if (enabled())
@@ -445,6 +645,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::updateRobotPose(
         const DebugDrawerTopic::VisuID& id,
         const Eigen::Matrix4f& pose, bool ignoreScale)
@@ -455,6 +656,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::updateRobotPose(
         const DebugDrawerTopic::VisuID& id,
         const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, bool ignoreScale)
@@ -462,6 +664,7 @@ namespace armarx
         updateRobotPose(id, math::Helpers::Pose(pos, ori), ignoreScale);
     }
 
+
     void DebugDrawerTopic::updateRobotConfig(
         const DebugDrawerTopic::VisuID& id, const std::map<std::string, float>& config)
     {
@@ -471,6 +674,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::updateRobotColor(
         const DebugDrawerTopic::VisuID& id, const DrawColor& color)
     {
@@ -480,6 +684,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::updateRobotNodeColor(
         const DebugDrawerTopic::VisuID& id,
         const std::string& nodeName, const DrawColor& color)
@@ -490,6 +695,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::removeRobot(const DebugDrawerTopic::VisuID& id)
     {
         if (enabled())
@@ -562,6 +768,7 @@ namespace armarx
         topic->setTriMeshVisu(layer(id), id.name, dd);
     }
 
+
     void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
             const VirtualRobot::TriMeshModel& trimesh,
             const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
@@ -571,6 +778,7 @@ namespace armarx
                               colorFace, lineWidth, colorEdge, ignoreLengthScale);
     }
 
+
     void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
             const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
             const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
@@ -608,6 +816,7 @@ namespace armarx
         }
     }
 
+
     void DebugDrawerTopic::drawTriMeshAsPolygons(
         const VisuID& id,
         const VirtualRobot::TriMeshModel& trimesh,
@@ -757,15 +966,10 @@ namespace armarx
         }
     }
 
-    bool DebugDrawerTopic::enabled() const
-    {
-        return topic;
-    }
-
 
     DebugDrawerTopic::operator bool() const
     {
-        return topic;
+        return enabled();
     }
 
     armarx::DebugDrawerTopic::operator DebugDrawerInterfacePrx& ()
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
index b95de6d43258d5a34246a639f6735ea4ee1ee1b4..c707e5cb242660136f2b0e4f0fd42ad68f5d617d 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -8,6 +8,18 @@
 
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
+namespace Eigen
+{
+    /**
+     * @brief A 3x2 matrix.
+     *
+     * Useful to represent axis-aligned bounding boxes (AABBs). When used as a
+     * AABB, column 0 contains the minimal x, y, z values and column 1 the
+     * maximal x, y, z values.
+     * Accordingly, the rows each contain the limits in x, y, z direction.
+     */
+    using Matrix32f = Matrix<float, 3, 2>;
+}
 
 namespace VirtualRobot
 {
@@ -15,7 +27,6 @@ namespace VirtualRobot
     class BoundingBox;
 }
 
-
 namespace armarx
 {
     // forward declaration
@@ -30,16 +41,17 @@ namespace armarx
      * types, and take care of conversion to Ice variants or data structures.
      * In addition, this class provides useful overloads for different use cases.
      *
-     * All methods check whether the internal topic proxy is set, and do
-     * nothing if it is not set. To disable visualization by this classs
-     * completely, just do not set the topic. To check whether visualization
-     * is enabled (i.e. a topic proxy is set), use `enabled()` or just convert
-     * `*this` to bool:
+     * Drawing is enabled if the internal topic proxy is set and an optional
+     * enabled flag is set (true by default). All methods check whether drawing
+     * is enabled and do nothing if drawing is disabled. To disable
+     * visualization by this class completely, use `setEnabled(true/false)` or
+     * just do not set the topic. To check whether visualization is enabled,
+     * use `enabled()` or just convert `*this` to bool:
      * @code
      * DebugDrawerTopic debugDrawer;
-     * if (debugDrawer)  // equivalent: if (debugDrawer.enabled())
+     * if (debugDrawer)  // Equivalent: if (debugDrawer.enabled())
      * {
-     *     // do stuff if visualization is enabled
+     *     // Do stuff if visualization is enabled.
      * }
      * @endcode
      *
@@ -122,7 +134,7 @@ namespace armarx
      * Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
      * std::string layer = "layer";
      * std::string name = "pose";
-     * debugDrawer.drawPose({name, layer}, pose);
+     * debugDrawer.drawPose({layer, name}, pose);
      * @endcode
      *
      *
@@ -141,45 +153,57 @@ namespace armarx
     {
     public:
 
+
+        /**
+         * @brief A visualisation ID.
+         *
+         * This constructor can be called in the following ways
+         * (with `draw(const VisuID& id, ...)` being any drawing method):
+         *
+         * @code
+         * std::string name = "pose";
+         * std::string layer = "layer";
+         * draw(name, ...);           // just the name, implicit call
+         * draw({name}, ...);         // just the name, call with initializer list
+         * draw({layer, name}, ...);  // layer and name, with initializer list
+         * @endcode
+         *
+         * (And of course by an explicit call if you want to be really verbose.)
+         * Not passing a layer will cause DebugDrawerTopic to use the
+         * preset layer.
+         */
         struct VisuID
         {
+        public:
+
             /// Empty constructor.
             VisuID();
 
-            /**
-             * @brief Construct a VisuID.
-             *
-             * This constructor can be called in the following ways
-             * (with `draw(const VisuID& id, ...)` being any drawing method):
-             *
-             * @code
-             * std::string name = "pose";
-             * std::string layer = "layer";
-             * draw(name, ...);           // just the name, implicit call
-             * draw({name}, ...);         // just the name, call with initializer list
-             * draw({name, layer}, ...);  // name and layer, with initializer list
-             * @endcode
-             *
-             * (And of course by an explicit call if you want to be really verbose.)
-             * Not passing a layer will cause DebugDrawerTopic to use the
-             * preset layer.
-             *
-             * @param name the name
-             * @param layer the layer name
-             */
-            VisuID(const std::string& name, const std::string& layer = "");
+            /// Construct a VisuID with given name (for drawing to the preset layer).
+            VisuID(const std::string& name);
+            /// Construct a VisuID with given name and layer.
+            VisuID(const std::string& layer, const std::string& name);
 
             /// Construct a VisuID from a non-std::string source (e.g. char[]).
             template <typename Source>
             VisuID(const Source& name) : VisuID(std::string(name))
             {}
 
-            std::string name = "";   ///< The visu name (empty by default).
-            std::string layer = "";  ///< The layer name (empty by default).
 
+            /// Get a `VisuID` with the given name and same layer as `*this.
+            VisuID withName(const std::string& name) const;
+
+            /// Streams a short human-readable description of `rhs` to `os`.
             friend std::ostream& operator<<(std::ostream& os, const VisuID& rhs);
+
+        public:
+
+            std::string layer = "";  ///< The layer name (empty by default).
+            std::string name = "";   ///< The visu name (empty by default).
         };
 
+
+        /// Default values for drawing functions.
         struct Defaults
         {
             DrawColor colorText { 0, 0, 0, 1 };
@@ -199,6 +223,11 @@ namespace armarx
 
             // Default value of DebugDrawerColoredPointCloud etc.
             float pointCloudPointSize = 3.0f;
+
+            float lineWidth = 2;
+
+            DrawColor boxEdgesColor { 0, 1, 1, 1 };
+            float boxEdgesWidth = 2;
         };
         static const Defaults DEFAULTS;
 
@@ -218,6 +247,14 @@ namespace armarx
         /// Get the topic.
         DebugDrawerInterfacePrx getTopic() const;
 
+        /**
+         * @brief Set whether drawing is enabled.
+         * Visualization is only truly enabled if the topic is set.
+         */
+        void setEnabled(bool enabled);
+        /// Indicate whether visualization is enabled, i.e. a topic is set and enabled flag is set.
+        bool enabled() const;
+
         /**
          * @brief Call offeringTopic([topicName]) on the given component.
          * @param component The component (`*this` when called from a component).
@@ -321,24 +358,69 @@ namespace armarx
                      bool ignoreLengthScale = false);
 
 
+        /// Remove a box.
+        void removeBox(const VisuID& id);
+
+
+        /// Draw box edges (as a line set).
+        void drawBoxEdges(const VisuID& id,
+                          const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+                          const Eigen::Vector3f& extents,
+                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+                          bool ignoreLengthScale = false);
+
+        /// Draw box edges (as a line set).
+        void drawBoxEdges(const VisuID& id,
+                          const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+                          bool ignoreLengthScale = false);
+
+        /// Draw edges of an axis aligned bounding box (as a line set).
+        void drawBoxEdges(const VisuID& id,
+                          const VirtualRobot::BoundingBox& boundingBox,
+                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+                          bool ignoreLengthScale = false);
+
+        /// Draw edges of an axis aligned bounding box (as a line set).
+        void drawBoxEdges(const VisuID& id,
+                          const Eigen::Matrix32f& aabb,
+                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+                          bool ignoreLengthScale = false);
+
+        /// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
+        void drawBoxEdges(const VisuID& id,
+                          const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+                          bool ignoreLengthScale = false);
+
+        /// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
+        void drawBoxEdges(const VisuID& id,
+                          const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
+                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+                          bool ignoreLengthScale = false);
+
+        /// Remove box edges (as a line set).
+        void removeboxEdges(const VisuID& id);
+
+
         /**
          * @brief Draw a cylinder with center and direction.
-         * @param length the full length (not half-length)
+         * @param length The full length (not half-length).
          */
         void drawCylinder(
             const VisuID& id,
-            const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float radius, float length,
+            const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float length, float radius,
             const DrawColor& color = DEFAULTS.colorCylinder,
             bool ignoreLengthScale = false);
 
         /**
          * @brief Draw a cylinder with center and orientation.
          * An identity orientation represents a cylinder with an axis aligned to the Y-axis.
-         * @param length the full length (not half-length)
+         * @param length The full length (not half-length).
          */
         void drawCylinder(
             const VisuID& id,
-            const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float radius, float length,
+            const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float length, float radius,
             const DrawColor& color = DEFAULTS.colorCylinder,
             bool ignoreLengthScale = false);
 
@@ -349,6 +431,9 @@ namespace armarx
             const DrawColor& color = DEFAULTS.colorCylinder,
             bool ignoreLengthScale = false);
 
+        /// Remove a cylinder.
+        void removeCylinder(const VisuID& id);
+
 
         /// Draw a sphere.
         void drawSphere(
@@ -357,6 +442,9 @@ namespace armarx
             const DrawColor& color = DEFAULTS.colorSphere,
             bool ignoreLengthScale = false);
 
+        /// Remove a sphere.
+        void removeSphere(const VisuID& id);
+
 
         /// Draw an arrow with position (start) and direction.
         void drawArrow(
@@ -384,6 +472,9 @@ namespace armarx
             float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
             bool ignoreLengthScale = false);
 
+        /// Remove a polygon.
+        void removePolygon(const VisuID& id);
+
 
         /// Draw a line from start to end.
         void drawLine(
@@ -392,6 +483,9 @@ namespace armarx
             float width, const DrawColor& color = DEFAULTS.colorLine,
             bool ignoreLengthScale = false);
 
+        /// Remove a line.
+        void removeLine(const VisuID& id);
+
 
         /// Draw a line set.
         void drawLineSet(
@@ -421,6 +515,9 @@ namespace armarx
             const std::vector<float>& intensitiesB,
             bool ignoreLengthScale = false);
 
+        /// Remove a line set.
+        void removeLineSet(const VisuID& id);
+
 
         // POSE
 
@@ -439,7 +536,7 @@ namespace armarx
                       float scale,
                       bool ignoreLengthScale = false);
 
-        /// Remove a pose visualization.
+        /// Remove a pose.
         void removePose(const VisuID& id);
 
 
@@ -614,16 +711,12 @@ namespace armarx
             bool ignoreLengthScale = false);
 
 
-        // STATUS
+        // OPERATORS
 
-        /// Indicate whether a topic is set, i.e. visualization is enabled.
-        bool enabled() const;
-        /// Indicate whether a topic is set, i.e. visualization is enabled.
+        /// Indicate whether visualization is enabled.
+        /// @see `enabled()`
         operator bool() const;
 
-
-        // OPERATORS
-
         /// Conversion operator to DebugDrawerInterfacePrx.
         operator DebugDrawerInterfacePrx& ();
         operator const DebugDrawerInterfacePrx& () const;
@@ -711,6 +804,9 @@ namespace armarx
         /// The debug drawer topic.
         DebugDrawerInterfacePrx topic = nullptr;
 
+        /// Whether drawing is enabled. (In comination with `topic`.
+        bool _enabled = true;
+
         /// The default layer (used if none is passed to the method).
         std::string _layer = DEFAULT_LAYER;