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 <dynamic-graph/eigen-io.h>
14 
15 #include <Eigen/Core>
16 #include <Eigen/Geometry>
17 #include <sot/core/api.hh>
18 
19 #define MRAWDATA(x) x.data()
20 
21 /* --------------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 /* --------------------------------------------------------------------- */
24 
25 namespace dynamicgraph {
26 namespace sot {
27 
28 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
29  \
30  typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
31  \
32  typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
33  \
34  typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
35 
36 #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
37  \
38  typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \
39  Matrix##Size##X##TypeSuffix; \
40  \
41  typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
42 
43 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
44  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
45  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
46  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
47  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
48  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
49  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
50  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
51  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
52 
56 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
57 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
58 
59 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
60 #undef EIGEN_MAKE_TYPEDEFS
61 
62 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
64 typedef Eigen::Map<MatrixRXd> SigMatrixXd;
65 typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
66 typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
67 typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
68 
69 typedef Eigen::Ref<Eigen::VectorXd> RefVector;
70 typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
71 typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
72 typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
73 
74 typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
76 typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
77 typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
78 typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
79 typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
80 typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
81 typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
82 typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;
83 
84 typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
85 typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
86 typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
87 
88 inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
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__ */
dynamicgraph::sot::QuaternionMap
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:86
dynamicgraph::sot::VectorQuaternion
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Definition: matrix-geometry.hh:78
dynamicgraph::sot::MatrixHomogeneous
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:75
SOT_CORE_EXPORT
#define SOT_CORE_EXPORT
Definition: api.hh:20
dynamicgraph
dynamicgraph::sot::ConstRefMatrix
const typedef Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:72
dynamicgraph::sot::SigVectorXd
Eigen::Map< Eigen::VectorXd > SigVectorXd
Definition: matrix-geometry.hh:65
R
R
f
void f(void)
Definition: test_mailbox.cpp:33
dynamicgraph::sot::ConstRefVector
const typedef Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:70
dynamicgraph::sot::buildFrom
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:88
dynamicgraph::sot::VectorRollPitchYaw
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
Definition: matrix-geometry.hh:80
eigen-io.h
dynamicgraph::sot::VectorUTheta
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:77
api.hh
size_type
Matrix::Index size_type
dynamicgraph::sot::SigMatrixXd
Eigen::Map< MatrixRXd > SigMatrixXd
Definition: matrix-geometry.hh:64
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
dynamicgraph::sot::VectorRotation
Eigen::Vector3d SOT_CORE_EXPORT VectorRotation
Definition: matrix-geometry.hh:79
dynamicgraph::sot::MatrixTwist
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:82
linear-algebra.h
dynamicgraph::sot::RefMatrix
Eigen::Ref< Eigen::MatrixXd > RefMatrix
Definition: matrix-geometry.hh:71
dynamicgraph::sot::RefVector
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:69
dynamicgraph::sot::Quaternion
Eigen::Quaternion< double > SOT_CORE_EXPORT Quaternion
Definition: matrix-geometry.hh:85
dynamicgraph::sot::MatrixRotation
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:76
dynamicgraph::sot::Vector7
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:84
dynamicgraph::sot::const_SigMatrixXd
const typedef Eigen::Map< const MatrixRXd > const_SigMatrixXd
Definition: matrix-geometry.hh:66
dynamicgraph::sot::MatrixForce
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixForce
Definition: matrix-geometry.hh:81
d
FCL_REAL d
dynamicgraph::sot::const_SigVectorXd
const typedef Eigen::Map< const Eigen::VectorXd > const_SigVectorXd
Definition: matrix-geometry.hh:67
EIGEN_MAKE_TYPEDEFS_ALL_SIZES
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix)
Definition: matrix-geometry.hh:43
dynamicgraph::sot::MatrixRXd
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixRXd
Definition: matrix-geometry.hh:63


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31