bugfix: platform position controller is turning into wrong direction

Tested: from orientation -1.3 to 1.5

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

@@ -22,6 +22,8 @@
  *             GNU General Public License
  */
 #include "NJointHolonomicPlatformGlobalPositionController.h"
+#include <cmath>
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
 
 namespace armarx
 {
@@ -34,7 +36,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 +80,28 @@ 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;
+        const Eigen::Vector2f measuredPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
+        const float measuredOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
 
-        float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
-        relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
+        const float targetOrientation = rtGetControlStruct().targetOrientation;
 
-        float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
-        relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
+        // measured and target orientation are now within bounds [-pi, pi]
 
+        // in which orientation to rotate? => angleBetween is not reliable yet!
+        // float angleBetween = targetOrientation - measuredOrientation;
 
-        pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+        // 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>(relativeGlobalOrientation), relativeTargetOrientation);
+        opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), targetOrientation);
+
+        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 * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
         target->velocityX = velocities.x();
         target->velocityY = velocities.y();
         target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -112,7 +118,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 +131,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;