Add AnytimePathShortening on top of Default RRTConnect

[amalnanavati]

Description

I've been noticing many more planning failures lately (summer onwards) than we had in our Jan study. It turns out, that when I switched our default planner to RRTConnect #181 (side-note: I should not have done it as part of an unrelated PR), I did not add path shortening. This was resulting in the outputted plans having large swivels, which out system rejected.

This PR addresses that.

Testing procedure

Empirically, I tried this with 8 bites of string cheese at various locations on the plate, and none of them had a planning failure.

Before opening a pull request

• Format your code using black formatter python3 -m black .
• Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..

Before Merging

• Squash & Merge

[jjaime2]

Changes seem trivial enough, only wonder if we should validate on cubed melon since we saw several failures on that food type during the showcase.

[siddhss5]

I don’t understand. The output of an RRT is never meant to be run directly on a robot. It’s meant to be post-processed by a path shortening algorithm and then sent to the re-timer. And yes, you’d never ever want to run RRT* over a biRRT-connect + path shortening. Fortunately OMPL also implements path-shortening.

[amalnanavati]

Ah, the path shortening algorithm is what I missed. Thanks for the correction!

I added AnytimePathShortening to our MoveIt config (ada_ros2#50) and empirically tested it on 8 pieces of string cheese. The planning failures are all resolved and the plans don't have swivels (big win), but empirically planning takes ~1.5x longer.

I'm currently working on a script to benchmark the "move above food" plans in sim, to get quantitative guidance for tuning AnytimePathShortening parameters, and to protect against future regressions.

[siddhss5]

Excellent. It’d be good to benchmark a few things:1. How expensive your collision checks are. 2. How much time you’re spending path shortening. ADA can sometimes be tricky as some of its joints have continuous rotations so you need to verify that the path-shortening actually understands that (does the mod 2pi when comparing two rotations).Additionally, if your goal is an end-effector transform, you can use IKFast to analytically compute a set of IK solutions and send the set of goals to your planner. Even for a 6-dof arm like the Jaco, you’ll have a finite set of degenerate IK solutions. The GeoDude team has a lot of experience speeding up planners.

[amalnanavati]

Quantitatively benchmarking on 192 "above food" plans (varying x, y, fork roll, and fork pitch), I'm not yet seeing the gains of Path Shortening + RRTConnect above RRT*. I'll play around with the path shortening parameters more (here), but let me know if you have suggestions.

Planner	Success Rate	Median Path Length (rad)	Success Rate w/ Path Length Cutoffs	Median Elapsed Time
AnytimePathShortening[RRTConnect]	0.89	3.29	0.67	2.73
RRT*	0.96	0.78	0.93	2.70
RRTConnect	0.97	3.68	0.58	1.00

[amalnanavati]

Some updates:

MoveIt2 by default runs hybridization and shortcutting (via OMPL’s implementations). It runs num_planning_attempts planners (up to 4 at a time in parallel), hybridizes their results, and then uses any remaining time in allowed_planning_time to shortcut the resultant path. I have verified this both by going through the source code and by tracking debug logs. As a result of this, wrapping RRTConnect in AnytimePathShortening is not necessary – even what we were doing before this PR was hybridizing and shortcutting the results of RRTConnect. (And, for what it’s worth, even what we were running in the January study did hybridization and shortcutting on top of RRT*).

The next question, then, is why we are seeing increased path length when switching from hybridized-and-shortcutted (HAS) RRT* (Jan study) to hybridized-and-shortcutted (HAS) RRTConnect (Sep study). I think some of that had to do with insufficiently-tuned parameters. I had spent some cycles in 2023 tuning range for RRT*, but didn’t do that anew for RRTConnect. After some parameter tuning on RRTConnect, I found that a range of 0.01 works best, coupled with 24 planning attempts (it makes sense that RRTConnect should have a smaller range than RRT*'s 3.0, since RRTConnect doesn’t rewire).

However, even with the parameter tuning, I'm seeing much more variability in HAS-RRTConnect than HAS-RRT*. Keeping all parameters fixed and running both planners on the same 192 pose goals, HAS-RRTConnect's median path length varies from 1.4 to 3.1 rads, whereas HAS-RRT*'s is consistently 1.2. Further, in general MoveIt2 isn't great about guarenteeing the planner will return within allowed_planning_time, but even so HAS-RRTConnect has more variability---it is 1.4-2.6s whereas HAS-RRT* is consistently 1.0s.

TODOs:

• Run it on lovelace, to see performance on production hardware (the above results were from weebo).
• Percolate any parameter changes (num_planning_attempts from 5-->24 and allowed_plkanning_time from 2.0-->0.5) to the main system.
• Decide whether our production system will use HAS-RRT* or HAS-RRTConnect.
• Create an issue for follow-up TODOs re. speeding up the planning pipeline as a whole.

[siddhss5]

In practice the con-con version of RRT-Connect, where the range is infinity and the extension has either REACHED or TRAPPED is the best version. Any variant with a finite range parameter that’s smaller than the dimension of the space is termed an “ext”. So you can have 4 different variants - ext-ext, ext-con, con-ext, and con-con. Again, con-con is almost always the best.