Constraints Library Refactor #1428

BryceStevenWilley · 2019-04-08T05:07:25Z

Continuation of #897 in smaller bits, starting with the constraint library.

In order to make an OMPL plugin framework, the ModelBasedPlanningContext (MBPC) has to be able to be initialized by a plugin (i.e. we can no longer directly pass it the OMPL state space, simple setup, etc.), meaning that the leaky interface between the MBPC and the OMPLPlannerManager needs to be cleaned up, starting with the constraints library.

This change moves the constraints library internal to the MBPC, so it can separately load the constraint library and add its interpolation function to the OMPL state space being used.

Additionally, this removes an unused OMPL service, as mentioned here.

Much of the documentation/tutorial updates will happen later, as there are still many changes to come including:

Moving the rest of the OMPL initialization to the MBPC
Making an OMPLPlanningContext abstract class and making separate ModelBased, JointBased, and PoseBased planning contexts.
Adding the actual pluginlib loading

Checklist

Required by CI: Code is auto formatted using clang-format
Testing: @v4hn I can't quite figure out how to generate constraints to test this change; would you mind trying this PR to make sure it doesn't break anything for you?
Should this be cherry-picked to https://github.com/ros-planning/moveit2? Or should we wait for all of the OMPL plugin-related changes to be in ROS 1 before moving over?

BryceStevenWilley · 2019-04-09T02:20:40Z

Fun fact: the new draft PR feature (https://help.github.com/en/articles/about-pull-requests#draft-pull-requests) apparently doesn't trigger Travis or notify CODEOWNERS when it's marked as ready. Sorry about that.

BryceStevenWilley · 2019-04-16T13:14:07Z

Friendly ping (not sure if email notifications got sent out at all) @davetcoleman @v4hn

henningkayser

I tested this with the current panda setup and a simple orientation constraint (gripper downwards).
Normal planning seems to work fine in general. I noticed that sometimes plans are also generated if the start state does not satisfy the constraints. Looking at the changes I'm not sure if this is a new issue.
Also constructing constraints does not work for me. Even after filling the MotionPlanRequest parameters (inside generate_state_database.cpp) the ConstraintsLibrary always crashes with a segfault.

...it_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h

BryceStevenWilley · 2019-05-15T04:12:13Z

Thanks for the feedback @henningkayser. How did you go about trying to generate the Constraints library? I didn't see it documented anywhere, and haven't had the time to dive into what format the constraints parameter to generate_state_database.cpp needs to be to test things.

v4hn

Given your goal to isolate the MBPC structure, this seems reasonable.
At least it is somewhat cleaner than the previous implementation. :-)

I didn't see it documented anywhere, and haven't had the time to dive into what format the constraints parameter to generate_state_database.cpp needs to be to test things.

Yes, that's my fault. :-/
I did not manage to write the tutorial (until now?) due to time constraints.
Dave scolded me for that before...
There are rather verbose commit messages though with a full example. :)
783cf74 3fc0789

I can't approve this because I did not have the time to test it.
I hope you and @henningkayser get around to fix this up in the close future.

v4hn · 2019-06-05T14:41:05Z

moveit_planners/ompl/ompl_interface/demos/generate_state_database.cpp

-      ompl_interface.getConstraintsLibrary().addConstraintApproximation(params.constraints, params.planning_group,
-                                                                        scene, params.construction_opts);
+      context->getConstraintsLibrary()->addConstraintApproximation(params.constraints, params.planning_group, scene,
+                                                                   params.construction_opts);


Specifying the constraints, planning group and scene here again feels weird when the context could/should already know them.

That's fair, but I can't see another way to pass those along to the constraints library without some weird state being used behind the scenes.

BryceStevenWilley · 2019-06-12T03:22:00Z

@v4hn Thanks for pointing out the commit messages! Those are just enough, I had just never thought to look there. :)

davetcoleman

Looks good, thanks!!

moveit_planners/ompl/ompl_interface/CMakeLists.txt

