Mitigate Ruckig overshoot #3376
Conversation
AndyZe
changed the title
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
AndyZe
force-pushed
the
andyz/ruckig_overshoot_mitigation
branch
3 times, most recently
from
fc3664a to
b85dce2
Compare
| { | ||
| duration_extension_factor *= DURATION_EXTENSION_FRACTION; | ||
| // Reset the trajectory | ||
| trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */); | ||
|
|
||
| const std::vector<int>& move_group_idx = group->getVariableIndexList(); | ||
| extendTrajectoryDuration(duration_extension_factor, num_waypoints, num_dof, move_group_idx, trajectory, | ||
| original_trajectory); | ||
| extendTrajectoryDuration(duration_extension_factor, num_waypoints, num_dof, move_group_idx, original_trajectory, |
| // re-calculate waypoint velocity and acceleration | ||
| auto target_state = trajectory.getWayPointPtr(time_stretch_idx); | ||
| const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1); | ||
| double timestep = trajectory.getAverageSegmentDuration(); |
Codecov ReportPatch coverage:
Additional details and impacted files@@ Coverage Diff @@
## master #3376 +/- ##
==========================================
- Coverage 61.74% 61.73% -0.00%
==========================================
Files 380 380
Lines 33595 33599 +4
==========================================
Hits 20740 20740
- Misses 12855 12859 +4
... and 8 files with indirect coverage changes Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here. ☔ View full report in Codecov by Sentry. |
AndyZe
force-pushed
the
andyz/ruckig_overshoot_mitigation
branch
from
7af85af to
6f04a58
Compare
|
I don't get your initial example: Halving the velocity at the target state isn't what you want, do you? The target state velocity is an input parameter, isn't it? |
|
Good question! Some people want the time-optimal solution which passes through the given waypoints. Others want to limit jerk while avoiding overshoot. An example of a few groups I've worked with who want to avoid overshoot are:
So that's why I intended to make the overshoot mitigation optional. |
| const double overshoot_threshold) | ||
| { | ||
| // For every timestep | ||
| for (double time_from_start = 0; time_from_start < ruckig_trajectory.get_duration(); |
There was a problem hiding this comment.
Is there a feasible way to not start from 0 every time?
There was a problem hiding this comment.
MoveIt2 has that feature already :) I guess I can backport it to MoveIt1
Co-authored-by: ibrahiminfinite <[email protected]>
AndyZe
force-pushed
the
andyz/ruckig_overshoot_mitigation
branch
from
a93a53b to
9cf61c8
Compare
AndyZe
force-pushed
the
andyz/ruckig_overshoot_mitigation
branch
from
9cf61c8 to
eec82f7
Compare
Description
When smoothing a trajectory, sometimes it's not possible to avoid overshoot. An example is:
current state is (position = 0, velocity = 0), target state is (position = 1, velocity = -1)
However, the overshoot can be reduced by extending the segment duration, thereby reducing the velocity at the goal state. Here's a graphical explanation.