WIP: Optimization Objective Setters in OMPL Interface

robowflex_library/include/robowflex_library/planning.h

@@ -314,6 +314,7 @@ namespace robowflex
             bool simplify_solutions;                ///< Whether or not planner should simplify solutions.
             bool hybridize_solutions;               ///< Whether or not planner should hybridize solutions.
             bool interpolate_solutions;             ///< Whether or not planner should interpolate solutions.
+            bool multi_query_planning_enabled;      ///< Whether or not to simultaneously plan.
             bool use_constraints_approximations;    ///< Absolute silliness.
             bool display_random_valid_states;       ///< N/A, defunct.
             std::string link_for_exploration_tree;  ///< N/A, defunct.

robowflex_library/src/planning.cpp

@@ -307,6 +307,7 @@ OMPL::Settings::Settings()
   , simplify_solutions(true)
   , hybridize_solutions(true)
   , interpolate_solutions(true)
+  , multi_query_planning_enabled(false)
   , use_constraints_approximations(false)
   , display_random_valid_states(false)
   , link_for_exploration_tree("")
@@ -324,6 +325,7 @@ void OMPL::Settings::setParam(IO::Handler &handler) const
     handler.setParam(prefix + "max_state_sampling_attempts", max_state_sampling_attempts);
     handler.setParam(prefix + "minimum_waypoint_count", minimum_waypoint_count);
     handler.setParam(prefix + "simplify_solutions", simplify_solutions);
+    handler.setParam(prefix + "multi_query_planning_enabled", multi_query_planning_enabled);
     handler.setParam(prefix + "use_constraints_approximations", use_constraints_approximations);
     handler.setParam(prefix + "display_random_valid_states", display_random_valid_states);
     handler.setParam(prefix + "link_for_exploration_tree", link_for_exploration_tree);

robowflex_ompl/include/robowflex_ompl/ompl_interface.h

@@ -115,7 +115,55 @@ namespace robowflex
              */
             void setPrePlanCallback(const PrePlanCallback &prePlanCallback);
 
+            /** \brief Sets whether or not the planner should hybridize solutions.
+             *  \param[in] hybridize If true, planner will hybridize solutions.
+             */
+            void setHybridize(bool hybridize);
+
+            /** \brief Sets whether or not the planner should interpolate solutions.
+             *  \param[in] interpolate If true, planner will interpolate solutions.
+             */
+            void setInterpolate(bool interpolate);
+
+            /** \brief Sets default optimization objective to minimize path length.
+             */
+            void usePathLengthObjective();
+
+            /** \brief Sets default optimization objective to maximize the minimum path clearance.
+             *  \param[in] weight The weight of clearance versus path length in the objective function. 1
+             *                    means all clearance, 0 means all path length.
+             */
+            void useMaxMinClearanceObjective(double weight = 0.5);
+
+            /** \brief Allocator function that allocates a new custom optimization objective.
+             */
+            using OptimizationObjectiveAllocator =
+                std::function<ompl::base::OptimizationObjectivePtr(const ompl::base::SpaceInformationPtr &)>;
+
+            /** \brief Use a custom objective function that is created from a provided allocator function.
+             *  \param[in] allocator Allocator function.
+             */
+            void useCustomObjective(const OptimizationObjectiveAllocator &allocator);
+
+            /** \brief Sets whether to use the optimization objective for the path simplifier.
+             *  \param[in] use_objective If true, planner will set simplification objective.
+             */
+            void setObjectiveSimplifier(bool use_objective);
+
         private:
+            /** \brief Optimization objectives for planning. */
+            enum Objective
+            {
+                PATH_LENGTH,        ///< Minimize path length objective.
+                MAX_MIN_CLEARANCE,  ///< Maximize minimum clearance objective.
+                CUSTOM              ///< Custom objective.
+            };
+
+            bool use_objective_simplifier_{false};    ///< Use the optimization objective for the simplifier.
+            double clearance_objective_weight_{0.5};  ///< Default weight to use in clearance objectives.
+            Objective objective_{PATH_LENGTH};        ///< Optimization objective to use.
+            OptimizationObjectiveAllocator custom_objective_;  ///< Custom objective allocator.
+
             std::unique_ptr<ompl_interface::OMPLInterface> interface_{nullptr};  ///< Planning interface.
             std::vector<std::string> configs_;                                   ///< Planning configurations.
             bool hybridize_;    ///< Whether or not planner should hybridize solutions.

robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp

@@ -29,11 +29,13 @@ int main(int argc, char **argv)
 
     // Setup a benchmarking request for the joint and pose motion plan requests.
     Profiler::Options options;
-    options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH;
-    Experiment experiment("fetch_scenes", options, 30.0, 2);
+    options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::CLEARANCE;
+
+    Experiment experiment("fetch_scenes", options, 30.0, 10);
+    experiment.enableMultipleRequests();  // Enable multiple requests for hybridization
 
     const std::size_t start = 1;
-    const std::size_t end = 10;
+    const std::size_t end = 3;
     for (std::size_t i = start; i <= end; i++)
     {
         const auto &scene_file =
@@ -50,21 +52,28 @@ int main(int argc, char **argv)
         }
 
         // Create the default planner for the Fetch.
