Skip to content

Replace implementation of RobotLink::createInertia that relies on gz_math with an implementation that uses Eigen #1527

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open

Replace implementation of RobotLink::createInertia that relies on gz_math with an implementation that uses Eigen #1527

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 4 additions & 5 deletions rviz_default_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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,
Expand Down
3 changes: 2 additions & 1 deletion rviz_default_plugins/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand All @@ -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>
Expand Down
92 changes: 49 additions & 43 deletions rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,11 +52,8 @@
#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 <Eigen/Eigenvalues> // NOLINT cpplint cannot handle include order here
#include <Eigen/Geometry> // NOLINT cpplint cannot handle include order here

#include "resource_retriever/retriever.hpp"

Expand Down Expand Up @@ -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::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()
Expand Down