diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 3243b686d7e5ca71a3e992150693ebc241321b8d..5fa5360258b34c23ddb0b79948fd8aa93dc77e3e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -54,23 +54,24 @@ namespace armarx
 
     void NJointHolonomicPlatformRelativePositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
     {
-        currentPose << sv->relativePositionX,
-                    sv->relativePositionY;
+        currentPosition << sv->relativePositionX, sv->relativePositionY;
+        currentOrientation = sv->relativePositionRotation;
         if (rtGetControlStruct().newTargetSet)
         {
-            startPose = currentPose;
-            orientationOffset = sv->relativePositionRotation;
+            startPosition = currentPosition;
+            startOrientation = currentOrientation;
             pid.reset();
             *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
         }
 
         //position pid
-        Eigen::Vector2f relativeCurrentPose = currentPose - startPose;
-        pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPose, rtGetControlStruct().target);
+        Eigen::Vector2f relativeCurrentPosition = currentPosition - startPosition;
+        pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPosition, rtGetControlStruct().target);
 
+        float relativeOrientation = currentOrientation - startOrientation;
         //rotation pid
         // Revert the rotation by rotating by the negative angle
-        Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-sv->relativePositionRotation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+        Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-currentOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
         //ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
 
         target->velocityX = localTargetVelocity[0];
@@ -78,7 +79,7 @@ namespace armarx
         //        target->velocityRotation = pid.getControlValue()[2] / rad2MMFactor;
         oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
         oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
-        oriCtrl.currentPosition = sv->relativePositionRotation - orientationOffset;
+        oriCtrl.currentPosition = relativeOrientation;
         oriCtrl.targetPosition = rtGetControlStruct().targetOrientation;
         oriCtrl.currentV = sv->velocityRotation;
         target->velocityRotation = oriCtrl.run();
@@ -101,9 +102,9 @@ namespace armarx
 
     void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
     {
-        startPose[0] = sv->relativePositionX;
-        startPose[1] = sv->relativePositionY;
-        orientationOffset = sv->relativePositionRotation;
+        startPosition[0] = sv->relativePositionX;
+        startPosition[1] = sv->relativePositionY;
+        startOrientation = sv->relativePositionRotation;
     }
 
     void NJointHolonomicPlatformRelativePositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index cf4e73188f070f801567d175b514703f728c9328..abbd25d999ae85f7ba500e49f13479d0a4688700 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -101,8 +101,9 @@ namespace armarx
         MultiDimPIDController pid;
         PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
         ControlTargetHolonomicPlatformVelocity* target;
-        Eigen::Vector2f startPose, currentPose;
-        float orientationOffset;
+        Eigen::Vector2f startPosition, currentPosition;
+        float startOrientation;
+        float currentOrientation;
         //        float rad2MMFactor;
     };
 } // namespace armarx