Improve armar7 platform movement

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

@@ -64,6 +64,7 @@ namespace armarx
         data.velocityX = velX;
         data.velocityY = velY;
         data.velocityRotation = velRot;
+        data.commandTimestamp = IceUtil::Time::now();
         setControlStruct(data);
         //        reinitTripleBuffer(data);
     }
@@ -136,10 +137,36 @@ namespace armarx
         float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
         factor = std::max(factor, posFactor);
 
-        float oriFactor = std::abs(delta(3)) / maxOrientationAcceleration / dt;
+        float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
         factor = std::max(factor, oriFactor);
 
+        if (factor < 1)
+        {
+            throw LocalException("Error factor negative!");
+        }
+
+        Eigen::Vector3f oldState = state;
         state += delta / factor;
+
+
+        if (target == Eigen::Vector3f(0, 0, 0))
+        {
+            //            if (state.norm() >= oldState.norm())
+            //            {
+            ARMARX_RT_LOGF_IMPORTANT("Target %d %d %d", target(0), target(1), target(2));
+            ARMARX_RT_LOGF_IMPORTANT("OStte  %d %d %d", oldState(0), oldState(1), oldState(2));
+            ARMARX_RT_LOGF_IMPORTANT("State  %d %d %d", state(0), state(1), state(2));
+            ARMARX_RT_LOGF_IMPORTANT("Delta  %d %d %d", delta(0), delta(1), delta(2));
+            ARMARX_RT_LOGF_IMPORTANT("Factor %d", factor);
+            if (state.norm() > oldState.norm())
+            {
+                ARMARX_RT_LOGF_IMPORTANT("!!!!NORM INVALID!!!!");
+            }
+
+            //                throw LocalException("haeee");
+            //            }
+        }
+
         return state;
     }