Fix totg bug giving invalid accelerations #1729
Conversation
JafarAbdi
force-pushed
the
pr-fix_totg_bug
branch
from
a632be6
to
d797a2b
Compare
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
|
Travis is failing due to eigenpy see |
| @@ -538,6 +538,12 @@ bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double | |||
|
|
|||
| if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first) | |||
| { | |||
| // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical | |||
| // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665) | |||
| if (path_pos - next_discontinuity->first < time_step_ * 1e-5) | |||
There was a problem hiding this comment.
Is it necessary to allow the trajectory steps to be much shorter than the time step? I worry slightly that this leaves the possibility for very rare errors.
There was a problem hiding this comment.
It seems that even with a timestep of 1e-8 an change of velocity of 1 will give an an acceleration of 100,000,000. Maybe when we eventually divide change in velocity by time-step, we cap the result to some maximum value?
There was a problem hiding this comment.
This should be a parameter then so this can be adjusted to the corresponding setup. I'm afraid this would be a magic parameter, then
| if (path_pos - next_discontinuity->first < time_step_ * 1e-5) | ||
| { | ||
| continue; | ||
| } | ||
| path_vel = old_path_vel + | ||
| (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos); | ||
| path_pos = next_discontinuity->first; |
There was a problem hiding this comment.
As an alternative
| path_pos = next_discontinuity->first; | |
| if (std::abs((path_vel - old_path_vel) / (path_pos - next_discontinuity->first)) > maximum_acceleration_) | |
| continue; | |
| path_pos = next_discontinuity->first; |
There was a problem hiding this comment.
This results in immediately dividing by zero
There was a problem hiding this comment.
My bad. If you moved this before the assignment it would work right?
There was a problem hiding this comment.
You would need to set maximum_acceleration_ to something reasonable
| @@ -538,6 +538,12 @@ bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double | |||
|
|
|||
| if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first) | |||
| { | |||
| // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical | |||
| // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665) | |||
| if (path_pos - next_discontinuity->first < time_step_ * 1e-5) | |||
There was a problem hiding this comment.
This should be a parameter then so this can be adjusted to the corresponding setup. I'm afraid this would be a magic parameter, then
moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
Show resolved
Hide resolved
|
Looks like this PR is close to ready, just need to expose the magic parameter? |
|
@mikeferguson Can you take a look at this and let us know if there is an issue with this approach? |
|
Merging since this has been considered at length already. If @mikeferguson has a better approach let us know. |
Description
Fix: #1665
Regarding the epsilon value
1e-8I picked it randomly, I think we should parameterize it depending on thetime_step_but I'm not sure how to do it exactlyChecklist