Join GitHub today
GitHub is home to over 36 million developers working together to host and review code, manage projects, and build software together.Sign up
Fix CurrentStateMonitor update callback for floating joints with non-identity joint origin #984
This PR fixes a bug in the CurrentStateMonitor::tfCallback() method. The old implementation does not take the joint's origin into account. This leads to wrong transformations in the PlanningScene of a PlanningSceneMonitor if the transformation to a floating joint's origin is not the identity.
A demo of the bug can be found here: https://github.com/xaver-k/floating_joint_test
A more in-depth explanation of the bug and the fix:
We calculate the pose of a child link relative to its parent as the concatenation of the transformations
In contrast, we update the joint variables of floating joints from TF. The data in TF is already the transform
The code in the PR corrects this by applying the inverse of the
Thoughts on the implementation:
Hmm, looks like Travis fails due to failed tests in
v4hn left a comment
Wow, that's a very thorough analysis of a very annoying bug.
LGTM, I approve.
I had hoped this would fix another bug with an offset between the robot model and the moveit planning_scene-based model in RViz whenever there is a virtual link in use, but that seems to be yet another issue...
@v4hn I could not find an issue about your offset bug, but maybe this information helps:
I have observed the same issue with robot cells with multiple floating joints. It looks like the transformations of the floating joints (and actually all other joints, too) are un-initialized when you start the PlanningScene plugin in rviz. They are first updated if you have a change (not just a new joint_state message) in one of the normal joints (rotational or prismatic). At that point, everything is updated to the proper poses.
I have not tested this, but my guess for this behavior is:
@xaver-k I added some minor improvement to your code (cdcc89a). Additionally, I took the chance to cleanup code in
Regarding updates of floating joints in rviz, I cannot confirm @xaver-k observations. I tested with his nicely prepared test package, which has two floating joints, but no 1-dof joints at all. On my side, the PlanningScene's robot is shown in a pose assuming Identity transforms for those floating joints as long as they were not yet published. Hence, it doesn't complain about the missing transforms! However, any TF update immediately triggers a change of the rviz display.
In contrast, rviz' internal RobotModel display explicitly complains about the missing transforms and visualizes the corresponding links at the origin as white objects.
Hi, sorry for the late reply.
I submitted a new issue for the rviz-related behavior I am experiencing and an example-setup: #1044 . I guess we should continue the discussion there.
I with regards to the subject of this PR, are we just waiting for a new review of @v4hn ? Or is there anything else you need?