From b567e6e04db25cc35fc3624bbb9ab67c892e533f Mon Sep 17 00:00:00 2001 From: Zak Kingston Date: Mon, 1 Nov 2021 14:05:26 -0500 Subject: [PATCH 1/5] Added optimization objective defaults to OMPL planner + hybridize / interpolate setters --- .../include/robowflex_ompl/ompl_interface.h | 26 ++++++++++++ robowflex_ompl/src/ompl_interface.cpp | 40 +++++++++++++++++++ 2 files changed, 66 insertions(+) diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 7924eb280..96e0e79b9 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -115,7 +115,33 @@ 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. + */ + void useMaxMinClearanceObjective(); + private: + /** \brief Optimization objectives for planning. */ + enum Objective + { + PATH_LENGTH, ///< Minimize path length objective. + MAX_MIN_CLEARANCE ///< Maximize minimum clearance objective. + }; + + Objective objective_{PATH_LENGTH}; ///< Optimization objective to use. std::unique_ptr interface_{nullptr}; ///< Planning interface. std::vector configs_; ///< Planning configurations. bool hybridize_; ///< Whether or not planner should hybridize solutions. diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index f80d0f66e..f56347883 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -1,3 +1,8 @@ +/* Author: Zachary Kingston */ + +#include +#include + #include #include @@ -129,6 +134,21 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, ss_ = context_->getOMPLSimpleSetup(); + // Set desired default optimization objective + ompl::base::OptimizationObjectivePtr obj; + switch (objective_) + { + case MAX_MIN_CLEARANCE: + obj = std::make_shared(ss_->getSpaceInformation()); + break; + case PATH_LENGTH: + default: + obj = std::make_shared(ss_->getSpaceInformation()); + break; + } + + ss_->getProblemDefinition()->setOptimizationObjective(obj); + last_scene_id_ = scene_id; last_request_hash_ = request_hash; @@ -158,3 +178,23 @@ void OMPL::OMPLInterfacePlanner::setPrePlanCallback(const PrePlanCallback &prePl { 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::useMaxMinClearanceObjective() +{ + objective_ = MAX_MIN_CLEARANCE; +} From 6567917e9f75ac3ec4056d84534ca10e5dbc30d4 Mon Sep 17 00:00:00 2001 From: Zachary Kingston Date: Tue, 2 Nov 2021 18:35:39 -0500 Subject: [PATCH 2/5] hybridization and query settings + testing --- .../include/robowflex_library/planning.h | 1 + robowflex_library/src/planning.cpp | 2 ++ .../include/robowflex_ompl/ompl_interface.h | 6 +++- .../scripts/fetch_ompl_scenes_benchmark.cpp | 25 +++++++++++----- robowflex_ompl/src/ompl_interface.cpp | 30 +++++++++++++++---- 5 files changed, 49 insertions(+), 15 deletions(-) diff --git a/robowflex_library/include/robowflex_library/planning.h b/robowflex_library/include/robowflex_library/planning.h index 71b4b1867..5f4ab09a6 100644 --- a/robowflex_library/include/robowflex_library/planning.h +++ b/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. diff --git a/robowflex_library/src/planning.cpp b/robowflex_library/src/planning.cpp index a7f33dfb7..9d33407f3 100644 --- a/robowflex_library/src/planning.cpp +++ b/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); diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 96e0e79b9..a92919590 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -130,8 +130,10 @@ namespace robowflex 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(); + void useMaxMinClearanceObjective(double weight = 0.5); private: /** \brief Optimization objectives for planning. */ @@ -141,7 +143,9 @@ namespace robowflex MAX_MIN_CLEARANCE ///< Maximize minimum clearance objective. }; + double clearance_objective_weight_{0.5}; ///< Default weight to use in clearance objectives. Objective objective_{PATH_LENGTH}; ///< Optimization objective to use. + std::unique_ptr interface_{nullptr}; ///< Planning interface. std::vector configs_; ///< Planning configurations. bool hybridize_; ///< Whether or not planner should hybridize solutions. diff --git a/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp b/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp index bdfff2627..047458188 100644 --- a/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp +++ b/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(fetch); - planner->initialize(); + auto default_planner = std::make_shared(fetch, "default"); + default_planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml"); + + auto clearance_planner = std::make_shared(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(planner, GROUP); + auto request = std::make_shared(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); diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index f56347883..a6f135a43 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -1,5 +1,6 @@ /* Author: Zachary Kingston */ +#include #include #include @@ -133,21 +134,35 @@ 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 MAX_MIN_CLEARANCE: - obj = std::make_shared(ss_->getSpaceInformation()); + { + auto pl_obj = std::make_shared(si); + auto cl_obj = std::make_shared(si); + + auto multi_obj = std::make_shared(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(ss_->getSpaceInformation()); + obj = std::make_shared(si); break; } - ss_->getProblemDefinition()->setOptimizationObjective(obj); + // Set optimization objective in problem definition and simplifier + auto pdef = ss_->getProblemDefinition(); + pdef->setOptimizationObjective(obj); + ss_->getPathSimplifier() = std::make_shared(si, pdef->getGoal(), obj); last_scene_id_ = scene_id; last_request_hash_ = request_hash; @@ -168,9 +183,8 @@ std::vector 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_; } @@ -194,7 +208,11 @@ void OMPL::OMPLInterfacePlanner::usePathLengthObjective() objective_ = PATH_LENGTH; } -void OMPL::OMPLInterfacePlanner::useMaxMinClearanceObjective() +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; } From 0f5862b88fdc8dd3d9bdd18ed79ae3c02b550e08 Mon Sep 17 00:00:00 2001 From: Zak Kingston Date: Wed, 3 Nov 2021 10:05:55 -0500 Subject: [PATCH 3/5] Add toggle for simplification objective --- .../include/robowflex_ompl/ompl_interface.h | 10 ++++++++-- robowflex_ompl/src/ompl_interface.cpp | 13 +++++++++++-- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index a92919590..5203320d1 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -135,6 +135,11 @@ namespace robowflex */ void useMaxMinClearanceObjective(double weight = 0.5); + /** \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 @@ -143,8 +148,9 @@ namespace robowflex MAX_MIN_CLEARANCE ///< Maximize minimum clearance objective. }; - double clearance_objective_weight_{0.5}; ///< Default weight to use in clearance objectives. - Objective objective_{PATH_LENGTH}; ///< Optimization objective to use. + 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. std::unique_ptr interface_{nullptr}; ///< Planning interface. std::vector configs_; ///< Planning configurations. diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index a6f135a43..d0beb81f4 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -159,10 +159,14 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, break; } - // Set optimization objective in problem definition and simplifier + // Set optimization objective in problem definition auto pdef = ss_->getProblemDefinition(); pdef->setOptimizationObjective(obj); - ss_->getPathSimplifier() = std::make_shared(si, pdef->getGoal(), obj); + + // Set objective in path simplifier + if (use_objective_simplifier_) + ss_->getPathSimplifier() = + std::make_shared(si, pdef->getGoal(), obj); last_scene_id_ = scene_id; last_request_hash_ = request_hash; @@ -208,6 +212,11 @@ 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.) From 018539524b86ac4dd2a86a0af82ef271b2fc9b85 Mon Sep 17 00:00:00 2001 From: Zachary Kingston Date: Wed, 15 Dec 2021 14:32:34 -0600 Subject: [PATCH 4/5] Added custom cost function callback --- .../include/robowflex_ompl/ompl_interface.h | 16 ++++++++++++++-- robowflex_ompl/src/ompl_interface.cpp | 10 ++++++++++ 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 5203320d1..20d1b78bd 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -135,6 +135,16 @@ namespace robowflex */ void useMaxMinClearanceObjective(double weight = 0.5); + /** \brief Allocator function that allocates a new custom optimization objective. + */ + using OptimizationObjectiveAllocator = + std::function; + + /** \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. */ @@ -144,13 +154,15 @@ namespace robowflex /** \brief Optimization objectives for planning. */ enum Objective { - PATH_LENGTH, ///< Minimize path length objective. - MAX_MIN_CLEARANCE ///< Maximize minimum clearance 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 interface_{nullptr}; ///< Planning interface. std::vector configs_; ///< Planning configurations. diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index d0beb81f4..451438b0a 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -140,6 +140,10 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, ompl::base::OptimizationObjectivePtr obj; switch (objective_) { + case CUSTOM: + obj = custom_objective_(si); + break; + case MAX_MIN_CLEARANCE: { auto pl_obj = std::make_shared(si); @@ -225,3 +229,9 @@ void OMPL::OMPLInterfacePlanner::useMaxMinClearanceObjective(double weight) objective_ = MAX_MIN_CLEARANCE; clearance_objective_weight_ = weight; } + +void OMPL::OMPLInterfacePlanner::useCustomObjective(const OptimizationObjectiveAllocator &allocator) +{ + objective_ = CUSTOM; + custom_objective_ = allocator; +} From 80688bf4d9a29f8b2ddb2a51ea342553aaed8086 Mon Sep 17 00:00:00 2001 From: Zachary Kingston Date: Wed, 15 Dec 2021 14:48:45 -0600 Subject: [PATCH 5/5] format --- robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp b/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp index 047458188..d00b776ea 100644 --- a/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp +++ b/robowflex_ompl/scripts/fetch_ompl_scenes_benchmark.cpp @@ -32,7 +32,7 @@ int main(int argc, char **argv) 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 + experiment.enableMultipleRequests(); // Enable multiple requests for hybridization const std::size_t start = 1; const std::size_t end = 3;