Allowing users to load and change dynamically new optimization objective for OMPL's planners #1436

AntoineDevop · 2022-07-20T09:13:15Z

These modification allow user to create new optimizer for OMPL and load it into MoveIt trough the parameter optimization_objective.

After creating his own custom optimizer, with a simple call to the macro defined in the new header, the user can load it in MoveIt.

Checklist

[ x] Required by CI: Code is auto formatted using clang-format
[ x] Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

Update the document accordingly the pull request #1436 (moveit/moveit2#1436)

v4hn

Great feature, I'm unsure about the concrete implementation.

v4hn · 2022-07-20T09:23:27Z

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -350,8 +354,26 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
    }
    else
    {
-      objective =
-          std::make_shared<ompl::base::PathLengthOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
+        pluginlib::ClassLoader<ompl_optimization_loader::OptimizationObjectiveLoader> poly_loader("moveit_core", "ompl_optimization_loader::OptimizationObjectiveLoader");


Did they address this issue in ROS2 or shouldn't you keep the ClassLoader instance alive?
Technically the OptimizationObjectiveLoader is correctly destroyed before the plugin, but I guess the usual assumption is that all the methods in the instantiated objective returned by getOptimizationObjective live in the same shared object as the loader and technically pluginlib is allowed to dlclose the object once the plugin is destroyed.

v4hn · 2022-07-20T09:25:07Z

moveit_planners/ompl/CMakeLists.txt

@@ -34,7 +34,7 @@ install(TARGETS moveit_ompl_interface moveit_ompl_planner_plugin
  INCLUDES DESTINATION include
 )
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(moveit_core ompl)
+ament_export_dependencies(moveit_core ompl moveit_ros_planning)


moveit/planning_interface/ lives in moveit_core, so there is no need for this new dependency.

Yes my bad, the include in top of the file was false. See new commit

I still don't see the need to depend on moveit_ros_planning unless that dependency was missing here before your patch?

For some reasons, the header inside moveit/ompl_interface are not found unless we add moveit_ros_planning to the export_dependendencies.

...nners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_optimization_objective_loader.h

v4hn · 2022-07-20T10:08:00Z

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

+        try
+        {
+            RCLCPP_DEBUG(LOGGER, "Using optimization objective : %s", optimizer.c_str());
+            std::shared_ptr<ompl_optimization_loader::OptimizationObjectiveLoader> obj = poly_loader.createSharedInstance(optimizer);


I believe this way the name for the objective you need to define in your config is

optimization_objective: your_package/YourObjectiveLoader

right? It would be nicer to leave the Loader part out of the config name to keep them in line with the other objective names.

It depend on how the user creates his plugin_description.xml because the name tag allows the user to define the name he wants to use to load and therefor he can give a name consistent with the other objective, without any Loader.
If he doesn't give any name, yes he has to give the fully qualified type to the optimization_objective parameter. But because we have no choice to use a loader because of the parameter given to the constructor of the class OtpimizationObjective, I could not think to another solution.

the name tag allows the user to define the name he wants to use to load

Indeed. I guess I assumed everyone always uses the same name for C++ class name and plugin name.
I guess it is ok to deviate from that norm here.

codecov · 2022-07-20T12:30:43Z

Codecov Report

Patch coverage: 33.34% and project coverage change: -0.01 ⚠️

Comparison is base (72dbc00) 50.47% compared to head (37c1128) 50.46%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1436      +/-   ##
==========================================
- Coverage   50.47%   50.46%   -0.01%     
==========================================
  Files         387      387              
  Lines       31819    31840      +21     
==========================================
+ Hits        16059    16066       +7     
- Misses      15760    15774      +14

Impacted Files	Coverage Δ
...veit/ompl_interface/model_based_planning_context.h	`91.12% <ø> (ø)`
...mpl_interface/src/model_based_planning_context.cpp	`57.72% <33.34%> (-0.96%)`	⬇️

... and 2 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

v4hn · 2022-07-20T12:42:42Z

It would really help to have an example for such a plugin in a tutorial because otherwise we have another undocumented feature with additional aspects to consider.

I guess my main open question is #1436 (comment)

Aside from that I'm wondering whether we could get around having to load the plugin for each planning request instead of once.
In theory the user could request a different plugin in each low-level call, but in practice these parameters are set in the ompl_planning.yaml, right?

ejalaa12 · 2022-07-20T13:08:49Z

It would really help to have an example for such a plugin in a tutorial because otherwise we have another undocumented feature with additional aspects to consider.

We updated the ompl_interface tutorial in one of the commit here
Is there anywhere else we could add this documentation ?

I guess my main open question is #1436 (comment)

I think we could add the pluginlib::ClassLoader<ompl_optimization_loader::OptimizationObjectiveLoader> as a member of the class. That way the ClassLoader is initialized only once and destroyed only when the context is destroyed.
Also, if

Aside from that I'm wondering whether we could get around having to load the plugin for each planning request instead of once. In theory the user could request a different plugin in each low-level call, but in practice these parameters are set in the ompl_planning.yaml, right?

The objective was re-created at each request https://github.com/ros-planning/moveit2/blob/a2a8b48d990bc840820d7c608b8b502c00094c84/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp#L325-L354

Or Is it because loading the plugin takes time compared to instantiating a class normally? In that case we might have to store a shared instance somewhere in the context to be "re-used" later.

mamoll · 2022-08-04T04:27:24Z

I like the idea behind this PR a lot! I share @v4hn's concern, though, that creating an optimization objective for every planning request seems redundant. It sounds like @ejalaa12 is saying that this is, in fact, what is currently done for all the predefined optimization objectives (so this PR would behave no differently). Is that right?

ejalaa12 · 2022-08-04T05:52:46Z

@mamoll yes exactly, the lines I've cited in my previous response shows that all the predefined objective are created at the moment of the request.
This PR would not behave differently.
However, I'm no expert in pluginlib, so my question was if the cost of instantiating an object using pluginlib is higher than just creating it "manually".
In any case, one improvement I see with this PR, is that we can at least move the creation of the pluginlib::ClassLoader to the constructor. There is no need to re-create a new ClassLoader which would then create a new OptimizationObjective for each request.

v4hn · 2022-08-04T09:15:58Z

I agree that the OptimizationObjectives are currently `new` created when they are needed. This is fine, we don't aim for an allocation-free pipeline here. But `ClassLoader` invokes pluginlib which starts to read xml files from plugin definitions of downstream packages it can find in the workspace (possibly recursively crawling folder structures) to figure out which plugins are available and map the passed plugin to a library it can dlopen and map into memory for dynamic function calls. I guess I made my point. :-) In practice that overhead is somewhat reduced because pluginlib is known to do some debatable caching (it avoids `dlclose`ing libraries and moves them [to a "graveyard"](https://github.com/ros/class_loader/blob/rolling/src/class_loader_core.cpp#L236-L244) instead for technical reasons), but that is definitely not part of the public API, so we should not rely on it.

ejalaa12 · 2022-08-09T08:06:17Z

One solution for this problem is the following:

we make the ClassLoader a member of model_based_planning_context
at construction we load a list of optimizers into a std::map<string, OptimizationObjective>. This list can be pre-defined (hard-coded or from ros-parameters or a combination of both
for each planning request we select the OptimizationObjective from the std::map, if it does not exist, we can try loading it and appending to the map.

That way, we minimize the amount of call to pluginlib.

We can provide a PR for that if this is ok for you.

v4hn · 2022-08-09T10:00:38Z

I did not look through all the details the proposed solution requires, but it does sound like a good idea.

AntoineDevop · 2022-08-09T16:28:26Z

In fact, OptimizationObjectives has indeed need to be created each time a planning is requested because it needs the updated version of the SpaceInformation and the only way to give it to it is by instantiated again.
So what we did is :

Create a map of std::map<string, OptimizationObjectiveLoader>at the creation of the planning context where is registered all the declared optimization objective loader plugin
When a planning request is set, if it's not one of the predefined objective, it search it in the map and created the corresponding OptimizationObjectives.

This method ensure that the call to PluginLib occurs only once at the creation of the planning context. One of the drawbacks is that it will not search for newly, during runtime, added plugin but this should not be a frequent case.

mergify · 2022-09-07T06:23:59Z

This pull request is in conflict. Could you fix it @AntoineDevop?

mergify · 2022-09-27T22:33:52Z

This pull request is in conflict. Could you fix it @AntoineDevop?

…he number of call to PluginLib

Update the document accordingly the pull request #1436 (moveit/moveit2#1436)

ejalaa12 · 2022-11-07T15:21:59Z

@mamoll @v4hn
Sorry for the late update, we just created a Pull Request with the instructions in the moveit2_tutorials repo: moveit/moveit2_tutorials#542.

We also went ahead and rebased our PR on main.

Thanks for reviewing it.

ejalaa12 · 2023-05-05T10:30:46Z

@mamoll Any update on this PR ? I've just rebased the changes on main again.

mamoll · 2023-05-05T14:02:22Z

@mamoll Any update on this PR ? I've just rebased the changes on main again.

@sjahr @henningkayser can you have a look at this? This seems relevant to our recent discussion.

mergify · 2023-06-02T09:47:49Z

This pull request is in conflict. Could you fix it @AntoineDevop?

mergify · 2023-08-11T16:25:19Z

This pull request is in conflict. Could you fix it @AntoineDevop?

tylerjw · 2023-08-23T16:52:33Z

I created an issue about this. I'll try to get someone to review this and provide more feedback. I'm sorry it looks like this has gone without reviews since you last updated it.

ejalaa12 · 2023-08-29T08:24:05Z

@tylerjw Hi, I can update this branch with what's necessary. I was waiting for a review and forgot about it ^^.

AntoineDevop added a commit to AntoineDevop/moveit2_tutorials that referenced this pull request Jul 20, 2022

Update ompl_interface_tutorial.rst

19e4b21

Update the document accordingly the pull request #1436 (moveit/moveit2#1436)

v4hn reviewed Jul 20, 2022

View reviewed changes

AntoineDevop added 7 commits November 7, 2022 16:09

Creation of the interface to load a optimizer objective

446f786

Modification in order to allow users to include moveit/ompl_interface

55425c5

Load the user's plugins as the new optimizer objective

3a2d8f4

Modification of the header of the file

bb13492

Fixing bad include

0d01d15

Loading OMPL's optimization objectives at launch in order to reduce t…

2225fab

…he number of call to PluginLib

Clang format

52fa9b7

ejalaa12 force-pushed the custom_ompl_optimization_objective branch from 8426df7 to 52fa9b7 Compare November 7, 2022 15:11

ejalaa12 pushed a commit to forssea-robotics/moveit2_tutorials that referenced this pull request Nov 7, 2022

Update ompl_interface_tutorial.rst

a619453

Update the document accordingly the pull request #1436 (moveit/moveit2#1436)

ejalaa12 mentioned this pull request Nov 7, 2022

Update ompl_interface_tutorial.rst moveit/moveit2_tutorials#542

Open

sjahr requested review from mamoll and v4hn November 7, 2022 17:01

Merge branch 'main' into custom_ompl_optimization_objective

37c1128

sjahr self-assigned this Jun 12, 2023

tylerjw mentioned this pull request Aug 23, 2023

Allowing users to load and change dynamically new optimization objective for OMPL's planners #2321

Closed

tylerjw added the discuss Highlight for discussion in Standup or Maintainer Meeting label Aug 29, 2023

henningkayser removed the discuss Highlight for discussion in Standup or Maintainer Meeting label Sep 5, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Allowing users to load and change dynamically new optimization objective for OMPL's planners #1436

Allowing users to load and change dynamically new optimization objective for OMPL's planners #1436

AntoineDevop commented Jul 20, 2022

v4hn left a comment

v4hn Jul 20, 2022

v4hn Jul 20, 2022

AntoineDevop Jul 20, 2022

v4hn Jul 20, 2022

ejalaa12 Jul 20, 2022

v4hn Jul 20, 2022

AntoineDevop Jul 20, 2022

v4hn Jul 20, 2022

codecov bot commented Jul 20, 2022 •

edited

Loading

v4hn commented Jul 20, 2022

ejalaa12 commented Jul 20, 2022

mamoll commented Aug 4, 2022

ejalaa12 commented Aug 4, 2022

v4hn commented Aug 4, 2022 via email

ejalaa12 commented Aug 9, 2022

v4hn commented Aug 9, 2022 via email

AntoineDevop commented Aug 9, 2022

mergify bot commented Sep 7, 2022

mergify bot commented Sep 27, 2022

ejalaa12 commented Nov 7, 2022

ejalaa12 commented May 5, 2023 •

edited

Loading

mamoll commented May 5, 2023

mergify bot commented Jun 2, 2023

mergify bot commented Aug 11, 2023

tylerjw commented Aug 23, 2023

ejalaa12 commented Aug 29, 2023

Allowing users to load and change dynamically new optimization objective for OMPL's planners #1436

Are you sure you want to change the base?

Allowing users to load and change dynamically new optimization objective for OMPL's planners #1436

Conversation

AntoineDevop commented Jul 20, 2022

Checklist

v4hn left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Jul 20, 2022 • edited Loading

Codecov Report

v4hn commented Jul 20, 2022

ejalaa12 commented Jul 20, 2022

mamoll commented Aug 4, 2022

ejalaa12 commented Aug 4, 2022

v4hn commented Aug 4, 2022 via email

ejalaa12 commented Aug 9, 2022

v4hn commented Aug 9, 2022 via email

AntoineDevop commented Aug 9, 2022

mergify bot commented Sep 7, 2022

mergify bot commented Sep 27, 2022

ejalaa12 commented Nov 7, 2022

ejalaa12 commented May 5, 2023 • edited Loading

mamoll commented May 5, 2023

mergify bot commented Jun 2, 2023

mergify bot commented Aug 11, 2023

tylerjw commented Aug 23, 2023

ejalaa12 commented Aug 29, 2023

codecov bot commented Jul 20, 2022 •

edited

Loading

ejalaa12 commented May 5, 2023 •

edited

Loading