Added license and more collision checking features

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2023, Queensland University of Technology (QUT) Centre for Robotics (QCR)
+Copyright (c) 2023, QUT Research Engineering Facility (REF)
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

armer/armer.py

@@ -14,10 +14,13 @@
 import yaml
 import roboticstoolbox as rtb
 import spatialmath as sm
+import timeit
 
 from typing import List, Dict, Any, Tuple
 from armer.utils import populate_transform_stamped
 from armer.robots import ROSRobot
+# Add cython global checker
+from armer.cython import collision_handler
 
 class Armer:
     """
@@ -66,10 +69,26 @@ def __init__(
 
         # Launch backend
         self.backend.launch(**(backend_args if backend_args else dict()))
-
+
+        # This is a global dictionary of dictionaries (per robot) for multi robot scenarios
+        self.global_collision_dict = dict()
         for robot in self.robots:
-            self.backend.add(robot)
+            # Add robot to backend (with low alpha for collision shapes)
+            self.backend.add(robot, collision_alpha=0.2)
 
+            # Resolve robot collision overlaps
+            robot.characterise_collision_overlaps()
+
+            # This method extracts all captured collision objects (dict) from each robot
+            # Needed to conduct global collision checking (if more than one robot instance is in play)
+            # NOTE: all robot instances read the same robot description param, so all robots will get the same description
+            #       this may not be the preferred implementation for future use cases.
+            self.global_collision_dict[robot.name] = robot.get_link_collision_dict()
+
+        # Handle any hardware required initialisation if available
+        if hasattr(self.backend, 'hw_initialise'):
+            self.backend.hw_initialise()
+
         for readonly, args in self.readonly_backends:
             readonly.launch(**args)
 
@@ -191,10 +210,128 @@ def load(nh, path: str) -> Armer:
             publish_transforms=publish_transforms,
             logging=logging
         )
+
+    def global_collision_check(self, robot: ROSRobot):
+        """
+        Conducts a full check for collisions
+        NOTE: takes a given robot object and runs its collision check (of its own dictionary) against the global dictionary
+                the global dictionary may have collision data from multiple robots (with different link data)
+        TODO: currently each robot is checked against its own link data. This is needed for self collision checking
+            but could be possibly optimised in some way as to not be overloaded with multiple instances
+        NOTE: [2023-10-31] Identified that this component is very inefficient for the panda (real test). Implemented 
+                a start and stop link (e.g., terminating search from end-effector to panda_link8, rather than full tree)
+        """
+        # Error handling on gripper name
+        if robot.gripper == None or robot.gripper == "":
+            robot.logger(f"Global Collision Check -> gripper name is invalid: {robot.gripper}", 'error')
+            return False
+
+        # Error handling on empty lick dictionary (should never happen but just in case)
+        if robot.link_dict == dict() or robot.link_dict == None:
+            robot.logger(f"Global Collision Check -> link dictionary is invalid: {robot.link_dict}", 'error')
+            return False
+
+        # Error handling on collision object dict and overlap dict
+        if robot.overlapped_link_dict == dict() or robot.overlapped_link_dict == None or \
+            robot.collision_dict == dict() or robot.collision_dict == None:
+            robot.logger(f"Global Collision Check -> collision or overlap dictionaries invalid: [{robot.collision_dict}] | [{robot.overlapped_link_dict}]", 'error')
+            return False
+
+        if robot.collision_sliced_links == None:
+            robot.logger(f"Global Collision Check -> could not get collision sliced links: [{robot.collision_sliced_links}]", 'error')
+            return False
+
+        # Debugging
+        # print(f"sliced links: {[link.name for link in robot.collision_sliced_links]}")
+        # print(f"col dict -> robots to check: {[robot for robot in self.global_collision_dict.keys()]}")
+        # print(f"col dict -> links to check as a dict: {[link for link in self.global_collision_dict.values()]}")
+        # print(f"panda_link5 check: {self.global_collision_dict['arm']['panda_link5']}")
+
+        start = timeit.default_timer()
+        # Creates a KD tree based on link locations (cartesian translation) to target link (sliced) and extracts 
+        # closest links to evaluate (based on dim; e.g., if dim=3, then the 3 closest links will be checked for that link)
+        # NOTE: this has yielded a notable improvement in execution without this method (approx. 120%). However, 
+        #       it is important to note that the link distances used in the tree are based on that link's origin point.
+        #       therefore, there may be cases where the size of the link (physically) is larger than its origin, meaning we miss it in eval
+        #       This is something to investigate to robustify this method.
+        check_links = robot.query_kd_nn_collision_tree(
+            sliced_links=robot.collision_sliced_links, 
+            dim=4,
+            debug=False
+        )
+        end = timeit.default_timer()
+        # print(f"[KD Setup] full collision check: {1/(end-start)} hz")
+        # print(f"[Check Links] -> {check_links}")
+
+        # Alternative Method
+        # NOTE: this has between 1-6% increase in speed of execution
+        start = timeit.default_timer()
+        col_link_id = collision_handler.global_check(
+            robot_name = robot.name,
+            robot_names = list(self.global_collision_dict.keys()),
+            len_robots = len(self.global_collision_dict.keys()),
+            robot_links = robot.collision_sliced_links,
+            len_links = len(robot.collision_sliced_links),
+            global_dict = self.global_collision_dict,
+            overlap_dict = robot.overlapped_link_dict,
+            check_links = check_links
+        )
+        end = timeit.default_timer()
+        # print(f"[Actual Link Check] full collision check: {1/(end-start)} hz")
+
+        if col_link_id >= 0:
+            # rospy.logwarn(f"Global Collision Check -> Robot [{robot.name}] in collision with link {robot.collision_sliced_links[col_link_id].name}")
+            return True
+        else:
+            # No collisions found with no errors identified.
+            return False
+
+    def update_dynamic_objects(self, robot: ROSRobot) -> None:
+        """
+        method to handle the addition and removal of dynamic objects per robot instance
+        """
+        # Check if the current robot has any objects that need removal
+        if robot.dynamic_collision_removal_dict:
+            for d_obj_name in list(robot.dynamic_collision_removal_dict.copy().keys()):
+                robot.logger(f"Removal of Dynamic Objects in Progress", 'warn')
+                # remove from backend
+                # NOTE: there is a noted bug in the swift backend that sets the object 
+                #       (in a separate dictionary called swift_objects) to None. In the self.backend.step()
+                #       method below, this attempts to run some methods that belong to the shape but cannot do so
+                #       as it is a NoneType.
+                shape_to_remove = robot.dynamic_collision_removal_dict[d_obj_name].shape
+                robot.logger(f"Remove object is: {shape_to_remove}")
+                # TODO: add this feature in once swift side is fixed 
+                #       should still work for ROS backend
+                # self.backend.remove(shape_to_remove)
+                # remove from robot dict
+                robot.dynamic_collision_removal_dict.pop(d_obj_name)
+                robot.logger(f"Removed successfully")
+        else:
+            # Check if the current robot has any newly added objects to add to the backend
+            # NOTE: this loop is run everytime at the moment (not an issue with limited shapes but needs better optimisation for scale)
+            for dynamic_obj in robot.dynamic_collision_dict.copy().values():
+                if dynamic_obj.is_added == False:
+                    robot.logger(f"Adding Dynamic Object: {dynamic_obj}")
+                    dynamic_obj.id = self.backend.add(dynamic_obj.shape)
+                    dynamic_obj.is_added = True
+                    robot.logger(f"Added Successfully")
 
     def step(self, dt: float, current_time: float) -> None:
         """Main step method - controlled by ROS2 node"""
         for robot in self.robots:
+            if self.global_collision_check(robot=robot) and robot.preempted == False:
+                # Current robot found to be in collision so preempt
+                robot.collision_approached = True
+                robot.preempt()
+
+            if robot.preempted == False:
+                # Set the safe state of robot for recovery on collisions if needed
+                robot.set_safe_state()
+
+            # Check if the current robot has any  dynamic objects that need backend update
+            self.update_dynamic_objects(robot=robot)
+
             robot.step(dt=dt)
 
         # with Timer('step'):

armer/entry_point.py

@@ -21,9 +21,8 @@
 __path__ = ament_index_python.packages.get_package_share_directory('armer')
 
 class ArmerNode(Node):
-  """Main ARMer Node
-  
-  The main Node running in ROS2
+  """
+  Main ARMer Node running in ROS2
   """
   def __init__(self):
     super().__init__('armer')
