pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
rpy.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS, INRIA
3 //
4 
5 #ifndef __pinocchio_math_rpy_hpp__
6 #define __pinocchio_math_rpy_hpp__
7 
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>
12 
13 #include <Eigen/Geometry>
14 
15 namespace pinocchio
16 {
17  namespace rpy
18  {
26  template<typename Scalar>
27  Eigen::Matrix<Scalar,3,3> rpyToMatrix(const Scalar r, const Scalar p, const Scalar y)
28  {
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())
34  ).toRotationMatrix();
35  }
36 
44  template<typename Vector3Like>
45  Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
46  rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
47  {
48  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, rpy, 3, 1);
49 
50  return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
51  }
52 
65  template<typename Matrix3Like>
66  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
67  matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
68  {
69  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);
70  typedef typename Matrix3Like::Scalar Scalar;
71  typedef Eigen::Matrix<Scalar,3,1> ResultType;
72  ResultType res;
73 
74  Scalar m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
75  Scalar p = atan2(-R(2, 0), m);
76  Scalar r, y;
77  if (fabs(fabs(p) - M_PI / 2.) < 0.001) {
78  r = 0;
79  y = -atan2(R(0, 1), R(1, 1));
80  } else {
81  y = atan2(R(1, 0), R(0, 0));
82  r = atan2(R(2, 1), R(2, 2));
83  }
84  res << r, p, y;
85  return res;
86  }
87  } // namespace rpy
88 }
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.
Definition: rpy.hpp:67
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar r, const Scalar p, const Scalar y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
Definition: rpy.hpp:27
Main pinocchio namespace.
Definition: treeview.dox:24