WIP: Feature/ompl rrt #86

marrts · 2021-09-09T22:46:15Z

OMPL interface for Descartes, still a work in progress, needs some cleanup but working at this point.

descartes_light/solvers/include/descartes_light/solvers/ompl/descartes_space.h

marip8 · 2021-09-13T15:12:39Z

descartes_light/solvers/include/descartes_light/solvers/ompl/descartes_space.h

+    /** \brief A state space representing the Descartes boost graph. The distance function is determined by the difference
+     * in rungs and indices unless the rungs are adjacent, then an actual cost is made calculated using an edge evaluator */


Suggested change

/** \brief A state space representing the Descartes boost graph. The distance function is determined by the difference

* in rungs and indices unless the rungs are adjacent, then an actual cost is made calculated using an edge evaluator */

/** \brief An OMPL state space representing the Descartes planning graph.

* \details A state in this space is pair of integers representing the rung and sample indices of each Descartes state in the planning graph.

* The distance function is determined by the difference in rung and sample indices. If the rungs are adjacent, then the actual cost is calculated using the appropriate edge evaluator */

Please add some more details about what this state space represents and how it works

Added some more details, let me know what you think

descartes_light/solvers/include/descartes_light/solvers/ompl/descartes_space.h

descartes_light/solvers/include/descartes_light/solvers/ompl/ompl_types.h

descartes_light/solvers/include/descartes_light/solvers/ompl/ompl_solver.h

marip8 · 2021-09-13T16:15:10Z

descartes_light/solvers/include/descartes_light/solvers/ompl/ompl_solver.h

+  SearchResult<FloatType> ompl_search(std::shared_ptr<ompl::base::Planner> ompl_planner);
+
+  void initOMPL();


Do these need to be separate from the constructor and/or search method?

I could combine initOMPL() with a redefinition of the constructor.

The ompl_search method is setup this way because I wanted the classes that inherit from it to easily call it without having to re-write everything in it. Also, it requires an input of a planner type, so it can't just inherit from search. This can't be just templated on ompl::base::Planner either because each planner has a unique set of parameters, so using the planner type as an input makes more sense to me.

marip8 · 2021-09-13T16:18:36Z

descartes_light/solvers/include/descartes_light/solvers/ompl/ompl_solver.h

+/**
+ * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
+ * algorithm with a default visitor to search the graph
+ */
+template <typename FloatType>
+class BGLOMPLRRTSolver : public BGLOMPLSolver<FloatType>
+{
+public:
+  using BGLOMPLSolver<FloatType>::BGLOMPLSolver;
+
+  SearchResult<FloatType> search() override;
+};
+
+using BGLOMPLRRTSolverF = BGLOMPLRRTSolver<float>;
+using BGLOMPLRRTSolverD = BGLOMPLRRTSolver<double>;
+
+template <typename FloatType>
+class BGLOMPLRRTConnectSolver : public BGLOMPLSolver<FloatType>
+{
+public:
+  using BGLOMPLSolver<FloatType>::BGLOMPLSolver;
+
+  SearchResult<FloatType> search() override;
+};
+
+using BGLOMPLRRTConnectSolverF = BGLOMPLRRTConnectSolver<float>;
+using BGLOMPLRRTConnectSolverD = BGLOMPLRRTConnectSolver<double>;


We should think about a way to make the specification of the search algorithm generic so we don't need to define new classes for every search algorithm we want to use. Depending on how exactly OMPL works, we could pass a search algorithm pointer into the constructor, or we could template this class on the type of search algorithm that should be used.

One concern of mine is I'm not sure how many ompl algorithms will actually work with this setup, but I know RRT and RRTConnect do work.

Your point still stands that it would be nice if one could just pass the ompl planner type. Unfortunately each planner has it's own unique set of parameters, so we would realistically need to account for every planner similar to how it's done in Tesseract. I think a better solution is to provide a method to getSpaceInformation() and getMaxDistance() from the base OMPLSolver class, which would allow a user to create the own instantiation of an ompl planner which then could be used to call ompl_search. It would look exactly like what is done in the search() method in OMPLRRTSolver and OMPLRRTConnectSolver but since it requires an input I think it should be kept separate from 'search()'

marip8 · 2021-09-13T16:22:58Z

descartes_light/solvers/include/descartes_light/solvers/ompl/impl/ompl_solver.hpp

+                                                                           max_dist_,
+                                                                           rung_to_rung_dist_);
+
+  dss_->setLongestValidSegmentFraction(0.5);


Should this be parameterized?

So I don't think this actually does anything in the context of how I have the DescartesStateSpace methods defined, but it is required for OMPL. Basically it takes the maximum extent from the state space and multiplies by this factor. Any segment longer than this is checked at intervals equal to this length. We only ever check from point A to point B in Descartes, so I just set this to be a high value. (It also cannot be 1.0 or 0.0)

marrts · 2021-09-13T18:15:41Z

I'm going to go ahead and convert this so that it doesn't have any boost dependencies, then I'll rebase off the re-organization that was recently merged. From there we can tackle some of your specific feedback points more directly.