@@ -49,10 +48,10 @@ def __init__(self):
     )
 
   def timer_callback(self):
-    """ROS2 Callback Mechanism
+    """
+    ROS2 Callback Mechanism
     
-    Set to configured frequency (init)
-    NOTE: 
+    Set to configured frequency (init) 
     """
     current_time = self.get_clock().now()
     # Get dt in seconds (REQUIRED)
@@ -67,6 +66,9 @@ def timer_callback(self):
     self.last_time = current_time
 
 def main(args=None):
+    """
+    Initialises and executes the node
+    """
     rclpy.init(args=args)
 
     try:

armer/robots/ROSRobot.py

@@ -17,6 +17,8 @@
 import numpy as np
 import yaml
 import time
+import timeit
+import json
 
 from typing import List, Any
 from threading import Lock, Event
@@ -33,7 +35,7 @@
 from armer_msgs.msg import ManipulatorState, JointVelocity, ServoStamped, Guards
 from armer_msgs.action import GuardedVelocity, Home, MoveToJointPose, MoveToNamedPose, MoveToPose
 from armer_msgs.srv import GetNamedPoses, AddNamedPose, RemoveNamedPose, AddNamedPoseConfig, RemoveNamedPoseConfig, GetNamedPoseConfigs 
-from geometry_msgs.msg import TwistStamped, Twist, PoseStamped, Pose
+from geometry_msgs.msg import TwistStamped, Twist, PoseStamped, Pose, Point
 from std_msgs.msg import Header, Float64MultiArray, Bool
 from sensor_msgs.msg import JointState
 
@@ -138,12 +140,6 @@ def __init__(self,
             self.logger(f"Configured gripper name {self.gripper} not in link tree -> defaulting to top of stack: {default_top_link_name}")
             self.gripper = default_top_link_name
 
-        link=self.link_dict[self.gripper]   
-        while link is not None:
-            self.sorted_links.append(link)
-            link=link.parent
-        self.sorted_links.reverse()
-
         # --- Collision Checking Setup Section --- #
         # Loops through links (as read in through URDF parser)
         # Extracts each link's list of collision shapes (of type sg.Shape). TODO: add a validity check on type here
@@ -162,6 +158,13 @@ def __init__(self,
         self.dynamic_collision_dict = dict()
         self.dynamic_collision_removal_dict = dict()
         self.collision_approached = False
+
+        link=self.link_dict[self.gripper]   
+        while link is not None:
+            self.collision_dict[link.name] = link.collision.data if link.collision.data else []
+            self.sorted_links.append(link)
+            link=link.parent
+        self.sorted_links.reverse()
         # Check for external links (from robot tree)
         # This is required to add any other links (not specifically part of the robot tree) 
         # to our collision dictionary for checking
@@ -1015,7 +1018,7 @@ def collision_marker_debugger(self, sliced_link_names: list = [], check_link_nam
                     else:
                         marker.header.frame_id = link
 
-                    marker.header.stamp = rospy.Time.now()
+                    # marker.header.stamp = self.get_time(as_float=False)
                     if shape.stype == 'sphere':
                         marker.type = 2
                         # Expects diameter (m)

armer/utils.py

@@ -89,10 +89,10 @@ def mjtg(robot: rtb.robot, qf: np.ndarray, max_speed: float=0.2, max_rot: float=
     #---------------- End
 
     move_time = max(linear_move_time, angular_move_time)
-    rospy.loginfo(f'Estimated move time of {move_time} (max of) | lin {linear_move_time} | ang {angular_move_time}')
+    robot.logger(f'Estimated move time of {move_time} (max of) | lin {linear_move_time} | ang {angular_move_time}')
     # Edited as part of branch hotfix/96fd293: termination on invalid trajectory
     if move_time == 0:
-        rospy.logerr(f"Trajectory is invalid --> Cannot Solve.")
+        robot.logger(f"Trajectory is invalid --> Cannot Solve.", 'error')
         return Trajectory(name='invalid', t=1, s=robot.q, sd=None, sdd=None, istime=False)
 
     # Obtain minimum jerk velocity profile of joints based on estimated end effector move time

cfg/panda_sim.yaml

@@ -3,6 +3,15 @@ robots:
     model: 
       urdf_file: franka_description/robots/panda_arm_hand.urdf.xacro
       qr: [0, -0.3, 0, -2.2, 0, 2.0, 0.78539816339]
+      # This is the definition of the default control end-effector link (change as needed)
+      gripper: panda_hand 
+      # NOTE: this defines the start and stop links for definig link collision checking window
+      # NOTE: this assumes the sorted links are in order (start is top of tree to stop at base)
+      # NOTE: start defaults to the robot's gripper if not defined
+      # NOTE: stop defaults to the robot's base link if not defined
+      # The closer this is to the end-effector (set in gripper) the faster the checking will perform
+      collision_check_start_link: panda_hand
+      collision_check_stop_link: panda_link7
     singularity_thresh: 0.02
 backend: 
   type: roboticstoolbox.backends.swift.Swift

package.xml

@@ -10,7 +10,7 @@
 
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>MIT</license>
+  <license>BSD3</license>
 
   <!-- ROS version handling -->
   <exec_depend>rclpy</exec_depend>