sot-core
4.11.4
Hierarchical task solver plug-in for dynamic-graph.
|
Go to the documentation of this file.
8 #ifndef __SOT_MATRIX_GEOMETRY_H__
9 #define __SOT_MATRIX_GEOMETRY_H__
13 #include <Eigen/Geometry>
14 #include <dynamic-graph/eigen-io.h>
15 #include <dynamic-graph/linear-algebra.h>
18 #define MRAWDATA(x) x.data()
27 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
29 typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
31 typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
33 typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
35 #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
37 typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \
38 Matrix##Size##X##TypeSuffix; \
40 typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
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)
58 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
59 #undef EIGEN_MAKE_TYPEDEFS
61 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
89 Eigen::Vector3d _t = MH.translation();
92 Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
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;
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:85
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Definition: matrix-geometry.hh:77
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
#define SOT_CORE_EXPORT
Definition: api.hh:20
Definition: abstract-sot-external-interface.hh:17
const typedef Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:71
Eigen::Map< Eigen::VectorXd > SigVectorXd
Definition: matrix-geometry.hh:64
const typedef Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:87
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
Definition: matrix-geometry.hh:79
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:76
Eigen::Map< MatrixRXd > SigMatrixXd
Definition: matrix-geometry.hh:63
Eigen::Vector3d SOT_CORE_EXPORT VectorRotation
Definition: matrix-geometry.hh:78
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:81
Eigen::Ref< Eigen::MatrixXd > RefMatrix
Definition: matrix-geometry.hh:70
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
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
const typedef Eigen::Map< const MatrixRXd > const_SigMatrixXd
Definition: matrix-geometry.hh:65
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixForce
Definition: matrix-geometry.hh:80
const typedef Eigen::Map< const Eigen::VectorXd > const_SigVectorXd
Definition: matrix-geometry.hh:66
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix)
Definition: matrix-geometry.hh:42
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixRXd
Definition: matrix-geometry.hh:62