New implementation for computeCartesianPath()#2916
Merged
sjahr merged 9 commits intomoveit:mainfrom Aug 13, 2024
Merged
Conversation
Return to initial pose after test
2dd55fb to
ffc38fb
Compare
sea-bass
reviewed
Aug 6, 2024
Contributor
sea-bass
left a comment
There was a problem hiding this comment.
Interesting! I like this promise of being able to run less IK solves with these (adjustable) checks. Only some minor comments.
moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
Show resolved
Hide resolved
moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
Show resolved
Hide resolved
...ng_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
Show resolved
Hide resolved
sea-bass
approved these changes
Aug 6, 2024
sjahr
approved these changes
Aug 8, 2024
Contributor
sjahr
left a comment
There was a problem hiding this comment.
Cool, looks good thank you!
Contributor
|
Thanks for the contribution @rhaschke! Any plans to backport this to humble? |
Contributor
Author
|
No. This PR involves a big API change. |
4 tasks
2 tasks
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This is a port of moveit/moveit#3618
The Cartesian interpolator uses a JumpThreshold parameter to detect and reject a switch between IK solution branches leading to larger joint-space jumps. However, this approach is rather fragile: It is difficult to find a suitable threshold parameter and the approach requires a long-enough trajectory to work (at least 10 waypoints).
Particularly if the last requirement is not met, joint-space jumps can still occur unnoticed leading to a dangerous execution of an unchecked, large-volume trajectory.
This PR proposes a fundamentally different way to check for joint-space jumps, which works robustly and locally for any two adjacent waypoints: If the two waypoints are distant in joint-space, their half-way interpolation state will typically lead to a very strong deviation from the straight-line Cartesian path between them. This PR measures this deviation and rejects a trajectory if it exceeds a "precision" threshold.
Actually, the approach (recursively) introduces more waypoints in between to improve the precision of the linear Cartesian path. Thus, the method doesn't need to be provided with a good estimate of the
MaxEEFStepparameter, it adjusts the step size dynamically to meet the given precision requirement. (This statement only holds if IK succeeds at all. In order to evaluate IK on a fine-grained grid and detect obstacles, one still needs theMaxEEFStepparameter.)The following plots illustrate the new behavior. They all use the extremely coarse
MaxEEFStep> 1m, resulting in a single initial step and then adjust the precision to introduce additional waypoints:I kept but deprecated the old JumpThreshold-based methods. I'm not sure how to best migrate the MoveGroup interface. To expose the new interface, we would need to introduce the
CartesianPrecisionargument as a message.For now, I just dropped the JumpThreshold argument and resort to the default precision, which is 1mm. Given a reasonable
MaxEEFStep, this should perfectly resolve the jump issues. I think, fiddling with the new parameter shouldn't be necessary in most cases.