Feature/collision mgmt multiprocess

body/stretch_body/dynamixel_hello_XL430.py

@@ -106,6 +106,8 @@ def __init__(self, name, chain=None, usb=None):
             self.total_range = abs(self.ticks_to_world_rad(self.params['range_t'][0]) - self.ticks_to_world_rad(self.params['range_t'][1]))
             self.in_collision_stop = {'pos': False, 'neg': False}
             self.ts_collision_stop = {'pos': 0.0, 'neg': 0.0}
+            self.forced_collision_stop_override = {'pos': False, 'neg':False}
+            self._in_collision_stop_override = False
         except KeyError:
             self.motor=None
 
@@ -449,22 +451,33 @@ def step_collision_avoidance(self,in_collision):
         #     print('Invalid IN_COLLISION for joint %s'%self.name)
         #     return
 
+        if True in self.forced_collision_stop_override.values():
+            if not self._in_collision_stop_override:
+                self.quick_stop()
+                self._in_collision_stop_override = True
+                return
+
+        if not True in self.forced_collision_stop_override.values():
+            self._in_collision_stop_override = False
+
+
         for dir in ['pos','neg']:
             if in_collision[dir] and not self.in_collision_stop[dir]:
                 # Stop current motion
                 self.ts_collision_stop[dir] = time.time()
-                self.quick_stop()
+                if not self.was_runstopped:
+                    self.quick_stop()
                 self.in_collision_stop[dir] = True
-                self.last_collision_pair_min_dist = in_collision['las_cp_min_dist']
+                # self.last_collision_pair_min_dist = in_collision['las_cp_min_dist']
+                # self.last_cfg_thresh = in_collision['last_joint_cfg_thresh']
 
             #Reset if out of collision (at least 1s after collision)
-            if self.in_collision_stop[dir]  and not in_collision[dir] and time.time()-self.ts_collision_stop[dir]>1.0:
-                # Check if the minimum distance between the last active collision pair has changed before reset
-                if in_collision['las_cp_min_dist']:
-                    print(f"[{self.name}] Joint in collision {in_collision['las_cp_min_dist']}")
-                    if self.last_collision_pair_min_dist['pair_name']==in_collision['las_cp_min_dist']['pair_name']:
-                        if abs(self.last_collision_pair_min_dist['dist'] - in_collision['las_cp_min_dist']['dist'])>0.03:
-                            self.in_collision_stop[dir] = False
+            if self.in_collision_stop[dir]  and not in_collision[dir] and time.time()-self.ts_collision_stop[dir]>1:
+                self.in_collision_stop[dir] = False
+                # if abs(self.last_cfg_thresh - in_collision['last_joint_cfg_thresh']) > 0.001:
+                #     self.in_collision_stop[dir] = False
+                # if  abs(self.status['vel'])<0.001:
+                #     self.in_collision_stop[dir] = False
 
     def get_braking_distance(self,acc=None):
         """Compute distance to brake the joint from the current velocity"""
@@ -591,6 +604,14 @@ def set_velocity(self,v_des,a_des=None):
             self.enable_torque()
         v = min(self.params['motion']['max']['vel'],abs(v_des))
 
+        if self.forced_collision_stop_override['pos'] and v_des>0:
+            self.logger.warning(f"Forced Collision stop")
+            return
+
+        if self.forced_collision_stop_override['neg'] and v_des<0:
+            self.logger.warning(f"Forced Collision stop")
+            return
+
         if self.in_collision_stop['pos'] and v_des>0:
             self.logger.warning('set_velocity in collision . Motion disabled in direction %s for %s. Not executing set_velocity'%('pos',self.name))
             return

body/stretch_body/end_of_arm.py

@@ -128,7 +128,9 @@ def get_joint_configuration(self,braked=False):
                     dx=0
             ret[j] = motor.status['pos'] + dx
         return ret
-
+
+    def pre_stow(self,robot=None):
+        pass
 
 
 

body/stretch_body/end_of_arm_tools.py

@@ -75,7 +75,7 @@ def home(self):
         self.motors['wrist_roll'].move_to(0)
         self.motors['wrist_yaw'].home()
 
-    def pre_stow(self):
+    def pre_stow(self,robot=None):
         if 'wrist_pitch' in self.end_of_arm.joints:
             self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch'])
 
@@ -105,8 +105,10 @@ def home(self):
         self.motors['wrist_roll'].move_to(self.params['stow']['wrist_roll'])
         self.motors['wrist_yaw'].home()
 
-    def pre_stow(self):
-        if 'wrist_pitch' in self.end_of_arm.joints:
+    def pre_stow(self,robot=None):
+        if robot:
+            robot.end_of_arm.move_to('wrist_pitch', robot.end_of_arm.params['stow']['wrist_pitch'])
+        else:
             self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch'])
 
 class EOA_Wrist_DW3_Tool_SG3(EndOfArm):
