Adjusted relative platform position controller

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

@@ -57,19 +57,20 @@ namespace armarx
                     sv->relativePositionY;
         if (rtGetControlStruct().newTargetSet)
         {
-            offset = currentPose;
+            startPose = currentPose;
             orientationOffset = sv->relativePositionRotation;
             pid.reset();
             *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
         }
 
-        Eigen::Vector3f relativeCurrentPose = currentPose - offset;
+        //position pid
+        Eigen::Vector2f relativeCurrentPose = currentPose - startPose;
         pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPose, rtGetControlStruct().target);
 
-
+        //rotation pid
         // Revert the rotation by rotating by the negative angle
         Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-sv->relativePositionRotation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
-        //        ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
+        //ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
 
         target->velocityX = localTargetVelocity[0];
         target->velocityY = localTargetVelocity[1];
@@ -99,8 +100,8 @@ namespace armarx
 
     void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
     {
-        offset[0] = sv->relativePositionX;
-        offset[1] = sv->relativePositionY;
+        startPose[0] = sv->relativePositionX;
+        startPose[1] = sv->relativePositionY;
         orientationOffset = sv->relativePositionRotation;
     }
 

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

@@ -48,8 +48,8 @@ namespace armarx
     {
     public:
         std::string platformName;
-        float p = 0.75f;
-        float i = 0.05f;
+        float p = 0.5f;
+        float i = 0.0f;
         float d = 0.0f;
         float maxVelocity = 300;
         float maxAcceleration = 100;
@@ -60,7 +60,7 @@ namespace armarx
 
     struct NJointHolonomicPlatformRelativePositionControllerTarget
     {
-        Eigen::Vector3f target; // x,y
+        Eigen::Vector2f target; // x,y
         float targetOrientation;
         float translationAccuracy = 0.0f;
         float rotationAccuracy = 0.0f;
@@ -96,7 +96,7 @@ namespace armarx
         MultiDimPIDController pid;
         PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
         ControlTargetHolonomicPlatformVelocity* target;
-        Eigen::Vector3f offset, currentPose;
+        Eigen::Vector2f startPose, currentPose;
         float orientationOffset;
         //        float rad2MMFactor;