diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 46a12f1cab61355afeb751642be7970fca0f757c..5e9d4bfcefcbbbccaaa7738e8dc2af7c8a7fc0db 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -270,3 +270,21 @@ Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose)
     return pose.block<3, 3>(0, 0);
 }
 
+Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen)
+{
+    if(maxLen.rows() != 1 && maxLen.rows() != vec.rows())
+    {
+        throw std::invalid_argument("maxLen.rows != 1 and != maxLen.rows");
+    }
+    float scale = 1;
+    for(int i = 0; i < vec.rows(); i++)
+    {
+        int j = maxLen.rows() == 1 ? 0 : i;
+        if(std::abs(vec(i)) > maxLen(j) && maxLen(j) >= 0)
+        {
+            scale = std::min(scale, maxLen(j) / std::abs(vec(i)));
+        }
+    }
+    return vec / scale;
+}
+
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index e0f3d45c57863dc3694e5e48c39e82b166597d3d..4e30135dd0f376820bdfb5136a0149384c196e6e 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -65,6 +65,7 @@ namespace math
         static float Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor);
         static Eigen::Vector3f GetPosition(const Eigen::Matrix4f& pose);
         static Eigen::Matrix3f GetOrientation(const Eigen::Matrix4f& pose);
+        static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen);
 
     private:
     };