moveit_planners/ompl/ompl_interface/demos/generate_state_database.cpp

...it_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h

moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp

davetcoleman · 2019-07-03T00:18:52Z

@ommmid has been creating a new PlanningContext plugin for TrajOpt + MoveIt, and would make a good reviewer for this PR.

davetcoleman · 2019-07-24T18:38:06Z

ping @BryceStevenWilley could you address my requested changes?

BryceStevenWilley · 2019-07-24T19:17:07Z

Thanks for the ping Dave, this has been on my plate for a while, but I think I can address your comments by this weekend.

BryceStevenWilley · 2019-07-29T03:10:25Z

Don't have access to a machine that can build MoveIt right now, will probably need another few days to test.

BryceStevenWilley · 2019-08-16T03:02:16Z

Sorry for the wait, finally had time to test and finish up the generate state database script. Also added some of @v4hn's comments from commits into actual launch files and in code comments.

davetcoleman · 2019-08-21T18:21:33Z

@henningkayser could you give this a second review?

fyi @mamoll

In preparation to a larger change that moves much of the context initialization internal to the ModelBasedPlanningContext as well.

Also added launch file for ease of use.

moveit_planners/ompl/ompl_interface/CMakeLists.txt

mamoll · 2019-11-20T16:56:29Z

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h


-  const PlanningContextManager& context_manager_;
+  ModelBasedPlanningContext* context_;


I understand that context_ can't be const, but why can't it be a reference rather than a raw pointer? Why not use a shared_ptr if a reference is not possible?

