Skip to content

New implementation for computeCartesianPath()#3618

Merged
rhaschke merged 6 commits intomoveit:masterfrom
ubi-agni:computeCartesianPath
Aug 20, 2024
Merged

New implementation for computeCartesianPath()#3618
rhaschke merged 6 commits intomoveit:masterfrom
ubi-agni:computeCartesianPath

Conversation

@rhaschke
Copy link
Contributor

@rhaschke rhaschke commented Jun 30, 2024

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 MaxEEFStep parameter, 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 the MaxEEFStep parameter.)

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:

1 step, 0.3m precision 1 step, collision not detected!
1 step collision
1 + 1 steps, 0.1m precision 1 + 3 steps, 0.025m precision
2 steps 4 steps

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 CartesianPrecision argument 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.

@codecov-commenter
Copy link

codecov-commenter commented Jun 30, 2024

⚠️ Please install the 'codecov app svg image' to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 80.18018% with 22 lines in your changes missing coverage. Please review.

Project coverage is 47.21%. Comparing base (e8eb9c1) to head (3e348b6).
Report is 21 commits behind head on master.

Files with missing lines Patch % Lines
...it_core/robot_state/src/cartesian_interpolator.cpp 84.89% 13 Missing ⚠️
...ove_group_interface/src/wrap_python_move_group.cpp 0.00% 6 Missing ⚠️
...on/pick_place/src/approach_and_translate_stage.cpp 66.67% 1 Missing ⚠️
...g_interface/test/move_group_interface_cpp_test.cpp 66.67% 1 Missing ⚠️
...rviz_plugin/src/motion_planning_frame_planning.cpp 0.00% 1 Missing ⚠️

❗ 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.
📢 Have feedback on the report? Share it here.

@rhaschke rhaschke force-pushed the computeCartesianPath branch 2 times, most recently from 86be522 to 2b5a16c Compare July 1, 2024 11:37
@rhaschke rhaschke force-pushed the computeCartesianPath branch 3 times, most recently from bf7fbc2 to c0f0ef9 Compare July 3, 2024 09:40
@rhaschke rhaschke marked this pull request as ready for review July 3, 2024 13:34
@rhaschke rhaschke requested review from tylerjw and v4hn as code owners July 3, 2024 13:34
@rhaschke
Copy link
Contributor Author

rhaschke commented Jul 3, 2024

FYI, @LeroyR

rhaschke added 2 commits July 3, 2024 21:17
Return to initial pose after test
@captain-yoshi
Copy link
Contributor

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 min_fraction Achieved = 0.0. The same goals works with Pilz.

Here is my config (manually set in MTC cartesian_path.cpp). Using a version of Track-IK which is deterministic.

// image 1
moveit::core::CartesianPrecision precision;
precision.translational = 0.001;
precision.rotational = 0.01;
precision.max_resolution = 1e-5;

moveit::core::MaxEEFStep max(0, 0.0349066);

// image 2
moveit::core::CartesianPrecision precision;
precision.translational = 0.01;
precision.rotational = 0.01;
precision.max_resolution = 1e-5;

moveit::core::MaxEEFStep max(0, 0.0349066);

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.

image

image

@rhaschke
Copy link
Contributor Author

I suggest using a non-zero MaxEEFStep(0.1, 0.035). Trying to jump to the target with a single IK call, might lead to unavoidable jumps. Did you use zero translational step in the past?

@captain-yoshi
Copy link
Contributor

captain-yoshi commented Jul 19, 2024

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).

Solver Time (s) Waypoints
Pilz 3.6 6641
CartesianPath (MaxEEF 2 degree) 1.42 5197
CartesianPath (MaxEEF 0.5 degree) 2.92 5197
CartesianPath (MaxEEF 0 degree) 1.22 5208

Did you use zero translational step in the past?

Never used in the past as it failed for short trajectories.

@rhaschke
Copy link
Contributor Author

Wow this is a game changer

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.

@captain-yoshi
Copy link
Contributor

captain-yoshi commented Jul 19, 2024

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.

@sea-bass
Copy link

sea-bass commented Aug 6, 2024

Wow this is a game changer

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.

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!

Copy link

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My same comments on moveit/moveit2#2916 hold here.

@rhaschke rhaschke merged commit 9caa14e into moveit:master Aug 20, 2024
@rhaschke rhaschke deleted the computeCartesianPath branch August 20, 2024 12:10
sofiadig pushed a commit to sofiadig/moveit that referenced this pull request Aug 24, 2024
@topguns837
Copy link

I have observed some weird bot motion after the changes in this P.R. I have opened an issue here : #3657

@InigoMoreno
Copy link

I don't understand why any changes have been made to the API so close to the noetic EOL.

@rhaschke
Copy link
Contributor Author

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

@InigoMoreno
Copy link

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.

rhaschke added a commit that referenced this pull request Oct 31, 2024
#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...
rnzenha-s pushed a commit to shadow-robot/sr_interface that referenced this pull request Dec 11, 2024
dg-shadow pushed a commit to shadow-robot/sr_interface that referenced this pull request Jan 9, 2025
* 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>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

6 participants