diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 3cc0e13c37dfaae118d6cb4eba12d3e85a82a7e8..bc246422b98fbd89feac994a2ace52645532e22a 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -33,25 +33,19 @@ namespace armarx
         const RobotUnitPtr&,
         const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
         const VirtualRobot::RobotPtr&) :
-        pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
+        pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
+        opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration)
 
     {
         const SensorValueBase* sv = useSensorValue(cfg->platformName);
         this->sv = sv->asA<SensorValueHolonomicPlatform>();
         target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
-        ARMARX_CHECK_EXPRESSION(target) << "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity;
 
-        pid.threadSafe = false;
 
-        oriCtrl.maxV = cfg->maxRotationVelocity;
-        oriCtrl.acceleration = cfg->maxRotationAcceleration;
-        oriCtrl.deceleration = cfg->maxRotationAcceleration;
-        oriCtrl.maxDt = 0.1;
-        oriCtrl.pid->Kp = cfg->p;
-        oriCtrl.positionPeriodLo = -M_PI;
-        oriCtrl.positionPeriodHi = M_PI;
+        pid.threadSafe = false;
         pid.preallocate(2);
 
+        opid.threadSafe = false;
     }
 
     void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
@@ -63,18 +57,23 @@ namespace armarx
         if (rtGetControlStruct().newTargetSet)
         {
             pid.reset();
+            opid.reset();
             *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+            isAborted = false;
         }
-
-        if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
+        if (isAborted)
+        {
+            return;
+        }
+        else if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
         {
-
             // ARMARX_RT_LOGF_WARNING  << deactivateSpam(0.5) << "Waiting for global pos";
 
             target->velocityX = 0;
             target->velocityY = 0;
 
             target->velocityRotation = 0;
+            isAborted = true;
 
             return;
         }
@@ -82,36 +81,24 @@ namespace armarx
         float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
         Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
 
-        Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition + relativeCurrentPosition;
-        float updatedOrientation = rtGetControlStruct().globalOrientation + relativeOrientation;
-        updatedOrientation = std::atan2(std::sin(updatedOrientation), std::cos(updatedOrientation));
+        Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
+        float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
 
-        pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+        float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
+        relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
 
-        oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
-        oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
-        oriCtrl.currentPosition = updatedOrientation;
-        oriCtrl.targetPosition =  getWriterControlStruct().targetOrientation;
-        oriCtrl.currentV = sv->velocityRotation;
+        float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
+        relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
 
-        Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
-        target->velocityX = localTargetVelocity(0);
-        target->velocityY = localTargetVelocity(1);
-        target->velocityRotation = oriCtrl.run();
 
-        Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
-        if (posError.norm() < rtGetControlStruct().translationAccuracy)
-        {
-            // target->velocityX = 0;
-            // target->velocityY = 0;
-        }
+        pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+        //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
+        opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(relativeGlobalOrientation), relativeTargetOrientation);
 
-        float orientationError = oriCtrl.currentPosition - oriCtrl.targetPosition;
-        orientationError = std::atan2(std::sin(orientationError), std::cos(orientationError));
-        if (std::fabs(orientationError) < rtGetControlStruct().rotationAccuracy)
-        {
-            //target->velocityRotation = 0;
-        }
+        Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+        target->velocityX = velocities.x();
+        target->velocityY = velocities.y();
+        target->velocityRotation = static_cast<float>(opid.getControlValue());
     }
 
     void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
@@ -135,7 +122,7 @@ namespace armarx
 
     void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
     {
-        // ..todo:: lock :)
+        // ..todo: check if norm is too large
 
         getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
         getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
@@ -145,6 +132,8 @@ namespace armarx
 
         getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
         writeControlStruct();
+
+
     }
 
 } // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index c6c7e6cd9f044c814fae30bf216c005f674963ad..0f5943ee59f0574a16e3cc1fbab0cecbbeb11adb 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -51,10 +51,16 @@ namespace armarx
         float p = 1.0f;
         float i = 0.0f;
         float d = 0.0f;
-        float maxVelocity = 300;
-        float maxAcceleration = 500;
-        float maxRotationVelocity = 0.5;
-        float maxRotationAcceleration = 0.5;
+        float maxVelocity = 250;
+        float maxAcceleration = 250;
+
+        float p_rot = 0.7f;
+        float i_rot = 0.0f;
+        float d_rot = 0.0f;
+
+        float maxRotationVelocity = 0.7;
+        float maxRotationAcceleration = 0.7;
+
     };
 
     struct NJointHolonomicPlatformGlobalPositionControllerTarget
@@ -65,12 +71,12 @@ namespace armarx
         float rotationAccuracy = 0.0f;
         bool newTargetSet = false;
 
-
         Eigen::Vector2f startPosition;
         float startOrientation;
 
         Eigen::Vector2f globalPosition;
         float globalOrientation;
+
         IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
     };
 
@@ -108,13 +114,13 @@ namespace armarx
     protected:
         const SensorValueHolonomicPlatform* sv;
         MultiDimPIDController pid;
+        PIDController  opid;
 
-        PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
         ControlTargetHolonomicPlatformVelocity* target;
 
-
         Eigen::Vector2f currentPosition;
         float currentOrientation;
+        bool isAborted = false;
 
     };
 } // namespace armarx