diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 9e9bc57664..80e3547f0d 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 @@ -46,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/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 594d3ba9dc..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 @@ -1,43 +1,43 @@ /********************************************************************* -* 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 */ #pragma once #include -#include +#include #include #include @@ -188,43 +188,50 @@ 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); /** \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, - 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; + + /** \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. + * + * 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; + + /** \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; /** \brief The kinematic model for which motion plans are computed */ moveit::core::RobotModelConstPtr robot_model_; @@ -232,7 +239,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 44b15cee8b..e802e9ade1 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,8 @@ #include #include -#include #include -#include +#include using namespace std::placeholders; @@ -236,7 +235,6 @@ ompl_interface::PlanningContextManager::PlanningContextManager(moveit::core::Rob { cached_contexts_.reset(new CachedContexts()); registerDefaultPlanners(); - registerDefaultStateSpaces(); } ompl_interface::PlanningContextManager::~PlanningContextManager() = default; @@ -292,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); @@ -311,16 +303,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,16 +332,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); - - // 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); } } @@ -367,47 +356,92 @@ 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 +bool ompl_interface::PlanningContextManager::doesGroupHaveIKSolver(const std::string& group_name) 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; + 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 variable_count = 0; + unsigned int bijection_count = 0; + for (const auto& jt : ik_solver_pair.second) + { + variable_count += jt.first->getVariableCount(); + bijection_count += jt.second.bijection_.size(); + } + if (variable_count == jmg->getVariableCount() && variable_count == bijection_count) + ik = true; + } + return ik; + } else { - ROS_ERROR_NAMED(LOGNAME, "Factory of type '%s' was not found", factory_type.c_str()); - static const ModelBasedStateSpaceFactoryPtr EMPTY; - return EMPTY; + ROS_ERROR_NAMED("planning_context_manager", "planning group '%s' not found when testing for IK Solver.", + group_name.c_str()); + return false; } } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory2( - const std::string& group, const moveit_msgs::MotionPlanRequest& req) const +const std::string& ompl_interface::PlanningContextManager::selectStateSpaceType( + const moveit_msgs::MotionPlanRequest& req, bool enforce_joint_model_state_space) 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) + // the user forced us to plan in joint space + if (enforce_joint_model_state_space) { - int priority = it->second->canRepresentProblem(group, req, robot_model_); - if (priority > prev_priority) - { - best = it; - prev_priority = priority; - } + 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 ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE; } - if (best == state_space_factories_.end()) + // 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)) { - ROS_ERROR_NAMED(LOGNAME, "There are no known state spaces that can represent the given planning " - "problem"); - static const ModelBasedStateSpaceFactoryPtr EMPTY; - return EMPTY; + return ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE; } + // in all other cases, plan in joint space else { - ROS_DEBUG_NAMED(LOGNAME, "Using '%s' parameterization for solving problem", best->first.c_str()); - return best->second; + return ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE; + } +} + +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) + { + state_space = std::make_shared(space_spec); + state_space->computeLocations(); + return state_space; + } + else if (parameterization_type == ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE) + { + state_space = std::make_shared(space_spec); + state_space->computeLocations(); + return state_space; + } + else + { + ROS_ERROR_NAMED(LOGNAME, "Unkown state space parameterization type '%s'. No state space created.", + parameterization_type.c_str()); + return ompl_interface::ModelBasedStateSpacePtr(); } } @@ -456,20 +490,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. + 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) { 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..36fbaa8fc9 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -0,0 +1,218 @@ +/********************************************************************* + * 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 +#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_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_); + + // 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; + request_.planner_id = "RRTConnect"; + 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_; + pconfig.config = { { "enforce_joint_model_state_space", "false" } }; + + 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 PandaTests : public LoadTestRobotBaseClass +{ +protected: + PandaTests() : LoadTestRobotBaseClass("panda", "panda_arm") + { + } +}; + +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 + * ************************************************************************/ +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