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/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index db246518fd..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 @@ -150,6 +150,12 @@ struct CollisionRequest * are included. */ std::string group_name = ""; + /** \brief If true, use padded collision environment */ + bool pad_environment_collisions = true; + + /** \brief If true, do self collision check with padded robot links */ + 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 3d71a117e7..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 @@ -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,11 +365,7 @@ 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). */ @@ -387,99 +376,69 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \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..2c3241c4ae 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,17 +417,15 @@ 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, static_cast(robot_state), acm); } void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, @@ -430,24 +434,64 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the padded version - getCollisionEnv()->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 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; + } // 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.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) { - if (getCurrentState().dirtyCollisionBodyTransforms()) - { - checkCollisionUnpadded(req, res, getCurrentStateNonConst(), getAllowedCollisionMatrix()); - } - else - { - checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix()); - } + collision_detection::CollisionRequest new_req = req; + new_req.pad_environment_collisions = 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.pad_environment_collisions = 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.pad_environment_collisions = 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.pad_environment_collisions = 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.pad_environment_collisions = false; + checkCollision(new_req, res, static_cast(robot_state), acm); } void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, @@ -455,14 +499,59 @@ 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.pad_environment_collisions = 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()); +} + +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()) + { + 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()) { - getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); + 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.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_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp index e55b0f15d4..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,7 +363,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), @@ -375,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, @@ -392,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( @@ -410,7 +413,6 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in collision otherwise false. )") - .def("check_self_collision", py::overload_cast( &planning_scene::PlanningScene::checkSelfCollision), @@ -481,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 diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 252d2a98d6..a53772b130 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.pad_environment_collisions = 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..65c46322cc 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.pad_environment_collisions = 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; }