diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 4a67d95babb7cefba0d5cd201c45eb853c2ff230..30989790f5d1942afa0a452efc95bd60330c655a 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -295,6 +295,11 @@ namespace VirtualRobot
         return m;
     }
 
+    Eigen::Matrix4f MathTools::rpy2eigen4f(const Eigen::Vector3f& rpy)
+    {
+        return rpy2eigen4f(rpy(0), rpy(1), rpy(2));
+    }
+
     Eigen::Matrix3f MathTools::rpy2eigen3f(float r, float p, float y)
     {
         Eigen::Matrix3f m;
@@ -322,6 +327,11 @@ namespace VirtualRobot
         return m;
     }
 
+    Eigen::Matrix3f MathTools::rpy2eigen3f(const Eigen::Vector3f& rpy)
+    {
+        return rpy2eigen3f(rpy(0), rpy(1), rpy(2));
+    }
+
     void MathTools::posrpy2eigen4f(const float x[6], Eigen::Matrix4f& m)
     {
         rpy2eigen4f(x[3], x[4], x[5], m);
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index f4359124d3dd0b02acb33386a69be042ddce9ba1..91576c632f64dd130f8f5e3cb876a3ed7f4355e4 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -127,7 +127,10 @@ namespace VirtualRobot
         */
         void VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen4f(float r, float p, float y, Eigen::Matrix4f& m);
         Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen4f(float r, float p, float y);
+        Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen4f(const Eigen::Vector3f& rpy);
+
         Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen3f(float r, float p, float y);
+        Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen3f(const Eigen::Vector3f& rpy);
 
         void VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(const float x[6], Eigen::Matrix4f& m);
         void VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(const Eigen::Vector3f& pos, const Eigen::Vector3f& rpy, Eigen::Matrix4f& m);