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

invert waypoint velocities on reverse #1335

Conversation

Projects
None yet
4 participants
@v4hn
Copy link
Member

commented Feb 1, 2019

This addresses a long-standing issue in the trajectory container.

Without this patch, the trajectory's time profile is broken after calling reverse()
and robot controllers that respect velocities in specified trajectories,
e.g. default PR2 and TIAGo, will generate execution trajectories with highly oscillating velocity-profile.

This should be cherry-picked to melodic-devel of course.

invert waypoint velocities on reverse
Otherwise the trajectories profile will be broken
and robot controllers that respect velocities in specified trajectories,
e.g. default PR2 and TIAGo, will generate highly oscillating profiles.
@henningkayser
Copy link
Contributor

left a comment

Nice that this is fixed!
It's not that critical but maybe we should document or create an issue that reverse() does not recompute acceleration/effort values.

@jrgnicho jrgnicho self-requested a review Feb 1, 2019

@jrgnicho
Copy link
Contributor

left a comment

Thanks for the contribution

@rhaschke
Copy link
Collaborator

left a comment

While inversing the velocities is fine, why don't you also inverse the accelerations?
They also change sign when reversing time.

@v4hn

This comment has been minimized.

Copy link
Member Author

commented Feb 6, 2019

While inversing the velocities is fine, why don't you also inverse the accelerations?
They also change sign when reversing time.

Accelerations change sign twice on reversal because we reverse the trajectory and also the velocity.
Thus, the sign is actually good as is.

Here is a small test that reverses a valid trajectory. With the proposed patch this is the output on our fanuc configuration:

reversed_trajectory

Without the patch the velocity is not inverted for traj_reversed.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Feb 6, 2019

@v4hn While I was tempting to believe you on phone, from the figures I conclude that acceleration should change sign as well (and in your figure it actually does).
Turning a bell-shaped velocity profile (+acc -> -acc) into an inverted bell-shaped velocity profile, also means to start with negative acceleration (to reach negative velocity) and finish with positive (to bring velocity back to zero from negative).

@v4hn

This comment has been minimized.

Copy link
Member Author

commented Feb 6, 2019

@v4hn While I was tempting to believe you on phone, from the figures I conclude that acceleration should change sign as well (and in your figure it actually does).

The figure was generated for the normal and inverted trajectory with the proposed pull-request as-is.
Do you think the trajectory profile in the screenshot looks bad?
The acceleration profile for traj_reversed looks like the sign was inverted, but actually only the waypoints are reversed (along the time axis).

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Feb 6, 2019

You (and your colleague) are right. Let's assume the trajectory is given as p(t), where t runs from 0 to tmax. The reversed trajectory then is q(t)=p(s(t)), where s(t)=tmax-t. The velocity and acceleration profiles are then given by the derivatives using chain rule as follows:

v(t)=dq/dt=dp/ds * ds/dt
a(t)=dv/dt=dv/ds * ds/dt

The term ds/dt = -1 creates the required sign change - once for velocity, twice for acceleration, three times for jerk, etc.

@rhaschke rhaschke merged commit fe1afc7 into ros-planning:kinetic-devel Feb 6, 2019

1 check passed

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

This comment has been minimized.

Copy link
Member Author

commented Feb 6, 2019

three times for jerk

Yes, if we ever want to extend the representation to jerk, we will have to account for it here.

Thanks for the reviews and thanks for spelling out the formalism @rhaschke 👍

rhaschke added a commit that referenced this pull request Feb 6, 2019

invert waypoint velocities on reverse (#1335)
Otherwise the trajectories profile will be broken and robot controllers that respect velocities in specified trajectories, e.g. default PR2 and TIAGo, will generate oscillating profiles.

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

invert waypoint velocities on reverse (ros-planning#1335)
Otherwise the trajectories profile will be broken and robot controllers that respect velocities in specified trajectories, e.g. default PR2 and TIAGo, will generate oscillating profiles.
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.