Replace implementation of createInertia that relies on gz_math with an implementation that uses Eigen and thus removes the gz_math dependency

Author: atsogias

rviz_default_plugins/CMakeLists.txt

@@ -66,11 +66,10 @@ if(${QT_VERSION} VERSION_LESS 5.15.0)
   endfunction()
 endif()
 
-find_package(geometry_msgs REQUIRED)
-
-find_package(gz_math_vendor REQUIRED)
-find_package(gz-math REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
 
+find_package(geometry_msgs REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(interactive_markers REQUIRED)
 find_package(laser_geometry REQUIRED)
@@ -278,7 +277,7 @@ target_link_libraries(rviz_default_plugins PUBLIC
 )
 
 target_link_libraries(rviz_default_plugins PRIVATE
-  gz-math::core
+  Eigen3::Eigen
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,

rviz_default_plugins/package.xml

@@ -25,9 +25,11 @@
   <author email="[email protected]">William Woodall</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <build_depend>qtbase5-dev</build_depend>
   <build_depend>rviz_ogre_vendor</build_depend>
+  <build_depend>eigen</build_depend>
 
   <build_export_depend>rviz_ogre_vendor</build_export_depend>
 
@@ -38,7 +40,6 @@
   <exec_depend>rviz_ogre_vendor</exec_depend>
 
   <depend>geometry_msgs</depend>
-  <depend>gz_math_vendor</depend>
   <depend>image_transport</depend>
   <depend>interactive_markers</depend>
   <depend>laser_geometry</depend>

rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp

@@ -38,6 +38,9 @@
 #include <string>
 #include <vector>
 
+#include <Eigen/Eigenvalues>
+#include <Eigen/Geometry>
+
 #include <OgreEntity.h>
 #include <OgreMaterial.h>
 #include <OgreMaterialManager.h>
@@ -52,12 +55,6 @@
 #include <QFileInfo>  // NOLINT cpplint cannot handle include order here
 #include <QString>  // NOLINT: cpplint is unable to handle the include order here
 
-#include <gz/math/Inertial.hh>
-#include <gz/math/MassMatrix3.hh>
-#include <gz/math/Pose3.hh>
-#include <gz/math/Quaternion.hh>
-#include <gz/math/Vector3.hh>
-
 #include "resource_retriever/retriever.hpp"
 
 #include "rviz_default_plugins/robot/robot_joint.hpp"
@@ -880,46 +877,54 @@ void RobotLink::createMass(const urdf::LinkConstSharedPtr & link)
 
 void RobotLink::createInertia(const urdf::LinkConstSharedPtr & link)
 {
-  if (link->inertial) {
-    const gz::math::Vector3d i_xx_yy_zz(
-      link->inertial->ixx,
-      link->inertial->iyy,
-      link->inertial->izz);
-    const gz::math::Vector3d Ixyxzyz(
-      link->inertial->ixy,
+  if (!link->inertial) {
+    return;
+  }
+
+  const Eigen::Matrix3d inertia{(Eigen::Matrix3d{} << link->inertial->ixx, link->inertial->ixy,
       link->inertial->ixz,
-      link->inertial->iyz);
-    gz::math::MassMatrix3d mass_matrix(link->inertial->mass, i_xx_yy_zz, Ixyxzyz);
-
-    gz::math::Vector3d box_scale;
-    gz::math::Quaterniond box_rot;
-    if (!mass_matrix.EquivalentBox(box_scale, box_rot)) {
-      // Invalid inertia, load with default scale
-      if (link->parent_joint && link->parent_joint->type != urdf::Joint::FIXED) {
-        // Do not show error message for base link or static links
-        RVIZ_COMMON_LOG_ERROR_STREAM(
-          "The link " << link->name << " has unrealistic "
-            "inertia, so the equivalent inertia box will not be shown.\n");
-      }
-      return;
+      link->inertial->ixy, link->inertial->iyy, link->inertial->iyz,
+      link->inertial->ixz, link->inertial->iyz, link->inertial->izz).finished()};
+  const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver{inertia};
+  if (link->inertial->mass <= 0 || link->inertial->ixx <= 0 ||
+    link->inertial->ixx * link->inertial->iyy - std::pow(link->inertial->ixy, 2) <= 0 ||
+    inertia.determinant() <= 0 || eigen_solver.info() != Eigen::ComputationInfo::Success)
+  {
+    // Invalid inertia, load with default scale
+    if (link->parent_joint && link->parent_joint->type != urdf::Joint::FIXED) {
+      // Do not show error message for base link or static links
+      RVIZ_COMMON_LOG_ERROR_STREAM(
+        "The link " << link->name << " has unrealistic "
+          "inertia, so the equivalent inertia box will not be shown.\n");
     }
-    Ogre::Vector3 translate(
-      link->inertial->origin.position.x,
-      link->inertial->origin.position.y,
-      link->inertial->origin.position.z);
-
-    double x, y, z, w;
-    link->inertial->origin.rotation.getQuaternion(x, y, z, w);
-    Ogre::Quaternion originRotate(w, x, y, z);
-
-    Ogre::Quaternion rotate(box_rot.W(), box_rot.X(), box_rot.Y(), box_rot.Z());
-    Ogre::SceneNode * offset_node = inertia_node_->createChildSceneNode(
-      translate, originRotate * rotate);
-    inertia_shape_ = new Shape(Shape::Cube, scene_manager_, offset_node);
-
-    inertia_shape_->setColor(1, 0, 0, 1);
-    inertia_shape_->setScale(Ogre::Vector3(box_scale.X(), box_scale.Y(), box_scale.Z()));
-  }
+    return;
+  }
+
+  const Eigen::Vector3d & eigen_values{eigen_solver.eigenvalues()};
+  Eigen::Vector3d box_size{(eigen_values.y() + eigen_values.z() - eigen_values.x()),
+    (eigen_values.x() + eigen_values.z() - eigen_values.y()),
+    (eigen_values.x() + eigen_values.y() - eigen_values.z())};
+  box_size = (6 / link->inertial->mass * box_size).cwiseSqrt();
+  const Eigen::Vector3f box_size_float{box_size.cast<float>()};
+
+  const Eigen::Quaternionf box_rotation_eigen{Eigen::Quaterniond{eigen_solver.eigenvectors()}.cast<float>()};
+  const Ogre::Quaternion box_rotation{box_rotation_eigen.w(), box_rotation_eigen.x(),
+    box_rotation_eigen.y(), box_rotation_eigen.z()};
+
+  const urdf::Pose & pose{link->inertial->origin};
+  const Ogre::Vector3 translation{static_cast<float>(pose.position.x),
+    static_cast<float>(pose.position.y),
+    static_cast<float>(pose.position.z)};
+  const Ogre::Quaternion origin_rotation{static_cast<float>(pose.rotation.w),
+    static_cast<float>(pose.rotation.x), static_cast<float>(pose.rotation.y),
+    static_cast<float>(pose.rotation.z)};
+  const Ogre::Quaternion rotation{origin_rotation * box_rotation};
+  const Ogre::Vector3 scale{box_size_float.x(), box_size_float.y(), box_size_float.z()};
+
+  Ogre::SceneNode * offset_node = inertia_node_->createChildSceneNode(translation, rotation);
+  inertia_shape_ = new Shape(Shape::Cube, scene_manager_, offset_node);
+  inertia_shape_->setColor(1, 0, 0, 1);
+  inertia_shape_->setScale(scale);
 }
 
 void RobotLink::createSelection()