Choreonoid  1.1
EigenUtil.h
[詳解]
1 
5 #ifndef CNOID_UTIL_EIGEN_UTIL_H_INCLUDED
6 #define CNOID_UTIL_EIGEN_UTIL_H_INCLUDED
7 
8 #include "EigenTypes.h"
9 #include "EigenYaml.h"
10 #include "exportdecl.h"
11 
12 namespace cnoid {
13 
14  const double PI = 3.14159265358979323846;
15  const double PI_2 = 1.57079632679489661923;
16 
17  inline double degree(double rad) { return (180.0 * rad / PI); }
18  inline double radian(double deg) { return (PI * deg / 180.0); }
19 
20  template<typename Derived>
21  inline Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1>
22  rpyFromRot(const Eigen::MatrixBase<Derived>& R) {
23  Vector3 ea = R.eulerAngles(2, 1, 0);
24  return Vector3(ea[2], ea[1], ea[0]);
25  }
26 
27  CNOID_EXPORT Matrix3 rotFromRpy(double r, double p, double y);
28 
29  inline Matrix3 rotFromRpy(const Vector3& rpy) {
30  return rotFromRpy(rpy[0], rpy[1], rpy[2]);
31  }
32 
34 
35  inline Matrix3 hat(const Vector3& x) {
36  Matrix3 M;
37  M << 0.0, -x(2), x(1),
38  x(2), 0.0, -x(0),
39  -x(1), x(0), 0.0;
40  return M;
41  }
42 }
43 
44 #endif
const double PI
Definition: EigenUtil.h:14
double radian(double deg)
Definition: EigenUtil.h:18
Vector3 omegaFromRot(const Matrix3 &R)
Definition: EigenUtil.cpp:24
double degree(double rad)
Definition: EigenUtil.h:17
Matrix3 hat(const Vector3 &x)
Definition: EigenUtil.h:35
Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 > rpyFromRot(const Eigen::MatrixBase< Derived > &R)
Definition: EigenUtil.h:22
Definition: EasyScanner.h:16
Matrix3 rotFromRpy(double r, double p, double y)
Definition: EigenUtil.cpp:6
const double PI_2
Definition: EigenUtil.h:15
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25