New implementation for computeCartesianPath()#3618
Conversation
|
Codecov ReportAttention: Patch coverage is
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## master #3618 +/- ##
==========================================
+ Coverage 47.16% 47.21% +0.05%
==========================================
Files 603 604 +1
Lines 59196 59238 +42
Branches 7024 7021 -3
==========================================
+ Hits 27915 27964 +49
+ Misses 30863 30856 -7
Partials 418 418 ☔ View full report in Codecov by Sentry. |
86be522 to
2b5a16c
Compare
bf7fbc2 to
c0f0ef9
Compare
|
FYI, @LeroyR |
Return to initial pose after test
c0f0ef9 to
1815e5f
Compare
|
Very Nice! Always wanted to benchmark against Pilz :) I tested on my setup, and it now works reliably with short trajectories. But for larger distance (see photos below), I often get Here is my config (manually set in MTC cartesian_path.cpp). Using a version of Track-IK which is deterministic. Do you have any insight on why this could happen ? I would assume that the min_fraction would be greater then 0 for these cases. |
|
I suggest using a non-zero |
|
Wow this is a game changer :) With the proposed config, this PR passes my huge Task which consist of + 1000 stages and generates 5200 waypoints ! Some limited benchmark (5 runs each).
Never used in the past as it failed for short trajectories. |
I'm curious: where do you see a dramatic change? I was just expecting some more robustness when generating linear Cartesian trajectories. Particularly, I wasn't expecting faster computation. |
|
Robustness is the key + same configuration for a wide variety of poses. Like I said, the solver went from a no go (from my perspective and project needs) to a potential alternative to Pilz. It seems faster, but would have to change my configuration to match Pilz. Gotta check if it passes through the same planning plugins also. EDIT : With MTC I can now use the min/max distance to stop early on collisions, which cannot be used with Pilz. |
If you have successive points where a joint interpolation is sufficient (and within tolerance), you can avoid having to solve IK at that point. So I can totally see the speedups that way! |
sea-bass
left a comment
There was a problem hiding this comment.
My same comments on moveit/moveit2#2916 hold here.
|
I have observed some weird bot motion after the changes in this P.R. I have opened an issue here : #3657 |
|
I don't understand why any changes have been made to the API so close to the noetic EOL. |
Reaching EOL doesn't mean stopping development. ROS1 is to be continued: https://github.com/ros-o/ros-o |
|
Yes, but the binaries are going to be frozen, and there are probably packages that use moveit that will not have time to update its binaries before that. |
#3615 made PoseModelStateSpace identical to ModelBasedStateSpace, both using joint-space interpolation and distance measurements. This resulted in planning timeouts for code that previously worked. Obviously, using joint-space interpolation much more often results in the rejection of tight path constraints than Cartesian interpolation did. The problem with potential jumps due to Cartesian interpolation in principle remains, which is a dilemma of the current design: - We do need Cartesian interpolation to reduce the risk of state rejection due to a failing path constraint, which currently cannot be checked during interpolation. - Ideally, we would do joint interpolation and only resort to Cartesian interpolation if the path constraint is violated, i.e. follow a similar approach as in #3618. However, this would require access to the path constraint...
* Fix Left arm jumps (#843) * Make new palm flesh default for hands E and G * Make STF (MST) the default fingertip sensors * Rename 'underaction_error_reporter' node. Ensure 'robot_description' and 'robot_description_semantic' parameters exists at node launch * Lint fixes * Fix ROS tests to conform to latest api changes to moveit_commander: moveit/moveit#3618 * Update license year --------- Co-authored-by: rnzenha-s <rodrigo@shadowrobot.com>


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.