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