diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 72cf1d622c09e31294a57bc44ce9c6c77b00cd56..bd2444c23ebaf8825edc8a3c02d2625691680f83 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -300,6 +300,10 @@ namespace VirtualRobot
 
         posrpy2eigen4f(x, m);
     }
+    void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::posrpy2eigen4f(float x, float y, float z,float roll, float pitch, float yaw, Eigen::Matrix4f& m)
+    {
+        posrpy2eigen4f(Eigen::Vector3f{x, y, z}, Eigen::Vector3f{roll, pitch, yaw}, m);
+    }
 
     Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::posrpy2eigen4f(const Eigen::Vector3f& pos, const Eigen::Vector3f& rpy)
     {
@@ -307,6 +311,10 @@ namespace VirtualRobot
         posrpy2eigen4f(pos, rpy,m );
         return m;
     }
+    Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::posrpy2eigen4f(float x, float y, float z,float roll, float pitch, float yaw)
+    {
+        return posrpy2eigen4f(Eigen::Vector3f{x, y, z}, Eigen::Vector3f{roll, pitch, yaw});
+    }
 
     Eigen::Vector3f MathTools::getTranslation(const Eigen::Matrix4f& m)
     {
@@ -2003,6 +2011,15 @@ namespace VirtualRobot
         return q;*/
     }
 
+    Eigen::Matrix4f VirtualRobot::MathTools::posquat2eigen4f(float x, float y, float z, float qx, float qy, float qz, float qw)
+    {
+        Eigen::Matrix4f r = quat2eigen4f(qx,qy,qz,qw);
+        r(0,3)=x;
+        r(1,3)=y;
+        r(2,3)=z;
+        return r;
+    }
+
 
 } // namespace
 
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index 17b8a1d2747fd8df4053888732d4ed15f0204af9..b274d43027786fa6e75884036520bf279ef51712 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -115,7 +115,10 @@ namespace VirtualRobot
 
         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);
+        void VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(float x, float y, float z,float roll, float pitch, float yaw, Eigen::Matrix4f& m);
         Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(const Eigen::Vector3f& pos, const Eigen::Vector3f& rpy);
+        Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(float x, float y, float z,float roll, float pitch, float yaw);
+        Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT posquat2eigen4f(float x, float y, float z, float qx, float qy, float qz, float qw);
 
         /*!
             Convert homogeneous matrix to translation and rpy rotation.