fix lift runstop drop and adjust homing sensitivity

stretch4_body/behavior/routines/routine.py

@@ -21,7 +21,7 @@ def startup(self):
         return Device.startup(self)
 
     def check_runstop(self):
-        if 'power_periph' in self.robot.status and self.robot.status['power_periph']['runstop_event']:
+        if not self.is_canceled and 'power_periph' in self.robot.status and self.robot.status['power_periph']['runstop_event']:
             self.logger.warning(f"Routine {self.name} canceled during `check_runstop`.")
             self.cancel()
 
@@ -33,10 +33,10 @@ def update_controller(self):
             return False
         self.check_runstop()
         if self.is_canceled:
-            self.logger.warning(f"Routine {self.name} canceled during `update_controller`.")
-            return False
+            self.logger.warning(f"Routine {self.name} exiting from `update_controller`.")
+            #return False
         do_continue= self.robot.cb_routine_update_controller()
-        return do_continue and not self.is_timed_out()
+        return do_continue and not self.is_timed_out() and not self.is_canceled
 
     def get_run_time(self):
         return time.time()-self.ts_start

stretch4_body/behavior/routines/routine_homing.py

@@ -189,7 +189,6 @@ def _run_homing(self,cmd_id,*args, **kwargs):
         self.wait_duration(0.5)
         self.joint.motor.mark_position_on_contact(x)
         self.update_controller()
-
         if self.wait_until_contact(self.joint.motor, timeout=15.0, t_ignore=0.5):
             self.wait_duration(t=1.0)
             self.logger.info(f'Hardstop detected at motor position (rad) {self.joint.motor.status["pos"]}')
@@ -206,19 +205,20 @@ def _run_homing(self,cmd_id,*args, **kwargs):
                 self.logger.warning('%s homing failed. Failed to detect contact' % self.name.capitalize())
             success = False
         self.wait_duration(0.5)  # Allow time to settle
-        if success:
-            self.joint.move_to(x_m=end_pos, req_calibration=False)
-            if not self.wait_until_at_setpoint(self.joint.motor, timeout=10.0, t_ignore=0.5):
-                self.logger.warning('%s failed to reach final position' % self.joint.name.capitalize())
-                success = False
-
         # Restore previous modes
         self.joint.motor.gains['enable_guarded_mode'] = prev_enable_guarded_mode
         self.joint.motor.gains['enable_sync_mode'] = prev_enable_sync_mode
         self.joint.motor.gains['safety_hold']=prev_safety_hold
         self.joint.motor.gains['safety_stiffness'] = prev_safety_stiffness
         self.joint.motor.set_gains()
         self.update_controller()
+        if success:
+            self.joint.move_to(x_m=end_pos, req_calibration=False)
+            if not self.wait_until_at_setpoint(self.joint.motor, timeout=10.0, t_ignore=0.5):
+                self.logger.warning('%s failed to reach final position' % self.joint.name.capitalize())
+                success = False
+
+
 
         if success:
             self.logger.info('%s homing successful' % self.joint.name.capitalize())

stretch4_body/behavior/sentries/sentry_joint_runaway.py

@@ -13,7 +13,7 @@ def __init__(self, robot):
 
     def step(self):
         if self.robot.get_subsystem('lift') is not None:
-            if abs(self.robot.lift.status['vel'])>self.params['lift_runaway_vel'] and self.robot.lift.motor.hw_valid:
+            if self.robot.lift.status['vel']<0 and abs(self.robot.lift.status['vel'])>self.params['lift_runaway_vel'] and self.robot.lift.motor.hw_valid:
                 self.logger.warning('Runaway velocity of lift of %f m/s. Shutting down actuator!'%self.robot.lift.status['vel'])
                 self.logger.warning('Stop the server with stretch_body_server --kill')
                 self.logger.warning('Run REx_actuator_control --lift to re-enable joint')

stretch4_body/core/prismatic_joint.py

@@ -511,6 +511,8 @@ def wait_for_contact(self, timeout=5.0):
             self.pull_status()
             if self.motor.status['in_guarded_event']:
                 return True
+            if self.motor.status['runstop_on']:
+                return False
             time.sleep(0.01)
         return False
 
@@ -675,6 +677,7 @@ def home(self):
         else:
             self.logger.warning('%s homing failed. Failed to detect contact' % self.name.capitalize())
             self.motor.reset_mark_position_on_contact()
+            self.motor.enable_safety()
             self.push_command()
             success = False
         # input('Enter to continue2')

stretch4_body/robot/robot_params_SE4.py

@@ -1065,11 +1065,11 @@
         'usb_name': '/dev/hello-motor-lift',
         'use_vel_traj': 1,
         'calibration_range_bounds': [1.197,1.203 ],
-        'i_feedforward': 1.5,
+        'i_feedforward': 1.7,
         # Absolute hardstop limits. Tune carefully to prevent physical damage.
         'range_m' : [0.0, 1.20], #Calder no shells, [0.0, 1.21], #Dali w/ shells, [0.0, 1.20]
         # Homing procedure settings. contact_sensitivity adjusts how hard the lift hits the hardstop.
-        'homing': {'contact_sensitivity': 0.0,'end_pos':0.5,'v_m':0.15,'a_m':0.3,'to_positive_stop':True,'safety_hold':1,'safety_stiffness':0.1},
+        'homing': {'contact_sensitivity': 0.5,'end_pos':0.5,'v_m':0.15,'a_m':0.3,'to_positive_stop':True,'safety_hold':1,'safety_stiffness':0.2},
         #'homing': {'contact_sensitivity': 0.2,'end_pos':0.5,'v_m':0.15,'a_m':0.3,'to_positive_stop':True,'safety_hold':1,'safety_stiffness':0.7},
         'belt_pitch_m': 0.005,
           'motion':{