I believe because we are now constructing the constraints library from inside the ModelBasedPlanningContext, with a call to this (a9e91ab#diff-d949b241fc2b4781b617fdb53844ece5R85). Since this is naturally a raw pointer, I just kept it like that, and am a little hesitant to try to convert this to a reference or a shared pointer.

BryceStevenWilley · 2019-11-20T20:38:41Z

Passing Travis, and 2 reviews, in it goes!

mlautman · 2019-11-23T05:26:22Z

I believe this PR introduced regressions.

Loading a constraint database now results in

ros.moveit_core.planning_request_adapter: Exception caught executing *final* adapter 'Fix Start State Path Constraints': Character [-] at element [10] is not valid in Graph Resource Name [/home/mike-picknik/.../cadb].  Valid characters are a-z, A-Z, 0-9, / and _.

Fixup for #1428.

BryceStevenWilley changed the title ~~WIP: Constraints Library Refactor.~~ Constraints Library Refactor. Apr 9, 2019

BryceStevenWilley changed the title ~~Constraints Library Refactor.~~ Constraints Library Refactor Apr 9, 2019

BryceStevenWilley marked this pull request as ready for review April 9, 2019 02:11

BryceStevenWilley force-pushed the ompl_plugin_interface branch from 9d25f40 to d161b1e Compare April 9, 2019 02:19

henningkayser reviewed May 6, 2019

View reviewed changes

...it_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h Outdated Show resolved Hide resolved

v4hn reviewed Jun 5, 2019

View reviewed changes

davetcoleman reviewed Jul 3, 2019

View reviewed changes

davetcoleman requested a review from ommmid July 3, 2019 00:19

BryceStevenWilley force-pushed the ompl_plugin_interface branch from 9e65649 to f6f7b8f Compare August 16, 2019 03:01

BryceStevenWilley requested review from davetcoleman and henningkayser August 16, 2019 03:02

davetcoleman approved these changes Aug 21, 2019

View reviewed changes

davetcoleman mentioned this pull request Aug 21, 2019

Add OMPL planner 'AnytimePathShortening' #1629

Closed

BryceStevenWilley added the awaits 2nd review one maintainer approved this request label Sep 16, 2019

BryceStevenWilley added 6 commits November 20, 2019 12:00

Moved constraints library internal to the ModelBasedPlanningContext

a9e91ab

In preparation to a larger change that moves much of the context initialization internal to the ModelBasedPlanningContext as well.

Removed unused OMPLPlannerService.

08aad1f

Fixed tests since ModelBasedStateSpace is now virtual

fa1725f

Corrected switched comments

2d81cde

Addressed PR comments

c3c2f0f

Fixed compile issues, and got construct database to work.

d503692

Also added launch file for ease of use.

BryceStevenWilley force-pushed the ompl_plugin_interface branch from f6f7b8f to d503692 Compare November 20, 2019 17:02

BryceStevenWilley requested a review from mamoll as a code owner November 20, 2019 17:02

mamoll reviewed Nov 20, 2019

View reviewed changes

Only install the generate state db script to CATKIN_PACKAGE_BIN_DEST

ba8ef74

mamoll self-requested a review November 20, 2019 18:07

mamoll approved these changes Nov 20, 2019

View reviewed changes

BryceStevenWilley removed the awaits 2nd review one maintainer approved this request label Nov 20, 2019

BryceStevenWilley merged commit cd202ab into moveit:master Nov 20, 2019

BryceStevenWilley deleted the ompl_plugin_interface branch November 20, 2019 20:39

mlautman mentioned this pull request Nov 23, 2019

fixing CADB loading bug #1768

Merged

rhaschke pushed a commit that referenced this pull request Nov 23, 2019

Fix infinite recursive call to loadConstraintApproximations (#1768)

1c770f3

Fixup for #1428.

rhaschke pushed a commit that referenced this pull request Nov 24, 2019

Fix infinite recursive call to loadConstraintApproximations (#1768)

fd82ce5

Fixup for #1428.

lepalom mentioned this pull request Jun 17, 2021

Using OMPL plugin #2723

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Constraints Library Refactor #1428

Constraints Library Refactor #1428

BryceStevenWilley commented Apr 8, 2019 •

edited

Loading

BryceStevenWilley commented Apr 9, 2019

BryceStevenWilley commented Apr 16, 2019

henningkayser left a comment

BryceStevenWilley commented May 15, 2019

v4hn left a comment

v4hn Jun 5, 2019

BryceStevenWilley Aug 16, 2019

BryceStevenWilley commented Jun 12, 2019

davetcoleman left a comment

davetcoleman commented Jul 3, 2019

davetcoleman commented Jul 24, 2019

BryceStevenWilley commented Jul 24, 2019

BryceStevenWilley commented Jul 29, 2019

BryceStevenWilley commented Aug 16, 2019

davetcoleman commented Aug 21, 2019

mamoll Nov 20, 2019

BryceStevenWilley Nov 20, 2019

BryceStevenWilley commented Nov 20, 2019

mlautman commented Nov 23, 2019


		const PlanningContextManager& context_manager_;
		ModelBasedPlanningContext* context_;

Constraints Library Refactor #1428

Constraints Library Refactor #1428

Conversation

BryceStevenWilley commented Apr 8, 2019 • edited Loading

Checklist

BryceStevenWilley commented Apr 9, 2019

BryceStevenWilley commented Apr 16, 2019

henningkayser left a comment

Choose a reason for hiding this comment

BryceStevenWilley commented May 15, 2019

v4hn left a comment

Choose a reason for hiding this comment

v4hn Jun 5, 2019

Choose a reason for hiding this comment

BryceStevenWilley Aug 16, 2019

Choose a reason for hiding this comment

BryceStevenWilley commented Jun 12, 2019

davetcoleman left a comment

Choose a reason for hiding this comment

davetcoleman commented Jul 3, 2019

davetcoleman commented Jul 24, 2019

BryceStevenWilley commented Jul 24, 2019

BryceStevenWilley commented Jul 29, 2019

BryceStevenWilley commented Aug 16, 2019

davetcoleman commented Aug 21, 2019

mamoll Nov 20, 2019

Choose a reason for hiding this comment

BryceStevenWilley Nov 20, 2019

Choose a reason for hiding this comment

BryceStevenWilley commented Nov 20, 2019

mlautman commented Nov 23, 2019

BryceStevenWilley commented Apr 8, 2019 •

edited

Loading