- Aldebaran documentation |
- C++ Libraries |
- index

libalmath 2.8.7.4

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
urdfeigen.h
Go to the documentation of this file.
1 /*
2 * Copyright 2015 Aldebaran. All rights reserved.
3 *
4 */
5
6 #ifndef LIB_ALMATH_SCENEGRAPH_URDFEIGEN_H
7 #define LIB_ALMATH_SCENEGRAPH_URDFEIGEN_H
8
9 #include <almath/scenegraph/urdf.h>
10 #include <Eigen/Geometry>
11 #include <almath/scenegraph/bodymass.h>
12
13 namespace AL {
14 namespace Math {
15
16 inline Eigen::Quaterniond eigenQuaternionFromUrdfRpy(
17 const AL::urdf::Array3d &rpy) {
18 return Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *
19 Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) *
20 Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX());
21 }
22
23 template <typename T>
24 AL::urdf::Array3d urdfRpyFromEigenMatrix3(const T &t) {
25 Eigen::Vector3d ypr = t.eulerAngles(2, 1, 0);
26 return AL::urdf::Array3d{ypr[2], ypr[1], ypr[0]};
27 }
28
29 inline Eigen::Isometry3d toEigenTransform(const AL::urdf::Pose &p) {
30 typedef Eigen::Map<const Eigen::Vector3d> Vector3dMap;
31 return Eigen::Translation3d(Vector3dMap(p.xyz().data())) *
32 eigenQuaternionFromUrdfRpy(p.rpy());
33 }
34
35 inline Eigen::AffineCompact3d toEigenAffineCompact3(const AL::urdf::Pose &p) {
36 typedef Eigen::Map<const Eigen::Vector3d> Vector3dMap;
37 return Eigen::Translation3d(Vector3dMap(p.xyz().data())) *
38 eigenQuaternionFromUrdfRpy(p.rpy());
39 }
40
41 inline Eigen::Isometry3d toEigenIsometry3(const AL::urdf::Pose &p) {
42 typedef Eigen::Map<const Eigen::Vector3d> Vector3dMap;
43 return Eigen::Translation3d(Vector3dMap(p.xyz().data())) *
44 eigenQuaternionFromUrdfRpy(p.rpy());
45 }
46
47 template <typename T>
48 AL::urdf::Pose urdfPoseFromEigenTransform(const T &t) {
49 return AL::urdf::Pose({t.translation()[0], t.translation()[1], t.translation()[2]},
50 urdfRpyFromEigenMatrix3(t.linear()));
51 }
52
53
54 inline Eigen::Matrix3d toEigenMatrix3(const AL::urdf::Inertial &inertial) {
55 Eigen::Matrix3d m;
56 m(0, 0) = inertial.ixx();
57 m(0, 1) = m(1, 0) = inertial.ixy();
58 m(0, 2) = m(2, 0) = inertial.ixz();
59 m(1, 1) = inertial.iyy();
60 m(1, 2) = m(2, 1) = inertial.iyz();
61 m(2, 2) = inertial.izz();
62 return m;
63 }
64
65 // squash inertial `a` and `b` to create inertial `c`.
66 // All of them follow the urdf convention: frame origin is the center of mass.
67 // pose_a and pose_b shall be defined with respect to the same frame.
68 // pose_c is defined such that
69 // 1/ its origin is the center of mass of the compound body
70 // 2/ its basis is the same as pose_a one.
71 template <typename Scalar, typename EPose, typename Derived1>
72 void squashInertial(Scalar mass_a, const EPose &pose_a,
73 const Eigen::MatrixBase<Derived1> &inertia_a, Scalar mass_b,
74 const EPose &pose_b,
75 const Eigen::MatrixBase<Derived1> &inertia_b,
76 Scalar &mass_c, EPose &pose_c,
77 Eigen::MatrixBase<Derived1> &inertia_c) {
78 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived1, 3, 3)
79 EIGEN_STATIC_ASSERT(
80 (Eigen::internal::is_same<Scalar, typename EPose::Scalar>::value),
81 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
82 EIGEN_STATIC_ASSERT(
83 (Eigen::internal::is_same<Scalar, typename Derived1::Scalar>::value),
84 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
85 typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
86 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
87
88 // define total mass
89 mass_c = mass_a + mass_b;
90
91 // define the new frame
92 pose_c.translation() =
93 (mass_a * pose_a.translation() + mass_b * pose_b.translation()) / mass_c;
94 pose_c.linear() = pose_a.linear();
95
96 // add inertia from a
97 inertia_c = inertia_a;
98 Vector3 displ = pose_c.translation() - pose_a.translation();
99 addPointMassInertia(mass_a, pose_c.linear().transpose() * displ, inertia_c);
100
101 // add inertia from b
102 Matrix3 rot_cb = pose_c.linear().transpose() * pose_b.linear();
103 inertia_c += rot_cb.transpose() * inertia_b * rot_cb;
104 displ = pose_c.translation() - pose_b.translation();
105 addPointMassInertia(mass_b, pose_c.linear().transpose() * displ, inertia_c);
106 }
107
108 template <typename Scalar, typename Derived0, typename Derived1>
109 void urdfInertialToEigen(const AL::urdf::Inertial &inertial, Scalar &mass,
110 Eigen::MatrixBase<Derived0> &com_a,
111 Eigen::MatrixBase<Derived1> &inertia_a) {
112 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 3, 1)
113 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived1, 3, 3)
114 EIGEN_STATIC_ASSERT(
115 (Eigen::internal::is_same<Scalar, typename Derived0::Scalar>::value),
116 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
117 EIGEN_STATIC_ASSERT(
118 (Eigen::internal::is_same<Scalar, typename Derived1::Scalar>::value),
119 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
120 typedef Eigen::Transform<Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign> T;
121 typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
122
123 mass = inertial.mass();
124 T t_ab = toEigenTransform(inertial.origin()).cast<Scalar>();
125 com_a = t_ab.translation();
126 Matrix3 inertia_b = toEigenMatrix3(inertial).cast<Scalar>();
127 inertia_a = t_ab.linear().transpose() * inertia_b * t_ab.linear();
128 }
129 }
130 }
131 #endif
AL::urdf::Inertial::ixx
double ixx() const
AL::urdf::Inertial::iyz
double iyz() const
AL::urdf::Inertial::ixz
double ixz() const
AL::Math::addPointMassInertia
void addPointMassInertia(Scalar mass, const Eigen::MatrixBase< Derived0 > &position, Eigen::MatrixBase< Derived1 > &output)
Definition: bodymass.h:17
AL::urdf::Inertial::izz
double izz() const
AL::urdf::Inertial::ixy
double ixy() const
AL::urdf::Pose
Definition: urdf.h:319
AL::urdf::Array3d
std::array< double, 3 > Array3d
Definition: urdf.h:66
AL::urdf::Inertial::iyy
double iyy() const
AL::Math::toEigenTransform
Eigen::Isometry3d toEigenTransform(const AL::urdf::Pose &p)
Definition: urdfeigen.h:29
AL::Math::toEigenMatrix3
Eigen::Matrix3f toEigenMatrix3(const Math::Transform &tr)
Definition: almatheigen.h:34
bodymass.h
AL::urdf::Inertial::mass
double mass() const
AL::Math::urdfPoseFromEigenTransform
AL::urdf::Pose urdfPoseFromEigenTransform(const T &t)
Definition: urdfeigen.h:48
AL::urdf::Inertial::origin
Pose origin() const
AL::urdf::Inertial
Definition: urdf.h:385
AL::Math::eigenQuaternionFromUrdfRpy
Eigen::Quaterniond eigenQuaternionFromUrdfRpy(const AL::urdf::Array3d &rpy)
Definition: urdfeigen.h:16
AL::Math::toEigenIsometry3
Eigen::Isometry3d toEigenIsometry3(const AL::urdf::Pose &p)
Definition: urdfeigen.h:41
AL::Math::urdfInertialToEigen
void urdfInertialToEigen(const AL::urdf::Inertial &inertial, Scalar &mass, Eigen::MatrixBase< Derived0 > &com_a, Eigen::MatrixBase< Derived1 > &inertia_a)
Definition: urdfeigen.h:109
urdf.h
AL::Math::squashInertial
void squashInertial(Scalar mass_a, const EPose &pose_a, const Eigen::MatrixBase< Derived1 > &inertia_a, Scalar mass_b, const EPose &pose_b, const Eigen::MatrixBase< Derived1 > &inertia_b, Scalar &mass_c, EPose &pose_c, Eigen::MatrixBase< Derived1 > &inertia_c)
Definition: urdfeigen.h:72
AL::Math::urdfRpyFromEigenMatrix3
AL::urdf::Array3d urdfRpyFromEigenMatrix3(const T &t)
Definition: urdfeigen.h:24
AL::Math::toEigenAffineCompact3
Eigen::AffineCompact3f toEigenAffineCompact3(const Math::Transform &tr)
Definition: almatheigen.h:49