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.

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

Fix CurrentStateMonitor update callback for floating joints with non-identity joint origin #984

Conversation

Projects
None yet
4 participants
@xaver-k
Copy link

commented Jul 13, 2018

Description

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 parent -> joint origin and joint origin -> child, where joint origin -> child is dependent on the joint variable(s). With 1D-joint (rotational, prismatic) we can update the joint variable directly from the /joint_states topic.

In contrast, we update the joint variables of floating joints from TF. The data in TF is already the transform child -> parent. The old implementation saves this transformation as the joint's variables. This means that we practically apply the child -> joint origin-transform twice when calculating the link poses internally. This does not show as long as your floating joint's origin is just the identity, but leads to problems otherwise.

The code in the PR corrects this by applying the inverse of the child -> joint origin-transformation before storing the transform in the joint variables.

Thoughts on the implementation:

  • Calculating the inverse during each update cycle should not be too bad, since practically we transpose and multiply (no numeric inverse). I did not experience any performance issues.
  • If more performance is required, we could store the inverse of the joint origin in the moveit::core::LinkModel and provide a new method getJointOriginTransformInverse().

Checklist

  • Required: Code is auto formatted using clang-format
    Note: There are some changes that clang-format makes but none are related to the code-changes in the PR.
  • Extended the tutorials / documentation, if necessary (not applicable)
  • Include a screenshot if changing a GUI (not applicable)
  • Optional: Created tests, which fail without this PR reference
  • Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)
@xaver-k

This comment has been minimized.

Copy link
Author

commented Jul 16, 2018

Hmm, looks like Travis fails due to failed tests in moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp . As the current state of the kinetic-devel-branch has the exact same failures and the mesh_filter-package is not dependent on planning_scene_monitor, I guess this is not related to the changes in the PR.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Jul 17, 2018

There was a pending PR #982 to fix (disable) the broken test. I know merged this PR and retriggered your Travis build.

@xaver-k xaver-k force-pushed the xaver-k:fix_CurrentStateMonitor_update_of_floating_joints branch from 6abe8b0 to fa53a61 Jul 17, 2018

@xaver-k

This comment has been minimized.

Copy link
Author

commented Jul 17, 2018

Travis now succeeds after a rebase of the feature branch.

@v4hn

v4hn approved these changes Aug 20, 2018

Copy link
Member

left a comment

Wow, that's a very thorough analysis of a very annoying bug.
You are probably one of the first people to ever try this.

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...

@xaver-k

This comment has been minimized.

Copy link
Author

commented Aug 21, 2018

@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:

Because planning_scene_monitor::CurrentStateMonitor::jointStateCallback() skips the update callbacks if no 1D-joints have changed, the transformations are never initialized until at least one rotational or prismatic joint is updated. If you have a robot that does not have at least one of these joints, the transforms in your plugin are never updated.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Aug 23, 2018

@xaver-k I added some minor improvement to your code (cdcc89a). Additionally, I took the chance to cleanup code in RobotState::updateLinkTransformsInternal() to reduce code duplication (cdcc89a).
@v4hn Please have another look.

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.
@v4hn Is that what you mean by "offset between the robot model and the planning-scene model"?

@xaver-k

This comment has been minimized.

Copy link
Author

commented Sep 2, 2018

Hi, sorry for the late reply.
@rhaschke the planning_scene_user-node in my test-package does not publish its maintained planning scene, so the rviz display does not actually get any data from the node.

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?

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Sep 2, 2018

Yes, I'm just waiting for a second approval by e.g. @v4hn.

@v4hn

v4hn approved these changes Sep 3, 2018

Copy link
Member

left a comment

Your changes look good to me too @rhaschke , except for a nitpick.
Please merge after adding a comment.

@rhaschke rhaschke merged commit fa44b78 into ros-planning:kinetic-devel Sep 7, 2018

1 check was pending

continuous-integration/travis-ci/pr The Travis CI build is in progress
Details

rhaschke added a commit that referenced this pull request Sep 7, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.