diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index a4ee8ad724be379aee10d469fa72d8e8b3e5f180..c1bb7243955f6a7ef7559338c00e77809d1c2a98 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -75,6 +75,11 @@ void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f&
     debugDrawerPrx->setTextVisu(layerName, name, text, makeGlobal(p1), color, size);
 }
 
+void DebugDrawerHelper::drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width)
+{
+    debugDrawerPrx->setArrowVisu(layerName, name, makeGlobal(pos), makeGlobalDirection(direction), color, length, width);
+}
+
 PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose)
 {
     return new Pose(rn->getGlobalPose(pose));
@@ -84,3 +89,8 @@ Vector3Ptr DebugDrawerHelper::makeGlobal(const Eigen::Vector3f& position)
 {
     return new Vector3(rn->getGlobalPosition(position));
 }
+
+Vector3Ptr DebugDrawerHelper::makeGlobalDirection(const Eigen::Vector3f& direction)
+{
+    return new Vector3(math::Helpers::TransformDirection(rn->getGlobalPose(), direction));
+}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 3bdaa4933cfb47767bf7e64575a01c32507ef7d3..5d95dd9c8e604b830bb7be33abab9ba96ff9d910 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -59,8 +59,11 @@ namespace armarx
 
         void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
 
+        void drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width);
+
         PosePtr makeGlobal(const Eigen::Matrix4f& pose);
         Vector3Ptr makeGlobal(const Eigen::Vector3f& position);
+        Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction);
 
 
         Defaults defaults;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 629d427a66fb670940910bcd559459e39939bd72..a54cf2fe86393f94a679b1d09d45cb5a7d48f292 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -27,6 +27,8 @@
 
 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
+#include <VirtualRobot/math/Helpers.h>
+
 using namespace armarx;
 
 PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
@@ -57,7 +59,7 @@ void PositionControllerHelper::update()
     }
     Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
     velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
-    if(autoClearFeedForward)
+    if (autoClearFeedForward)
     {
         clearFeedForwardVelocity();
     }
@@ -147,6 +149,11 @@ const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
     return waypoints.at(currentWaypointIndex);
 }
 
+const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const
+{
+    return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
+}
+
 size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
 {
     float dist = FLT_MAX;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 92d20fa3133d90535234780b23c7a4c8899b1161..b90b2a615f0dafabae4a7a42fb4024a8f1e84d0d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -75,6 +75,7 @@ namespace armarx
         bool isLastWaypoint() const;
 
         const Eigen::Matrix4f& getCurrentTarget() const;
+        const Eigen::Vector3f getCurrentTargetPosition() const;
 
         size_t skipToClosestWaypoint(float rad2mmFactor);
 
@@ -84,7 +85,7 @@ namespace armarx
 
         struct NullspaceOptimizationArgs
         {
-            NullspaceOptimizationArgs(){}
+            NullspaceOptimizationArgs() {}
             int loops = 100;
             float stepLength = 0.05f;
             float eps = 0.001;