Display links' centers of mass

[aemarkov]

I add visualization of links' centers of mass in RobotModel display type.
It adds spheres in intertial origin positions.

Here it is:

(in this example urdf inertial origins is incorrect, don't worry)

You can set visibility of the CoMs and size of the markers.

Do you need this functionality?

[jlack1987]

I think additionally it would be nice to show inertial approximations with an ellipsoid

[chapulina]

As a reference, Gazebo displays the center of mass as a sphere sized as if it were a ball of lead with the same mass as the link, and the inertia as a box sized as if it were a box of uniform density that has the same inertia as the link.

An example:

Here's the CoM calculation.
Here's the inertia calculation, which uses ignition::math::MassMatrix3::EquivalentBox

[jlack1987]

Yeah something similar would be great for the RobotModel. I like that Gazebo shows, would be great if we had something on similar for when we're on hardware

[luator]

Is anyone working on this? It would be a very useful feature.
@Garrus007 Did you consider making a pull request with your feature? If not, is the code available somewhere?

[aemarkov]

I have a commit in my old forked repo. But there are several bugs (It can segfault because I forgot to check several parameters). I can update this to the current version of the RViz and create Pull-Request in few days.

But I like @chapulina comment. Maybe we should create more advanced view instead of simple balls with arbitrary set diameter.

[rhaschke]

You are very welcome to provide your solution and update with it with the pointers provided by @chapulina.

[yeshasvitirupachuri]

@Garrus007 do you have any update on this ?

[JuliusSustarevas]

Is there something stopping this? Or just bandwidth? I'd especially be interested int he full center of mass of the whole robot model

[briansoe66]

@Garrus007 @Yeshasvitvs @JuliusSustarevas I have created a Pull request for ros2 rviz, based on @Garrus007 fork. I would appreciate help testing the implementation. Let me know if you can help and want write access to the fork/branch.

[briansoe66]

ros2 rviz can now display links' center of mass and inertia, as of commit 2b551f8