diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 868184054..e09499914 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/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, diff --git a/rviz_default_plugins/package.xml b/rviz_default_plugins/package.xml index b8353775c..11dbad18e 100644 --- a/rviz_default_plugins/package.xml +++ b/rviz_default_plugins/package.xml @@ -25,9 +25,11 @@ William Woodall ament_cmake_ros + eigen3_cmake_module qtbase5-dev rviz_ogre_vendor + eigen rviz_ogre_vendor @@ -38,7 +40,6 @@ rviz_ogre_vendor geometry_msgs - gz_math_vendor image_transport interactive_markers laser_geometry diff --git a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp index f84d4b306..5da0920c2 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp @@ -52,11 +52,8 @@ #include // NOLINT cpplint cannot handle include order here #include // NOLINT: cpplint is unable to handle the include order here -#include -#include -#include -#include -#include +#include // NOLINT cpplint cannot handle include order here +#include // NOLINT cpplint cannot handle include order here #include "resource_retriever/retriever.hpp" @@ -880,46 +877,55 @@ 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_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()}; + + const Eigen::Quaternionf box_rotation_eigen{ + Eigen::Quaterniond{eigen_solver.eigenvectors()}.cast()}; + 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(pose.position.x), + static_cast(pose.position.y), + static_cast(pose.position.z)}; + const Ogre::Quaternion origin_rotation{static_cast(pose.rotation.w), + static_cast(pose.rotation.x), static_cast(pose.rotation.y), + static_cast(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()