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

@@ -22,7 +22,9 @@
  *             GNU General Public License
  */
 #include "NJointHolonomicPlatformGlobalPositionController.h"
+
 #include <cmath>
+
 #include <SimoxUtility/math/periodic/periodic_clamp.h>
 
 namespace armarx
@@ -87,7 +89,7 @@ namespace armarx
 
         const Eigen::Rotation2Df global_R_local(-measuredOrientation);
 
-        Eigen::Vector2f velocities = global_R_local * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+        Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
         target->velocityX = velocities.x();
         target->velocityY = velocities.y();
         target->velocityRotation = static_cast<float>(opid.getControlValue());