From 0fb1280df677f8cbeff060216d5358f3c9dd6a2a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 14 Nov 2024 13:14:29 +0100 Subject: [PATCH 01/10] Add use_padding flag + deprecate Unpadded... functions --- .../collision_detection/collision_common.h | 6 + .../moveit/planning_scene/planning_scene.h | 97 +++-------- .../planning_scene/src/planning_scene.cpp | 161 ++++++++++++++---- .../benchmarks/src/BenchmarkExecutor.cpp | 3 +- .../plan_execution/src/plan_execution.cpp | 9 +- 5 files changed, 169 insertions(+), 107 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index db246518fd..2c1805015a 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -150,6 +150,12 @@ struct CollisionRequest * are included. */ std::string group_name = ""; + /** \brief If true, use padded collision environment */ + bool use_padded_collision_environment = true; + + /** \brief If true, do self collision check with padded robot links */ + bool use_padded_self_collision = false; + /** \brief If true, compute proximity distance */ bool distance = false; diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 3d71a117e7..7ee02aa045 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -347,19 +347,12 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); /** \brief Check whether the current state is in collision. The current state is expected to be updated. */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const - { - checkCollision(req, res, getCurrentState()); - } + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const; /** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state) const - { - robot_state.updateCollisionBodyTransforms(); - checkCollision(req, res, static_cast(robot_state)); - } + moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e * robot_state are @@ -372,114 +365,80 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro a non-const \e robot_state and updates its link transforms if needed. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - robot_state.updateCollisionBodyTransforms(); - checkCollision(req, res, static_cast(robot_state), acm); - } + const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm). */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; + const collision_detection::AllowedCollisionMatrix& acm, bool validate_transforms = true) const; /** \brief Check whether the current state is in collision, but use a collision_detection::CollisionRobot instance that has no padding. Since the function is non-const, the current state transforms are also updated if needed. */ - void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res); + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); /** \brief Check whether the current state is in collision, but use a collision_detection::CollisionRobot instance that has no padding. */ - void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res) const - { - checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix()); - } + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) const; /** \brief Check whether a specified state (\e robot_state) is in collision, but use a collision_detection::CollisionRobot instance that has no padding. */ - void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state) const - { - checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix()); - } + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in collision, but use a collision_detection::CollisionRobot instance that has no padding. Update the link transforms of \e robot_state if needed. */ - void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state) const - { - robot_state.updateCollisionBodyTransforms(); - checkCollisionUnpadded(req, res, static_cast(robot_state), - getAllowedCollisionMatrix()); - } + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */ - void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - robot_state.updateCollisionBodyTransforms(); - checkCollisionUnpadded(req, res, static_cast(robot_state), acm); - } + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. */ - void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether the current state is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); /** \brief Check whether the current state is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res) const - { - checkSelfCollision(req, res, getCurrentState()); - } + collision_detection::CollisionResult& res) const; /** \brief Check whether a specified state (\e robot_state) is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state) const - { - robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); - } + moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state) const - { - // do self-collision checking with the unpadded version of the robot - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); - } + const moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), acm); - } + const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given allowed collision matrix (\e acm) */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - // do self-collision checking with the unpadded version of the robot - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); - } + const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Get the names of the links that are involved in collisions for the current state */ void getCollidingLinks(std::vector& links); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 8655117fb8..1015d50cbf 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -394,14 +394,20 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) { - if (getCurrentState().dirtyCollisionBodyTransforms()) - { - checkCollision(req, res, getCurrentStateNonConst()); - } - else - { - checkCollision(req, res, getCurrentState()); - } + checkCollision(req, res, getCurrentStateNonConst()); +} + +void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) const +{ + checkCollision(req, res, getCurrentState(), getAllowedCollisionMatrix()); +} + +void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const +{ + checkCollision(req, res, robot_state, getAllowedCollisionMatrix()); } void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, @@ -411,43 +417,86 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& checkCollision(req, res, robot_state, getAllowedCollisionMatrix()); } -void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res) +void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const { - if (getCurrentState().dirtyCollisionBodyTransforms()) + if (robot_state.dirtyCollisionBodyTransforms()) { - checkSelfCollision(req, res, getCurrentStateNonConst()); - } - else - { - checkSelfCollision(req, res, getCurrentState()); + robot_state.updateCollisionBodyTransforms(); } + checkCollision(req, res, robot_state, acm); } void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const + const collision_detection::AllowedCollisionMatrix& acm, + bool validate_transforms) const { + if (validate_transforms && robot_state.dirtyCollisionBodyTransforms()) + { + RCLCPP_WARN(getLogger(), "Robot state has dirty collision body transforms. Please update the collision body " + "transforms before checking collision."); + } // check collision with the world using the padded version - getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm); + req.use_padded_collision_environment ? getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm) : + getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); + + // Return early if a collision was found or the maximum number of allowed contacts is exceeded + if (res.collision || (req.contacts && res.contacts.size() >= req.max_contacts)) + { + return; + } // do self-collision checking with the unpadded version of the robot - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); + req.use_padded_self_collision ? getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm) : + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) { - if (getCurrentState().dirtyCollisionBodyTransforms()) - { - checkCollisionUnpadded(req, res, getCurrentStateNonConst(), getAllowedCollisionMatrix()); - } - else - { - checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix()); - } + collision_detection::CollisionRequest new_req = req; + new_req.use_padded_collision_environment = false; + checkCollision(req, res, getCurrentStateNonConst(), getAllowedCollisionMatrix()); +} + +void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) const +{ + collision_detection::CollisionRequest new_req = req; + new_req.use_padded_collision_environment = false; + checkCollision(new_req, res, getCurrentState(), getAllowedCollisionMatrix()); +} + +void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state) const +{ + collision_detection::CollisionRequest new_req = req; + new_req.use_padded_collision_environment = false; + checkCollision(new_req, res, robot_state, getAllowedCollisionMatrix()); +} + +void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const +{ + collision_detection::CollisionRequest new_req = req; + new_req.use_padded_collision_environment = false; + checkCollision(new_req, res, static_cast(robot_state), getAllowedCollisionMatrix()); +} + +void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const +{ + robot_state.updateCollisionBodyTransforms(); + collision_detection::CollisionRequest new_req = req; + new_req.use_padded_collision_environment = false; + checkCollision(new_req, res, static_cast(robot_state), acm); } void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, @@ -455,14 +504,60 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { - // check collision with the world using the unpadded version - getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); + collision_detection::CollisionRequest new_req = req; + new_req.use_padded_collision_environment = false; + checkCollision(req, res, robot_state, acm); +} - // do self-collision checking with the unpadded version of the robot - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) +void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) +{ + checkSelfCollision(req, res, getCurrentStateNonConst()); +} + +/** \brief Check whether the current state is in self collision */ +void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) const +{ + checkSelfCollision(req, res, getCurrentState()); +} + +void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const +{ + if (robot_state.dirtyCollisionBodyTransforms()) { - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); + robot_state.updateCollisionBodyTransforms(); } + checkSelfCollision(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); +} + +void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state) const +{ + checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); +} + +void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const +{ + if (robot_state.dirtyCollisionBodyTransforms()) + { + robot_state.updateCollisionBodyTransforms(); + } + checkSelfCollision(req, res, static_cast(robot_state), acm); +} + +void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const +{ + req.use_padded_self_collision ? getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm) : + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts) diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 252d2a98d6..be47a121a7 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1010,10 +1010,11 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, // compute correctness and clearance collision_detection::CollisionRequest req; + req.use_padded_collision_environment = false; for (std::size_t k = 0; k < p.getWayPointCount(); ++k) { collision_detection::CollisionResult res; - planning_scene_->checkCollisionUnpadded(req, res, p.getWayPoint(k)); + planning_scene_->checkCollision(req, res, p.getWayPoint(k)); if (res.collision) correct = false; if (!p.getWayPoint(k).satisfiesBounds()) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 6e0452f410..7c51894a0f 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -282,16 +282,17 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP std::size_t wpc = t.getWayPointCount(); collision_detection::CollisionRequest req; req.group_name = t.getGroupName(); + req.use_padded_collision_environment = false; for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i) { collision_detection::CollisionResult res; if (acm) { - plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); + plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm); } else { - plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i)); + plan.planning_scene->checkCollision(req, res, t.getWayPoint(i)); } if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false)) @@ -306,11 +307,11 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP res.clear(); if (acm) { - plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm); + plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm); } else { - plan.planning_scene->checkCollisionUnpadded(req, res, t.getWayPoint(i)); + plan.planning_scene->checkCollision(req, res, t.getWayPoint(i)); } return false; } From 685d2d54ad9c3d418daff03a5883cc269b3615f2 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 14 Nov 2024 14:01:57 +0100 Subject: [PATCH 02/10] Fix CI --- moveit_core/planning_scene/src/planning_scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 1015d50cbf..9c508692c8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -425,7 +425,7 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& { robot_state.updateCollisionBodyTransforms(); } - checkCollision(req, res, robot_state, acm); + checkCollision(req, res, static_cast(robot_state), acm); } void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, From ea609e609a84bbee9af2abb8702278a2332cf41e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 14 Nov 2024 18:49:39 +0100 Subject: [PATCH 03/10] Remove deprecated functions from python API --- .../src/moveit/moveit_core/planning_scene/planning_scene.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 e55b0f15d4..407d4da71f 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 @@ -360,7 +360,8 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") - +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" .def("check_collision_unpadded", py::overload_cast( &planning_scene::PlanningScene::checkCollisionUnpadded), @@ -410,7 +411,7 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") - +#pragma GCC diagnostic pop .def("check_self_collision", py::overload_cast( &planning_scene::PlanningScene::checkSelfCollision), From 382f6dd847b9d4d0d119a0b4588300e104662dae Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 14 Nov 2024 19:20:11 +0100 Subject: [PATCH 04/10] Update #pragmas --- .../moveit_core/planning_scene/planning_scene.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) 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 407d4da71f..b6120cd58e 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 @@ -101,6 +101,9 @@ void initPlanningScene(py::module& m) { py::module planning_scene = m.def_submodule("planning_scene"); +// Remove once checkCollisionUnpadded is removed +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" py::class_>(planning_scene, "PlanningScene", R"( @@ -360,8 +363,7 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + // DEPRECATED! Use check_collision instead .def("check_collision_unpadded", py::overload_cast( &planning_scene::PlanningScene::checkCollisionUnpadded), @@ -376,7 +378,7 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") - + // DEPRECATED! Use check_collision instead .def("check_collision_unpadded", py::overload_cast(&planning_scene::PlanningScene::checkCollisionUnpadded, @@ -393,7 +395,7 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") - + // DEPRECATED! Use check_collision instead .def("check_collision_unpadded", py::overload_cast( @@ -411,7 +413,6 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") -#pragma GCC diagnostic pop .def("check_self_collision", py::overload_cast( &planning_scene::PlanningScene::checkSelfCollision), @@ -482,6 +483,7 @@ void initPlanningScene(py::module& m) Returns: bool: true if load from file was successful otherwise false. )"); +#pragma GCC diagnostic pop // TODO remove once checkCollisionUnpadded is removed } } // namespace bind_planning_scene } // namespace moveit_py From 57833b8078bdedab64508519c0e17595ab4a2b4f Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 15 Nov 2024 10:01:28 +0100 Subject: [PATCH 05/10] Address review --- .../collision_detection/collision_common.h | 4 +-- .../moveit/planning_scene/planning_scene.h | 2 +- .../planning_scene/src/planning_scene.cpp | 25 +++++++++---------- .../benchmarks/src/BenchmarkExecutor.cpp | 2 +- .../plan_execution/src/plan_execution.cpp | 2 +- 5 files changed, 17 insertions(+), 18 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 2c1805015a..0fcd4f9808 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -151,10 +151,10 @@ struct CollisionRequest std::string group_name = ""; /** \brief If true, use padded collision environment */ - bool use_padded_collision_environment = true; + bool pad_environment_collisions = true; /** \brief If true, do self collision check with padded robot links */ - bool use_padded_self_collision = false; + bool pad_self_collisions = false; /** \brief If true, compute proximity distance */ bool distance = false; diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 7ee02aa045..f98254281e 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -368,7 +368,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given - allowed collision matrix (\e acm). */ + allowed collision matrix (\e acm). If validate_transforms is set true, the given robot state is checked for dirty transforms. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, bool validate_transforms = true) const; diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 9c508692c8..788c51c18d 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -440,8 +440,8 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& "transforms before checking collision."); } // check collision with the world using the padded version - req.use_padded_collision_environment ? getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm) : - getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); + req.pad_environment_collisions ? getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm) : + getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); // Return early if a collision was found or the maximum number of allowed contacts is exceeded if (res.collision || (req.contacts && res.contacts.size() >= req.max_contacts)) @@ -450,15 +450,15 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& } // do self-collision checking with the unpadded version of the robot - req.use_padded_self_collision ? getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm) : - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); + req.pad_self_collisions ? getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm) : + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) { collision_detection::CollisionRequest new_req = req; - new_req.use_padded_collision_environment = false; + new_req.pad_environment_collisions = false; checkCollision(req, res, getCurrentStateNonConst(), getAllowedCollisionMatrix()); } @@ -466,7 +466,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR collision_detection::CollisionResult& res) const { collision_detection::CollisionRequest new_req = req; - new_req.use_padded_collision_environment = false; + new_req.pad_environment_collisions = false; checkCollision(new_req, res, getCurrentState(), getAllowedCollisionMatrix()); } @@ -475,7 +475,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR const moveit::core::RobotState& robot_state) const { collision_detection::CollisionRequest new_req = req; - new_req.use_padded_collision_environment = false; + new_req.pad_environment_collisions = false; checkCollision(new_req, res, robot_state, getAllowedCollisionMatrix()); } @@ -484,7 +484,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR moveit::core::RobotState& robot_state) const { collision_detection::CollisionRequest new_req = req; - new_req.use_padded_collision_environment = false; + new_req.pad_environment_collisions = false; checkCollision(new_req, res, static_cast(robot_state), getAllowedCollisionMatrix()); } @@ -495,7 +495,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR { robot_state.updateCollisionBodyTransforms(); collision_detection::CollisionRequest new_req = req; - new_req.use_padded_collision_environment = false; + new_req.pad_environment_collisions = false; checkCollision(new_req, res, static_cast(robot_state), acm); } @@ -505,7 +505,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR const collision_detection::AllowedCollisionMatrix& acm) const { collision_detection::CollisionRequest new_req = req; - new_req.use_padded_collision_environment = false; + new_req.pad_environment_collisions = false; checkCollision(req, res, robot_state, acm); } @@ -515,7 +515,6 @@ void PlanningScene::checkSelfCollision(const collision_detection::CollisionReque checkSelfCollision(req, res, getCurrentStateNonConst()); } -/** \brief Check whether the current state is in self collision */ void PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const { @@ -556,8 +555,8 @@ void PlanningScene::checkSelfCollision(const collision_detection::CollisionReque const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { - req.use_padded_self_collision ? getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm) : - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); + req.pad_self_collisions ? getCollisionEnv()->checkSelfCollision(req, res, robot_state, acm) : + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts) diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index be47a121a7..a53772b130 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1010,7 +1010,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, // compute correctness and clearance collision_detection::CollisionRequest req; - req.use_padded_collision_environment = false; + req.pad_environment_collisions = false; for (std::size_t k = 0; k < p.getWayPointCount(); ++k) { collision_detection::CollisionResult res; diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 7c51894a0f..65c46322cc 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -282,7 +282,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP std::size_t wpc = t.getWayPointCount(); collision_detection::CollisionRequest req; req.group_name = t.getGroupName(); - req.use_padded_collision_environment = false; + req.pad_environment_collisions = false; for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i) { collision_detection::CollisionResult res; From cfb96ae3feb868ce3a4d00a39637c1ad80429267 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 15 Nov 2024 13:13:30 +0100 Subject: [PATCH 06/10] Add CHANGELOG comment --- moveit_core/CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index bd26167d3c..b1fd4c3d0e 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -1,6 +1,10 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.12.0 (202X-XX-XX) +------------------- +* Add flags to control padding to CollisionRequest and deprecate PlanningScene::checkCollisionUnpadded(..) functions + 2.11.0 (2024-09-16) ------------------- From 3baa075317949a3604513804195dc0ae40cf77f8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 15 Nov 2024 15:13:43 +0100 Subject: [PATCH 07/10] Remove warning --- .../include/moveit/planning_scene/planning_scene.h | 4 ++-- moveit_core/planning_scene/src/planning_scene.cpp | 8 +------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index f98254281e..37d7c9787c 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -368,10 +368,10 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given - allowed collision matrix (\e acm). If validate_transforms is set true, the given robot state is checked for dirty transforms. */ + allowed collision matrix (\e acm). */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm, bool validate_transforms = true) const; + const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether the current state is in collision, but use a collision_detection::CollisionRobot instance that has no padding. diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 788c51c18d..986c552b5a 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -431,14 +431,8 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm, - bool validate_transforms) const + const collision_detection::AllowedCollisionMatrix& acm) const { - if (validate_transforms && robot_state.dirtyCollisionBodyTransforms()) - { - RCLCPP_WARN(getLogger(), "Robot state has dirty collision body transforms. Please update the collision body " - "transforms before checking collision."); - } // check collision with the world using the padded version req.pad_environment_collisions ? getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm) : getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); From eab7a2608b287bb82ac98dcf34efd26ec445f6d6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 18 Nov 2024 11:12:07 +0100 Subject: [PATCH 08/10] Update MIGRATION guide --- MIGRATION.md | 2 ++ moveit_core/CHANGELOG.rst | 4 ---- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 9af12e14c4..3aa22d5f05 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,8 @@ API changes in MoveIt releases ## ROS Rolling +- [11/2024] Added flags to control padding to CollisionRequest. This change deprecates PlanningScene::checkCollisionUnpadded(..) functions. Please use PlanningScene::checkCollision(..) with a req.pad_environment_collisions = false; + - [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse. The constructors have been replaced by builder methods so that errors can be communicated. Paths and trajectories now need to be created with `Path::Create()` and `Trajectory::Create()`. These methods now return an `std::optional` that needs to be checked for a valid value. `Trajectory` no longer has the `isValid()` method. If it's invalid, `Trajectory::Create()` will return `std::nullopt`. Finally, `Path` now takes the list of input waypoints as `std::vector`, instead of `std::list`. - [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index b1fd4c3d0e..bd26167d3c 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -1,10 +1,6 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.12.0 (202X-XX-XX) -------------------- -* Add flags to control padding to CollisionRequest and deprecate PlanningScene::checkCollisionUnpadded(..) functions - 2.11.0 (2024-09-16) ------------------- From adb21191db53b9e05f9fae747ebd7578c03f02b8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 18 Nov 2024 11:40:05 +0100 Subject: [PATCH 09/10] Update early exit logic in checkCollision --- moveit_core/planning_scene/src/planning_scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 986c552b5a..f9a0964c6d 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -438,7 +438,7 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); // Return early if a collision was found or the maximum number of allowed contacts is exceeded - if (res.collision || (req.contacts && res.contacts.size() >= req.max_contacts)) + if (res.collision && (!req.contacts || res.contacts.size() >= req.max_contacts)) { return; } From f78da0050b9984329867718e2a1bc9ab247b0dd4 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 18 Nov 2024 13:39:16 +0100 Subject: [PATCH 10/10] Update comment --- moveit_core/planning_scene/src/planning_scene.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index f9a0964c6d..2c3241c4ae 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -437,7 +437,8 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req.pad_environment_collisions ? getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm) : getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); - // Return early if a collision was found or the maximum number of allowed contacts is exceeded + // Return early if a collision was found and the number of contacts found already exceed `req.max_contacts`, if + // `req.contacts` is enabled. if (res.collision && (!req.contacts || res.contacts.size() >= req.max_contacts)) { return;