Skip to content

Commit

Permalink
Pressing runstop halts dxl waypoint traj updates
Browse files Browse the repository at this point in the history
  • Loading branch information
hello-binit committed Oct 6, 2021
1 parent 089fb01 commit b21bd01
Showing 1 changed file with 4 additions and 2 deletions.
6 changes: 4 additions & 2 deletions body/stretch_body/dynamixel_hello_XL430.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit b21bd01

Please sign in to comment.