Index: src/frames.hpp
===================================================================
--- src/frames.hpp	(revision 30002)
+++ src/frames.hpp	(working copy)
@@ -391,6 +391,10 @@
     //! the norm of (x,y,z,w) should be equal to 1
     static Rotation Quaternion(double x,double y,double z, double w);

+    //! Get the quaternion of this matrix
+    //! \post the norm of (x,y,z,w) is 1
+    void GetQuaternion(double& x,double& y,double& z, double& w);
+
     //! Sets the value of this object to a rotation specified with RPY convention:
     //! first rotate around X with roll, then around the
     //!               old Y with pitch, then around old Z with alfa
Index: src/frames.cpp
===================================================================
--- src/frames.cpp	(revision 30002)
+++ src/frames.cpp	(working copy)
@@ -187,6 +187,51 @@
		  2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2);
 }

+/* From the following sources:
+http://web.archive.org/web/20041029003853/http:/www.j3d.org/matrix_faq/matrfaq_latest.html
+http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
+RobOOP::quaternion.cpp
+ */
+void Rotation::GetQuaternion(double& x,double& y,double& z, double& w)
+{
+    double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2) + 1.0f;
+    if( trace > epsilon )
+	{
+        double s = 0.5f / sqrt(trace);
+        w = 0.25f / s;
+        x = ( (*this)(2,1) - (*this)(1,2) ) * s;
+        y = ( (*this)(0,2) - (*this)(2,0) ) * s;
+        z = ( (*this)(1,0) - (*this)(0,1) ) * s;
+    }
+	else
+	{
+        if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) )
+		{
+            float s = 2.0f * sqrtf( 1.0f + (*this)(0,0) - (*this)(1,1) - (*this)(2,2));
+            w = ((*this)(2,1) - (*this)(1,2) ) / s;
+            x = 0.25f * s;
+            y = ((*this)(0,1) + (*this)(1,0) ) / s;
+            z = ((*this)(0,2) + (*this)(2,0) ) / s;
+        }
+		else if ((*this)(1,1) > (*this)(2,2))
+		{
+            float s = 2.0f * sqrtf( 1.0f + (*this)(1,1) - (*this)(0,0) - (*this)(2,2));
+            w = ((*this)(0,2) - (*this)(2,0) ) / s;
+            x = ((*this)(0,1) + (*this)(1,0) ) / s;
+            y = 0.25f * s;
+            z = ((*this)(1,2) + (*this)(2,1) ) / s;
+        }
+		else
+		{
+            float s = 2.0f * sqrtf( 1.0f + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) );
+            w = ((*this)(1,0) - (*this)(0,1) ) / s;
+            x = ((*this)(0,2) + (*this)(2,0) ) / s;
+            y = ((*this)(1,2) + (*this)(2,1) ) / s;
+            z = 0.25f * s;
+        }
+    }
+}
+
 Rotation Rotation::RPY(double roll,double pitch,double yaw)
     {
         double ca1,cb1,cc1,sa1,sb1,sc1;
Index: tests/framestest.cpp
===================================================================
--- tests/framestest.cpp	(revision 30002)
+++ tests/framestest.cpp	(working copy)
@@ -151,7 +151,49 @@
	TestRotation2(Vector(3,4,5),10*deg2rad,20*deg2rad,30*deg2rad);
 }

+void FramesTest::TestQuaternion() {
+    Rotation    R;
+    Rotation    R2;
+    double      x,y,z,w;
+    double      x2,y2,z2,w2;

+    // identity R -> quat -> R2
+    R.GetQuaternion(x,y,z,w);
+    R2.Quaternion(x,y,z,w);
+	CPPUNIT_ASSERT_EQUAL(R,R2);
+
+    // 45 deg rotation in X
+    R = Rotation::EulerZYX(0,0,45*deg2rad);
+    R.GetQuaternion(x,y,z,w);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(x, sin((45*deg2rad)/2), epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(y, 0, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(z, 0, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(w, cos((45*deg2rad)/2), epsilon);
+    R2 = Rotation::Quaternion(x,y,z,w);
+	CPPUNIT_ASSERT_EQUAL(R,R2);
+
+    // direct 45 deg rotation in X
+    R2 = Rotation::Quaternion(sin((45*deg2rad)/2), 0, 0, cos((45*deg2rad)/2));
+	CPPUNIT_ASSERT_EQUAL(R,R2);
+    R2.GetQuaternion(x2,y2,z2,w2);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
+
+    // 45 deg rotation in X and in Z
+    R = Rotation::EulerZYX(45*deg2rad,0,45*deg2rad);
+    R.GetQuaternion(x,y,z,w);
+    R2 = Rotation::Quaternion(x,y,z,w);
+	CPPUNIT_ASSERT_EQUAL(R,R2);
+    R2.GetQuaternion(x2,y2,z2,w2);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2, epsilon);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
+}
+
+
 void FramesTest::TestFrame() {
	Vector   v(3,4,5);
	Wrench   w(Vector(7,-1,3),Vector(2,-3,3)) ;
Index: tests/framestest.hpp
===================================================================
--- tests/framestest.hpp	(revision 30002)
+++ tests/framestest.hpp	(working copy)
@@ -14,6 +14,7 @@
     CPPUNIT_TEST(TestTwist);
     CPPUNIT_TEST(TestWrench);
     CPPUNIT_TEST(TestRotation);
+    CPPUNIT_TEST(TestQuaternion);
     CPPUNIT_TEST(TestFrame);
     CPPUNIT_TEST(TestJntArray);

@@ -27,6 +28,7 @@
     void TestTwist();
     void TestWrench();
     void TestRotation();
+    void TestQuaternion();
     void TestFrame();
     void TestJntArray();
     void TestJntArrayWhenEmpty();