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.

Sign up for GitHub

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

Jump to bottom

Added missing robot state update to iterative spline parameterization #1298

Merged
merged 4 commits into from
Jan 30, 2019
Merged

Added missing robot state update to iterative spline parameterization #1298

merged 4 commits into from
Jan 30, 2019

Conversation

Martin-Oehler
Copy link
Contributor

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

rhaschke
rhaschke approved these changes Jan 9, 2019
View reviewed changes
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@Martin-Oehler
Copy link
Contributor Author

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
Copy link
Contributor

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

@Martin-Oehler
Copy link
Contributor Author

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.

@rhaschke rhaschke added the awaits 2nd review one maintainer approved this request label Jan 11, 2019
@v4hn
Copy link
Contributor

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
Copy link
Contributor

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

@Martin-Oehler
Copy link
Contributor Author

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

@rhaschke
Copy link
Contributor

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

@Martin-Oehler
Copy link
Contributor Author

I implemented the requested changes.

rhaschke
rhaschke approved these changes Jan 24, 2019
View reviewed changes
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good. Thanks a lot.

moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp Outdated Show resolved Hide resolved
rhaschke
rhaschke requested changes Jan 24, 2019
View reviewed changes
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please fix formatting.

rhaschke and others added 2 commits January 24, 2019 14:30
@rhaschke @Martin-Oehler
Updated comment
eb26a13 
Co-Authored-By: Martin-Oehler <martin.sven.oehler@gmail.com>
@Martin-Oehler
clang format
3215884
@Martin-Oehler
Copy link
Contributor Author

I added your comment change and formatted with clang.

henningkayser
henningkayser approved these changes Jan 30, 2019
View reviewed changes
Copy link
Member

@henningkayser henningkayser left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 moveit:kinetic-devel Jan 30, 2019
@welcome
Copy link

welcome bot commented Jan 30, 2019

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

rhaschke pushed a commit that referenced this pull request Jan 30, 2019
@Martin-Oehler @rhaschke
Added missing robot state update to iterative spline parameterization (…
3713df8 
…#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().
@rhaschke rhaschke mentioned this pull request Feb 15, 2019
Closed
pull bot pushed a commit to shadow-robot/moveit that referenced this pull request Sep 3, 2020
@Martin-Oehler @pull
Added missing robot state update to iterative spline parameterization (…
c8a8690 
…moveit#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().
This pull request was closed.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@henningkayser henningkayser henningkayser approved these changes

@rhaschke rhaschke rhaschke approved these changes

Assignees
No one assigned
Labels
awaits 2nd review one maintainer approved this request
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
4 participants
@Martin-Oehler @rhaschke @v4hn @henningkayser