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

Added missing robot state update to iterative spline parameterization #1298

Conversation

Projects
None yet
4 participants
@Martin-Oehler
Copy link
Contributor

commented Jan 9, 2019

Description

The iterative spline parameterization is missing an update to the robot states of the trajectory after iterating over each point. These leads to lots of warnings of the following kind:

[ WARN] [1547043223.679662634, 8.816000000] [/move_group]: Returning dirty collision body transforms

This PR adds the missing update() call (just one line).

I did not auto-format with clang as this changed lines unrelated to my PR.

Checklist

  • Required by CI: Code is auto formatted using clang-format
  • Extended the tutorials / documentation, if necessary reference
  • Include a screenshot if changing a GUI
  • Document API changes relevant to the user in the moveit/MIGRATION.md notes
  • Created tests, which fail without this PR reference
  • Decide if this should be cherry-picked to other current ROS branches
  • While waiting for someone to review your request, please consider reviewing another open pull request to support the maintainers
Martin Oehler
@rhaschke
Copy link
Collaborator

left a comment

The question is, why the time parameterization changes the joint positions in first place. I think, it shouldn't.
Fixup approved.

@Martin-Oehler

This comment has been minimized.

Copy link
Contributor Author

commented Jan 10, 2019

For some reason, the trajectory is first written to an internal data structure before doing the time parameterization. In this final step, the result is written back to the robot_trajectory::RobotTrajectory object.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Jan 10, 2019

Yes, I have seen this. The question is why? The joint positions shouldn't be modified, as this might invalidate the trajectory.

@Martin-Oehler

This comment has been minimized.

Copy link
Contributor Author

commented Jan 10, 2019

Without looking further into it, I have only two guesses:

  1. Performance: The data is restructured, maybe for faster access
  2. The code has been copied from somewhere else and this was the fastest way to make it run.
@v4hn

This comment has been minimized.

Copy link
Member

commented Jan 11, 2019

Updating all states can be quite a bit of unnecessary computation here and I would prefer to avoid it where possible.

Looking at the code, it conditionally includes two new waypoints if add_points is set to true and the code only ever modifies the positions of these two points.

So how about we skip the setVariablePosition calls and instead do them for the two new points only if add_points_ is true (and afterwards update them too?

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Jan 11, 2019

Thanks for looking deeper into this, @v4hn. I would agree to your proposal.

@Martin-Oehler

This comment has been minimized.

Copy link
Contributor Author

commented Jan 14, 2019

I agree as well and will update my PR accordingly (or should I open a new one?).

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Jan 14, 2019

Updating should be fine. You can also squash commits to cleanup history ;-)

@Martin-Oehler

This comment has been minimized.

Copy link
Contributor Author

commented Jan 24, 2019

I implemented the requested changes.

@rhaschke
Copy link
Collaborator

left a comment

Looks good. Thanks a lot.

@rhaschke
Copy link
Collaborator

left a comment

Please fix formatting.

rhaschke and others added some commits Jan 24, 2019

Updated comment
Co-Authored-By: Martin-Oehler <martin.sven.oehler@gmail.com>
Martin Oehler
@Martin-Oehler

This comment has been minimized.

Copy link
Contributor Author

commented Jan 24, 2019

I added your comment change and formatted with clang.

@henningkayser
Copy link
Contributor

left a comment

Works and looks good. I personally would remove the braces in the for loop but that's up to you.

@rhaschke rhaschke merged commit 9b56edd into ros-planning:kinetic-devel Jan 30, 2019

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
@welcome

This comment has been minimized.

Copy link

commented Jan 30, 2019

Congrats on getting your first MoveIt! pull request merged and improving open source robotics!

rhaschke added a commit that referenced this pull request Jan 30, 2019

Added missing robot state update to iterative spline parameterization (
…#1298)

... to prevent warnings.
The two new additional waypoints added in the beginning and end of the trajectory (which are then updated by the time parameterization to have a smooth start / stop), missed a RobotState::update().

ggupta9777 added a commit to ggupta9777/moveit that referenced this pull request Mar 17, 2019

Added missing robot state update to iterative spline parameterization (
…ros-planning#1298)

... to prevent warnings. 
The two new additional waypoints added in the beginning and end of the trajectory (which are then updated by the time parameterization to have a smooth start / stop), missed a RobotState::update().
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.