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