[Python] Add Allowed Collision Matrix to planning scene and optional …

…planning scene during planning (moveit#2387)
tylerjw · Oct 24, 2023 · 7b621d7 · 7b621d7
1 parent 606722e
commit 7b621d7
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 8 deletions.
diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi
@@ -29,3 +29,5 @@ class PlanningScene:
     def robot_model(self) -> Any: ...
     @property
     def transforms(self) -> Any: ...
+    @property
+    def allowed_collision_matrix(self) -> Any: ...
diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp
@@ -112,7 +112,8 @@ void init_planning_scene(py::module& m)
                     py::return_value_policy::move)
 
       .def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr)
-
+      .def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix, nullptr,
+                    py::return_value_policy::move)
       // methods
       .def("__copy__",
            [](const planning_scene::PlanningScene* self) {

diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp
@@ -45,6 +45,7 @@ planning_interface::MotionPlanResponse
 plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
      std::shared_ptr<moveit_cpp::PlanningComponent::PlanRequestParameters>& single_plan_parameters,
      std::shared_ptr<moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>& multi_plan_parameters,
+     std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
      std::optional<const moveit::planning_pipeline_interfaces::SolutionSelectionFunction> solution_selection_function,
      std::optional<moveit::planning_pipeline_interfaces::StoppingCriterionFunction> stopping_criterion_callback)
 {
@@ -61,7 +62,7 @@ plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
     std::shared_ptr<const moveit_cpp::PlanningComponent::PlanRequestParameters> const_single_plan_parameters =
         std::const_pointer_cast<const moveit_cpp::PlanningComponent::PlanRequestParameters>(single_plan_parameters);
 
-    return planning_component->plan(*const_single_plan_parameters);
+    return planning_component->plan(*const_single_plan_parameters, planning_scene);
   }
   else if (multi_plan_parameters)
   {
@@ -73,25 +74,32 @@ plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
     if (solution_selection_function && stopping_criterion_callback)
     {
       return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function),
-                                      *stopping_criterion_callback);
+                                      *stopping_criterion_callback, planning_scene);
     }
     else if (solution_selection_function)
     {
-      return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function));
+      return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function), nullptr,
+                                      planning_scene);
     }
     else if (stopping_criterion_callback)
     {
       return planning_component->plan(*const_multi_plan_parameters,
                                       moveit::planning_pipeline_interfaces::getShortestSolution,
-                                      *stopping_criterion_callback);
+                                      *stopping_criterion_callback, planning_scene);
     }
     else
     {
-      return planning_component->plan(*const_multi_plan_parameters);
+      return planning_component->plan(*const_multi_plan_parameters,
+                                      moveit::planning_pipeline_interfaces::getShortestSolution, nullptr,
+                                      planning_scene);
     }
   }
   else
   {
+    if (planning_scene)
+    {
+      throw std::invalid_argument("Cannot specify planning scene without specifying plan parameters");
+    }
     return planning_component->plan();
   }
 }
@@ -322,8 +330,9 @@ void init_planning_component(py::module& m)
 
       // TODO (peterdavidfagan): improve the plan API
       .def("plan", &moveit_py::bind_planning_component::plan, py::arg("single_plan_parameters") = nullptr,
-           py::arg("multi_plan_parameters") = nullptr, py::arg("solution_selection_function") = nullptr,
-           py::arg("stopping_criterion_callback") = nullptr, py::return_value_policy::move,
+           py::arg("multi_plan_parameters") = nullptr, py::arg("planning_scene") = nullptr,
+           py::arg("solution_selection_function") = nullptr, py::arg("stopping_criterion_callback") = nullptr,
+           py::return_value_policy::move,
            R"(
            Plan a motion plan using the current start and goal states.
 

diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h
@@ -62,6 +62,7 @@ planning_interface::MotionPlanResponse
 plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
      std::shared_ptr<moveit_cpp::PlanningComponent::PlanRequestParameters>& single_plan_parameters,
      std::shared_ptr<moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>& multi_plan_parameters,
+     std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
      std::optional<const moveit::planning_pipeline_interfaces::SolutionSelectionFunction> solution_selection_function,
      std::optional<moveit::planning_pipeline_interfaces::StoppingCriterionFunction> stopping_criterion_callback);