bugfix: platform position controller is turning into wrong direction

Changes

• Orientation PID controller with 'limitless' flag to clamp error to [-pi, pi]
• Clamping incoming angles (target orientation and current orientation) to [-pi, pi]
• Simplified implementation (removed the buggy/incorrect code)

Tests

Tested in simulation: from orientation -1.3 to 1.5

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp

@@ -80,24 +80,10 @@ namespace armarx
             return;
         }
 
+        const float measuredOrientation = rtGetControlStruct().globalOrientation;
 
-        const Eigen::Vector2f measuredPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
-        const float measuredOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
-
-        const float targetOrientation = rtGetControlStruct().targetOrientation;
-
-        // measured and target orientation are now within bounds [-pi, pi]
-
-        // in which orientation to rotate? => angleBetween is not reliable yet!
-        // float angleBetween = targetOrientation - measuredOrientation;
-
-        // angleBetween = simox::math::periodic_clamp(angleBetween, -M_PIf32, M_PIf32);
-        // targetOrientation = measuredOrientation + angleBetween;
-        // -> now angleBetween is reliable
-
-        pid.update(timeSinceLastIteration.toSecondsDouble(), measuredPosition, rtGetControlStruct().target);
-        //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
-        opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), targetOrientation);
+        pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+        opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
 
         const Eigen::Rotation2Df global_R_local(-measuredOrientation);