Choreonoid  1.5
EigenUtil.h
Go to the documentation of this file.
1 
5 #ifndef CNOID_UTIL_EIGEN_UTIL_H
6 #define CNOID_UTIL_EIGEN_UTIL_H
7 
8 #include "EigenTypes.h"
9 
10 #if defined(WIN32) && !defined(BOOST_NO_CXX11_ALLOCATOR)
11 #include <boost/version.hpp>
12 #if (BOOST_VERSION >= 105900)
13 #define BOOST_NO_CXX11_ALLOCATOR
14 #endif
15 #endif
16 
17 #include <boost/make_shared.hpp>
18 #include "exportdecl.h"
19 
20 namespace cnoid {
21 
22 const double PI = 3.14159265358979323846;
23 const double PI_2 = 1.57079632679489661923;
24 
25 const double TO_DEGREE = 180.0 / PI;
26 const double TO_RADIAN = PI / 180.0;
27 
28 inline double degree(double rad) { return TO_DEGREE * rad; }
29 inline double radian(double deg) { return TO_RADIAN * deg; }
30 inline float degree(float rad) { return (float)TO_DEGREE * rad; }
31 inline float radian(float deg) { return (float)TO_RADIAN * deg; }
32 inline double radian(int deg) { return TO_RADIAN * deg; }
33 
34 /*
35  Since version 3.2, the behavior of Eigen's eulerAngles function was slightly modified;
36  The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi].
37  This is not good for using the returned angles to interpolate attitdues.
38  Now our own implementation is used for getting R-P-Y angles.
39 */
40 /*
41  template<typename Derived>
42  inline Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1>
43  rpyFromRot(const Eigen::MatrixBase<Derived>& R) {
44  Vector3 ea = R.eulerAngles(2, 1, 0);
45  return Vector3(ea[2], ea[1], ea[0]); // exchange element order to be our conventional one !
46  }
47 */
48 
50 
51 CNOID_EXPORT Matrix3 rotFromRpy(double r, double p, double y);
52 
53 inline Matrix3 rotFromRpy(const Vector3& rpy) {
54  return rotFromRpy(rpy[0], rpy[1], rpy[2]);
55 }
56 
58 
59 inline Matrix3 hat(const Vector3& x) {
60  Matrix3 M;
61  M << 0.0, -x(2), x(1),
62  x(2), 0.0, -x(0),
63  -x(1), x(0), 0.0;
64  return M;
65 }
66 
67 CNOID_EXPORT std::string str(const Vector3& v);
68 CNOID_EXPORT bool toVector3(const std::string& s, Vector3& out_v);
69 
70 
74 
75 template<class T>
76 boost::shared_ptr<T> make_shared_aligned() {
77  return boost::allocate_shared<T>(Eigen::aligned_allocator<T>());
78 }
79 template<class T, class P1>
80 boost::shared_ptr<T> make_shared_aligned(const P1& p1) {
81  return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1);
82 }
83 template<class T, class P1, class P2>
84 boost::shared_ptr<T> make_shared_aligned(const P1& p1, const P2& p2) {
85  return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1, p2);
86 }
87 template<class T, class P1, class P2, class P3>
88 boost::shared_ptr<T> make_shared_aligned(const P1& p1, const P2& p2, const P3& p3) {
89  return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1, p2, p3);
90 }
91 template<class T, class P1, class P2, class P3, class P4>
92 boost::shared_ptr<T> make_shared_aligned(const P1& p1, const P2& p2, const P3& p3, const P4& p4) {
93  return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), p1, p2, p3, p4);
94 }
95 
96 }
97 
98 #endif
const double PI
Definition: EigenUtil.h:22
double radian(double deg)
Definition: EigenUtil.h:29
void normalizeRotation(Matrix3 &R)
Definition: EigenUtil.cpp:116
Vector3 omegaFromRot(const Matrix3 &R)
Definition: EigenUtil.cpp:67
double degree(double rad)
Definition: EigenUtil.h:28
const double TO_RADIAN
Definition: EigenUtil.h:26
boost::shared_ptr< T > make_shared_aligned()
Definition: EigenUtil.h:76
Eigen::Affine3d Affine3
Definition: EigenTypes.h:64
bool toVector3(const std::string &s, Vector3 &out_v)
Definition: EigenUtil.cpp:95
Matrix3 hat(const Vector3 &x)
Definition: EigenUtil.h:59
const double TO_DEGREE
Definition: EigenUtil.h:25
std::string str(const Vector3 &v)
Definition: EigenUtil.cpp:90
Eigen::Transform< double, 3, Eigen::AffineCompact > Position
Definition: EigenTypes.h:73
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Vector3 rpyFromRot(const Matrix3 &R)
Definition: EigenUtil.cpp:27
Matrix3 rotFromRpy(double r, double p, double y)
Definition: EigenUtil.cpp:9
const double PI_2
Definition: EigenUtil.h:23
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57