SmoothPath fails for ADA

[Stefanos Nikolaidis]

SmoothPath appears to fail for Ada: Specifically, ADA keeps moving back and forth when RRTConnect is called, indicating a possible issue with shortcutting. It also appears not to respect collision constraints. @hjzh4

[Hejia Zhang]

It seems, when we use ompl RRTConnect, we need to set planner range by calling ::ompl::geometric::RRTConnect::setRange(double distance) otherwise the planner range will be inf by default and the distance between adjacent nodes of random exploration tree will be very large which results in a jerky path with a few waypoints (3~4).

[Aditya Vamsikrishna Mandalika]

That's right. When you call an OMPL planner, you will need to set the planner parameters as per your choosing. A few questions before I solve the issue:

1. @Stefanos19, is the robot moving back and forth along the same motion? Or is it that it moves along a windy path (which would be the expected behavior if the range is high)?

2. @hjzh4, are you using planner/ompl/OMPLConfigurationToConfigurationPlanner to set up the planner or are you calling the function utility function provided in robot/util/planToConfiguration()?

[Hejia Zhang]

Hi Aditya, I directly called util/planToConfiguration()

[Stefanos Nikolaidis]

Hi Aditya, Thanks for the prompt response! It's along a windy path, basically as if you run up and down the stairs before grabbing a seat in a chair in front of you :)

[Aditya Vamsikrishna Mandalika]

Right. Sounds like the issue is then in setting up the planner parameters.

AIKIDO now allows defining Problems and Planners for planning. If you have a planner/ConfigurationToConfiguration problem, you could use a planner/ompl/OMPLConfigurationToConfigurationPlanner, a wrapper for OMPL planners. It exposes the underlying planner to allow setting planner specific parameters. You could instantiate an RRTConnect as follows:

auto mPlanner = std::make_shared<OMPLConfigurationToConfigurationPlanner<RRTConnect>>(adaArmSpace);
auto omplPlanner = mPlanner->getOMPLPlanner()->as<RRTConnect>();
omplPlanner->setRange(range);
omplPlanner->setAnyPlannerParam(value);

Your own method planToConfiguration() could then just be mPlanner->plan(problem).

Two more points:

1. We generally set the RRTConnect range to be equal to the collision checking resolution without loss in performance.
2. I would recommend checking out planner/SequenceMetaPlanner if you are setting up multiple planners to solve the problem.

[Aditya Vamsikrishna Mandalika]

Setting the planner parameters did the trick. Closing the issue.