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

@@ -23,6 +23,10 @@
  */
 #include "NJointHolonomicPlatformGlobalPositionController.h"
 
+#include <cmath>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
@@ -34,7 +38,7 @@ namespace armarx
         const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
         const VirtualRobot::RobotPtr&) :
         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)
+        opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
 
     {
         const SensorValueBase* sv = useSensorValue(cfg->platformName);
@@ -78,24 +82,14 @@ namespace armarx
             return;
         }
 
-        float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
-        Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
-
-        Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
-        float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
-
-        float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
-        relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
-
-        float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
-        relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
+        const float measuredOrientation = rtGetControlStruct().globalOrientation;
 
+        pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+        opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
 
-        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);
+        const Eigen::Rotation2Df global_R_local(-measuredOrientation);
 
-        Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * 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());
@@ -112,7 +106,7 @@ namespace armarx
         std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
 
         getWriterControlStruct().target << x, y;
-        getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+        getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
         getWriterControlStruct().translationAccuracy = translationAccuracy;
         getWriterControlStruct().rotationAccuracy = rotationAccuracy;
         getWriterControlStruct().newTargetSet = true;
@@ -125,7 +119,7 @@ namespace armarx
         // ..todo: check if norm is too large
 
         getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
-        getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
+        getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
 
         getWriterControlStruct().startPosition = currentPosition;
         getWriterControlStruct().startOrientation = currentOrientation;