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 7924eb280..20d1b78bd 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/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; + + /** \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 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..d00b776ea 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 f80d0f66e..451438b0a 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -1,3 +1,9 @@ +/* Author: Zachary Kingston */ + +#include +#include +#include + #include #include @@ -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(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(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(si, pdef->getGoal(), obj); last_scene_id_ = scene_id; last_request_hash_ = request_hash; @@ -148,9 +191,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_; } @@ -158,3 +200,38 @@ 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::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; +}