diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi index a18ecb7b3b..e5f2363780 100644 --- 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 index e4ec30b900..4caff8f7f2 100644 --- 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 index 5d61b6e789..3e6488ca31 100644 --- 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& planning_component, std::shared_ptr& single_plan_parameters, std::shared_ptr& multi_plan_parameters, + std::shared_ptr& planning_scene, std::optional solution_selection_function, std::optional stopping_criterion_callback) { @@ -61,7 +62,7 @@ plan(std::shared_ptr& planning_component, std::shared_ptr const_single_plan_parameters = std::const_pointer_cast(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& 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 index 615a3facc5..fd727b9d7c 100644 --- 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& planning_component, std::shared_ptr& single_plan_parameters, std::shared_ptr& multi_plan_parameters, + std::shared_ptr& planning_scene, std::optional solution_selection_function, std::optional stopping_criterion_callback);