TrajectoryFollowingController: floating point rounding fix

source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp

@@ -112,13 +112,15 @@ namespace armarx::navigation::traj_ctrl
         twist.linear /= scaleMax;
         twist.angular /= scaleMax;
 
+        constexpr float eps = 0.001;
+
         // pedantic checks
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular);
+        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
+        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
+        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
+        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
+        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
+        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
 
         return twist;
     }