Allow restricting collision pairs to a group (moveit#2987)

v4hn · Dec 10, 2021 · 9bfb2c7 · 9bfb2c7
1 parent 9d78be4
commit 9bfb2c7
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 13 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
@@ -560,37 +560,42 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
     getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
   }
 
-  /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */
+  /** \brief Get the names of the links that are involved in collisions for the state \e robot_state.
+   *  Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         const moveit::core::RobotState& robot_state) const
+                         const moveit::core::RobotState& robot_state, const std::string& group_name = "") const
   {
-    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
+    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
   }
 
   /** \brief Get the names of the links that are involved in collisions for the state \e robot_state.
-      Update the link transforms for \e robot_state if needed. */
+      Update the link transforms for \e robot_state if needed.
+      Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         moveit::core::RobotState& robot_state) const
+                         moveit::core::RobotState& robot_state, const std::string& group_name = "") const
   {
     robot_state.updateCollisionBodyTransforms();
-    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
+    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix(),
+                      group_name);
   }
 
   /** \brief  Get the names of the links that are involved in collisions for the state \e robot_state given the
-      allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */
+      allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed.
+      Can be restricted to links part of or updated by \e group_name*/
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         moveit::core::RobotState& robot_state,
-                         const collision_detection::AllowedCollisionMatrix& acm) const
+                         moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm,
+                         const std::string& group_name = "") const
   {
     robot_state.updateCollisionBodyTransforms();
-    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm);
+    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm, group_name);
   }
 
   /** \brief  Get the names of the links that are involved in collisions for the state \e robot_state given the
-      allowed collision matrix (\e acm) */
+      allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                          const moveit::core::RobotState& robot_state,
-                         const collision_detection::AllowedCollisionMatrix& acm) const;
+                         const collision_detection::AllowedCollisionMatrix& acm,
+                         const std::string& group_name = "") const;
 
   /**@}*/
 

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -539,12 +539,14 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont
 
 void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                                       const moveit::core::RobotState& robot_state,
-                                      const collision_detection::AllowedCollisionMatrix& acm) const
+                                      const collision_detection::AllowedCollisionMatrix& acm,
+                                      const std::string& group_name) const
 {
   collision_detection::CollisionRequest req;
   req.contacts = true;
   req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
   req.max_contacts_per_pair = 1;
+  req.group_name = group_name;
   collision_detection::CollisionResult res;
   checkCollision(req, res, robot_state, acm);
   res.contacts.swap(contacts);