Improve armar7 platform movement

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

@@ -140,12 +140,32 @@ namespace armarx
         float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
         factor = std::max(factor, oriFactor);
 
-        if (factor < 0)
+        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())
+            //            {
+            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);
+
+            //                throw LocalException("haeee");
+            //            }
+        }
+
         return state;
     }