Improved Collision Checking and Recovery

Added service and feature to recover along executed trajectory back to safe state (q). This is a moving window of n length array (n being the DOF of the robot). The larger this window is, the more the arm will move back to a defined safe position. Default size of window is 200
Clean up and functionality of trajectory checking of collisions with dynamically added shapes
Added cuboid input of dynamic shapes (supported with cylinders and spheres)
Added initial controller switching function to ROS backend. Needs testing with real robot
Confirmed functionality with Panda and xarm (simulated). Noted issue with UR for ghost robot to be investigated

armer/armer.py

@@ -306,7 +306,7 @@ def global_collision_check(self, robot: ROSRobot):
             debug=True
         )
         end = timeit.default_timer()
-        print(f"[KD Setup] full collision check: {1/(end-start)} hz")
+        # print(f"[KD Setup] full collision check: {1/(end-start)} hz")
         # print(f"[Check Links] -> {check_links}")
 
         # Alternative Method
@@ -326,7 +326,7 @@ def global_collision_check(self, robot: ROSRobot):
         # 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}")
+            # 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.
@@ -463,14 +463,14 @@ def run(self) -> None:
                     # TODO: Each robot performs its own self check (possibly saves some time?)
                     # Each robot is then checked with a global collision check (environment and other robots)
                     # If collisions are found (self or with global) preemption signal sent to each robot object.
-                    # start = timeit.default_timer()
                     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()
-
-                    # end = timeit.default_timer()
-                    # print(f"[Current] full collision check: {1/(end-start)} hz")  
+
+                    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)

armer/backends/ROS/ROS.py

@@ -107,8 +107,18 @@ def step(self, dt=0.01):
             # NOTE: the following two lines are needed in order for collision checking to work
             self.robots[robot_name]._update_link_tf()
             self.robots[robot_name]._propogate_scene_tree()
-            # Publishes out to ROS via configured control topic (joint_group_vel_controller)
-            self.robots[robot_name].publish()
+
+
+            if self.robots[robot_name].controller_type_request == 1:
+                # Joint Trajectory control
+                self.robots[robot_name].controller_select(1)
+                self.robots[robot_name].execute_trajectory()
+
+                # Reset back to standard joint group velocity controller
+                self.robots[robot_name].controller_select(0)
+            else:
+                # Publishes out to ROS via configured control topic (joint_group_vel_controller)
+                self.robots[robot_name].publish()
 
         # Update world transforms of dynamic objects
         # TODO: test with real robot