5 #ifndef __pinocchio_math_rpy_hpp__ 6 #define __pinocchio_math_rpy_hpp__ 8 #include "pinocchio/math/fwd.hpp" 9 #include "pinocchio/math/comparison-operators.hpp" 10 #include "pinocchio/math/sincos.hpp" 11 #include <boost/type_traits.hpp> 13 #include <Eigen/Geometry> 26 template<
typename Scalar>
27 Eigen::Matrix<Scalar,3,3>
rpyToMatrix(
const Scalar r,
const Scalar p,
const Scalar y)
29 typedef Eigen::AngleAxis<Scalar> AngleAxis;
30 typedef Eigen::Matrix<Scalar,3,1> Vector3s;
31 return (AngleAxis(y, Vector3s::UnitZ())
32 * AngleAxis(p, Vector3s::UnitY())
33 * AngleAxis(r, Vector3s::UnitX())
44 template<
typename Vector3Like>
45 Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
48 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, rpy, 3, 1);
65 template<
typename Matrix3Like>
66 Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
69 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);
70 typedef typename Matrix3Like::Scalar Scalar;
71 typedef Eigen::Matrix<Scalar,3,1> ResultType;
74 Scalar m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
75 Scalar p = atan2(-R(2, 0), m);
77 if (fabs(fabs(p) - M_PI / 2.) < 0.001) {
79 y = -atan2(R(0, 1), R(1, 1));
81 y = atan2(R(1, 0), R(0, 0));
82 r = atan2(R(2, 1), R(2, 2));
89 #endif //#ifndef __pinocchio_math_rpy_hpp__ Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, Matrix3Like::Options > matrixToRpy(const Eigen::MatrixBase< Matrix3Like > &R)
Convert from Transformation Matrix to Roll, Pitch, Yaw.
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar r, const Scalar p, const Scalar y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
Main pinocchio namespace.