Improve armar7 platform movement

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

@@ -92,10 +92,20 @@ namespace armarx
                               rtGetControlStruct().velocityY,
                               rtGetControlStruct().velocityRotation);
 
-            Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
-            target->velocityX = result(0);
-            target->velocityY = result(1);
-            target->velocityRotation = result(2);
+            if (x.norm() < 100)
+            {
+                target->velocityX = 0;
+                target->velocityY = 0;
+                target->velocityRotation = 0;
+            }
+            else
+            {
+
+                Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+                target->velocityX = result(0);
+                target->velocityY = result(1);
+                target->velocityRotation = result(2);
+            }
         }
     }
 
@@ -149,18 +159,20 @@ namespace armarx
         state += delta / factor;
 
 
-        if (target == Eigen::Vector3f(0, 0, 0))
+        if (target == Eigen::Vector3f::Zero() && state != Eigen::Vector3f::Zero())
         {
             //            if (state.norm() >= oldState.norm())
             //            {
-            std::stringstream ss;
-            ss << "====TargetZero:\nTarget:\n"
-               << target << "\nstate:\n"
-               << state << "\noldst:\n"
-               << oldState << "\ndelta:\n"
-               << delta << "\nfactor: " << factor << "\n====\n";
-            std::string s = ss.str();
-            ARMARX_RT_LOGF_IMPORTANT("%s", s.c_str());
+            ARMARX_RT_LOGF_IMPORTANT("Target %.04f %.04f %.04f", target(0), target(1), target(2));
+            ARMARX_RT_LOGF_IMPORTANT(
+                "OStte  %.04f %.04f %.04f", oldState(0), oldState(1), oldState(2));
+            ARMARX_RT_LOGF_IMPORTANT("State  %.04f %.04f %.04f", state(0), state(1), state(2));
+            ARMARX_RT_LOGF_IMPORTANT("Delta  %.04f %.04f %.04f", delta(0), delta(1), delta(2));
+            ARMARX_RT_LOGF_IMPORTANT("Factor %.04f", factor);
+            if (state.norm() > oldState.norm())
+            {
+                ARMARX_RT_LOGF_IMPORTANT("!!!!NORM INVALID!!!!");
+            }
 
             //                throw LocalException("haeee");
             //            }