diff --git a/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp
index 9bd7c2422c8266721924c8048f6c970f354e8279..38b7040eba4d38c69a5d7b201fc7ff3405998617 100644
--- a/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp
@@ -3,46 +3,95 @@
 #include <VirtualRobot/math/Helpers.h>
 
 
+#include "Pose.h"
+
+
 namespace armarx
 {
     DebugDrawerTopic::VisuID::VisuID() = default;
-    
+
     DebugDrawerTopic::VisuID::VisuID(const std::string& name) :
         name(name)
     {}
-    
+
     DebugDrawerTopic::VisuID::VisuID(const std::string& layer, const std::string& name) :
         layer(layer), name(name)
     {}
 
-    
+
     const std::string DebugDrawerTopic::TOPIC_NAME;
 
     DebugDrawerTopic::DebugDrawerTopic()
     {}
-    
+
     DebugDrawerTopic::DebugDrawerTopic(const DebugDrawerInterfacePrx& topic) : topic(topic)
     {}
-    
+
     void DebugDrawerTopic::offeringTopic(ManagedIceObject& component) const
     {
         component.offeringTopic(TOPIC_NAME);
     }
-    
+
     void DebugDrawerTopic::getTopic(ManagedIceObject& component)
     {
         setTopic(component.getTopic<DebugDrawerInterfacePrx>(TOPIC_NAME));
     }
-    
+
+    void DebugDrawerTopic::setPoseScale(float scale)
+    {
+        this->poseScale = scale;
+    }
+
     void DebugDrawerTopic::setTopic(const DebugDrawerInterfacePrx& topic)
     {
         this->topic = topic;
     }
-    
+
     DebugDrawerInterfacePrx DebugDrawerTopic::getTopic() const
     {
         return topic;
     }
-    
+
+
+    void DebugDrawerTopic::drawPose(const Eigen::Matrix4f& pose, const DebugDrawerTopic::VisuID& id)
+    {
+        if (!topic)
+        {
+            return;
+        }
+        if (poseScale >= 1 && poseScale <= 1)  // squelch compiler warning that == is unsafe
+        {
+            topic->setPoseVisu(id.layer, id.name, new Pose(pose));
+        }
+        else
+        {
+            topic->setScaledPoseVisu(id.layer, id.name, new Pose(pose), poseScale);
+        }
+    }
+
+    void DebugDrawerTopic::drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const DebugDrawerTopic::VisuID& id)
+    {
+        drawPose(math::Helpers::Pose(pos, ori), id);
+    }
+
+    void DebugDrawerTopic::drawArrow(const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
+                                     float width, const DrawColor& color, const DebugDrawerTopic::VisuID& id)
+    {
+        if (!topic)
+        {
+            return;
+        }
+        topic->setArrowVisu(id.layer, id.name, new armarx::Vector3(position), new armarx::Vector3(direction),
+                            color, length, width);
+    }
+
+    void DebugDrawerTopic::drawArrowFromTo(
+        const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+        float width, const DrawColor& color, const DebugDrawerTopic::VisuID& id)
+    {
+        Eigen::Vector3f dir = (to - from);
+        float length = dir.norm();
+        drawArrow(from, dir / length, length, width, color, id);
+    }
 
 }
diff --git a/source/RobotAPI/libraries/core/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/DebugDrawerTopic.h
index 6faafb45ffe3ede1fba9d49929b441c6f65bffd5..151ea784d0f60add1f3eb51c3543a943430ab85b 100644
--- a/source/RobotAPI/libraries/core/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/DebugDrawerTopic.h
@@ -45,15 +45,32 @@ namespace armarx
         void offeringTopic(ManagedIceObject& component) const;
         /// Get the topic by calling getTopic([topicName]) on the given component.
         void getTopic(ManagedIceObject& component);
+
+
+        /// Set the scale for pose visualization.
+        /// This value will be used for all successive calls to drawPose().
+        void setPoseScale(float scale);
+        
+        
+        void drawPose(const Eigen::Matrix4f& pose, const VisuID& id);
+        void drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const VisuID& id);
         
+        void drawArrow(const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
+                       float width, const DrawColor& color, const VisuID& id);
+        void drawArrowFromTo(const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+                             float width, const DrawColor& color, const VisuID& id);
         
 
+
     private:
 
         static const std::string TOPIC_NAME;
         
         DebugDrawerInterfacePrx topic = nullptr;
+        
+        float poseScale = 1;
 
+        
     };
 
 }