diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 5e9d4bfcefcbbbccaaa7738e8dc2af7c8a7fc0db..66c19effdc8f39b3e81815eff7f695b56b253a9e 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -288,3 +288,12 @@ Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eig
     return vec / scale;
 }
 
+float Helpers::rad2deg(float rad)
+{
+    return rad * (float)(180.0 / M_PI);
+}
+
+float Helpers::deg2rad(float deg)
+{
+    return deg * (float)(M_PI / 180.0);
+}
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index 4e30135dd0f376820bdfb5136a0149384c196e6e..10e6b9e62c31f3883f04d3d9f77d0f528f46a05e 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -67,6 +67,9 @@ namespace math
         static Eigen::Matrix3f GetOrientation(const Eigen::Matrix4f& pose);
         static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen);
 
+        static float rad2deg(float rad);
+        static float deg2rad(float deg);
+
     private:
     };
 }