NJointHolonomicPlatformGlobalPositionController: using new sensor device

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

@@ -59,45 +59,64 @@ namespace armarx
 
     void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
     {
-        currentPosition << sv->relativePositionX, sv->relativePositionY;
-        currentOrientation = sv->relativePositionRotation;
+        const auto global_T_robot = sv->global_T_root;
 
+        const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(global_T_robot);
+        const float global_orientation = rpy.z();
+        const Eigen::Vector2f global_P_robot = global_T_robot.block<2,1>(0,3);
 
         if (rtGetControlStruct().newTargetSet)
         {
             pid.reset();
             opid.reset();
 
-        getWriterControlStruct().newTargetSet = false;
-        writeControlStruct();
+            getWriterControlStruct().newTargetSet = false;
+            writeControlStruct();
 	
-            isAborted = false;
+            isTargetSet = true;
         }
-        if (isAborted)
+
+
+        // if ((sv->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;
+        // }
+
+        if (not isTargetSet)
         {
+            target->velocityX = 0;
+            target->velocityY = 0;
+            target->velocityRotation = 0;
+
             return;
         }
-        else if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
-        {
-            // ARMARX_RT_LOGF_WARNING  << deactivateSpam(0.5) << "Waiting for global pos";
 
+        if(not sv->isAvailable())
+        {
+            // ARMARX_RT_LOGF_INFO << deactivateSpam(1) << "global pose not available";
             target->velocityX = 0;
             target->velocityY = 0;
-
             target->velocityRotation = 0;
-            isAborted = true;
 
             return;
         }
 
-        const float measuredOrientation = rtGetControlStruct().globalOrientation;
+        const float measuredOrientation = global_orientation;
 
-        pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+        pid.update(timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
         opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
 
         const Eigen::Rotation2Df global_R_local(-measuredOrientation);
+        const Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
 
-        Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
         target->velocityX = velocities.x();
         target->velocityY = velocities.y();
         target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -105,6 +124,9 @@ namespace armarx
 
     void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
     {
+        target->velocityX = 0;
+        target->velocityY = 0;
+        target->velocityRotation = 0;
     }
 
     void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
@@ -115,28 +137,12 @@ namespace armarx
 
         getWriterControlStruct().target << x, y;
         getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
+
         getWriterControlStruct().translationAccuracy = translationAccuracy;
         getWriterControlStruct().rotationAccuracy = rotationAccuracy;
+        
         getWriterControlStruct().newTargetSet = true;
         writeControlStruct();
     }
 
-
-    void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
-    {
-        // ..todo: check if norm is too large
-
-        getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
-        getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
-
-        getWriterControlStruct().startPosition = currentPosition;
-        getWriterControlStruct().startOrientation = currentOrientation;
-
-        getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
-        writeControlStruct();
-
-
-    }
-
 } // namespace armarx
-

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

@@ -34,6 +34,7 @@
 #include "../SensorValues/SensorValueHolonomicPlatform.h"
 
 #include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
 #include <RobotAPI/libraries/core/PIDController.h>
 
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
@@ -71,11 +72,11 @@ namespace armarx
         float rotationAccuracy = 0.0f;
         bool newTargetSet = false;
 
-        Eigen::Vector2f startPosition;
-        float startOrientation;
+        // Eigen::Vector2f startPosition;
+        // float startOrientation;
 
-        Eigen::Vector2f globalPosition;
-        float globalOrientation;
+        // Eigen::Vector2f globalPosition;
+        // float globalOrientation;
 
         IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
     };
@@ -108,21 +109,14 @@ namespace armarx
 
         void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
 
-        void setGlobalPos(const PlatformPose& currentPose);
-
-
     protected:
-        const SensorValueHolonomicPlatform* sv;
+        const GlobalRobotLocalizationSensorDevice::SensorValueType* sv;
         MultiDimPIDController pid;
         PIDController  opid;
 
         ControlTargetHolonomicPlatformVelocity* target;
 
-        Eigen::Vector2f currentPosition;
-        float currentOrientation;
-        bool isAborted = false;
+        bool isTargetSet = false;
 
     };
 } // namespace armarx
-
-