From 18a19b0fb28918ba72bba96c88a88d547f1bbb1c Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Mon, 13 Jul 2020 19:37:11 +0200 Subject: [PATCH 01/13] add comments --- .../ompl/ompl_interface/src/planning_context_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 44b15cee8b..845ab133b1 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -341,7 +341,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana context_spec.config_ = config.config; context_spec.planner_selector_ = getPlannerSelector(); context_spec.constraint_sampler_manager_ = constraint_sampler_manager_; - context_spec.state_space_ = factory->getNewStateSpace(space_spec); + context_spec.state_space_ = + factory->getNewStateSpace(space_spec); /* only line where getNewStateSpace is called?? */ // Choose the correct simple setup type to load context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); From 89a509ddd7c5868b4a579258abe590ed9264022c Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Wed, 15 Jul 2020 18:02:05 +0200 Subject: [PATCH 02/13] add new state space selection methods --- .../ompl_interface/planning_context_manager.h | 7 ++ .../src/planning_context_manager.cpp | 66 +++++++++++++++++++ 2 files changed, 73 insertions(+) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 594d3ba9dc..2d8696ea49 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -226,6 +226,13 @@ class PlanningContextManager const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory2(const std::string& group_name, const moveit_msgs::MotionPlanRequest& req) const; + /** \brief Check whether a joint model group has an inverse kinematics solver available. **/ + bool doesGroupHaveIKSolver(const std::string& group_name) const; + + ModelBasedStateSpacePtr selectAndCreateStateSpace(const moveit_msgs::MotionPlanRequest& req, + const ModelBasedStateSpaceSpecification& space_spec, + bool enforce_joint_model_state_space = false) const; + /** \brief The kinematic model for which motion plans are computed */ moveit::core::RobotModelConstPtr robot_model_; diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 845ab133b1..2d18394dce 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -412,6 +412,72 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo } } + bool ompl_interface::PlanningContextManager::doesGroupHaveIKSolver(const std::string& group_name) const + { + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); + if (jmg) + { + const std::pair& ik_solver_pair = jmg->getGroupKinematics(); + bool ik = false; + // check that we have a direct means to compute IK + if (ik_solver_pair.first) + { + ik = jmg->getVariableCount() == ik_solver_pair.first.bijection_.size(); + } + // or an IK solver for each of the subgroups + else if (!ik_solver_pair.second.empty()) + { + unsigned int vc = 0; + unsigned int bc = 0; + for (const auto& jt : ik_solver_pair.second) + { + vc += jt.first->getVariableCount(); + bc += jt.second.bijection_.size(); + } + if (vc == jmg->getVariableCount() && vc == bc) + ik = true; + } + return ik; + } + else + { + ROS_ERROR_NAMED("planning_context_manager", "planning group '%s' not found when testing for IK Solver.", + group_name.c_str()); + return false; + } + } + + ompl_interface::ModelBasedStateSpacePtr ompl_interface::PlanningContextManager::selectAndCreateStateSpace( + const moveit_msgs::MotionPlanRequest& req, const ompl_interface::ModelBasedStateSpaceSpecification& space_spec, + bool enforce_joint_model_state_space) const + { + // the user forced us to plan in joint space + if (enforce_joint_model_state_space) + { + return std::make_shared(space_spec); + } + + // if there are joint or visibility constraints, we need to plan in joint space + else if (req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty()) + { + return std::make_shared(space_spec); + } + + // if we have path constraints and an inverse kinematics solver, we prefer interpolating in pose space + else if ((!req.path_constraints.position_constraints.empty() || + !req.path_constraints.orientation_constraints.empty()) && + doesGroupHaveIKSolver(req.group_name)) + { + return std::make_shared(space_spec); + } + // in all other cases, plan in joint space + else + { + return std::make_shared(space_spec); + } + } + ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, bool use_constraints_approximation) const From 33faa9bf0fe6db49d955060d3c6fad5c1b8ca619 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Wed, 15 Jul 2020 19:28:35 +0200 Subject: [PATCH 03/13] split new state space selection method in ompl interface --- .../ompl_interface/planning_context_manager.h | 8 +- .../src/planning_context_manager.cpp | 125 ++++++++++-------- 2 files changed, 76 insertions(+), 57 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 2d8696ea49..a43a21da54 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -229,9 +229,11 @@ class PlanningContextManager /** \brief Check whether a joint model group has an inverse kinematics solver available. **/ bool doesGroupHaveIKSolver(const std::string& group_name) const; - ModelBasedStateSpacePtr selectAndCreateStateSpace(const moveit_msgs::MotionPlanRequest& req, - const ModelBasedStateSpaceSpecification& space_spec, - bool enforce_joint_model_state_space = false) const; + const std::string& selectStateSpaceType(const moveit_msgs::MotionPlanRequest& req, + bool enforce_joint_model_state_space = false) const; + + ModelBasedStateSpacePtr createStateSpace(const std::string& parameterization_type, + const ModelBasedStateSpaceSpecification& space_spec) const; /** \brief The kinematic model for which motion plans are computed */ moveit::core::RobotModelConstPtr robot_model_; diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 2d18394dce..6a17acbf2c 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -412,71 +412,88 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo } } - bool ompl_interface::PlanningContextManager::doesGroupHaveIKSolver(const std::string& group_name) const +bool ompl_interface::PlanningContextManager::doesGroupHaveIKSolver(const std::string& group_name) const +{ + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); + if (jmg) { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); - if (jmg) + const std::pair& ik_solver_pair = jmg->getGroupKinematics(); + bool ik = false; + // check that we have a direct means to compute IK + if (ik_solver_pair.first) { - const std::pair& ik_solver_pair = jmg->getGroupKinematics(); - bool ik = false; - // check that we have a direct means to compute IK - if (ik_solver_pair.first) - { - ik = jmg->getVariableCount() == ik_solver_pair.first.bijection_.size(); - } - // or an IK solver for each of the subgroups - else if (!ik_solver_pair.second.empty()) - { - unsigned int vc = 0; - unsigned int bc = 0; - for (const auto& jt : ik_solver_pair.second) - { - vc += jt.first->getVariableCount(); - bc += jt.second.bijection_.size(); - } - if (vc == jmg->getVariableCount() && vc == bc) - ik = true; - } - return ik; + ik = jmg->getVariableCount() == ik_solver_pair.first.bijection_.size(); } - else + // or an IK solver for each of the subgroups + else if (!ik_solver_pair.second.empty()) { - ROS_ERROR_NAMED("planning_context_manager", "planning group '%s' not found when testing for IK Solver.", - group_name.c_str()); - return false; + unsigned int vc = 0; + unsigned int bc = 0; + for (const auto& jt : ik_solver_pair.second) + { + vc += jt.first->getVariableCount(); + bc += jt.second.bijection_.size(); + } + if (vc == jmg->getVariableCount() && vc == bc) + ik = true; } + return ik; + } + else + { + ROS_ERROR_NAMED("planning_context_manager", "planning group '%s' not found when testing for IK Solver.", + group_name.c_str()); + return false; } +} - ompl_interface::ModelBasedStateSpacePtr ompl_interface::PlanningContextManager::selectAndCreateStateSpace( - const moveit_msgs::MotionPlanRequest& req, const ompl_interface::ModelBasedStateSpaceSpecification& space_spec, - bool enforce_joint_model_state_space) const +const std::string& ompl_interface::PlanningContextManager::selectStateSpaceType( + const moveit_msgs::MotionPlanRequest& req, bool enforce_joint_model_state_space) const +{ + // the user forced us to plan in joint space + if (enforce_joint_model_state_space) { - // the user forced us to plan in joint space - if (enforce_joint_model_state_space) - { - return std::make_shared(space_spec); - } + return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE; + } - // if there are joint or visibility constraints, we need to plan in joint space - else if (req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty()) - { - return std::make_shared(space_spec); - } + // if there are joint or visibility constraints, we need to plan in joint space + else if (req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty()) + { + return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE; + } - // if we have path constraints and an inverse kinematics solver, we prefer interpolating in pose space - else if ((!req.path_constraints.position_constraints.empty() || - !req.path_constraints.orientation_constraints.empty()) && - doesGroupHaveIKSolver(req.group_name)) - { - return std::make_shared(space_spec); - } - // in all other cases, plan in joint space - else - { - return std::make_shared(space_spec); - } + // if we have path constraints and an inverse kinematics solver, we prefer interpolating in pose space + else if ((!req.path_constraints.position_constraints.empty() || + !req.path_constraints.orientation_constraints.empty()) && + doesGroupHaveIKSolver(req.group_name)) + { + return ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE; + } + // in all other cases, plan in joint space + else + { + return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE; + } +} + +ompl_interface::ModelBasedStateSpacePtr ompl_interface::PlanningContextManager::createStateSpace( + const std::string& parameterization_type, const ModelBasedStateSpaceSpecification& space_spec) const +{ + if (parameterization_type == ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE) + { + return std::make_shared(space_spec); + } + else if (parameterization_type == ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE) + { + return std::make_shared(space_spec); } + else + { + ROS_ERROR_NAMED("planning_context_manager", "Unkown state space parameterization type '%s'", parameterization_type); + return ompl_interface::ModelBasedStateSpacePtr(); + } +} ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, From 11bb3c447980e48de9b538c8ed4fdc803b7c10dc Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Wed, 15 Jul 2020 19:39:24 +0200 Subject: [PATCH 04/13] use new state space selection method in ompl interface --- .../ompl_interface/planning_context_manager.h | 3 +- .../src/planning_context_manager.cpp | 36 +++++++++---------- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index a43a21da54..82e7dd234e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include #include @@ -218,7 +219,7 @@ class PlanningContextManager /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */ ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, - const StateSpaceFactoryTypeSelector& factory_selector, + const std::string& state_space_parameterization_type, const moveit_msgs::MotionPlanRequest& req) const; const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory1(const std::string& group_name, diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 6a17acbf2c..5afeae2175 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -70,6 +70,7 @@ #include #include #include +#include using namespace std::placeholders; @@ -311,16 +312,15 @@ void ompl_interface::PlanningContextManager::setPlannerConfigurations( ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( const planning_interface::PlannerConfigurationSettings& config, - const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& /*req*/) const + const std::string& state_space_parameterization_type, const moveit_msgs::MotionPlanRequest& req) const { - const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group); - // Check for a cached planning context ModelBasedPlanningContextPtr context; { std::unique_lock slock(cached_contexts_->lock_); - auto cached_contexts = cached_contexts_->contexts_.find(std::make_pair(config.name, factory->getType())); + auto cached_contexts = + cached_contexts_->contexts_.find(std::make_pair(config.name, state_space_parameterization_type)); if (cached_contexts != cached_contexts_->contexts_.end()) { for (const ModelBasedPlanningContextPtr& cached_context : cached_contexts->second) @@ -341,17 +341,14 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana context_spec.config_ = config.config; context_spec.planner_selector_ = getPlannerSelector(); context_spec.constraint_sampler_manager_ = constraint_sampler_manager_; - context_spec.state_space_ = - factory->getNewStateSpace(space_spec); /* only line where getNewStateSpace is called?? */ - - // Choose the correct simple setup type to load + context_spec.state_space_ = createStateSpace(state_space_parameterization_type, space_spec); context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); ROS_DEBUG_NAMED(LOGNAME, "Creating new planning context"); context.reset(new ModelBasedPlanningContext(config.name, context_spec)); { std::unique_lock slock(cached_contexts_->lock_); - cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context); + cached_contexts_->contexts_[std::make_pair(config.name, state_space_parameterization_type)].push_back(context); } } @@ -540,20 +537,19 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana // Check if sampling in JointModelStateSpace is enforced for this group by user. // This is done by setting 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml. // - // Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via IK. - // However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be flipped, - // leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling - // in JointModelStateSpace. - StateSpaceFactoryTypeSelector factory_selector; + // Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via + // IK. However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be + // flipped, leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection + // sampling in JointModelStateSpace. StateSpaceFactoryTypeSelector factory_selector; + bool enforce_joint_model_state_space = false; auto it = pc->second.config.find("enforce_joint_model_state_space"); - if (it != pc->second.config.end() && boost::lexical_cast(it->second)) - factory_selector = std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1, - JointModelStateSpace::PARAMETERIZATION_TYPE); - else - factory_selector = std::bind(&PlanningContextManager::getStateSpaceFactory2, this, std::placeholders::_1, req); + { + enforce_joint_model_state_space = true; + } - ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory_selector, req); + const std::string state_space_type = selectStateSpaceType(req, enforce_joint_model_state_space); + ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, state_space_type, req); if (context) { From 1b2985137ae6635166d32e57111089429b159fd5 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Wed, 15 Jul 2020 19:40:21 +0200 Subject: [PATCH 05/13] clang format licence in ompl_interface/planning_context_manager.h --- .../ompl_interface/planning_context_manager.h | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 82e7dd234e..20de8b55ef 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ From 51f6e14b876914f0bf745fb2e938e3a6be60b058 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Wed, 15 Jul 2020 20:02:50 +0200 Subject: [PATCH 06/13] remove everyting related to the now unused state space factories in the ompl interface --- .../ompl/ompl_interface/CMakeLists.txt | 3 - .../joint_model_state_space_factory.h | 54 ----------- .../model_based_state_space_factory.h | 76 ---------------- .../pose_model_state_space_factory.h | 54 ----------- .../ompl_interface/planning_context_manager.h | 20 ----- .../joint_model_state_space_factory.cpp | 56 ------------ .../model_based_state_space_factory.cpp | 45 ---------- .../pose_model_state_space_factory.cpp | 90 ------------------- .../src/planning_context_manager.cpp | 55 +----------- 9 files changed, 1 insertion(+), 452 deletions(-) delete mode 100644 moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h delete mode 100644 moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h delete mode 100644 moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h delete mode 100644 moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp delete mode 100644 moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp delete mode 100644 moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 9e9bc57664..f8e314abc9 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -5,11 +5,8 @@ add_library(${MOVEIT_LIB_NAME} src/planning_context_manager.cpp src/model_based_planning_context.cpp src/parameterization/model_based_state_space.cpp - src/parameterization/model_based_state_space_factory.cpp src/parameterization/joint_space/joint_model_state_space.cpp - src/parameterization/joint_space/joint_model_state_space_factory.cpp src/parameterization/work_space/pose_model_state_space.cpp - src/parameterization/work_space/pose_model_state_space_factory.cpp src/detail/threadsafe_state_storage.cpp src/detail/state_validity_checker.cpp src/detail/projection_evaluators.cpp diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h deleted file mode 100644 index 7386856fc6..0000000000 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ /dev/null @@ -1,54 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include - -namespace ompl_interface -{ -class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory -{ -public: - JointModelStateSpaceFactory(); - - int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const override; - -protected: - ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; -}; -} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h deleted file mode 100644 index 4b71b32e0f..0000000000 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ /dev/null @@ -1,76 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include -#include -#include - -namespace ompl_interface -{ -MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory); - -class ModelBasedStateSpaceFactory -{ -public: - ModelBasedStateSpaceFactory() - { - } - - virtual ~ModelBasedStateSpaceFactory() - { - } - - ModelBasedStateSpacePtr getNewStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const; - - const std::string& getType() const - { - return type_; - } - - /** \brief Decide whether the type of state space constructed by this factory could represent problems specified by - the user - request \e req for group \e group. The group \e group must always be specified and takes precedence over \e - req.group_name, which may be different */ - virtual int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const = 0; - -protected: - virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const = 0; - std::string type_; -}; -} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h deleted file mode 100644 index e6c34ebba9..0000000000 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ /dev/null @@ -1,54 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include - -namespace ompl_interface -{ -class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory -{ -public: - PoseModelStateSpaceFactory(); - - int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const override; - -protected: - ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; -}; -} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 20de8b55ef..c155b1d44b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -38,7 +38,6 @@ #include #include -#include #include #include @@ -189,30 +188,17 @@ class PlanningContextManager known_planners_[planner_id] = pa; } - void registerStateSpaceFactory(const ModelBasedStateSpaceFactoryPtr& factory) - { - state_space_factories_[factory->getType()] = factory; - } - const std::map& getRegisteredPlannerAllocators() const { return known_planners_; } - const std::map& getRegisteredStateSpaceFactories() const - { - return state_space_factories_; - } - ConfiguredPlannerSelector getPlannerSelector() const; protected: - typedef std::function StateSpaceFactoryTypeSelector; - ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const; void registerDefaultPlanners(); - void registerDefaultStateSpaces(); template void registerPlannerAllocatorHelper(const std::string& planner_id); @@ -222,11 +208,6 @@ class PlanningContextManager const std::string& state_space_parameterization_type, const moveit_msgs::MotionPlanRequest& req) const; - const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory1(const std::string& group_name, - const std::string& factory_type) const; - const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory2(const std::string& group_name, - const moveit_msgs::MotionPlanRequest& req) const; - /** \brief Check whether a joint model group has an inverse kinematics solver available. **/ bool doesGroupHaveIKSolver(const std::string& group_name) const; @@ -242,7 +223,6 @@ class PlanningContextManager constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; std::map known_planners_; - std::map state_space_factories_; /** \brief All the existing planning configurations. The name of the configuration is the key of the map. This name can diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp deleted file mode 100644 index 2c5952640c..0000000000 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -ompl_interface::JointModelStateSpaceFactory::JointModelStateSpaceFactory() : ModelBasedStateSpaceFactory() -{ - type_ = JointModelStateSpace::PARAMETERIZATION_TYPE; -} - -int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( - const std::string& /*group*/, const moveit_msgs::MotionPlanRequest& /*req*/, - const moveit::core::RobotModelConstPtr& /*robot_model*/) const -{ - return 100; -} - -ompl_interface::ModelBasedStateSpacePtr -ompl_interface::JointModelStateSpaceFactory::allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const -{ - return std::make_shared(space_spec); -} diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp deleted file mode 100644 index de23960b62..0000000000 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Ioan Sucan */ - -#include - -ompl_interface::ModelBasedStateSpacePtr -ompl_interface::ModelBasedStateSpaceFactory::getNewStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const -{ - ModelBasedStateSpacePtr ss = allocStateSpace(space_spec); - ss->computeLocations(); - return ss; -} diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp deleted file mode 100644 index 5d912db8f2..0000000000 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -ompl_interface::PoseModelStateSpaceFactory::PoseModelStateSpaceFactory() : ModelBasedStateSpaceFactory() -{ - type_ = PoseModelStateSpace::PARAMETERIZATION_TYPE; -} - -int ompl_interface::PoseModelStateSpaceFactory::canRepresentProblem( - const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const -{ - const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); - if (jmg) - { - const std::pair& slv = jmg->getGroupKinematics(); - bool ik = false; - // check that we have a direct means to compute IK - if (slv.first) - ik = jmg->getVariableCount() == slv.first.bijection_.size(); - else if (!slv.second.empty()) - { - // or an IK solver for each of the subgroups - unsigned int vc = 0; - unsigned int bc = 0; - for (const auto& jt : slv.second) - { - vc += jt.first->getVariableCount(); - bc += jt.second.bijection_.size(); - } - if (vc == jmg->getVariableCount() && vc == bc) - ik = true; - } - - if (ik) - { - // if we have path constraints, we prefer interpolating in pose space - if ((!req.path_constraints.position_constraints.empty() || - !req.path_constraints.orientation_constraints.empty()) && - req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty()) - return 150; - else - return 50; - } - } - return -1; -} - -ompl_interface::ModelBasedStateSpacePtr -ompl_interface::PoseModelStateSpaceFactory::allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const -{ - return std::make_shared(space_spec); -} diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 5afeae2175..183094e544 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -67,9 +67,7 @@ #include #include -#include #include -#include #include using namespace std::placeholders; @@ -237,7 +235,6 @@ ompl_interface::PlanningContextManager::PlanningContextManager(moveit::core::Rob { cached_contexts_.reset(new CachedContexts()); registerDefaultPlanners(); - registerDefaultStateSpaces(); } ompl_interface::PlanningContextManager::~PlanningContextManager() = default; @@ -293,12 +290,6 @@ void ompl_interface::PlanningContextManager::registerDefaultPlanners() registerPlannerAllocatorHelper("geometric::TRRT"); } -void ompl_interface::PlanningContextManager::registerDefaultStateSpaces() -{ - registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new JointModelStateSpaceFactory())); - registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new PoseModelStateSpaceFactory())); -} - ompl_interface::ConfiguredPlannerSelector ompl_interface::PlanningContextManager::getPlannerSelector() const { return std::bind(&PlanningContextManager::plannerSelector, this, std::placeholders::_1); @@ -365,50 +356,6 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana return context; } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory1( - const std::string& /* dummy */, const std::string& factory_type) const -{ - auto f = factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type); - if (f != state_space_factories_.end()) - return f->second; - else - { - ROS_ERROR_NAMED(LOGNAME, "Factory of type '%s' was not found", factory_type.c_str()); - static const ModelBasedStateSpaceFactoryPtr EMPTY; - return EMPTY; - } -} - -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory2( - const std::string& group, const moveit_msgs::MotionPlanRequest& req) const -{ - // find the problem representation to use - auto best = state_space_factories_.end(); - int prev_priority = 0; - for (auto it = state_space_factories_.begin(); it != state_space_factories_.end(); ++it) - { - int priority = it->second->canRepresentProblem(group, req, robot_model_); - if (priority > prev_priority) - { - best = it; - prev_priority = priority; - } - } - - if (best == state_space_factories_.end()) - { - ROS_ERROR_NAMED(LOGNAME, "There are no known state spaces that can represent the given planning " - "problem"); - static const ModelBasedStateSpaceFactoryPtr EMPTY; - return EMPTY; - } - else - { - ROS_DEBUG_NAMED(LOGNAME, "Using '%s' parameterization for solving problem", best->first.c_str()); - return best->second; - } -} - bool ompl_interface::PlanningContextManager::doesGroupHaveIKSolver(const std::string& group_name) const { const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); @@ -540,7 +487,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana // Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via // IK. However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be // flipped, leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection - // sampling in JointModelStateSpace. StateSpaceFactoryTypeSelector factory_selector; + // sampling in JointModelStateSpace. bool enforce_joint_model_state_space = false; auto it = pc->second.config.find("enforce_joint_model_state_space"); if (it != pc->second.config.end() && boost::lexical_cast(it->second)) From 87ec89618386d0e2b5985bf80c508ea69da509b4 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 28 Jul 2020 12:58:08 +0200 Subject: [PATCH 07/13] ompl interface fix log statement in planning context manager --- .../ompl/ompl_interface/src/planning_context_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 183094e544..f78a8e62f7 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -434,7 +434,8 @@ ompl_interface::ModelBasedStateSpacePtr ompl_interface::PlanningContextManager:: } else { - ROS_ERROR_NAMED("planning_context_manager", "Unkown state space parameterization type '%s'", parameterization_type); + ROS_ERROR_NAMED("planning_context_manager", "Unkown state space parameterization type '%s'", + parameterization_type.c_str()); return ompl_interface::ModelBasedStateSpacePtr(); } } From c304010dc5671abde527cb940e6bf2a140111e13 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 28 Jul 2020 14:06:28 +0200 Subject: [PATCH 08/13] change some 2 letter variable names in ompl interface --- .../ompl_interface/src/planning_context_manager.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index f78a8e62f7..6b9751a996 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -372,14 +372,14 @@ bool ompl_interface::PlanningContextManager::doesGroupHaveIKSolver(const std::st // or an IK solver for each of the subgroups else if (!ik_solver_pair.second.empty()) { - unsigned int vc = 0; - unsigned int bc = 0; + unsigned int variable_count = 0; + unsigned int bijection_count = 0; for (const auto& jt : ik_solver_pair.second) { - vc += jt.first->getVariableCount(); - bc += jt.second.bijection_.size(); + variable_count += jt.first->getVariableCount(); + bijection_count += jt.second.bijection_.size(); } - if (vc == jmg->getVariableCount() && vc == bc) + if (variable_count == jmg->getVariableCount() && variable_count == bijection_count) ik = true; } return ik; From 393c7e718887da88173dcc4b811d442ee9a08bf0 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 28 Jul 2020 14:19:56 +0200 Subject: [PATCH 09/13] add documentation to ompl interface planning_context_manager.h --- .../ompl_interface/planning_context_manager.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index c155b1d44b..b76b3c41e0 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -211,9 +211,25 @@ class PlanningContextManager /** \brief Check whether a joint model group has an inverse kinematics solver available. **/ bool doesGroupHaveIKSolver(const std::string& group_name) const; + /** \brief Get as suitable parameterization type of a given planning problem (joint space or Cartesian space). + * + * \param enforce_joint_model_state_space The user can enforce joint space parameterization. This is done by setting + * 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml. + * + * Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via + * IK. However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be + * flipped, leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection + * sampling in JointModelStateSpace. + * */ const std::string& selectStateSpaceType(const moveit_msgs::MotionPlanRequest& req, bool enforce_joint_model_state_space = false) const; + /** \Brief Create a state space of the given parameterization type using the specifications. + * + * Supported parameterization types are: + * - JointModelStateSpace::PARAMETERIZATION_TYPE for joint space parameterization. + * - PoseModelStateSpace::PARAMETERIZATION_TYPE for Cartesian space parameterization. + */ ModelBasedStateSpacePtr createStateSpace(const std::string& parameterization_type, const ModelBasedStateSpaceSpecification& space_spec) const; From 35687559279c32a61833ffbf158854312f6e284f Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 4 Aug 2020 09:29:59 +0200 Subject: [PATCH 10/13] ompl interface: add state_space->computeLocations(); on state space creation + small doc update --- .../moveit/ompl_interface/planning_context_manager.h | 8 ++++---- .../ompl_interface/src/planning_context_manager.cpp | 11 ++++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index b76b3c41e0..5f9c1e595b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -216,10 +216,10 @@ class PlanningContextManager * \param enforce_joint_model_state_space The user can enforce joint space parameterization. This is done by setting * 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml. * - * Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via - * IK. However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be - * flipped, leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection - * sampling in JointModelStateSpace. + * Why would you use this feature? Some planning problems like orientation path constraints are represented in + * PoseModelStateSpace and sampled via IK. However consecutive IK solutions are not checked for proximity at the + * moment and sometimes happen to be flipped, leading to invalid trajectories. This workaround lets the user prevent + * this problem by forcing rejection sampling in JointModelStateSpace. * */ const std::string& selectStateSpaceType(const moveit_msgs::MotionPlanRequest& req, bool enforce_joint_model_state_space = false) const; diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 6b9751a996..2ce54e7caf 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -424,17 +424,22 @@ const std::string& ompl_interface::PlanningContextManager::selectStateSpaceType( ompl_interface::ModelBasedStateSpacePtr ompl_interface::PlanningContextManager::createStateSpace( const std::string& parameterization_type, const ModelBasedStateSpaceSpecification& space_spec) const { + ompl_interface::ModelBasedStateSpacePtr state_space; if (parameterization_type == ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE) { - return std::make_shared(space_spec); + state_space = std::make_shared(space_spec); + state_space->computeLocations(); + return state_space; } else if (parameterization_type == ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE) { - return std::make_shared(space_spec); + state_space = std::make_shared(space_spec); + state_space->computeLocations(); + return state_space; } else { - ROS_ERROR_NAMED("planning_context_manager", "Unkown state space parameterization type '%s'", + ROS_ERROR_NAMED(LOGNAME, "Unkown state space parameterization type '%s'. No state space created.", parameterization_type.c_str()); return ompl_interface::ModelBasedStateSpacePtr(); } From 26a2b3ced7551e01142b881e2885f19f5902e0e7 Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 4 Aug 2020 13:46:45 +0200 Subject: [PATCH 11/13] add test for planning context manager --- .../ompl/ompl_interface/CMakeLists.txt | 8 + .../test/test_planning_context_manager.cpp | 196 ++++++++++++++++++ .../test/test_planning_context_manager.test | 6 + 3 files changed, 210 insertions(+) create mode 100644 moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp create mode 100644 moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index f8e314abc9..80e3547f0d 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -43,9 +43,17 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(moveit_resources REQUIRED) + find_package(rostest REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_state_space test/test_state_space.cpp) target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + + add_rostest_gtest(test_planning_context_manager + test/test_planning_context_manager.test + test/test_planning_context_manager.cpp) + target_link_libraries(test_planning_context_manager ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + set_target_properties(test_planning_context_manager PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") endif() diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp new file mode 100644 index 0000000000..e390433d68 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -0,0 +1,196 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +/** \brief Load a robot model and executes tests on it. + * + * This class contains all test implementations. + * The tests can be executed on different robots by creating a derived class and overwriting the constructor, as show at + * the bottom of this file. + * */ +class LoadTestRobotBaseClass : public testing::Test +{ +protected: + LoadTestRobotBaseClass(const std::string& robot_name, const std::string& group_name) + : group_name_(group_name), robot_name_(robot_name) + { + } + + void SetUp() override + { + // I need a ROS node handle to create the PlanningContextManager + nh_ = ros::NodeHandle("/planning_context_manager_test"); + + // load robot + robot_model_ = moveit::core::loadTestingRobotModel(robot_name_); + robot_state_ = std::make_shared(robot_model_); + joint_model_group_ = robot_state_->getJointModelGroup(group_name_); + planning_scene_ = std::make_shared(robot_model_); + + // extract useful parameters for tests + num_dofs_ = joint_model_group_->getVariableCount(); + ee_link_name_ = joint_model_group_->getLinkModelNames().back(); + base_link_name_ = robot_model_->getRootLinkName(); + + // fill in the MotionPlanRequest with useful values + request_.group_name = group_name_; + request_.allowed_planning_time = 5.0; + moveit::core::RobotState start_state = getDeterministicState(); + moveit::core::robotStateToRobotStateMsg(start_state, request_.start_state); + + // auto start_pose = start_state.getGlobalLinkTransform(ee_link_name_); + + moveit::core::RobotState goal_state(start_state); + goal_state.setVariablePosition(0, goal_state.getVariablePosition(0) + 0.2); + moveit_msgs::Constraints goal = + kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_, 0.001); + request_.goal_constraints.push_back(goal); + }; + + void TearDown() override + { + } + + /** \brief Create a joint position vector with values 0.1, 0.2, 0.3, ... where the length depends on the number of + * joints in the robot. * */ + moveit::core::RobotState getDeterministicState() + { + Eigen::VectorXd joint_positions(num_dofs_); + double value = 0.1; + for (std::size_t i = 0; i < num_dofs_; ++i) + { + joint_positions[i] = value; + value += 0.1; + } + moveit::core::RobotState state(robot_model_); + state.setToDefaultValues(); + state.copyJointGroupPositions(joint_model_group_, joint_positions); + return state; + } + + void testStateSpaceSelection() + { + planning_interface::PlannerConfigurationSettings pconfig; + pconfig.group = group_name_; + pconfig.name = group_name_; + + planning_interface::PlannerConfigurationMap pconfig_map; + pconfig_map[pconfig.name] = pconfig; + + auto constraint_sampler_manager = std::make_shared(); + + ompl_interface::PlanningContextManager planning_context_manager(robot_model_, constraint_sampler_manager); + planning_context_manager.setPlannerConfigurations(pconfig_map); + + moveit_msgs::MoveItErrorCodes error_code; + auto pc = planning_context_manager.getPlanningContext(planning_scene_, request_, error_code, nh_, false); + EXPECT_EQ(pc->getOMPLStateSpace()->getParameterizationType(), + ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE); + + // create an orientation constraint + moveit_msgs::OrientationConstraint ocm; + ocm.link_name = ee_link_name_; + ocm.header.frame_id = base_link_name_; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.1; + ocm.absolute_y_axis_tolerance = 0.1; + ocm.absolute_z_axis_tolerance = 0.1; + ocm.weight = 1.0; + // moveit_msgs::Constraints test_constraints; + // test_constraints.orientation_constraints.push_back(ocm); + request_.path_constraints.orientation_constraints.push_back(ocm); + + pc = planning_context_manager.getPlanningContext(planning_scene_, request_, error_code, nh_, false); + EXPECT_EQ(pc->getOMPLStateSpace()->getParameterizationType(), + ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE); + + EXPECT_TRUE(true); + } + +protected: + const std::string group_name_; + const std::string robot_name_; + + ros::NodeHandle nh_; + moveit::core::RobotModelPtr robot_model_; + robot_state::RobotStatePtr robot_state_; + const robot_state::JointModelGroup* joint_model_group_; + planning_scene::PlanningScenePtr planning_scene_; + + std::size_t num_dofs_; + std::string base_link_name_; + std::string ee_link_name_; + + planning_interface::MotionPlanRequest request_; +}; + +/*************************************************************************** + * Run all tests on the Panda robot + * ************************************************************************/ +class PandaCopyStateTest : public LoadTestRobotBaseClass +{ +protected: + PandaCopyStateTest() : LoadTestRobotBaseClass("panda", "panda_arm") + { + } +}; + +TEST_F(PandaCopyStateTest, testStateSpaceSelection) +{ + testStateSpaceSelection(); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "planning_context_manager_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test new file mode 100644 index 0000000000..2e311078fb --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file From 4e779af6d96be9cb0e091f9858213bf5503e7b6c Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 4 Aug 2020 14:33:02 +0200 Subject: [PATCH 12/13] fix error in selectStateSpaceType --- .../ompl/ompl_interface/src/planning_context_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 2ce54e7caf..e802e9ade1 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -402,7 +402,7 @@ const std::string& ompl_interface::PlanningContextManager::selectStateSpaceType( } // if there are joint or visibility constraints, we need to plan in joint space - else if (req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty()) + else if (!req.path_constraints.joint_constraints.empty() && !req.path_constraints.visibility_constraints.empty()) { return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE; } From 9decc4025734827ab45d8bb745aa18c741756daa Mon Sep 17 00:00:00 2001 From: JeroenDM Date: Tue, 4 Aug 2020 15:12:21 +0200 Subject: [PATCH 13/13] try other approach for planing context manager test --- .../test/test_planning_context_manager.cpp | 30 ++++++++++++++++--- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index e390433d68..36fbaa8fc9 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -39,6 +39,7 @@ #include +#include #include #include #include @@ -67,7 +68,10 @@ class LoadTestRobotBaseClass : public testing::Test nh_ = ros::NodeHandle("/planning_context_manager_test"); // load robot - robot_model_ = moveit::core::loadTestingRobotModel(robot_name_); + // robot_model_ = moveit::core::loadTestingRobotModel(robot_name_); + robot_model_loader::RobotModelLoaderPtr robot_model_loader( + new robot_model_loader::RobotModelLoader("robot_description")); + robot_model_ = robot_model_loader->getModel(); robot_state_ = std::make_shared(robot_model_); joint_model_group_ = robot_state_->getJointModelGroup(group_name_); planning_scene_ = std::make_shared(robot_model_); @@ -80,6 +84,7 @@ class LoadTestRobotBaseClass : public testing::Test // fill in the MotionPlanRequest with useful values request_.group_name = group_name_; request_.allowed_planning_time = 5.0; + request_.planner_id = "RRTConnect"; moveit::core::RobotState start_state = getDeterministicState(); moveit::core::robotStateToRobotStateMsg(start_state, request_.start_state); @@ -118,6 +123,7 @@ class LoadTestRobotBaseClass : public testing::Test planning_interface::PlannerConfigurationSettings pconfig; pconfig.group = group_name_; pconfig.name = group_name_; + pconfig.config = { { "enforce_joint_model_state_space", "false" } }; planning_interface::PlannerConfigurationMap pconfig_map; pconfig_map[pconfig.name] = pconfig; @@ -172,19 +178,35 @@ class LoadTestRobotBaseClass : public testing::Test /*************************************************************************** * Run all tests on the Panda robot * ************************************************************************/ -class PandaCopyStateTest : public LoadTestRobotBaseClass +class PandaTests : public LoadTestRobotBaseClass { protected: - PandaCopyStateTest() : LoadTestRobotBaseClass("panda", "panda_arm") + PandaTests() : LoadTestRobotBaseClass("panda", "panda_arm") { } }; -TEST_F(PandaCopyStateTest, testStateSpaceSelection) +TEST_F(PandaTests, testStateSpaceSelection) { testStateSpaceSelection(); } +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +// class FanucTests : public LoadTestRobotBaseClass +// { +// protected: +// FanucTests() : LoadTestRobotBaseClass("fanuc", "manipulator") +// { +// } +// }; + +// TEST_F(FanucTests, testStateSpaceSelection) +// { +// testStateSpaceSelection(); +// } + /*************************************************************************** * MAIN * ************************************************************************/