Add use_padding flag + deprecate checkCollisionUnpadded() functions (#3088)

* Add use_padding flag + deprecate Unpadded... functions

* Fix CI

* Remove deprecated functions from python API

* Update #pragmas

* Address review

* Add CHANGELOG comment

* Remove warning

* Update MIGRATION guide

* Update early exit logic in checkCollision

* Update comment

Author: sjahr

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

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;
 

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<const moveit::core::RobotState&>(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<const moveit::core::RobotState&>(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<const moveit::core::RobotState&>(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<const moveit::core::RobotState&>(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<const moveit::core::RobotState&>(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<const moveit::core::RobotState&>(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<std::string>& links);