Enable dxl thread by default

body/stretch_body/dynamixel_X_chain.py

@@ -23,6 +23,7 @@ def __init__(self, usb, name):
         Device.__init__(self, name)
         self.usb = usb
         self.pt_lock = threading.RLock()
+        self.thread_rate_hz = 15.0
 
         try:
             prh.LATENCY_TIMER = self.params['dxl_latency_timer']
@@ -87,12 +88,20 @@ def startup(self, threaded=False):
                 return False
         return True
 
+    def _thread_loop(self):
+        self.pull_status()
+        for motor in self.motors:
+            self.motors[motor].update_trajectory()
+
     def stop(self):
         Device.stop(self)
         if not self.hw_valid:
             return
-        for mk in self.motors.keys():
-            self.motors[mk].stop()
+        for motor in self.motors:
+            self.motors[motor]._waypoint_ts = None
+            self.motors[motor]._waypoint_vel = None
+            self.motors[motor]._waypoint_accel = None
+            self.motors[motor].stop()
         self.hw_valid = False
 
 

body/stretch_body/dynamixel_hello_XL430.py

@@ -177,7 +177,7 @@ def _thread_loop(self):
 
     def stop(self):
         Device.stop(self)
-        self._waypoint_ts = None
+        self._waypoint_ts, self._waypoint_vel, self._waypoint_accel = None, None, None
         if self.hw_valid:
             self.disable_torque()
             self.hw_valid = False
@@ -538,7 +538,7 @@ 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 right protocol
+        # check if joint valid and previous trajectory not executing
         if not self.hw_valid or self._waypoint_ts is None:
             return
 

body/stretch_body/end_of_arm.py

@@ -19,6 +19,8 @@ def __init__(self, name='end_of_arm'):
             dynamixel_device = getattr(importlib.import_module(module_name), class_name)(chain=self)
             self.add_motor(dynamixel_device)
 
+    def startup(self, threaded=True):
+        DynamixelXChain.startup(self, threaded=threaded)
 
     def get_joint(self, joint_name):
         """Retrieves joint by name.

body/stretch_body/head.py

@@ -17,6 +17,9 @@ def __init__(self):
                       'wheels': [deg_to_rad(0), deg_to_rad(-90)], 'left': [deg_to_rad(90), deg_to_rad(0)],
                       'up': [deg_to_rad(0), deg_to_rad(30)]}
 
+    def startup(self, threaded=True):
+        DynamixelXChain.startup(self, threaded=threaded)
+
     def get_joint(self, joint_name):
         """Retrieves joint by name.
 

body/stretch_body/stretch_gripper.py

@@ -18,6 +18,9 @@ def __init__(self, chain=None):
                       'open': self.pct_max_open,
                       'close': -100}
 
+    def startup(self, threaded=True):
+        DynamixelHelloXL430.startup(self, threaded=threaded)
+
     def home(self,move_to_zero=True):
         DynamixelHelloXL430.home(self,single_stop=True,move_to_zero=move_to_zero,delay_at_stop=3.0)
 

body/stretch_body/wrist_yaw.py

@@ -14,6 +14,9 @@ def __init__(self, chain=None):
                       'forward': deg_to_rad(0.0),
                       'stow': deg_to_rad(180.0)}
 
+    def startup(self, threaded=True):
+        DynamixelHelloXL430.startup(self, threaded=threaded)
+
     def home(self):
         """
         Home to hardstops

body/test/test_end_of_arm.py

@@ -18,7 +18,7 @@ def test_homing(self):
         class_name = e.robot_params[tool_name]['py_class_name']
         e = getattr(importlib.import_module(module_name), class_name)()
         self.assertTrue('stretch_gripper' in e.joints)
-        self.assertTrue(e.startup())
+        self.assertTrue(e.startup(threaded=False))
 
         e.home()
         e.pull_status()

body/test/test_end_of_arm_tools.py

@@ -30,8 +30,8 @@ def test_catch_startup_exception(self):
         if b==115200:
             e = stretch_body.end_of_arm_tools.ToolNone()
             e.params['baud']=57600
-            self.assertFalse(e.startup())
+            self.assertFalse(e.startup(threaded=False))
         else:
             e = stretch_body.end_of_arm_tools.ToolNone()
             e.params['baud'] = 115200
-            self.assertFalse(e.startup())
+            self.assertFalse(e.startup(threaded=False))

tools/bin/stretch_gripper_home.py

@@ -10,7 +10,7 @@
 args=parser.parse_args()
 
 g=gripper.StretchGripper()
-if not g.startup():
+if not g.startup(threaded=False):
     exit()
 g.home()
 time.sleep(3.0)

tools/bin/stretch_gripper_jog.py

@@ -12,7 +12,7 @@
 args=parser.parse_args()
 
 g=gripper.StretchGripper()
-if not g.startup():
+if not g.startup(threaded=False):
     exit()
 g.pull_status()
 v_des=g.params['motion']['default']['vel']

tools/bin/stretch_head_jog.py

@@ -12,7 +12,7 @@
 args=parser.parse_args()
 
 h=head.Head()
-if not h.startup():
+if not h.startup(threaded=False):
     exit()
 
 v_des=[h.motors['head_pan'].params['motion']['default']['vel'], h.motors['head_tilt'].params['motion']['default']['vel']]

tools/bin/stretch_wrist_yaw_home.py

@@ -9,7 +9,7 @@
 args=parser.parse_args()
 
 g=wrist_yaw.WristYaw()
-if not g.startup():
+if not g.startup(threaded=False):
     exit()
 g.home()
 g.stop()

tools/bin/stretch_wrist_yaw_jog.py

@@ -13,7 +13,7 @@
 
 poses = {'zero':0, 'left':deg_to_rad(90), 'right': deg_to_rad(-45)}
 w=wrist_yaw.WristYaw()
-if not w.startup():
+if not w.startup(threaded=False):
     exit()
 
 v_des=w.params['motion']['default']['vel']