Improved overlap capture for multi-robot handling

qcr · Oct 25, 2023 · 1a8deab · 1a8deab
1 parent f200ffd
commit 1a8deab
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 7 deletions.
diff --git a/armer/armer.py b/armer/armer.py
@@ -288,6 +288,7 @@ def global_collision_check(self, robot: ROSRobot):
         for robot_name in self.global_collision_dict.keys():
             # print(f"Checking {robot.name} against robot in dict: {robot_name}")
             for link_name in self.global_collision_dict[robot_name]:
+                # Handle Self Checking with known Overlaps
                 if robot.name == robot_name and link_name in robot.overlapped_link_dict.keys():
                     ignore_list = robot.overlapped_link_dict[link_name]
                 else:

diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py
@@ -1748,26 +1748,48 @@ def characterise_collision_overlaps(self) -> bool:
             rospy.logerr(f"Characterise Collision Overlaps -> collision dictionary is invalid: [{self.collision_dict}]")
             return False
 
-        # Iterate backwards starting with gripper
-        # NOTE: starts at gripper and ends at robot base
-        #       the assumption here is that other external links are not 'part' of the arm
-        #link=self.link_dict[self.gripper]   
-        #while link is not None:
+        # Iterate forwards starting at base of tree
         for link in self.links:
             collision = True
             links_in_collision_list = []
             while collision:
                 # print(f"CHECKING Link name: {link.name}")
                 # Check link collision of current link
-                col_link, collision = self.check_link_collision(target_link=link.name, stop_link=self.gripper, check_list=self.collision_dict[link.name], ignore_list=links_in_collision_list)
+                col_link, collision = self.check_link_collision(
+                    target_link=link.name, 
+                    stop_link=self.gripper, 
+                    check_list=self.collision_dict[link.name], 
+                    ignore_list=links_in_collision_list
+                )
 
                 if collision:    
                     # print(f"INIT: collision found for {link.name} with {col_link.name}")
                     links_in_collision_list.append(col_link.name)
 
             rospy.loginfo(f"Characterise Collision Overlaps -> Completed collision check for link: {link.name} -> found links in collision: {links_in_collision_list}")
             self.overlapped_link_dict[link.name] = links_in_collision_list
-            # link=link.parent
+
+        # Iterate over gripper links
+        # TODO: need a better way over two sequential loops, but not really a big issue as only at initialisation is this run
+        for glink in self.grippers:
+            collision = True
+            links_in_collision_list = []
+            while collision:
+                # print(f"CHECKING Link name: {link.name}")
+                # Check link collision of current link
+                col_link, collision = self.check_link_collision(
+                    target_link=glink.name, 
+                    stop_link=self.gripper, 
+                    check_list=self.collision_dict[glink.name], 
+                    ignore_list=links_in_collision_list
+                )
+
+                if collision:    
+                    # print(f"INIT: collision found for {link.name} with {col_link.name}")
+                    links_in_collision_list.append(col_link.name)
+
+            rospy.loginfo(f"Characterise Collision Overlaps -> Completed collision check for glink: {glink.name} -> found links in collision: {links_in_collision_list}")
+            self.overlapped_link_dict[glink.name] = links_in_collision_list
 
         # Reached end in success
         return True