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

Merged
merged 4 commits into from Jan 30, 2019

Conversation

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

Martin-Oehler 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
Contributor

rhaschke 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

Martin-Oehler 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
Contributor

rhaschke 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

Martin-Oehler 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

v4hn 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
Contributor

rhaschke 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

Martin-Oehler 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
Contributor

rhaschke 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

Martin-Oehler commented Jan 24, 2019

I implemented the requested changes.

@rhaschke
Copy link
Contributor

rhaschke left a comment

Looks good. Thanks a lot.

@rhaschke
Copy link
Contributor

rhaschke 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

Martin-Oehler commented Jan 24, 2019

I added your comment change and formatted with clang.

@henningkayser
Copy link
Contributor

henningkayser 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

welcome bot 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.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.