Improvements: Inertia

[ahcorde]

The current implementation for inertia is pretty simple, because it only takes into account the diagonal terms.

Using something like ign-math's EquivalentBox would also take the off-diagonal terms into account.

How maintainers feel about adding ign-math as a dependency of RViz for that? @clalancette @wjwwood @jacobperron @mjeronimo

libignition-math it's available in rosdep.

[jacobperron]

Are you referring to a particular RViz display when you talk about inertia?

[clalancette]

Also note that we need to figure out how to deliver ign-math on Windows and macOS. If we can figure that out (and answers' @jacobperron 's question), I'd be OK with adding a dependency.

[wjwwood]

In general, I'm not opposed to adding a dependency like this, but I do have two concerns:

• If the dependency can be avoided trivially, like if the math is simple, then that's probably better, just because avoiding dependencies of any kind is generally a good idea.
• If we depend on it via rosdep how do we get it for macOS (presumably available via Homebrew) and Windows?

[chapulina]

we need to figure out how to deliver ign-math on Windows and macOS.

We ship Ignition Math binaries through Homebrew and conda-forge, but I understand ROS may not be able to consume either directly. We're happy to do the work needed to provide binaries to the ROS ecosystem though. I'm talking to @nuclearsandwich and @j-rivero about options.

Are you referring to a particular RViz display when you talk about inertia?

Mass properties, see https://github.com/ros-visualization/rviz/issues/1204 and https://github.com/ros2/rviz/pull/714

if the math is simple

It's certainly copiable, but I wouldn't say it's simple, see ignition::math::MassMatrix3

[briansoe66]

Would Eigen3 dependency be easier to deliver?

[chapulina]

Would Eigen3 dependency be easier to deliver?

I believe Eigen is already a dependency of ROS 2 core, but I don't think it provides the functionality that's being requested here.

We're happy to do the work needed to provide binaries to the ROS ecosystem though.

As a first approach, we'll create vendor packages for ignition_cmake2 and ignition_math6, so that they're compiled from source on macOS and Windows. So if there's interest in using them here, they will be available soon.

We're discussing what's the best way to use upstream dependencies in ROS 2 in the future without the need for vendor packages.

[briansoe66]

I believe the math requires an eigen-decomposition of the inertia matrix, and a rotation matrix multiplication. This will be very simple to do with Eigen3.

[briansoe66]

// get the principal moments and axis of inertia
Eigen::Matrix3d inertia << link->inertial->ixx, link->inertial->ixy, link->inertial->ixz, ... 
EigenSolver<MatrixXd> es(inertia);
double ixx = es.eigenvalues()(0);
double iyy = es.eigenvalues()(1);
double izz = es.eigenvalues()(2);
double length_x = sqrt(6 / link->inertial->mass * (iyy + izz - ixx));
double length_y = sqrt(6 / link->inertial->mass * (ixx + izz - iyy));
double length_z = sqrt(6 / link->inertial->mass * (ixx + iyy - izz));

// rotate the axis of inertia by the link pose
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);
Ogre::Matrix3d r1 = // convert rotate to ogre matrix3d
Ogre::Matrix3d r2 = // convert es->eigenvalues() to ogre matrix3d
Ogre::Matrix3d r3 = r2 * r1; // or maybe r2.transpose*r1 or maybe r1 * r2
Ogre::Quaternion q = // convert r3 to ogre quaternion

Ogre::SceneNode * offset_node = inertia_node_->createChildSceneNode(translate, q);
inertia_shape_ = new Shape(Shape::Cube, scene_manager_, offset_node);
inertia_shape_->setScale(Ogre::Vector3(length_x, length_y, length_z));

[chapulina]

This will be very simple to do with Eigen3.

Simple is relative :wink: It's understandable if the maintainers want to maintain the logic in this repo in order to avoid a new dependency. Our suggestion was mainly because Ignition Math already provides this out of the box, with lots of error checking and unit tests.

[wjwwood]

I'm fine with the dependency but it's up to the maintainers as it will be on them to deal with on going work related to this (either way they choose).

[chapulina]

In case you're interested, there's now an ignition_math6_vendor package for Rolling that should be available on all platforms:

https://github.com/ignition-release/ignition_math6_vendor

[clalancette]

@jacobperron @mjeronimo for comments on making ignition a dependency of rviz.

[jacobperron]

I believe this can be closed, since #751 has been merged.

@ahcorde If I'm mistaken and there's more to be done, we can reopen this ticket.