bug fix for head control jittering in head_and_arm_control_demo; refractored code to separate generic dynamixel and ur controller and lab demo hardware specific setup into different files

Author: zhigenzhao

scripts/arm_control.py

@@ -1,4 +1,4 @@
-from teleop_demo_python.hardware.ur import DualArmURController
+from teleop_demo_python.hardware import DualArmURController
 from teleop_demo_python.utils.pico_client import PicoClient
 
 

scripts/head_and_arm_control_demo.py

@@ -1,15 +1,16 @@
 import argparse
-from teleop_demo_python.hardware.dynamixel import DynamixelHeadController
-from teleop_demo_python.utils.pico_client import PicoClient
-from teleop_demo_python.hardware.ur import DualArmURController
-
 import threading
+import time
+
+from teleop_demo_python.hardware import (
+    DualArmURController,
+    DynamixelHeadController,
+)
+from teleop_demo_python.utils.pico_client import PicoClient
 
 
 def main():
-    parser = argparse.ArgumentParser(
-        description="Run head and dual arm teleoperation control."
-    )
+    parser = argparse.ArgumentParser(description="Run head and dual arm teleoperation control.")
     parser.add_argument(
         "--reset",
         action="store_true",
@@ -18,11 +19,8 @@ def main():
     args = parser.parse_args()
 
     pico_client = PicoClient()
-
-    # Initialize controllers after parsing args, in case reset needs them
-    # Or, if reset can be done without full init, adjust accordingly
     head_controller = DynamixelHeadController(pico_client)
-    arm_controller = DualArmURController(pico_client)
+    arm_controller = DualArmURController(pico_client, visualize_placo=False)
 
     if args.reset:
         print("Reset flag detected. Running arm controller reset procedure...")
@@ -31,7 +29,6 @@ def main():
             print("Arm controller reset procedure completed.")
         except Exception as e:
             print(f"Error during arm_controller.reset(): {e}")
-
     else:
         print("No reset flag detected. Proceeding with normal operation.")
         arm_controller.calc_target_joint_position()
@@ -49,22 +46,28 @@ def main():
             target=arm_controller.run_right_controller_thread,
             args=(stop_signal,),
         )
+        ik_thread = threading.Thread(
+            target=arm_controller.run_ik_thread,
+            args=(stop_signal,),
+        )
 
         # Start the threads
         head_thread.start()
         left_arm_thread.start()
         right_arm_thread.start()
+        ik_thread.start()
 
         while not stop_signal.is_set():
             try:
-                arm_controller.calc_target_joint_position()
+                time.sleep(0.01)
             except KeyboardInterrupt:
                 print("KeyboardInterrupt detected. Exiting...")
                 stop_signal.set()  # Trigger the stop signal for all threads
 
         head_thread.join()
         left_arm_thread.join()
         right_arm_thread.join()
+        ik_thread.join()
 
     head_controller.close()
     arm_controller.close()

scripts/head_control.py

@@ -1,8 +1,8 @@
-from teleop_demo_python.hardware.dynamixel import DynamixelHeadController
-from teleop_demo_python.utils.pico_client import PicoClient
-
-import time
 import threading
+import time
+
+from teleop_demo_python.hardware import DynamixelHeadController
+from teleop_demo_python.utils.pico_client import PicoClient
 
 
 def main():
@@ -20,7 +20,7 @@ def main():
 
     while True:
         try:
-            time.sleep(0.1)
+            time.sleep(0.01)
         except KeyboardInterrupt:
             print("Stopping head control...")
             stop_signal.set()

teleop_demo_python/hardware/__init__.py

@@ -0,0 +1,8 @@
+from teleop_demo_python.hardware.dual_arm_ur_controller import (
+    DualArmURController,
+)
+from teleop_demo_python.hardware.dynamixel import DynamixelController
+from teleop_demo_python.hardware.dynamixel_head_controller import (
+    DynamixelHeadController,
+)
+from teleop_demo_python.hardware.ur import URController