sot-core  4.11.2
Hierarchical task solver plug-in for dynamic-graph.
matrix-geometry.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,2019
3  * CNRS/AIST
4  * François Bleibel, Olivier Stasse, François Bailly
5  *
6  */
7 
8 #ifndef __SOT_MATRIX_GEOMETRY_H__
9 #define __SOT_MATRIX_GEOMETRY_H__
10 
11 /* --- Matrix --- */
12 #include <Eigen/Core>
13 #include <Eigen/Geometry>
14 #include <dynamic-graph/eigen-io.h>
15 #include <dynamic-graph/linear-algebra.h>
16 #include <sot/core/api.hh>
17 
18 #define MRAWDATA(x) x.data()
19 
20 /* --------------------------------------------------------------------- */
21 /* --------------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 
24 namespace dynamicgraph {
25 namespace sot {
26 
27 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
28  \
29  typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
30  \
31  typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
32  \
33  typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
34 
35 #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
36  \
37  typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \
38  Matrix##Size##X##TypeSuffix; \
39  \
40  typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
41 
42 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
43  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
44  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
45  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
46  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
47  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
48  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
49  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
50  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
51 
55 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
56 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
57 
58 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
59 #undef EIGEN_MAKE_TYPEDEFS
60 
61 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
63 typedef Eigen::Map<MatrixRXd> SigMatrixXd;
64 typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
65 typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
66 typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
67 
68 typedef Eigen::Ref<Eigen::VectorXd> RefVector;
69 typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
70 typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
71 typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
72 
73 typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
75 typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
76 typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
77 typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
78 typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
79 typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
80 typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
81 typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;
82 
83 typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
84 typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
85 typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
86 
87 inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
88 
89  Eigen::Vector3d _t = MH.translation();
90  MatrixRotation R(MH.linear());
91  Eigen::Matrix3d Tx;
92  Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
93  Eigen::Matrix3d sk;
94  sk = Tx * R;
95 
96  MT.block<3, 3>(0, 0) = R;
97  MT.block<3, 3>(0, 3) = sk;
98  MT.block<3, 3>(3, 0).setZero();
99  MT.block<3, 3>(3, 3) = R;
100 }
101 
102 } // namespace sot
103 } // namespace dynamicgraph
104 
105 #endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
const Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixRXd
Definition: matrix-geometry.hh:62
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Definition: matrix-geometry.hh:77
const Eigen::Map< const Eigen::VectorXd > const_SigVectorXd
Definition: matrix-geometry.hh:66
Eigen::Map< Eigen::VectorXd > SigVectorXd
Definition: matrix-geometry.hh:64
#define SOT_CORE_EXPORT
Definition: api.hh:20
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:87
const Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:71
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
Definition: matrix-geometry.hh:79
Eigen::Map< MatrixRXd > SigMatrixXd
Definition: matrix-geometry.hh:63
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:76
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:81
Eigen::Vector3d SOT_CORE_EXPORT VectorRotation
Definition: matrix-geometry.hh:78
Eigen::Ref< Eigen::MatrixXd > RefMatrix
Definition: matrix-geometry.hh:70
Eigen::Quaternion< double > SOT_CORE_EXPORT Quaternion
Definition: matrix-geometry.hh:84
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:83
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
const Eigen::Map< const MatrixRXd > const_SigMatrixXd
Definition: matrix-geometry.hh:65
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:85
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixForce
Definition: matrix-geometry.hh:80
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix)
Definition: matrix-geometry.hh:42
Definition: abstract-sot-external-interface.hh:17