Using pinocchio with RViz2

[shikajiko]

Hello,

I'm currently working on visualizing the kinematic tree of a humanoid robot in RViz2. My approach so far is:

Parse the robot’s URDF to build the Pinocchio model.
Run forwardKinematics to compute the joint placements.
Iterate over model.njoints and broadcast the joint positions using a TF2 broadcaster

However, I've encountered some issues:

1. The transforms I obtain from forwardKinematics are expressed in the global (world) frame. RViz, however, expects TF transforms to be provided as the transform from each parent frame to its child frame. As a result, when I broadcast the global positions directly, RViz interprets them as if they were relative to the parent, which gives incorrect results.

2. Since forwardKinematics only updates the transforms for joints, the fixed joints are not being broadcast. This means parts of the robot are missing from the visualization.

How can I compute or convert the global joint placements from Pinocchio into the proper relative (parent-to-child) transforms expected by RViz2?
What is the recommended way to incorporate fixed joints (or frames) into the TF tree? Is there a better approach other than iterating from model.njoints?

Thanks in advance

[cmastalli]

Hi @shikajiko!

We have developed such a thing and more. You can see the project here: https://github.com/loco-3d/whole_body_state_rviz_plugin

However, this code supports ROS-1 at the moment. Could you help us support ROS-2 as well?

Cheers,
Carlos

[shikajiko]

Hi @cmastalli ,

Thanks for sharing the project!
However, at the moment I don’t have the capability to help with the ROS 2 support since I’m still new to robotics. For now, I’ll focus on fixing the issue using what’s available in Pinocchio natively.

[EtienneAr]

Hello

1. The array data.liMi[j] gives you the placement of the i-th joint relatively to its parent.
2. Fixed joint are merged with their parent joint for optimization purposes. However, they are replaced by a frame so that you can still deduce their placement.

(there's a RVizViewer in pinocchio, but it should be refactored. It supports only python, ros1 and uses markers instead of RobotState and TF ...)

[shikajiko]

Thanks for the explanation @EtienneAr, That helped fixed the position issue.

Now, after loading the robot, how can I update or move the joint positions?

Thanks