update

Massage/MassageControl/MassageRobot_nova5.py

@@ -336,7 +336,7 @@ class MassageRobot:
             # 线程
             self.exit_event.clear()
             self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
-            self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
+            self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -496,11 +496,12 @@ if __name__ == "__main__":
     try:
         robot_thread = threading.Thread(target=robot.start)
         robot_thread.start()    
+
     except KeyboardInterrupt:
         print("用户中断操作。")
     except Exception as e:
         print("Exception occurred at line:", e.__traceback__.tb_lineno)
         print("发生异常：", e)
-    
+        
-    
+    robot_thread.join()  # 等待机械臂线程结束
     # robot.arm.disableRobot()

Massage/MassageControl/config/admittance.yaml

@@ -8,7 +8,9 @@ mass_rot: [0.11, 0.11, 0.11]
 # P
 # stiff_tran: [300, 300, 300]
 # stiff_rot: [7, 7, 7]
-stiff_tran: [270, 270, 270]
+# stiff_tran: [270, 270, 270]
+# stiff_rot: [11, 11, 11]
+stiff_tran: [0, 0, 0]
 stiff_rot: [11, 11, 11]
 # stiff_tran: [100, 100, 100]
 # stiff_rot: [1, 1, 1]

Massage/MassageControl/hardware/force_sensor.py

@@ -1,9 +1,11 @@
 import struct
 import serial
+import serial.tools.list_ports
 import numpy as np
 import atexit
 import threading
 import time
+import yaml
 
 import subprocess
 import psutil