@@ -138,8 +140,10 @@ def home(self):
         self.motors['wrist_yaw'].home()
         self.motors['stretch_gripper'].home()
 
-    def pre_stow(self):
-        if 'wrist_pitch' in self.end_of_arm.joints:
+    def pre_stow(self,robot=None):
+        if robot:
+            robot.end_of_arm.move_to('wrist_pitch', robot.end_of_arm.params['stow']['wrist_pitch'])
+        else:
             self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch'])
 
 class EOA_Wrist_DW3_Tool_Tablet_12in(EndOfArm):
@@ -156,7 +160,7 @@ def __init__(self, name='eoa_wrist_dw3_tool_tablet_12in'):
             'joint_wrist_pitch': 'wrist_pitch',
             'joint_wrist_roll':'wrist_roll' #Not mapping fingers for collision mgmt yet
         }
-    
+
     def move_by(self, joint, x_r, v_r=None, a_r=None, enable_wrist_roll = False):
         # Lock wrist roll by default
         if joint=='wrist_roll':
@@ -228,4 +232,20 @@ def home(self):
         self.motors['wrist_yaw'].home()
         self.motors['wrist_pitch'].move_to(self.params['stow']['wrist_pitch'])
         self.motors['wrist_yaw'].move_to(1.57)
-        time.sleep(1)
+        time.sleep(2)
+
+    def step_sentry(self, robot):
+        super().step_sentry(robot)
+        if robot.collision.running:
+            wrist_p = self.get_joint('wrist_pitch').status['pos']
+            wrist_y = self.get_joint('wrist_yaw').status['pos']
+            # TODO: Add more special conditions around this part
+            if wrist_p > -0.3 and wrist_y < 0.18:
+                # print("In Special Stop")
+                self.get_joint('wrist_yaw').forced_collision_stop_override = {'pos': False, 'neg':True}
+                self.get_joint('wrist_pitch').forced_collision_stop_override = {'pos': True, 'neg':False}
+
+            else:
+                # print("Out Special Stop")
+                self.get_joint('wrist_yaw').forced_collision_stop_override = {'pos': False, 'neg':False}
+                self.get_joint('wrist_pitch').forced_collision_stop_override = {'pos': False, 'neg':False}

body/stretch_body/gamepad_controller.py

@@ -334,7 +334,7 @@ def main():
             for k in state.keys():
                 print(k, ' : ', state[k])
             print('------------------------------')
-            time.sleep(1.0)
+            time.sleep(0.1)
     except (KeyboardInterrupt, SystemExit):
         pass
 

body/stretch_body/gamepad_teleop.py

