[math] matrixToRpy

src/math/rpy.hpp

@@ -60,6 +60,42 @@ namespace pinocchio
 
       return rpyToMatrix(v[0],v[1],v[2]);
     }
+
+    ///
+    /// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw
+    ///
+    /// Given a rotation matrix \f$R\f$, the angles \f$r, p, y\f$ are given
+    /// so that \f$ R = R_z(y)R_y(p)R_x(r) \f$,
+    /// where \f$R_{\alpha}(\theta)\f$ denotes the rotation of \f$\theta\f$ degrees
+    /// around axis \f$\alpha\f$.
+    /// The angles are guaranteed to be in the ranges \f$r\in[-\pi,\pi]\f$
+    /// \f$p\in[-\frac{\pi}{2},\frac{\pi}{2}]\f$ \f$y\in[-\pi,\pi]\f$.
+    /// As this is a specialized implementation, it is expected to be very efficient.
+    ///
+    /// \warning the method assumes \f$R\f$ is a rotation matrix. If it is not, the result is undefined.
+    ///
+    template<typename Matrix3Like>
+    Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
+    matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
+    {
+      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);
+      typedef typename Matrix3Like::Scalar Scalar;
+      typedef Eigen::Matrix<Scalar,3,1> ResultType;
+      ResultType res;
+
+      Scalar m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
+      Scalar p = atan2(-R(2, 0), m);
+      Scalar r, y;
+      if (fabs(fabs(p) - M_PI / 2.) < 0.001) {
+        r = 0;
+        y = -atan2(R(0, 1), R(1, 1));
+      } else {
+        y = atan2(R(1, 0), R(0, 0));
+        r = atan2(R(2, 1), R(2, 2));
+      }
+      res << r, p, y;
+      return res;
+    }
   } // namespace rpy
 }
 #endif //#ifndef __pinocchio_math_rpy_hpp__

unittest/rpy.cpp

@@ -34,5 +34,20 @@ BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
   BOOST_CHECK(Rv.isApprox(R_Eig));
 }
 
+BOOST_AUTO_TEST_CASE(test_matrixToRpy)
+{
+  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
+  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
+  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
+
+  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
+
+  Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
+
+  BOOST_CHECK_CLOSE(r,v[0],1e-12);
+  BOOST_CHECK_CLOSE(p,v[1],1e-12);
+  BOOST_CHECK_CLOSE(y,v[2],1e-12);
+}
+
 BOOST_AUTO_TEST_SUITE_END()