Improve armar7 platform movement

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

@@ -61,10 +61,11 @@ namespace armarx
         state << velX, velY, 0, 0, 0, velRot;
         ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
 
-        getWriterControlStruct().velocityX = velX;
-        getWriterControlStruct().velocityY = velY;
-        getWriterControlStruct().velocityRotation = velRot;
-        writeControlStruct();
+        NJointHolonomicPlatformVelocityControllerControlData data;
+        data.velocityX = velX;
+        data.velocityY = velY;
+        data.velocityRotation = velRot;
+        reinitTripleBuffer(data);
     }
 
     void
@@ -72,29 +73,29 @@ namespace armarx
         const IceUtil::Time& sensorValuesTimestamp,
         const IceUtil::Time& timeSinceLastIteration)
     {
-        auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+        //        auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
 
-        if (commandAge > maxCommandDelay && // command must be recent
-            (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
-             rtGetControlStruct().velocityRotation !=
-                 0.0f)) // only throw error if any command is not zero
-        {
-            throw LocalException(
-                "Platform target velocity was not set for a too long time: delay: ")
-                << commandAge.toSecondsDouble()
-                << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
-        }
-        else
-        {
-            Eigen::VectorXf x(6);
-            x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
-                rtGetControlStruct().velocityRotation;
+        //        if (commandAge > maxCommandDelay && // command must be recent
+        //            (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
+        //             rtGetControlStruct().velocityRotation !=
+        //                 0.0f)) // only throw error if any command is not zero
+        //        {
+        //            throw LocalException(
+        //                "Platform target velocity was not set for a too long time: delay: ")
+        //                << commandAge.toSecondsDouble()
+        //                << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+        //        }
+        //        else
+        //        {
+        Eigen::VectorXf x(6);
+        x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
+            rtGetControlStruct().velocityRotation;
 
-            Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
-            target->velocityX = result(0);
-            target->velocityY = result(1);
-            target->velocityRotation = result(5);
-        }
+        Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+        target->velocityX = result(0);
+        target->velocityY = result(1);
+        target->velocityRotation = result(5);
+        //        }
     }
 
     void