diff --git a/body/stretch_body/dynamixel_hello_XL430.py b/body/stretch_body/dynamixel_hello_XL430.py index 384bbb44..47ccc9ec 100644 --- a/body/stretch_body/dynamixel_hello_XL430.py +++ b/body/stretch_body/dynamixel_hello_XL430.py @@ -345,11 +345,11 @@ def pretty_print(self): #self.motor.pretty_print() def step_sentry(self, robot): - if self.hw_valid and self.robot_params['robot_sentry']['dynamixel_stop_on_runstop'] and self.params['enable_runstop']: is_runstopped = robot.pimu.status['runstop_event'] if is_runstopped is not self.was_runstopped: if is_runstopped: + self.stop_trajectory() self.disable_torque() else: self.enable_torque() @@ -538,9 +538,11 @@ def update_trajectory(self): with `self.startup(threaded=True)`, a background thread is launched for this. Otherwise, the user must handle calling this method. """ - # check if joint valid and previous trajectory not executing + # check if joint valid, previous trajectory not executing, and not runstopped if not self.hw_valid or self._waypoint_ts is None: return + if self.was_runstopped: + return if (time.time() - self._waypoint_ts) < self.trajectory[-1].time: p1, _, _ = self.trajectory.evaluate_at(time.time() - self._waypoint_ts)