Change priority for the constrained planning state space

moveit · Jun 1, 2022 · 4984a13 · 4984a13
1 parent 8209509
commit 4984a13
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 7 deletions.
diff --git a/...it/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h b/...it/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/...l_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp b/...l_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp
@@ -45,11 +45,19 @@ ConstrainedPlanningStateSpaceFactory::ConstrainedPlanningStateSpaceFactory() : M
   type_ = ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE;
 }
 
-int ConstrainedPlanningStateSpaceFactory::canRepresentProblem(
-    const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& /*req*/,
-    const moveit::core::RobotModelConstPtr& /*robot_model*/) const
+int ConstrainedPlanningStateSpaceFactory::canRepresentProblem(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;