-        auto planner = std::make_shared<OMPL::FetchOMPLPipelinePlanner>(fetch);
-        planner->initialize();
+        auto default_planner = std::make_shared<OMPL::OMPLInterfacePlanner>(fetch, "default");
+        default_planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml");
+
+        auto clearance_planner = std::make_shared<OMPL::OMPLInterfacePlanner>(fetch, "clearance");
+        clearance_planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml");
+        // clearance_planner->setHybridize(true);
+        clearance_planner->useMaxMinClearanceObjective();
 
         // Create an empty motion planning request.
-        auto request = std::make_shared<robowflex::MotionRequestBuilder>(planner, GROUP);
+        auto request = std::make_shared<robowflex::MotionRequestBuilder>(fetch, GROUP);
         if (not request->fromYAMLFile(request_file))
         {
             RBX_ERROR("Failed to read file: %s for request", request_file);
             continue;
         }
 
-        request->setConfig("PRMstar");
+        request->setNumPlanningAttempts(4);
+        request->setConfig("RRTConnectkConfigDefault");
 
         // Add request
-        experiment.addQuery(log::format("scene%1$04d", i), scene, planner, request);
+        experiment.addQuery("scene_default", scene, default_planner, request);
+        experiment.addQuery("scene_clearance", scene, clearance_planner, request);
     }
 
     auto dataset = experiment.benchmark(1);

robowflex_ompl/src/ompl_interface.cpp

@@ -1,3 +1,9 @@
+/* Author: Zachary Kingston */
+
+#include <ompl/geometric/PathSimplifier.h>
+#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
+#include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
+
 #include <moveit/ompl_interface/model_based_planning_context.h>
 
 #include <robowflex_library/macros.h>
@@ -128,6 +134,43 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene,
 #endif
 
     ss_ = context_->getOMPLSimpleSetup();
+    const auto &si = ss_->getSpaceInformation();
+
+    // Set desired default optimization objective
+    ompl::base::OptimizationObjectivePtr obj;
+    switch (objective_)
+    {
+        case CUSTOM:
+            obj = custom_objective_(si);
+            break;
+
+        case MAX_MIN_CLEARANCE:
+        {
+            auto pl_obj = std::make_shared<ompl::base::PathLengthOptimizationObjective>(si);
+            auto cl_obj = std::make_shared<ompl::base::MaximizeMinClearanceObjective>(si);
+
+            auto multi_obj = std::make_shared<ompl::base::MultiOptimizationObjective>(si);
+            multi_obj->addObjective(pl_obj, 1. - clearance_objective_weight_);
+            multi_obj->addObjective(cl_obj, clearance_objective_weight_);
+            multi_obj->lock();
+
+            obj = multi_obj;
+            break;
+        }
+        case PATH_LENGTH:
+        default:
+            obj = std::make_shared<ompl::base::PathLengthOptimizationObjective>(si);
+            break;
+    }
+
+    // Set optimization objective in problem definition
+    auto pdef = ss_->getProblemDefinition();
+    pdef->setOptimizationObjective(obj);
+
+    // Set objective in path simplifier
+    if (use_objective_simplifier_)
+        ss_->getPathSimplifier() =
+            std::make_shared<ompl::geometric::PathSimplifier>(si, pdef->getGoal(), obj);
 
     last_scene_id_ = scene_id;
     last_request_hash_ = request_hash;
@@ -148,13 +191,47 @@ std::vector<std::string> OMPL::OMPLInterfacePlanner::getPlannerConfigs() const
 ompl_interface::OMPLInterface &OMPL::OMPLInterfacePlanner::getInterface() const
 {
     if (!interface_)
-    {
         RBX_WARN("Interface is not initialized before call to OMPLInterfacePlanner::initialize.");
-    }
+
     return *interface_;
 }
 
 void OMPL::OMPLInterfacePlanner::setPrePlanCallback(const PrePlanCallback &prePlanCallback)
 {
     pre_plan_callback_ = prePlanCallback;
 }
+
+void OMPL::OMPLInterfacePlanner::setHybridize(bool hybridize)
+{
+    hybridize_ = hybridize;
+}
+
+void OMPL::OMPLInterfacePlanner::setInterpolate(bool interpolate)
+{
+    interpolate_ = interpolate;
+}
+
+void OMPL::OMPLInterfacePlanner::usePathLengthObjective()
+{
+    objective_ = PATH_LENGTH;
+}
+
+void OMPL::OMPLInterfacePlanner::setObjectiveSimplifier(bool use_objective)
+{
+    use_objective_simplifier_ = use_objective;
+}
+
+void OMPL::OMPLInterfacePlanner::useMaxMinClearanceObjective(double weight)
+{
+    if (weight > 1. or weight < 0.)
+        throw std::runtime_error("Weight must be [0, 1]");
+
+    objective_ = MAX_MIN_CLEARANCE;
+    clearance_objective_weight_ = weight;
+}
+
+void OMPL::OMPLInterfacePlanner::useCustomObjective(const OptimizationObjectiveAllocator &allocator)
+{
+    objective_ = CUSTOM;
+    custom_objective_ = allocator;
+}