diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h index e83c7cfb11..e7a8067266 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h @@ -48,9 +48,11 @@ class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory /** \brief Return a priority that this planner should be used for this specific planning problem. * - * This state space factory is currently only used if `use_ompl_constrained_state_space` was set to `true` in - * ompl_planning.yaml. In that case it is the only factory to choose from, so the priority does not matter. - * It returns a low priority so it will never be chosen when others are available. + * This state space factory is currently only used if there is exactly one position or orientation constraint, + * or if `enforce_constrained_state_space` was set to `true` in ompl_planning.yaml. + * In the first case, we prefer planning in the constrained state space and return a priority of 200. + * In the second case, it is the only factory to choose from, so the priority does not matter, + * and it returns a low priority so it will never be chosen when others are available. * (The second lowest priority is -1 in the PoseModelStateSpaceFactory.) * * For more details on this state space selection process, see: diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp index c69e6adc78..7830f81c37 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp @@ -46,10 +46,18 @@ ConstrainedPlanningStateSpaceFactory::ConstrainedPlanningStateSpaceFactory() : M } int ConstrainedPlanningStateSpaceFactory::canRepresentProblem( - const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& /*req*/, + const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& /*robot_model*/) const { - // Return the lowest priority currently existing in the ompl interface. + // If we have exactly one position or orientation constraint, prefer the constrained planning state space + auto num_constraints = + req.path_constraints.position_constraints.size() + req.path_constraints.orientation_constraints.size(); + if (num_constraints == 1 && req.path_constraints.joint_constraints.empty() && + req.path_constraints.visibility_constraints.empty()) + { + return 200; + } + // Otherwise, return the lowest priority currently existing in the ompl interface. // This state space will only be selected if it is the only option to choose from. // See header file for more info. return -2; 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 55dcbdf0a3..16b7014556 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 @@ -63,6 +63,7 @@ #include #include #include +#include // static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_planning_context_manager"); @@ -115,10 +116,14 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public SCOPED_TRACE("testPathConstraints"); // create all the test specific input necessary to make the getPlanningContext call possible + const auto& joint_names = joint_model_group_->getJointModelNames(); + planning_interface::PlannerConfigurationSettings pconfig_settings; pconfig_settings.group = group_name_; pconfig_settings.name = group_name_; - pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; + pconfig_settings.config = { { "enforce_joint_model_state_space", "0" }, + { "projection_evaluator", "joints(" + joint_names[0] + "," + joint_names[1] + ")" }, + { "type", "geometric::PRM" } }; planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; moveit_msgs::msg::MoveItErrorCodes error_code; @@ -145,8 +150,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); - // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space - EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); planning_interface::MotionPlanDetailedResponse response; ASSERT_TRUE(pc->solve(response)); @@ -181,8 +186,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); - // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space - EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); // Create a new response, because the solve method does not clear the given respone planning_interface::MotionPlanDetailedResponse response2;