marrts · 2021-09-14T04:19:10Z

Updated to remove all boost dependencies and rebased to emulate the new directory structure.

I think I fixed all of your minor code tidiness concerns and I addressed all of the minor change recommendations.

I made some more detailed comments and tried to address most of your concerns about how I was describing things. Hopefully it is more clear what the various functions do now.

The biggest questions out there are the issue of max_dist_ and rung_to_rung_dist_, and how we plan to move forward with handling the interface, whether we keep just the RRT and RRTConnect built-in ones like I have now or try to move towards some sort of templating or passing in a pointer to an ompl planner type.

Levi-Armstrong · 2021-11-02T22:28:43Z

@marip8, @marrts, and @colin-lewis-19 what is left for this to be merged?

marrts · 2021-11-03T14:47:31Z

The ompl planners crash when running the unit tests and I haven't been able to figure out why.

…in first and last created additional rungs

Levi-Armstrong · 2023-07-03T21:25:43Z

What is the current status of this MR?

marrts · 2023-07-03T21:32:40Z

I haven't looked at this in a while, and I'm not aware of any plans for anyone to actually use this. I think it can be closed, unless someone wants to do a little bit of clean up work to merge it in.

Levi-Armstrong · 2023-07-03T21:35:56Z

I haven't looked at this in a while, and I'm not aware of any plans for anyone to actually use this. I think it can be closed, unless someone wants to do a little bit of clean up work to merge it in.

Didn't this perform better than the BGL version?

marrts · 2023-07-03T21:42:36Z

Didn't this perform better than the BGL version?

It had some really positive results, but it can be finicky. In the application, with a really large search space, we originally had in mind for it we did not end up using it as I could never get that good of results. I think it needs some refinements. Usually it would either converge very fast to a feasible solution, or timeout trying.

marip8 requested changes Sep 13, 2021

View reviewed changes

Tyler Marr and others added 12 commits September 13, 2021 16:48

Working ompl search using idx diff for distance

d630d5e

Working with edge evaluator passed through

712c538

Passes unit test with RRT

337c6a0

Working RRT Connect

9a11af4

Re-organized to nicer structure

8f38365

Commented descartes state space code

6e3cdce

Added comments to ompl solver code

5e5ba1f

clang formatting

7be480d

Added ignore warnings for ompl planner

089176a

Change from pair of ints to rung and idx defined separately

a7b889e

Removed boost graph dependencies from ompl solver

ebb8043

Clang formatting

33e8b5f

marrts force-pushed the feature/ompl-rrt branch from 4a9fde6 to 33e8b5f Compare September 13, 2021 22:11

Code cleanup and more detailed descriptions of classes and functions

21f8e6f

Fixed bug when rung sizes are different and fixed issue with nullptr …

ed5e07d

…in first and last created additional rungs

Levi-Armstrong mentioned this pull request Feb 7, 2024

RRT and OMPL interfaces #112

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

WIP: Feature/ompl rrt #86

WIP: Feature/ompl rrt #86

marrts commented Sep 9, 2021

marip8 Sep 13, 2021

marrts Sep 14, 2021

marip8 Sep 13, 2021

marrts Sep 13, 2021

marip8 Sep 13, 2021

marrts Sep 13, 2021

marip8 Sep 13, 2021

marrts Sep 13, 2021

marrts commented Sep 13, 2021

marrts commented Sep 14, 2021 •

edited

Loading

Levi-Armstrong commented Nov 2, 2021

marrts commented Nov 3, 2021

Levi-Armstrong commented Jul 3, 2023

marrts commented Jul 3, 2023

Levi-Armstrong commented Jul 3, 2023

marrts commented Jul 3, 2023

		/** \brief A state space representing the Descartes boost graph. The distance function is determined by the difference
		* in rungs and indices unless the rungs are adjacent, then an actual cost is made calculated using an edge evaluator */

-    /** \brief A state space representing the Descartes boost graph. The distance function is determined by the difference
-     * in rungs and indices unless the rungs are adjacent, then an actual cost is made calculated using an edge evaluator */
+    /** \brief An OMPL state space representing the Descartes planning graph.
+      * \details A state in this space is pair of integers representing the rung and sample indices of each Descartes state in the planning graph.
+      * The distance function is determined by the difference in rung and sample indices. If the rungs are adjacent, then the actual cost is calculated using the appropriate edge evaluator */

		SearchResult<FloatType> ompl_search(std::shared_ptr<ompl::base::Planner> ompl_planner);

		void initOMPL();

WIP: Feature/ompl rrt #86

Are you sure you want to change the base?

WIP: Feature/ompl rrt #86

Conversation

marrts commented Sep 9, 2021

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

marrts commented Sep 13, 2021

marrts commented Sep 14, 2021 • edited Loading

Levi-Armstrong commented Nov 2, 2021

marrts commented Nov 3, 2021

Levi-Armstrong commented Jul 3, 2023

marrts commented Jul 3, 2023

Levi-Armstrong commented Jul 3, 2023

marrts commented Jul 3, 2023

marrts commented Sep 14, 2021 •

edited

Loading