diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 6ad411f67b743fecaf8ed8fa82a31f0d3e0d0b8c..4658f41d72431535920580dbb6bda4e0c70c0b6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -468,4 +468,9 @@ namespace armarx
             }
         }
     }
+
+    int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+    {
+        return _publishWpsCur;
+    }
 }
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 9ed0b775c33ca9bc3a32c239d6635fd9d021044e..4933edd9d22e5138a7ef06d09fa15aa4f908e339 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -51,6 +51,7 @@ namespace armarx
     public:
         bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
         bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
+        int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
 
         void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
         void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 712399c5f77ec1a7c116b3e77b0ad2e4df616a87..bf00909d6f5b95ee23fec2f982aae5d4d6ed6ec8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -51,6 +51,7 @@ module armarx
     {
         idempotent bool hasReachedTarget();
         idempotent bool hasReachedForceLimit();
+        idempotent int getCurrentWaypointIndex();
 
         void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
         void setWaypoints(Eigen::Matrix4fSeq wps);