@@ -53,7 +53,7 @@ def __init__(self, robot_instance = True, print_dongle_status = True, lock=None,
 
         self.end_of_arm_tool =RobotParams().get_params()[1]['robot']['tool']
 
-        self.sleep = 1/50
+        self.sleep = 1/15
         self.print_mode = False
         self._i = 0
 

body/stretch_body/prismatic_joint.py

@@ -44,6 +44,7 @@ def __init__(self,name,usb=None):
         self.total_range = abs(self.params['range_m'][1] - self.params['range_m'][0])
         self.in_collision_stop = {'pos': False, 'neg': False}
         self.ts_collision_stop = {'pos': 0, 'neg': 0}
+        self.collision_till_zero_vel_counter = 0
 
     # ###########  Device Methods #############
     def startup(self, threaded=True):
@@ -591,24 +592,27 @@ def step_collision_avoidance(self,in_collision):
         # if in_collision['pos'] and in_collision['neg']:
         #     print('Invalid IN_COLLISION for joint %s'%self.name)
         #     return
-
         for dir in ['pos','neg']:
             if in_collision[dir] and not self.in_collision_stop[dir]:
                 # Stop current motion
                 self.motor.enable_safety()
                 self.push_command()
                 self.in_collision_stop[dir] = True
                 self.ts_collision_stop[dir]=time.time()
-                self.last_collision_pair_min_dist = in_collision['las_cp_min_dist']
+                self.collision_till_zero_vel_counter = 0
+                # self.last_cfg_thresh = in_collision['last_joint_cfg_thresh']
 
             #Reset if out of collision (at least 1s after collision)
             if self.in_collision_stop[dir]  and not in_collision[dir] and time.time()-self.ts_collision_stop[dir]>1.0:
-                # Check if the minimum distance between the last active collision pair has changed before reset
-                if in_collision['las_cp_min_dist']:
-                    print(f"[{self.name}] Joint in collision {in_collision['las_cp_min_dist']}")
-                    if self.last_collision_pair_min_dist['pair_name']==in_collision['las_cp_min_dist']['pair_name']:
-                        if abs(self.last_collision_pair_min_dist['dist'] - in_collision['las_cp_min_dist']['dist'])>0.03:
-                            self.in_collision_stop[dir] = False
+                self.in_collision_stop[dir] = False
+                # if abs(self.last_cfg_thresh - in_collision['last_joint_cfg_thresh']) > 0.001 and abs(self.status['vel'])<0.001:
+                #     self.in_collision_stop[dir] = False
+                # if abs(self.status['vel'])<0.001:
+                #     self.collision_till_zero_vel_counter = self.collision_till_zero_vel_counter + 1
+                # if self.collision_till_zero_vel_counter>50:
+                #     self.in_collision_stop[dir] = False
+                #     self.collision_till_zero_vel_counter = 0
+
 
 
 

body/stretch_body/robot.py

@@ -156,8 +156,8 @@ def step(self):
         if self.robot.params['use_sentry']:
             if (self.titr % self.sentry_downrate_int) == 0:
                 self.robot._step_sentry()
-        if self.robot.is_homed():
-            self.robot.collision.step()
+        # if self.robot.is_homed():
+        #     self.robot.collision.step()
         if (self.titr % self.trajectory_downrate_int) == 0:
             self.robot._update_trajectory_non_dynamixel()
         self.stats.mark_loop_end()
@@ -170,6 +170,36 @@ def run(self):
                 self.step()
         self.robot.logger.debug('Shutting down SystemMonitorThread')
 
+class CollisionMonitorThread(threading.Thread):
+    """
+    This thread runs at 25Hz.
+    It updates the status data of the Devices.
+    It also steps the Sentry, Monitor, and Collision functions
+    """
+    def __init__(self, robot, target_rate_hz=25.0):
+        threading.Thread.__init__(self, name = self.__class__.__name__)
+        self.robot=robot
+        self.robot_update_rate_hz = target_rate_hz
+        self.shutdown_flag = threading.Event()
+        self.stats = hello_utils.LoopStats(loop_name='CollisionMonitorThread',target_loop_rate=self.robot_update_rate_hz)
+        self.titr=0
+        self.running=False
+
+    def step(self):
+        self.titr = self.titr + 1
+        self.stats.mark_loop_start()
+        if self.robot.is_homed():
+            self.robot.collision.step()
+        self.stats.mark_loop_end()
+
+    def run(self):
+        self.running=True
+        while not self.shutdown_flag.is_set():
+            self.stats.wait_until_ready_to_run()
+            if not self.shutdown_flag.is_set():
+                self.step()
+        self.robot.logger.debug('Shutting down CollisionMonitorThread')
+
 class Robot(Device):
     """
     API to the Stretch Robot
@@ -228,6 +258,7 @@ def __init__(self):
         self.sys_thread = None
         self.dxl_head_thread = None
         self.event_loop_thread = None
+        self.collision_mgmt_thread = None
 
         self.eoa_name= self.params['tool']
         module_name = self.robot_params[self.eoa_name]['py_module_name']
@@ -309,6 +340,7 @@ def startup(self,start_non_dxl_thread=True,start_dxl_thread=True,start_sys_mon_t
         self.dxl_end_of_arm_thread = DXLEndOfArmStatusThread(self,target_rate_hz=self.params['rates']['DXLStatusThread_Hz'])
         self.sys_thread = SystemMonitorThread(self, target_rate_hz=self.params['rates']['SystemMonitorThread_Hz'])
         self.dxl_head_thread = DXLHeadStatusThread(self, target_rate_hz=self.params['rates']['DXLStatusThread_Hz'])
+        self.collision_mgmt_thread = CollisionMonitorThread(self, target_rate_hz=100)
 
         if start_non_dxl_thread:
             self.non_dxl_thread.setDaemon(True)
@@ -327,6 +359,10 @@ def startup(self,start_non_dxl_thread=True,start_dxl_thread=True,start_sys_mon_t
             self.sys_thread.setDaemon(True)
             self.sys_thread.start()
 
+        if start_sys_mon_thread and self.collision_mgmt_thread:
+            self.collision_mgmt_thread.setDaemon(True)
+            self.collision_mgmt_thread.start()
+
         return success
 
     def stop(self):
@@ -352,6 +388,10 @@ def stop(self):
             if self.sys_thread.running:
                 self.sys_thread.shutdown_flag.set()
                 self.sys_thread.join(1)
+        if self.collision_mgmt_thread:
+            if self.collision_mgmt_thread.running:
+                self.collision_mgmt_thread.shutdown_flag.set()
+                self.collision_mgmt_thread.join(1)
         for k in self.devices:
             if self.devices[k] is not None:
                 self.logger.debug('Shutting down %s'%k)