Skip to content
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

Jump to bottom

Computed inertia with ignition-math #751

Merged
merged 2 commits into from
Sep 22, 2021
Merged

Computed inertia with ignition-math #751

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
44ef166
Computed inertia with ignition-math
ahcorde Aug 24, 2021
ad247a4
Style
ahcorde Sep 21, 2021
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
4 changes: 4 additions & 0 deletions rviz_default_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,9 @@ find_package(rviz_ogre_vendor REQUIRED)
find_package(Qt5 REQUIRED COMPONENTS Widgets Test)

find_package(geometry_msgs REQUIRED)

find_package(ignition-math6 REQUIRED)

find_package(image_transport REQUIRED)
find_package(interactive_markers REQUIRED)
find_package(laser_geometry REQUIRED)
Expand Down Expand Up @@ -227,6 +230,7 @@ target_include_directories(rviz_default_plugins PUBLIC
target_link_libraries(rviz_default_plugins PUBLIC
rviz_ogre_vendor::OgreMain
rviz_ogre_vendor::OgreOverlay
ignition-math6
)

# Causes the visibility macros to use dllexport rather than dllimport,
Expand Down
1 change: 1 addition & 0 deletions rviz_default_plugins/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
<exec_depend>rviz_ogre_vendor</exec_depend>

<depend>geometry_msgs</depend>
<depend>ignition_math6_vendor</depend>
<depend>image_transport</depend>
<depend>interactive_markers</depend>
<depend>laser_geometry</depend>
Expand Down
50 changes: 31 additions & 19 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 @@ -30,6 +30,12 @@

#include "rviz_default_plugins/robot/robot_link.hpp"

#include <ignition/math/Inertial.hh>
#include <ignition/math/MassMatrix3.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>

#define _USE_MATH_DEFINES
#include <cmath>
#include <map>
Expand Down Expand Up @@ -841,29 +847,35 @@ void RobotLink::createMass(const urdf::LinkConstSharedPtr & link)
void RobotLink::createInertia(const urdf::LinkConstSharedPtr & link)
{
if (link->inertial) {
// display a box sized as if it were a box of uniform density
// with the same inertia as the link
// Ixx = mass/12 (ly^2 + lz^2)
// Iyy = mass/12 (lx^2 + lz^2)
// Izz = mass/12 (lx^2 + ly^2)
urdf::Pose pose = link->inertial->origin;
Ogre::Vector3 translate(pose.position.x, pose.position.y, pose.position.z);
Ogre::Quaternion rotate(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
const ignition::math::Vector3d i_xx_yy_zz(
link->inertial->ixx,
link->inertial->iyy,
link->inertial->izz);
const ignition::math::Vector3d Ixyxzyz(
link->inertial->ixy,
link->inertial->ixz,
link->inertial->iyz);
ignition::math::MassMatrix3d mass_matrix(link->inertial->mass, i_xx_yy_zz, Ixyxzyz);

ignition::math::Vector3d box_scale;
ignition::math::Quaterniond box_rot;
if (!mass_matrix.EquivalentBox(box_scale, box_rot)) {
// Invalid inertia, load with default scale
RVIZ_COMMON_LOG_ERROR_STREAM(
"The link is static or has unrealistic "
"inertia, so the equivalent inertia box will not be shown.\n");
return;
}
Ogre::Vector3 translate(
link->inertial->origin.position.x,
link->inertial->origin.position.y,
link->inertial->origin.position.z);
Ogre::Quaternion rotate(box_rot.W(), box_rot.X(), box_rot.Y(), box_rot.Z());
Ogre::SceneNode * offset_node = inertia_node_->createChildSceneNode(translate, rotate);
inertia_shape_ = new Shape(Shape::Cube, scene_manager_, offset_node);

double length_x = sqrt(
6 / link->inertial->mass * (link->inertial->iyy + link->inertial->izz -
link->inertial->ixx));
double length_y = sqrt(
6 / link->inertial->mass * (link->inertial->ixx + link->inertial->izz -
link->inertial->iyy));
double length_z = sqrt(
6 / link->inertial->mass * (link->inertial->ixx + link->inertial->iyy -
link->inertial->izz));

inertia_shape_->setColor(1, 0, 0, 1);
inertia_shape_->setScale(Ogre::Vector3(length_x, length_y, length_z));
inertia_shape_->setScale(Ogre::Vector3(box_scale.X(), box_scale.Y(), box_scale.Z()));
}
}

Expand Down