Increase priority for constrained planning state space (#1300)

* Change priority for the constrained planning state space

* Fix constrained planning tests

* Use PRM instead of RRTConnect

---------

Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>

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:

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;

moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp

@@ -63,6 +63,7 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
 #include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h>
+#include <moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h>
 
 // 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<ompl_interface::JointModelStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
+    // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
+    EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(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<ompl_interface::JointModelStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
+    // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
+    EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
 
     // Create a new response, because the solve method does not clear the given respone
     planning_interface::MotionPlanDetailedResponse response2;