xiugai yidui

Language/LLM/scripts/LLM.py

@@ -100,9 +100,6 @@ class DashscopeClient:
         with open('LLM/config/trajectory_config/body_part_prompts.txt', 'r') as f:
             self.body_part_prompts = f.read().strip()
             self.body_part_message = [{'role': 'system', 'content': self.body_part_prompts}]
-        with open('LLM/config/trajectory_config/reason.txt', 'r') as f:
-            self.reason_prompts = f.read().strip()
-            self.reason_message = [{'role': 'system', 'content': self.reason_prompts}]
 
         with open('LLM/config/trajectory_config/shoulder_limit_prompts.txt', 'r') as f:
             self.shoulder_limit_prompts = f.read().strip()

Massage/Massage.py

@@ -360,6 +360,9 @@ class SharedResources:
         self.jump_mode = True
         self.jump_mode_default = True
 
+        self.is_headOn = True
+        self.is_headOn_default = True
+
         self.cal_acu = False
         self.get_picture = True
         self.manual_start = False
@@ -696,6 +699,10 @@ class SharedResources:
             cycles = task.get('cycles')
             direction = task.get('direction')
             jump_mode = task.get('jump_mode')
+            try:
+                is_headOn = task.get('headOn')
+            except:
+                is_headOn = True
             if  body_part == "shoulder":
                 pathBodypart = body_part
             else:
@@ -713,7 +720,8 @@ class SharedResources:
                 pathBodypart,
                 start_point_raw,
                 end_point_raw,
-                jump_mode
+                jump_mode,
+                is_headOn
             ])
         if self.mode_commands_list is None:
             self.send_instruction(instruction={"message": "没有检测到穴位点<br>请重新拍照"}, target="ui_on_message")
@@ -1094,14 +1102,15 @@ class SharedResources:
         self.logger.log_yellow("运动到拍照位置")
         self.robot.arm.disable_servo()
         time.sleep(0.5) 
-        code = self.robot.arm.move_joint(self.robot.arm.cam_pose, max_retry_count = 4,wait=True)
+        # code = self.robot.arm.move_joint(self.robot.arm.cam_pose, max_retry_count = 4,wait=True)
+        code = self.robot.set_joint(joint = self.robot.arm.cam_pose)
         if code == -1:
             self.logger.log_error("运动到拍照位置失败")
             self.stop_event.set()
             return "emergency"
         time.sleep(1)
         self.cam_position,self.cam_quaternion = self.robot.arm.get_end_position()
-        self.logger.log_info(f"获取最新的位置:{self.cam_position}和姿态:{self.cam_quaternion}")
+        self.logger.log_info(f"获取cam的位置:{self.cam_position}和姿态:{self.cam_quaternion}")
         if self.stop_event.is_set():
             self.logger.log_info(f"检测到停止事件")
             return "stop"
@@ -1344,7 +1353,8 @@ class SharedResources:
         self.robot.is_waitting = True
         self.robot.arm.disable_servo()
         time.sleep(1)
-        code = self.robot.arm.move_joint(pose, max_retry_count = 4)
+        # code = self.robot.arm.move_joint(pose, max_retry_count = 4)
+        code = self.robot.set_joint(joint = pose)
         if code == -1:
             self.logger.log_error("运动到standby位置失败")
             self.stop_event.set()
@@ -1852,7 +1862,7 @@ class TaskBase(smach.State):
         
         if self.resources.robot.force_sensor:
             self.resources.robot.force_sensor.stop_background_reading()
-        self.resources.robot.force_sensor = XjcSensor(arm_ip=self.resources.robot.arm_config['arm_ip']) # 初始化实例化力传感器
+        self.resources.robot.force_sensor = XjcSensor() # 初始化实例化力传感器
         time.sleep(0.2)
         code = self.resources.robot.sensor_set_zero()
         if code != 0:
@@ -1979,6 +1989,7 @@ class TaskBase(smach.State):
             use_algorithm = self.resources.use_algorithm
             if self.resources.choose_task != "ion" and self.resources.motion_task[5] == "point":
                 self.resources.use_algorithm = "hybridPid"
+                print("self.resources.use_algorithm:",self.resources.use_algorithm)
             code = self.resources.robot.move_to_point(pose=self.resources.target_points, t=self.resources.motion_task[6], timeout=0.5, interpolation="circle", algorithm=self.resources.use_algorithm,is_interrupt=False,is_switch_controller = self.resources.is_switch_controller)
             self.resources.use_algorithm = use_algorithm
         else:
@@ -2198,7 +2209,7 @@ class Thermotherapy_Task(TaskBase):
             self.resources.logger.log_info("Executing Thermotherapy_Task")
             self.move_to_startpos()
             self.resources.logger.log_info('启动热疗按摩')	    
-            self.resources.robot.thermotherapy.set_working_status(cur_level=self.resources.gear, heat_level=self.resources.temperature, vib_level=self.resources.shake)
+            # self.resources.robot.thermotherapy.set_working_status(cur_level=self.resources.gear, heat_level=self.resources.temperature, vib_level=self.resources.shake)
             self.tasks_queue_execution() # 按照任务队列执行任务
             return "stop"
         
@@ -2711,6 +2722,8 @@ class Stop(smach.State):
         time.sleep(1)
         self.resources.robot.arm.disable_servo()
         self.resources.robot.arm.disable_servo()
+        self.resources.robot.stop_thread()
+
         if self.resources.next_points is not None:
             time.sleep(1)
             position,quat_rot = self.resources.robot.arm.get_arm_position()
@@ -2718,21 +2731,32 @@ class Stop(smach.State):
             target_point = np.zeros(6)
             target_point[:3] = copy.deepcopy(position)
             target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False))
+            # target_point[:3] = copy.deepcopy(self.resources.robot.arm_state.arm_position_command)
+            # target_point[3:] = copy.deepcopy(R.from_quat(self.resources.robot.arm_state.arm_orientation_command).as_euler('xyz',degrees=False))
             # target_point[3:] = [150 * (np.pi / 180), 0, 180 * (np.pi / 180)]
             target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.10], self.resources.robot.arm.level_base)
-            self.resources.logger.log_blue(f"set_position: {target_point}")
+            self.resources.logger.log_blue(f"set_position: {target_point},{position}")
             code = self.resources.robot.set_position(pose=target_point,is_wait=True)
+
         self.resources.next_points = None
         time.sleep(1)
         pose = copy.deepcopy(self.resources.robot.arm.standby_pos)
         self.resources.logger.log_yellow("self.resources.robot.arm.disable_servo()")
         time.sleep(1)
         if not self.resources.is_pause:
-            self.resources.robot.arm.move_joint(pose)
+            # self.resources.robot.arm.move_joint(pose)
+            code = self.resources.robot.set_joint(pose)
+            if code == -1:
+                self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+                return "emergency"
         pose = copy.deepcopy(self.resources.robot.arm.init_pos)
         time.sleep(0.1)
         if not self.resources.is_pause:
-            self.resources.robot.arm.move_joint(pose)
+            # self.resources.robot.arm.move_joint(pose)
+            code = self.resources.robot.set_joint(pose)
+            if code == -1:
+                self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+                return "emergency"
         time.sleep(1)
 
         if self.resources.choose_task == "thermotherapy":
@@ -2849,6 +2873,8 @@ class Pause(smach.State):
         self.resources.robot.is_waitting = True
         time.sleep(1)
         self.resources.robot.arm.disable_servo()
+        self.resources.robot.stop_thread()
+
         if self.resources.next_points is not None:
             time.sleep(1)
             position,quat_rot = self.resources.robot.arm.get_arm_position()
@@ -2856,18 +2882,29 @@ class Pause(smach.State):
             target_point = np.zeros(6)
             target_point[:3] = copy.deepcopy(position)
             target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False))
+            # target_point[:3] = copy.deepcopy(self.resources.robot.arm_state.arm_position_command)
+            # target_point[3:] = copy.deepcopy(R.from_quat(self.resources.robot.arm_state.arm_orientation_command).as_euler('xyz',degrees=False))
             # target_point[3:] = [150 * (np.pi / 180), 0, 180 * (np.pi / 180)]
             target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.15], self.resources.robot.arm.level_base)
             self.resources.logger.log_blue(f"set_position: {target_point}")
             code = self.resources.robot.set_position(pose=target_point,is_wait=True)
+
         self.resources.next_points = None
         time.sleep(1)
         pose = copy.deepcopy(self.resources.robot.arm.standby_pos)
         time.sleep(0.1)
-        self.resources.robot.arm.move_joint(pose)
+        # self.resources.robot.arm.move_joint(pose)
+        code = self.resources.robot.set_joint(pose)
+        if code == -1:
+            self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+            return "emergency"
         pose = copy.deepcopy(self.resources.robot.arm.init_pos)
         time.sleep(0.1)
-        self.resources.robot.arm.move_joint(pose)
+        # self.resources.robot.arm.`move_joint`(pose)
+        code = self.resources.robot.set_joint(pose)
+        if code == -1:
+            self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+            return "emergency"
         time.sleep(1)
 
         if self.resources.choose_task == "thermotherapy":
@@ -3027,8 +3064,9 @@ class EmergencyExitState(smach.State):
         self.resources.send_instruction(instruction="emergency_stop", target="language")
         # 这里可以处理一些清理工作
         self.resources.logger.log_error("Emergency exit triggered, shutting down...")
+        self.resources.robot.stop_thread()
         self.resources.robot.stop()
-        self.resources.robot.arm.power_off()
+        # self.resources.robot.arm.power_off()
         self.resources.stop_event.clear()
         # 或者使用 sys.exit() 直接退出程序
         # sys.exit(1)

Massage/MassageControl/MassageRobot_nova5.py

@@ -74,6 +74,10 @@ except:
 import matplotlib.pyplot as plt
 from matplotlib.animation import FuncAnimation 
 
+import select
+
+import importlib.util
+
 current_file_path = os.path.abspath(__file__)
 MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
 print(MassageRobot_Dobot_Path)
@@ -86,6 +90,75 @@ from VortXDB.client import VTXClient
 当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。
 """
 
+# 定义模块加载的函数
+def load_class_from_pyc(pyc_path, class_name, module_name):
+    """
+    动态加载 .pyc 文件并尝试访问其中的类。
+    
+    :param pyc_path: .pyc 文件路径
+    :param class_name: 需要访问的类名
+    :param module_name: 模块的名称（用于调试和错误信息）
+    :return: 类对象（如果加载和访问成功）
+    :raises: 文件未找到或类未定义等异常
+    """
+    # 检查文件是否存在
+    if not os.path.exists(pyc_path):
+        raise FileNotFoundError(f"Module file '{pyc_path}' not found.")
+    
+    try:
+        # 动态加载模块
+        spec = importlib.util.spec_from_file_location(module_name, pyc_path)
+        module = importlib.util.module_from_spec(spec)
+        sys.modules[module_name] = module
+        spec.loader.exec_module(module)
+        
+        # 尝试获取指定的类
+        class_obj = getattr(module, class_name, None)
+        if class_obj is None:
+            raise AttributeError(f"Class '{class_name}' not found in module '{module_name}'.")
+        
+        return class_obj  # 返回类对象
+    except Exception as e:
+        raise RuntimeError(f"Failed to load class '{class_name}' from module '{module_name}': {str(e)}")
+
+# 定义 .pyc 文件的路径
+shockwave_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/shockwave.pyc'
+thermotherapy_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/thermotherapy.pyc'
+stone_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/stone.pyc'
+ion_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/ion.pyc'
+heat_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/heat.pyc'
+
+# 使用封装的函数加载各个模块和类
+try:
+    Shockwave = load_class_from_pyc(shockwave_pyc_path, "Shockwave", "shockwave")
+    print("Shockwave class loaded successfully.")
+except Exception as e:
+    print(f"Error loading Shockwave: {str(e)}")
+
+try:
+    Thermotherapy = load_class_from_pyc(thermotherapy_pyc_path, "Thermotherapy", "thermotherapy")
+    print("Thermotherapy class loaded successfully.")
+except Exception as e:
+    print(f"Error loading Thermotherapy: {str(e)}")
+
+try:
+    Stone = load_class_from_pyc(stone_pyc_path, "Stone", "stone")
+    print("Stone class loaded successfully.")
+except Exception as e:
+    print(f"Error loading Stone: {str(e)}")
+
+try:
+    Ion = load_class_from_pyc(ion_pyc_path, "Ion", "ion")
+    print("Ion class loaded successfully.")
+except Exception as e:
+    print(f"Error loading Ion: {str(e)}")
+
+try:
+    Heat = load_class_from_pyc(heat_pyc_path, "Heat", "heat")
+    print("Heat class loaded successfully.")
+except Exception as e:
+    print(f"Error loading Heat: {str(e)}")
+
 def track_function(function_name,log = False):
     def before(self, *args, **kwargs):
         self.current_function = function_name
@@ -113,15 +186,11 @@ class MassageRobot:
         self.arm_state = ArmState()
         self.arm_config = read_yaml(arm_config_path)
         # arm 实例化时机械臂类内部进行通讯连接
-        # 初始化坐标系 TOOL=0 BASE=1
         self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
-        self.arm.start()
-
-        self.arm.chooseRightFrame()
-
+        # self.arm.start()
+        # self.arm.chooseRightFrame()
         self.arm.init()
 
-
         self.thermotherapy = None
         self.shockwave = None
         self.stone = None
@@ -129,33 +198,21 @@ class MassageRobot:
         self.ion = None
         self.force_plan = ForcePlanner()
         # 传感器
-        # self.force_sensor = None
-        self.force_sensor = XjcSensor()
+        self.force_sensor = None
+        # self.force_sensor = XjcSensor()
         # self.force_sensor.connect()
         # 控制器,初始为导纳控制
         self.controller_manager = ControllerManager(self.arm_state)
         self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
-        # self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
-        self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1])
-        # self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
-        # self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
-        # self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
-        # self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
-        # self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
-        
+        self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
+        self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2])
+        self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
+        self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
+        self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
+        self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
+        self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
         self.controller_manager.switch_controller('admittance')
         
-
-        # 读取数据
-        self.gravity_base = None
-        self.force_zero = None
-        self.torque_zero = None
-        self.tool_position = None
-        self.mass_center_position = None
-        self.s_tf_matrix_t = None
-        arm_orientation_set0 = np.array([-180,-30,0])
-        self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
-        
         # 频率数据赋值
         self.control_rate = Rate(self.arm_config['control_rate'])
         self.sensor_rate = Rate(self.arm_config['sensor_rate'])
@@ -190,6 +247,7 @@ class MassageRobot:
             file_address = massage_head_dir + '/' + file
             play_load = read_yaml(file_address)
             self.playload_dict[play_load['name']] = play_load
+
         # self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
         # print(self.playload_dict)
         self.current_head = 'none'
@@ -211,9 +269,13 @@ class MassageRobot:
         self.x_tcp = np.zeros(6)
         self.P_tcp  = np.eye(6)
         # 过程噪声协方差矩阵
-        self.Q_tcp = np.eye(6) * 0.01
+        # self.Q_tcp = np.eye(6) * 0.01
+        self.Q_tcp = np.eye(6) * 0.05
+
         # 测量噪声协方差矩阵
-        self.R_tcp = np.eye(6) * 0.1
+        # self.R_tcp = np.eye(6) * 0.1
+        self.R_tcp = np.diag([0.5, 0.5, 0.5, 0.05, 0.05, 0.05])
+
         # 传感器故障计数器
         self.sensor_fail_count = 0 
         # 机械臂初始化,适配中间层
@@ -224,7 +286,7 @@ class MassageRobot:
         self.tool_position = None
         self.mass_center_position = None
         self.s_tf_matrix_t = None
-        arm_orientation_set0 = np.array([-180,0,-90])
+        arm_orientation_set0 = np.array([-180,-30,0])
         self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
 
         
@@ -237,6 +299,7 @@ class MassageRobot:
         # ---
         self.last_time = -1
         self.cur_time = -1
+        self.tool_num = 0
 
     # 预测步骤
     def kalman_predict(self,x, P, Q):
@@ -264,7 +327,9 @@ class MassageRobot:
         return x_update, P_update
     def init_hardwares(self,ready_pose):
         self.ready_pose = np.array(ready_pose)
-        self.switch_payload(self.current_head)
+        self.arm_state.desired_position = self.ready_pose[:3]
+        euler_angles = self.ready_pose[3:]
+        self.arm_state.desired_orientation = R.from_euler('xyz',euler_angles).as_quat()
         print(self.arm.get_arm_position())
         time.sleep(0.5)
         
@@ -357,6 +422,7 @@ class MassageRobot:
         return 0 #读取成功并置零成功
 
     def sensor_enable(self):
+        print("^^^^^^^^^^^^^^^^^^^^^^^^^")
         code  = self.force_sensor.enable_active_transmission()
         max_try = 3
         while code!= 0 and max_try > 0:
@@ -374,50 +440,61 @@ class MassageRobot:
             )
             # sys.exit(0)
             return -1
+        print("^^^^^^^^^^^^^^^^^^^^^^^^^")
         return 0
     
 
     
     def update_wrench(self):
         # 当前的机械臂到末端转换 (实时)
+        # print("com111111")
         b_rotation_s = R.from_quat(self.arm_state.arm_orientation).as_matrix()
         # 读取数据
+        # print("com2222222222")
         sensor_data = self.force_sensor.read()
+        # print("com3333")
         # self.logger.log_info(f"传感器数据{sensor_data}")
         if sensor_data is None:
             self.force_sensor.stop_background_reading()
             self.logger.log_error("传感器数据读取失败")
             return -1
+        # print("com44444444444444444444444444444444444444444")
         # 反重力补偿
         sensor_data[:3] = sensor_data[:3] + self.force_zero + self.b_rotation_s_set0.T @ self.gravity_base
         sensor_data[3:] = sensor_data[3:] + self.torque_zero + np.cross(self.mass_center_position, self.b_rotation_s_set0.T @ self.gravity_base)
         # 传感器数据通过矫正计算得到外来施加力 传感器坐标系下
         # 重力
+        # print("com5555555")
         gravity_in_sensor = b_rotation_s.T @ self.gravity_base
         s_force = sensor_data[:3] - self.force_zero - gravity_in_sensor
         # 力矩
         s_torque = sensor_data[3:] - self.torque_zero - np.cross(self.mass_center_position,gravity_in_sensor)
         wrench = np.concatenate([s_force,s_torque])
+        # print("com66666")
         # 传感器到TCP
         wrench = self.wrench_coordinate_conversion(self.s_tf_matrix_t,wrench)
         # 交给ARM STATE集中管理
         self.arm_state.external_wrench_tcp = wrench
+        # print("com777777777777777777")
         # 对tcp坐标系下的外力外矩进行平滑
         x_tcp_predict, P_tcp_predict = self.kalman_predict(x = self.x_tcp, 
                                                            P = self.P_tcp, 
                                                            Q = self.Q_tcp)
+        # print("com88888888888888888888")
         self.x_tcp, self.P_tcp = self.kalman_update(x_predict = x_tcp_predict, 
                                                     P_predict = P_tcp_predict, 
                                                     z = self.arm_state.external_wrench_tcp, 
                                                     R = self.R_tcp)
+        # print("com999999999999")
         self.arm_state.external_wrench_tcp = self.x_tcp
         self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
+        # print("update data wrench :", self.arm_state.external_wrench_tcp)
         return 0
 		
     def switch_payload(self,name):
         if name in self.playload_dict:
-            self.stop()
             self.current_head = name
+            self.logger.log_blue(f"Into switch_payload,change to{name}")
             
             compensation_config = self.playload_dict[self.current_head]
 
@@ -429,13 +506,23 @@ class MassageRobot:
             self.mass_center_position = np.array(compensation_config['mass_center_position'])
             self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
 
-            tcp_offset = self.playload_dict[name]["tcp_offset"]
-            tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
-            print("tcp_offset_str",tcp_offset_str)
-            self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
-            self.arm.chooseEndEffector(i=9)
-            print(self.arm.get_arm_position())
-            self.logger.log_info(f"切换到{name}按摩头")		
+            self.logger.log_blue(f"current position and orientation:{self.arm_state.arm_position},{self.arm_state.arm_orientation}")
+            self.logger.log_yellow("---------------------change----------------------")
+            self.arm.change_end_effector(self.playload_dict[name])
+
+            self.logger.log_info(f"切换到{name}按摩头,{self.arm.tcp_offset}")
+            R_matrix = R.from_euler('xyz',self.ready_pose[3:] ,degrees=False).as_matrix()
+            ready_position = self.ready_pose[:3] + R_matrix @ self.playload_dict[self.current_head]['tcp_offset'][:3]
+            ready_orientation =  R_matrix @ R.from_euler('xyz',self.playload_dict[self.current_head]['tcp_offset'][3:],degrees=False).as_matrix()
+            ready_orientation_euler = R.from_matrix(ready_orientation).as_euler('xyz',degrees=False)
+            print("ready_pose",self.ready_pose)
+            print("desired_position", ready_position)
+            print("desired_orientation",ready_orientation_euler)
+            # TODO 
+            self.arm_state.desired_position = ready_position 
+            self.arm_state.desired_orientation = R.from_euler('xyz',ready_orientation_euler,degrees=False).as_quat()
+            self.controller_manager.switch_controller('position')
+	
     
     # ####################### 位姿伺服 ##########################
     
@@ -451,6 +538,7 @@ class MassageRobot:
                         self.logger.log_error(f"传感器线程数据读取失败-{self.sensor_fail_count}")
                         if self.sensor_fail_count > 10:
                             self.logger.log_error("传感器线程数据读取失败超过10次,程序终止")
+                            self.stop_thread()
                             self.stop()
                             break
                     else:
@@ -527,7 +615,7 @@ class MassageRobot:
                     if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                         print("机械臂为暂停状态")
                         res = self.arm.dashboard.Continue()
-                        code = self.arm.parseResultId(res)[0]
+                        code = self.arm.parseResultprocess(res)[0]
                         if code == 0:
                             print("机械臂继续已暂停的运动指令")
 
@@ -538,12 +626,16 @@ class MassageRobot:
     
     def arm_command_loop(self):
         self.logger.log_info("机械臂控制线程启动")
+        self.arm.filter_matirx = np.zeros((1,7)) # 位姿伺服用
+        recv_time = 0
+        # self.arm.filter_matrix = np.zeros((1,6)) # 角度伺服用
         while (not self.arm.is_exit) and (not self.exit_event.is_set()):
             try:
                 if not self.is_waitting:
                     process_start_time = time.time()
                     control_cycle = self.control_rate.to_sec() 
                     self.controller_manager.step(control_cycle)
+
                     command_pose = np.concatenate([
                         self.arm_state.arm_position_command * 1000, # 转毫米
                         self.arm_state.arm_orientation_command
@@ -553,9 +645,17 @@ class MassageRobot:
                         command_pose[:3],
                         command_pose[3:]
                     )  
-                    # pose_processed = command_pose
-                    print(f"pose_processed:{pose_processed}")  
-                    print(self.arm.feedbackData.robotMode)
+
+                    temp_pose = copy.deepcopy(pose_processed)
+
+                    # print("self.arm.tcp_euler_inv:",self.arm.tcp_euler_inv,temp_pose)
+
+                    target_matrix = R.from_euler('xyz', temp_pose[3:]*np.pi/180, degrees=False).as_matrix() @ self.arm.tcp_euler_inv
+                    pose_processed[:3] = temp_pose[:3] - target_matrix @ np.array(self.arm.tcp_offset[:3])*1000
+                    pose_processed[3:] = R.from_matrix(target_matrix).as_euler("xyz",degrees = True)
+                    
+                    # print(f"pose_processed:{pose_processed},{self.arm_state.arm_position_command * 1000}")  
+                    
                     tcp_command = (
                     f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
                     f"{pose_processed[2]:.3f},{pose_processed[3]:.3f},"
@@ -564,17 +664,54 @@ class MassageRobot:
                     ).encode()
                     
                     run_time = time.time() - process_start_time
-                    sleep_duration = self.control_rate.to_sec() - run_time
+                    sleep_duration = self.control_rate.to_sec() - run_time - recv_time
                     b2 =time.time()
                     if sleep_duration > 0:
                         time.sleep(sleep_duration)
-                    print(f"real sleep:{time.time()-b2}")
-                    self.arm.dashboard.socket_dobot.sendall(tcp_command)
-                    print("currentCommandID",self.arm.feedbackData.robotCurrentCommandID)
+                    # print(f"real sleep:{time.time()-b2}")
+                    
+                    # print("self111:",self.arm.feedbackData.robotMode,tcp_command)
+                    retry_count = 0
+                    while retry_count < 3:
+                        if self.arm.is_exit or self.exit_event.is_set():
+                            break
+                        try:
+                            # self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                            self.arm.dashboard.ServoPos(pose_processed[0],pose_processed[1],pose_processed[2],pose_processed[3],pose_processed[4],pose_processed[5],t=0.02, aheadtime=20,gain=200)
+                            recv_start_time = time.time()
+                            # self.arm.dashboard.socket_dobot.recv(1024)
+                            # recv_time = time.time() - recv_start_time
+                            break
+                        except Exception as e:
+                            print(f"发送指令错误：{e}")
+                            # 你可以选择在这里close socket，也可以让reConnect里自动处理
+                            self.arm.dashboard.socket_dobot = self.arm.dashboard.reConnectWithRetry(self.arm.dashboard.ip, self.arm.dashboard.port, 2, 0.1)
+                            retry_count += 1
+                    else:
+                        # 只有没break才会走这里，重新raise最后的异常
+                        raise Exception(f"sendall连续{retry_count}次都失败，机械臂指令发送失败！")
+
+                    # # ⬇️ 将 ToolVectorActual 写入 txt 文件
+                    # try:
+                    #     with open("tool_vector_log.txt", "a") as f:
+                    #         f.write(",".join([str(x) for x in self.arm_state.arm_position_command]) + "\n")
+                    # except Exception as e:
+                    #     print("写入 ToolVectorActual 失败:", e)
+                    # # 自定义添加所需反馈数据
+
+                    # print("self111222:",self.arm.feedbackData.robotMode,tcp_command)
+                    if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState or self.arm.feedbackData.ErrorStatus == True: 
+                        self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode},{self.arm.feedbackData.EnableState},{self.arm.feedbackData.ErrorStatus}")
+                        self.arm.dashboard.Stop()
+                        self.arm.dashboard.EmergencyStop(mode=1)
+                        self.stop_thread()
+                        self.stop()
+                        return -1 
+                    if self.arm.feedbackData.EnableState and self.arm.feedbackData.robotMode == 10:
                         if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                             print("机械臂为暂停状态")
                             res = self.arm.dashboard.Continue()
-                        code = self.arm.parseResultId(res)[0]
+                            code = self.arm.parseResultprocess(res)[0]
                             if code == 0:
                                 print("机械臂继续已暂停的运动指令")
 
@@ -591,6 +728,9 @@ 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)
+
+            self.controller_manager.switch_controller('admittance')
+
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -603,11 +743,12 @@ class MassageRobot:
             self.arm_state.arm_orientation = quat_rot
             for i in range(20):
                 self.controller_manager.step(self.control_rate.to_sec())
-                self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
-                self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
+                # self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
+                # self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
                 position, quat_rot = self.arm.get_arm_position()
-                self.logger.log_blue(f"position current:{position}")
-                self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
+                # self.logger.log_blue(f"position current:{position}")
+                # self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
+                # print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode)
                 self.command_rate.precise_sleep()
                 
             position ,quat_rot = self.arm.get_arm_position()
@@ -627,7 +768,7 @@ class MassageRobot:
             self.logger.log_info("MassageRobot启动")
             time.sleep(0.5)
 
-    def stop(self):
+    def stop_thread(self):
         if not self.exit_event.is_set():
             self.exit_event.set()
             self.interrupt_event.clear()
@@ -636,11 +777,51 @@ class MassageRobot:
             for i in range(3):
                 self.force_sensor.disable_active_transmission()
             self.force_sensor.start_background_reading()
+            # clear_time = time.time()
+            # self.clear_socket_buffer(self.arm.dashboard.socket_dobot)
+            # print(f"==={time.time()-clear_time}===")
+
+    def stop(self):
         self.arm.stop_motion()
         self.logger.log_info("MassageRobot停止")
         
 
         self.controller_manager.switch_controller('position')
+        # else:
+        #     self.logger.log_error(f"未找到{name}按摩头")
+
+    # def clear_socket_buffer(self, sock):
+    #     # 使用 select 检查 socket 是否有数据
+    #     print("clear_socket_buffer")
+    #     while True:
+    #         ready, _, _ = select.select([sock], [], [], 0)
+    #         if ready:
+    #             try:
+    #                 data = sock.recv(4096)
+    #                 if not data:
+    #                     break  # 对方关闭连接
+    #             except BlockingIOError:
+    #                 break
+    #         else:
+    #             print("socket_buffer EMPTY")
+    #             break  # 没有更多数据了
+
+    # def stop(self):
+    #     if not self.exit_event.is_set():
+    #         self.exit_event.set()
+    #         self.interrupt_event.clear()
+    #         self.arm_control_thread.join()
+    #         self.arm_measure_thread.join()
+    #         for i in range(3):
+    #             self.force_sensor.disable_active_transmission()
+    #         self.force_sensor.start_background_reading()
+    #         self.arm.stop_motion()
+    #         self.logger.log_info("MassageRobot停止")
+            
+
+    #         self.controller_manager.switch_controller('position')
+    #     # else:
+    #     #     self.logger.log_error(f"未找到{name}按摩头")
 
     def log_thread(self):
         while True:
@@ -669,12 +850,29 @@ class MassageRobot:
     
     @track_function("set_position",True)
     def set_position(self,pose,is_wait = False):
-        self.logger.log_info(f"set postion:{pose}")
+
         x,y,z = pose[0],pose[1],pose[2]
         roll,pitch,yaw = pose[3],pose[4],pose[5]
-        pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]])
+        pose_command = np.concatenate([np.array([x,y,z]), np.array([roll,pitch,yaw])])
+
+        temp_pose = copy.deepcopy(pose_command)
+        pose_processed = copy.deepcopy(pose_command)
+        target_matrix = R.from_euler('xyz', temp_pose[3:], degrees=False).as_matrix() @ self.arm.tcp_euler_inv
+        pose_processed[:3] = temp_pose[:3] - target_matrix @ np.array(self.arm.tcp_offset[:3])
+        pose_processed[3:] = R.from_matrix(target_matrix).as_euler("xyz",degrees = False)
+    
+        self.logger.log_info(f"set postion:{pose},{pose_processed}")
+
         time.sleep(0.1)
-        code = self.arm.RunPoint_P_inPose_M_RAD(poses_list = pose_command)
+        code = self.arm.RunPoint_P_inPose_M_RAD(pose = pose_processed)
+        time.sleep(0.1)
+        return code
+    
+    @track_function("set_joint",True)
+    def set_joint(self,joint,is_wait = False):
+        self.logger.log_info(f"set joint:{joint}")
+        time.sleep(0.1)
+        code = self.arm.move_joint(q = joint)
         time.sleep(0.1)
         return code
 
@@ -1349,6 +1547,52 @@ class MassageRobot:
         result_points.append(copy.deepcopy(center_point))
         return result_points
     
+    def user_adjust(self,pose_increment = None, force = None, temperature = None, speed = None, direction = None ,gear = None, shake = None,press = None,frequency = None):
+        self.logger.log_info("用户调整")
+        if force is not None:
+            self.adjust_wrench = np.array(force)
+            self.adjust_wrench_envent.set()
+        if temperature is not None:
+            if self.thermotherapy is not None:
+                self.thermotherapy.set_working_status(heat_level=temperature)
+            elif self.stone is not None:
+                self.stone.set_working_status(heat_level=temperature)
+            elif self.ion is not None:
+                self.ion.set_working_status(goal_state=temperature)
+            elif self.heat is not None:
+                self.heat.set_working_status(heat_level=temperature)
+        if speed is not None:
+            if self.stone is not None:
+                self.stone.set_working_status(speed_level=speed)
+        if direction is not None:
+            if self.stone is not None:
+                self.stone.set_working_status(rotate_direction=direction)
+        if gear is not None:
+            if self.thermotherapy is not None:
+                self.thermotherapy.set_working_status(cur_level=gear)
+        if shake is not None:
+            if self.thermotherapy is not None:
+                self.thermotherapy.set_working_status(vib_level=shake)
+        if press is not None:
+            if self.shockwave is not None:
+                self.shockwave.p_set(level=press)
+        if frequency is not None:
+            if self.shockwave is not None:
+                self.shockwave.f_set(level=frequency)
+
+    def user_interrupt(self):
+        self.logger.log_info("用户打断")
+        self.interrupt_event.set()
+
+    def user_pause(self):
+        self.logger.log_info("用户暂停")
+        self.pause_envent.set()
+
+    def user_skip(self):
+        self.logger.log_info("用户跳过")
+        self.skip_envent.set()
+    
+    
     def traj_generate(self, t: Union[int, float, List[float]], pose = None, wrench = None, interpolation: Literal["linear", "cubic","circle","cloud_point"] = 'linear',**kwargs):
         """
         轨迹生成
@@ -1405,7 +1649,7 @@ class MassageRobot:
                     pos_traj, quat_traj = spline_interpolate(positions=pose_matrix[:, :3], quaternions=pose_matrix[:, 3:], time_points=time_points, time_step=dt)
                 elif interpolation == 'circle':
                     if self.ion is not None:
-                        pos_traj = circle_trajectory(center=pose, radius=0.07,time_points=time_points,time_step=dt,**kwargs)
+                        pos_traj = circle_trajectory(center=pose, radius=0.09,time_points=time_points,time_step=dt,**kwargs)
                     else:
                         pos_traj = circle_trajectory(center=pose,time_points=time_points,time_step=dt,**kwargs)
                     quat_traj = np.tile(R.from_euler('xyz', np.array(pose[0][3:])).as_quat(), (pos_traj.shape[0], 1))
@@ -1578,6 +1822,26 @@ class MassageRobot:
         }
         return traj
 
+    # 工具函数
+    ############################################################################################################
+    def convert_to_7d(self,matrix):
+        matrix = np.array(matrix)  
+        positions = matrix[:, :3]  
+        rpy_angles = matrix[:, 3:]  
+        # 将RPY角度转换为四元数
+        quaternions = R.from_euler('xyz', rpy_angles).as_quat()
+        # 将位置和四元数组合成一个新的矩阵
+        result_matrix = np.hstack([positions, quaternions])
+        # 加入当前位置
+        current_quat = self.arm_state.arm_orientation
+        current_position  = self.arm_state.arm_position
+        if quaternions[0].dot(current_quat) < 0:
+            current_quat = -current_quat
+        current_pose = np.hstack([current_position,current_quat])
+        result_matrix = np.vstack([current_pose,result_matrix])
+        return result_matrix
+
+    
 
 
 if __name__ == "__main__":
@@ -1609,8 +1873,8 @@ if __name__ == "__main__":
     # robot.dt = ts[1] - ts[0]
 
     
-    ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
-    robot.current_head = 'finger_head'
+    ready_pose = [0.4543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
+    robot.current_head = 'none'
 
     robot.force_sensor.disable_active_transmission()
     time.sleep(0.5)
@@ -1621,24 +1885,66 @@ if __name__ == "__main__":
     robot.force_sensor.set_zero()
     time.sleep(0.5)
     robot.force_sensor.enable_active_transmission()
+    # robot.set_position(pose = ready_pose)
     
     robot.init_hardwares(ready_pose)
     time.sleep(3)
+    robot.switch_payload("finger_head")
+    print("111111111111111111111111111111111111111111111111111111111111111111111111111111")
     robot.controller_manager.switch_controller('admittance')
+    # position,quat_rot = robot.arm.get_arm_position()
+    # euler_rot = R.from_quat(quat_rot).as_euler('xyz')
+    # # target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]]
+    # target_point = [0.12191350715303009, -0.1636467057977184, 0.007338312730729687, 8.37758040957278e-05, 1.5708137800874167, -1.5708329787091884]
+    
+    # # target_point = [0.3543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
+    # # robot.move_to_point(pose=target_point,t=60)
+    # # robot.arm.move_joint(q = robot.arm.off_pos)
+    # # robot.arm.move_joint(q = robot.arm.init_pos)
+    # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
+    # pose1 = [0.3736677762541832, 0.11722785977006844, 0.3835904171566223, -3.141592653589793, -0.5235987755982988, 7.330382858376184e-05]
+    # pose1 = [0.4486,0.1172,0.2536,np.pi,-0.5236,0]
+    # pose1 = [0.44866777605267877, 0.11723335755720732, 0.2536866065889565, 2.7069577869199146, -0.7176783797852828, 0.0]
+
+    # robot.set_position(pose = pose1)
+    # print("get_pos:",robot.ge)
+    print("arm_position1:",robot.arm.get_arm_position())
+    print("end_position1:",robot.arm.get_end_position())
+
+    robot.shockwave = Shockwave()
+    robot.shockwave.on()
+    
+    # print("joint1:",robot.arm.getAngel())
+    # robot.arm.move_joint(q=robot.arm.init_pos)
+    # # robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0])
+    # print("arm_position2:",robot.arm.get_arm_position())
+    # print("end_position2:",robot.arm.get_end_position())
+    # print("joint2:",robot.arm.getAngel())
+
+    pose={593.358197,165.740423,216.449797,-173.468246,-33.240047,2.178380}
+
+
     # robot.controller_manager.switch_controller('position')
     
     
     
     atexit.register(robot.force_sensor.disconnect)
-    robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
+    # robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
     try:
-        robot_thread = threading.Thread(target=robot.start)
-        robot_thread.start()    
+        # robot_thread = threading.Thread(target=robot.start)
+        # robot_thread.start()   
+        while True:
+            time.sleep(1) 
     except KeyboardInterrupt:
+        # robot_thread.join()
+        robot.shockwave.off()
         print("用户中断操作。")
     except Exception as e:
+        # robot_thread.join()
+        robot.shockwave.off()
         print("Exception occurred at line:", e.__traceback__.tb_lineno)
         print("发生异常：", e)
     
     
+    
     # robot.arm.disableRobot()

Massage/MassageControl/algorithms/admittance_controller.py

@@ -68,6 +68,10 @@ class AdmittanceController(BaseController):
             self.state.arm_desired_twist[3:] * dt
         ])
         self.clip_command(delta_pose, "pose")
+
+        # # update position
+        # delta_pose[:3] = arm_ori_mat @ delta_pose[:3]
+
         # 更新四元数
         delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
         arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
@@ -77,6 +81,70 @@ class AdmittanceController(BaseController):
         # 更新位置
         self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
 
+        # print("currentAdmit:",self.state.arm_position_command,self.state.arm_position,self.state.arm_orientation_command,self.state.arm_orientation)
+
+        # if time.time() - self.laset_print_time > 1:
+        #     print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_desired_acc)
+
+    # def step(self,dt):
+    #     # 计算误差 位置直接作差，姿态误差以旋转向量表示
+
+    #     temp_pose_error = self.state.arm_position - self.state.desired_position 
+    #     # if time.time() - self.laset_print_time > 5:
+    #     #     print(f'temp_pose_error: {temp_pose_error} ||| arm_position: {self.state.arm_position} ||| desired_position: {self.state.desired_position}')
+    #     if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+    #         self.state.arm_orientation = -self.state.arm_orientation
+
+    #     self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error
+    #     # if time.time() - self.laset_print_time > 5:
+    #     #     print("pose_error:",self.state.pose_error[:3])
+    #     # 计算误差 位置直接作差，姿态误差以旋转向量表示
+    #     #rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T
+    #     rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
+    #     # print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}')
+    #     rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
+    #     self.state.pose_error[3:] = -rot_err_rotvex
+
+    #     #wrench_err = self.state.external_wrench_base - self.state.desired_wrench
+    #     wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+    #     if time.time() - self.laset_print_time > 5:
+    #         print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}')
+    #     self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
+    #     # if time.time() - self.laset_print_time > 5:
+    #     #     print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
+    #     self.clip_command(self.state.arm_desired_acc,"acc")
+    #     self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist
+    #     self.clip_command(self.state.arm_desired_twist,"vel")
+    #     delta_pose = self.state.arm_desired_twist * dt
+
+    #     delta_pose[:3] = self.pos_scale_factor * delta_pose[:3]
+    #     delta_pose[3:] = self.rot_scale_factor * delta_pose[3:]
+    #     # if time.time() - self.laset_print_time > 5:
+    #     #     print("delta_pose:",delta_pose)
+        
+    #     delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3]
+
+    #     # if time.time() - self.laset_print_time > 5:
+    #     #     print("tf_delta_pose:",delta_pose)
+    #     self.clip_command(delta_pose,"pose")
+
+    #     # testlsy
+    #     delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
+
+    #     #arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
+    #     arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat 
+    #     self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
+
+    #     # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:]
+    #     # self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat()
+        
+    #     # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
+    #     self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+
+    #     if time.time() - self.laset_print_time > 1:
+    #         print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_position)
+        
     def step_traj(self,dt):
         # 方向统一
         if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:

Massage/MassageControl/algorithms/arm_state.py

@@ -25,6 +25,7 @@ class ArmState:
         self.desired_wrench = np.zeros(6,dtype=np.float64)
         self.desired_twist = np.zeros(6,dtype=np.float64)
         self.desired_acc = np.zeros(6,dtype=np.float64)
+        self.massage_wrench = np.zeros(6,dtype=np.float64)
 
         # 导纳计算过程变量
         self.arm_desired_twist = np.zeros(6,dtype=np.float64) 

Massage/MassageControl/algorithms/hybridAdmit_controller.py

@@ -59,6 +59,8 @@ class HybridAdmitController(BaseController):
         wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
         # z方向力导纳控制
         self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2])
+        # print("self.state.arm_desired_acc[2]:",self.state.arm_desired_acc[2],self.M_z,wrench_err[2],self.K_z,self.D_z)
+
         self.pose_integral_error +=  self.state.pose_error * dt
         # 位控制
         self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2]

Massage/MassageControl/algorithms/interpolation.py

@@ -359,7 +359,7 @@ def resample_curve_strict(points, num_resampled_points):
 
 
 
-def circle_trajectory(center, omega=8.0, radius=0.04, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None):
+def circle_trajectory(center, omega=8.0, radius=0.06, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None):
 
 
     if time_points is None or len(time_points) < 2:

Massage/MassageControl/config/hybrid.yaml

@@ -5,9 +5,9 @@ stiff_rot: [11, 11, 11]
 damp_rot: [1.1, 1.1, 1.1]
 desired_xi: 1.0
 
-mass_z: [3.0]
+mass_z: [7.0]
 stiff_z: [0]
-damp_z: [30]
+damp_z: [75]
 
 # 位控参数
 Kp: [20,20]

Massage/MassageControl/config/hybridAdmit.yaml

@@ -1,8 +1,9 @@
 name: 'hybridAdmit'
 
-mass_z: [3.0]
+mass_z: [7.0]
 stiff_z: [0]
-damp_z: [30]
+damp_z: [75]
+desired_xi: 1.0
 
 # 位控参数
 Kp: [20,20]

Massage/MassageControl/config/robot_config.yaml

@@ -2,11 +2,17 @@
 arm_ip: '192.168.5.1'
 
 # controller
-controller: ['Massage/MassageControl/config/admittance.yaml',
-              'Massage/MassageControl/config/position.yaml']
+controller: ['MassageControl/config/admittance.yaml',
+              'MassageControl/config/hybrid.yaml',
+              'MassageControl/config/position.yaml',
+              'MassageControl/config/admithybrid.yaml',
+              'MassageControl/config/hybridPid.yaml',
+              'MassageControl/config/hybridAdmit.yaml',
+              'MassageControl/config/positionerSensor.yaml',
+              'MassageControl/config/admittanceZ.yaml',]
 
 # massage heads diretory
-massage_head_dir: 'Massage/MassageControl/config/massage_head'
+massage_head_dir: '/home/jsfb/jsfb_ws/global_config/massage_head'
 
 control_rate: 55
 sensor_rate: 55

Massage/MassageControl/hardware/capture.py

@@ -1,124 +0,0 @@
-import os
-import sys
-import cv2
-import threading
-import numpy as np
-import time
-
-current_file_path = os.path.abspath(__file__)
-dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path))
-sys.path.append(dobot_nova5_path)
-
-from hardware.dobot_nova5 import dobot_nova5
-from hardware.remote_cam import ToolCamera
-
-class Getpose:
-    def __init__(self):
-        self.running = True
-        self.arm = dobot_nova5()
-        self.arm.start()
-        self.arm.init()
-        self.cam = ToolCamera(host='127.0.0.1')
-        self.cam.start()
-        time.sleep(2)
-        
-        self.pose_data = []
-        self.image_data = []
-        # self.arm.start_drag()
-        self.rgb = None
-            
-    def test_show(self):
-        self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
-        if self.rgb is not None:
-            cv2.imshow("Video Feed", self.rgb)
-            cv2.waitKey(1000)
-    
-    def add_data(self):
-        angle = self.arm.getAngel()
-        self.pose_data.append(angle)
-        print(f"Current angle: {angle}")
-    
-    def save_data(self):
-        if self.pose_data:
-            data = np.array(self.pose_data)
-            np.savetxt('pose.txt', data, fmt='%.5f')
-    
-
-if __name__ == "__main__":
-    save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
-    if not os.path.exists(save_directory):
-        os.makedirs(save_directory)
-    
-    myTest = Getpose()
-    
-    myTest.arm.start_drag()
-    
-    time.sleep(0.5)
-    try:
-        count = 1
-        i = 1
-        while True:
-            # 读取摄像头的帧
-            r, d, intrinsics = myTest.cam.get_latest_frame()
-            # ret, frame = cam.read()
-            frame = r
-
-            # if not ret:
-            #     print("Error: Could not read frame from video device.")
-            #     break
-
-                        # 显示摄像头的帧
-            cv2.imshow('found', frame)
-
-            # 等待按键事件
-            key = cv2.waitKey(1) & 0xFF
-
-
-            # if key == ord('r'):
-            #     pose = increase_dof(poselist[index])
-            #     index+=1
-            #     code = cp.arm.move_joint(pose, 4,wait=True)
-            #     if code == -1:
-            #         print("运动到拍照位置失败")
-            #     else:
-
-            # 按下's'键保存照片
-            if key == ord('s'):
-                img_path = f'photo{i}'
-                if not os.path.exists(os.path.join(save_directory, img_path)):
-                    os.makedirs(os.path.join(save_directory, img_path))
-                filename = os.path.join(save_directory, f'{img_path}/rgb_image.png')
-                cv2.imwrite(filename, frame) # 保存照片
-                filename1 = os.path.join(save_directory, f'{img_path}/depth_image.png')
-                cv2.imwrite(filename1, d) # 保存照片
-                angle = myTest.arm.Get_feedInfo_angle()
-                print("angle = ",angle)
-                filename2 = os.path.join(save_directory, f'{img_path}/angle.txt')
-                np.savetxt(filename2, angle.shape(-1,6),delimiter=',')
-                print(f'Saved {filename}')
-                
-                # pose = increase_dof(poselist[index])
-                # index+=1
-                # code = cp.arm.move_joint(poselist[index], 4,wait=True)
-                # if code == -1:
-                #     print("运动到拍照位置失败")
-                count += 1 # 增加照片计数
-                i += 1
-            # 按下'q'键退出循环
-            elif key == ord('q'):
-                break
-            
-
-    except KeyboardInterrupt:
-        # 处理Ctrl+C中断
-        print("\nCamera session ended by user.")
-    finally:
-        # 释放资源
-        myTest.arm.stop_drag()
-        time.sleep(0.5)
-        myTest.arm.disableRobot()
-        myTest.cam.stop()
-        cv2.destroyAllWindows()
-
-        # 总结拍摄的照片
-        print(f'Total number of images captured: {count}')

Massage/MassageControl/hardware/dobot_api.py

@@ -6,6 +6,8 @@ import json
 import threading
 import time
 from time import sleep
+import time
+import select
 
 alarmControllerFile = "files/alarmController.json"
 alarmServoFile = "files/alarmServo.json"
@@ -127,6 +129,7 @@ class DobotApi:
         if self.port == 29999 or self.port == 30004 or self.port == 30005:
             try:
                 self.socket_dobot = socket.socket()
+                self.socket_dobot.settimeout(5) # set time out 5s
                 self.socket_dobot.connect((self.ip, self.port))
                 self.socket_dobot.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 144000)
             except socket.error:
@@ -151,7 +154,7 @@ class DobotApi:
                     self.socket_dobot.send(str.encode(string, 'utf-8'))
                     break
                 except Exception:
-                    sleep(1)
+                    sleep(0.1)
 
     def wait_reply(self):
         """
@@ -159,7 +162,10 @@ class DobotApi:
         """
         data = ""
         try:
+            # print("time1:",time.time())
             data = self.socket_dobot.recv(1024)
+            # print("end_time:",time.time())
+            # print('recvdata:',data)
         except Exception as e:
             print(e)
             self.socket_dobot = self.reConnect(self.ip, self.port)
@@ -172,16 +178,62 @@ class DobotApi:
             # self.log(f'Receive from {self.ip}:{self.port}: {data_str}')
             return data_str
         
+    # def flush_recv_buffer(self):
+    #     """非阻塞清空接收缓冲区"""
+    #     self.socket_dobot.setblocking(0)
+    #     try:
+    #         while True:
+    #             ready = select.select([self.socket_dobot], [], [], 0.01)
+    #             if ready[0]:
+    #                 try:
+    #                     self.socket_dobot.recv(1024)
+    #                 except:
+    #                     break
+    #             else:
+    #                 break
+    #     finally:
+    #         self.socket_dobot.setblocking(1)
+    
+    def flush_recv_buffer(self):
+        """极速清空接收缓冲区"""
+        self.socket_dobot.setblocking(False)
+        try:
+            while True:
+                try:
+                    self.socket_dobot.recv(4096)  # 一次最多清64KB
+                except BlockingIOError:
+                    break
+        finally:
+            self.socket_dobot.setblocking(True) 
+
     def close(self):
         """
-        Close the port
+        Close the socket connection safely
         """
-        if (self.socket_dobot != 0):
+        if isinstance(self.socket_dobot, socket.socket):
             try:
                 self.socket_dobot.shutdown(socket.SHUT_RDWR)
-                self.socket_dobot.close()
             except socket.error as e:
-                print(f"Error while closing socket: {e}")
+                print(f"[WARN] Error during socket shutdown: {e}")
+            try:
+                self.socket_dobot.close()
+                print("[INFO] Socket closed successfully")
+            except socket.error as e:
+                print(f"[ERROR] Error while closing socket: {e}")
+            finally:
+                self.socket_dobot = None  # 清理引用
+
+
+    # def close(self):
+    #     """
+    #     Close the port
+    #     """
+    #     if (self.socket_dobot != 0):
+    #         try:
+    #             self.socket_dobot.shutdown(socket.SHUT_RDWR)
+    #             self.socket_dobot.close()
+    #         except socket.error as e:
+    #             print(f"Error while closing socket: {e}")
 
     def sendRecvMsg(self, string):
         """
@@ -189,10 +241,21 @@ class DobotApi:
         """
         with self.__globalLock:
             self.send_data(string)
+            send_time = time.time()
+            # print(f"send_data:{string}")
             recvData = self.wait_reply()
-            self.ParseResultId(recvData)
+            print(f"===={time.time()-send_time}s====recvData:{string}")
+            if not self.ParseResultId(recvData):
+                return None
             return recvData
         
+    def sendMsg(self, string):
+        """
+        send Sync
+        """
+        self.send_data(string)
+
+
     def __del__(self):
         self.close()
 
@@ -206,6 +269,20 @@ class DobotApi:
                 sleep(1)
         return socket_dobot
     
+    def reConnectWithRetry(self, ip, port, max_retries=5, retry_interval=1):
+        last_exception = None
+        for attempt in range(1, max_retries + 1):
+            try:
+                socket_dobot = socket.socket()
+                socket_dobot.connect((ip, port))
+                return socket_dobot
+            except Exception as e:
+                last_exception = e
+                print(f"第{attempt}次重连失败：{e}")
+                sleep(retry_interval)
+        # 如果走到这里，说明所有尝试都失败
+        raise Exception(f"reConnect在{max_retries}次后仍然失败，最后一次异常：{last_exception}")
+
 # 控制及运动指令接口类
 # Control and motion command interface
 
@@ -215,6 +292,23 @@ class DobotApiDashboard(DobotApi):
     def __init__(self, ip, port, *args):
         super().__init__(ip, port, *args)
 
+    def is_connected(self):
+        try:
+            self.socket_dobot.send(b'')  # 尝试发送空数据
+            return True
+        except:
+            return False
+
+    def Disconnect(self):
+        if self.socket_dobot:
+            try:
+                self.socket_dobot.shutdown(socket.SHUT_RDWR)
+                self.socket_dobot.close()
+            except Exception as e:
+                print(f"断开连接时出错: {e}")
+            finally:
+                self.socket_dobot = None
+
     def EnableRobot(self, load=0.0, centerX=0.0, centerY=0.0, centerZ=0.0, isCheck=-1,):
         """
             可选参数
@@ -1857,6 +1951,7 @@ class DobotApiDashboard(DobotApi):
         else:
             print("coordinateMode param is wrong")
             return ""
+        print("movJ:string,",string)
         params = []
         if user != -1:
             params.append('user={:d}'.format(user))
@@ -1871,6 +1966,9 @@ class DobotApiDashboard(DobotApi):
         for ii in params:
             string = string + ','+ii
         string = string + ')'
+        start_time = time.time()
+        self.flush_recv_buffer()
+        print(f"+++++++{time.time()-start_time}s++++++")
         return self.sendRecvMsg(string)
 
     def MovL(self, a1, b1, c1, d1, e1, f1, coordinateMode, user=-1, tool=-1, a=-1, v=-1, speed=-1, cp=-1, r=-1):
@@ -1972,6 +2070,38 @@ class DobotApiDashboard(DobotApi):
             string = string + ','+ii
         string = string + ')'
         return self.sendRecvMsg(string)
+
+    def ServoPos(self, X, Y, Z, RX, RY, RZ, t=-1.0,aheadtime=-1.0, gain=-1.0):
+        """
+        参数名 类型 含义 是否必填 参数范围
+        X double X 轴位置，单位：毫米 是
+        Y double Y 轴位置，单位：毫米 是
+        Z double Z 轴位置，单位：毫米 是
+        Rx double Rx 轴位置，单位：度 是
+        Ry double Ry 轴位置，单位：度 是
+        Rz double Rz 轴位置，单位：度 是
+        t float 该点位的运行时间，默认0.1,单位：s 否 [0.004,3600.0]
+        aheadtime float 作用类似于PID的D项，默认50，标量，无单位 否 [20.0,100.0]
+        gain float 目标位置的比例放大器，作用类似于PID的P项，默认500，标量，无单位 否 [200.0,1000.0]
+        Pose string  Target point posture variables. The reference coordinate system is the global user and tool coordinate system, see the User and Tool command descriptions in Settings command (the default values are both 0
+        t float Optional parameter.Running time of the point, unit: s, value range: [0.02,3600.0], default value:0.1
+        aheadtime float Optional parameter.Advanced time, similar to the D in PID control. Scalar, no unit, valuerange: [20.0,100.0], default value: 50.
+        gain float Optional parameter.Proportional gain of the target position, similar to the P in PID control.Scalar, no unit, value range: [200.0,1000.0], default value: 500.
+        """
+        string = ""
+        string = "ServoP({:f},{:f},{:f},{:f},{:f},{:f}".format(X, Y, Z, RX, RY, RZ)
+        params = []
+        if t != -1:
+            params.append('t={:f}'.format(t))
+        if aheadtime != -1:
+            params.append('aheadtime={:f}'.format(aheadtime))
+        if gain != -1:
+            params.append('gain={:f}'.format(gain))
+        for ii in params:
+            string = string + ','+ii
+        string = string + ')'
+        return self.sendMsg(string)    
+
     def ServoP(self, X, Y, Z, RX, RY, RZ, t=-1.0,aheadtime=-1.0, gain=-1.0):
         """
         参数名 类型 含义 是否必填 参数范围
@@ -2731,11 +2861,12 @@ class DobotApiDashboard(DobotApi):
         """
         if valueRecv.find("Not Tcp") != -1:  # 通过返回值判断机器是否处于tcp模式 Judge whether the robot is in TCP mode by the return value
             print("Control Mode Is Not Tcp")
-            return
+            return False
         recvData = re.findall(r'-?\d+', valueRecv)
         recvData = [int(num) for num in recvData]
         if len(recvData) > 0:
             if recvData[0] != 0:
+                print("recvData is ", recvData)
                 # 根据返回值来判断机器处于什么状态 Judge what status the robot is in based on the return value
                 if recvData[0] == -1:
                     print("Command execution failed")
@@ -2747,8 +2878,11 @@ class DobotApiDashboard(DobotApi):
                     print("The robot is in power down state")
                 else:
                     print("ErrorId is ", recvData[0])
+                return False
         else:
             print("ERROR VALUE")
+            return False
+        return True
 
     ###################################460新增#############################
     
@@ -3033,10 +3167,32 @@ class DobotApiDashboard(DobotApi):
 
 class DobotApiFeedBack(DobotApi):
     def __init__(self, ip, port, *args):
+        try:
             super().__init__(ip, port, *args)
+        except Exception as e:
+            print(f"[错误] 无法连接 Dobot（IP: {ip}, Port: {port}）：{e}")
+            # 可根据需要抛出异常或安全退出
+            raise  # 或者：self.connected = False; return
+        # super().__init__(ip, port, *args)
         self.__MyType = []
         self.last_recv_time = time.perf_counter()
 
+    def is_connected(self):
+        try:
+            self.socket_dobot.send(b'')  # 尝试发送空数据
+            return True
+        except:
+            return False
+
+    def Disconnect(self):
+        if self.socket_dobot:
+            try:
+                self.socket_dobot.shutdown(socket.SHUT_RDWR)
+                self.socket_dobot.close()
+            except Exception as e:
+                print(f"断开连接时出错: {e}")
+            finally:
+                self.socket_dobot = None
 
     def feedBackData(self):
         """

Massage/MassageControl/hardware/dobot_message.py

@@ -0,0 +1,39 @@
+# 定义机器人模式字典
+robot_mode_types = {
+    -1: "ROBOTMODE: 初始化状态，未检测到机械臂，请检查连接是否正确",
+    1: "INIT: 初始化",
+    2: "BREAK_OPEN: 抱闸松开",
+    3: "POWER_OFF: 本体下电",
+    4: "DISABLED: 未使能(抱闸未松)",
+    5: "ENABLE: 使能(空闲)",
+    6: "BACKDRIVE: 拖拽",
+    7: "RUNNING: 运行状态(脚本/TCP)",
+    8: "SINGLE_MOVE: 单次运动(点动)",
+    9: "ERROR: 错误状态",
+    10: "PAUSE: 暂停状态",
+    11: "COLLISION: 碰撞状态"
+}
+
+# 定义通用错误码字典
+general_error_codes = {
+    0: "无错误：命令下发成功",
+    -1: "命令执行失败：已收到命令，但执行失败了",
+    -2: "机器人处于报警状态：机器人报警状态下无法执行指令，需要清除报警后重新下发指令。",
+    -3: "机器人处于急停状态：机器人急停状态下无法执行指令，需要松开急停并清除报警后重新下发指令。",
+    -4: "机器人处于下电状态：机器人下电状态下无法执行指令，需要先给机器人上电。",
+    -5: "机器人处于脚本运行状态：拒绝执行部分指令，需要先暂停/停止脚本。",
+    -6: "MoveJog指令运动轴与运动类型不匹配：修改coordtype参数值，详见MoveJog指令说明。",
+    -7: "机器人处于脚本暂停状态：拒绝执行部分指令，需要先停止脚本。",
+    -8: "机器人认证过期：机器人处于不可用状态，需联系FAE处理。",
+    -10000: "命令错误：下发的命令不存在",
+    -20000: "参数数量错误：下发命令中的参数数量错误",
+    -30001: "必选参数类型错误（第1个）：有名称的为命名参数类型错误，否则为第1个参数类型错误",
+    -30002: "必选参数类型错误（第2个）：不带名称的第2个必选参数的类型错误",
+    -40001: "必选参数范围错误（第1个）：带名称或第1个必选参数超出范围",
+    -40002: "必选参数范围错误（第2个）：第2个必选参数的参数范围错误",
+    -50001: "可选参数类型错误（第1个）：命名参数类型错误如 user=\"ss\"，否则为第1个可选参数的类型错误",
+    -50002: "可选参数类型错误（第2个）：不带名称的第2个可选参数类型错误",
+    -60001: "可选参数范围错误（第1个）：命名参数如 a=200 超出范围，或第1个参数超范围",
+    -60002: "可选参数范围错误（第2个）：第2个可选参数超出范围"
+}
+

Massage/MassageControl/hardware/force_sensor.py

@@ -15,9 +15,8 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro
 
 
 class XjcSensor:
-    def __init__(self, port=None, baudrate=115200, rate=250):
-        # self.port = '/dev/ttyUSB0'
-        self.port = '/dev/ttyS0'
+    def __init__(self, port='/dev/ttyUSB0', baudrate=115200, rate=250):
+        self.port = port
         self.baudrate = baudrate
         self.rate = rate
         
@@ -296,6 +295,56 @@ class XjcSensor:
             print(f"Serial communication error: {e}")
             return None    
         
+    # def read(self):
+    #     """
+    #     Read the sensor's data.
+    #     """
+    #     if self.ser is None:
+    #         print("Serial port not initialized.")
+    #         return None
+
+    #     try:
+    #         # 清空缓冲区
+    #         if self.ser.in_waiting > 0:
+    #             self.ser.read(self.ser.in_waiting)
+
+    #         # 尝试寻找帧头（0x20 0x4E）
+    #         for _ in range(100):  # 最多尝试100次避免死循环
+    #             if self.ser.in_waiting >= 2:
+    #                 byte1 = self.ser.read(1)
+    #                 if int.from_bytes(byte1, 'big') == 0x20:
+    #                     byte2 = self.ser.read(1)
+    #                     if int.from_bytes(byte2, 'big') == 0x4E:
+    #                         response = bytearray([0x20, 0x4E])
+    #                         break
+    #             else:
+    #                 continue
+    #         else:
+    #             print("Frame header not found.")
+    #             return None
+
+    #         # 读取剩余14个字节
+    #         remaining = self.ser.read(14)
+    #         if len(remaining) < 14:
+    #             print("Incomplete data received.")
+    #             return None
+    #         response.extend(remaining)
+
+    #         # CRC 校验
+    #         Lo_check, Hi_check = self.crc16(response[:-2])
+    #         if response[-1] != Hi_check or response[-2] != Lo_check:
+    #             print("CRC check failed!")
+    #             return None
+
+    #         # 数据解析
+    #         sensor_data = self.parse_data_passive(response)
+    #         return sensor_data
+
+    #     except serial.SerialException as e:
+    #         print(f"Serial communication error: {e}")
+    #         return None
+
+    
 
     def parse_data_passive(self, buffer):
         values = [
@@ -426,11 +475,11 @@ if __name__ == "__main__":
     xjc_sensor.set_zero()
     xjc_sensor.set_zero()
     xjc_sensor.set_zero()
-    # time.sleep(1)
-    # xjc_sensor.enable_active_transmission()
+    time.sleep(1)
+    xjc_sensor.enable_active_transmission()
     while True:
-        # sensor_data = xjc_sensor.read()
-        sensor_data = xjc_sensor.read_data_f32()
+        sensor_data = xjc_sensor.read()
+        # sensor_data = xjc_sensor.read_data_f32()
         current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
         if sensor_data is None:
            print('failed to get force sensor data!')

Massage/MassageControl/hardware/test.py

@@ -1,40 +1,24 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
+import copy
 from scipy.spatial.transform import Rotation as R
+import math
 
-# 定义你的坐标系（原点 + 欧拉角，单位为角度）
-frames = [
-    {"origin": [0, 0, 0], "euler": [0, 0, 0]},
-    {"origin": [2, 2, 2], "euler": [0, 150, 90]},
-    {"origin": [3, 3, 3], "euler": [-30, -180, 0]},
-]
 
-def draw_frame(ax, origin, rot_matrix, length=0.5):
-    origin = np.array(origin)
-    x_axis = origin + rot_matrix @ np.array([length, 0, 0])
-    y_axis = origin + rot_matrix @ np.array([0, length, 0])
-    z_axis = origin + rot_matrix @ np.array([0, 0, length])
-    ax.quiver(*origin, *(x_axis - origin), color='r')
-    ax.quiver(*origin, *(y_axis - origin), color='g')
-    ax.quiver(*origin, *(z_axis - origin), color='b')
+print(math.sin(math.atan2(0.4,0.84))*0.93)
 
-fig = plt.figure()
-ax = fig.add_subplot(111, projection='3d')
-ax.set_box_aspect([1,1,1])
+# point = np.array([0.3670603416735574, -0.36714597755113015, -0.8546756909720299])
 
-for frame in frames:
-    origin = frame["origin"]
-    euler_deg = frame["euler"]
-    rotation = R.from_euler('xyz', euler_deg, degrees=True)
-    rot_matrix = rotation.as_matrix()
-    draw_frame(ax, origin, rot_matrix)
 
-ax.set_xlim(-1, 3)
-ax.set_ylim(-1, 3)
-ax.set_zlim(-1, 3)
-ax.set_xlabel("X")
-ax.set_ylabel("Y")
-ax.set_zlabel("Z")
-plt.title("Multiple 3D Coordinate Frames (Euler Angles)")
-plt.show()
+# temp_point = copy.deepcopy(point)
+# if point[2]<= 0:
+#     temp_point[0] = -math.atan2(point[1],-np.sqrt(1-point[1]*point[1]))
+# else:
+#     temp_point[0] = -math.atan2(point[1],np.sqrt(1-point[1]*point[1])) 
+# temp_point[1] = -math.atan2(point[0], -point[2])
+# temp_point[2] = 0
+# # temp_point[0] = -math.asin(point[1])
+# # temp_point[1] = -math.atan2(-point[0],point[2])
+# # temp_point[2] = 0
+# print(temp_point)

Massage/MassageControl/tools/auto_visual_calibration.py

@@ -0,0 +1,293 @@
+import cv2
+import os
+from datetime import datetime
+import numpy as np
+import sys
+from pathlib import Path
+import time
+from scipy.spatial.transform import Rotation as R
+
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/MassageControl')
+from hardware.remote_cam import ToolCamera
+from hardware.aubo_C5 import AuboC5
+
+np.set_printoptions(precision=8, suppress=True)
+# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
+criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)
+
+
+# 标定计算类  
+# -- 连接AuboC5 
+# -- 连接 ToolCamera   
+# -- 运动到初始位置 计算下一步运动位姿
+# -- 开始手眼标定
+# -- 输出结果 
+class Calibration:
+    def __init__(self, board_size, square_size, file_folder):
+        """
+        初始化标定计算类。
+        board_size: 标定板的格子数，格式为 (width, height)
+        square_size: 格子的大小，单位为米
+        """
+        self.board_size = board_size
+        self.square_size = square_size
+        self.objp = self._create_object_points()
+        self.image_points = []
+        self.robot_poses = []
+        self.intrinsics = None
+        self.distortion_cofficients = None
+        self.cam = ToolCamera(host='127.0.0.1')
+        self.cam.start()
+        time.sleep(5)
+        self.size = (400,640)
+        rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
+        if rgb_image is None :
+            print(f'===============相机连接失败请检查并重试==============')
+            sys.exit(1)
+        else:
+            print(f'===============相机连接成功==============')
+            self.size = rgb_image.shape[:2]
+        self.intrinsics = camera_intrinsics
+        
+        self.arm = AuboC5()
+        self.arm.init()
+        self.obj_points = []
+        print(f'===============机械臂连接成功==============')
+        date_time_str = time.strftime("%Y%m%d%H%M%S", time.localtime())
+        folder_name = f"calibration_data_{date_time_str}"
+        file_folder+=folder_name
+        os.makedirs(file_folder, exist_ok=True)
+        print(f'===============创建文件夹成功=============')   
+        self.file_address = file_folder
+        
+    def _create_object_points(self):
+        """创建标定板的3D点"""
+        objp = np.zeros((self.board_size[0] * self.board_size[1], 3), np.float32)
+        objp[:, :2] = np.mgrid[0:self.board_size[0], 0:self.board_size[1]].T.reshape(-1, 2) * self.square_size
+        return objp
+
+    def collect_data(self):
+        #移动到初始位置 
+        pose = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)]
+        code = self.arm.move_joint(pose,4,wait=True)
+        if code == -1:
+            print("运动到拍照位置失败")
+        #拍照
+        self.check_corners()
+        #运动到下一个位置
+        # 目前只设计了44条轨迹    
+        for i in range (0,45):
+            next_pose = self.get_next_pose_ugly(i)
+            self.arm.move_joint(next_pose, 4,wait=True)
+            if code == -1:
+                print("运动到拍照位置失败")
+            else:
+                self.check_corners()
+    
+    def check_corners(self):
+        rgb, depth, intrinsics = self.cam.get_latest_frame()
+        gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
+        ret, corners = cv2.findChessboardCorners(gray, self.board_size, None)
+        if ret:
+            robot_pose = self.get_pose_homomat()
+            self.add_sample(gray,corners,robot_pose)
+            print(f"robot_pose = {robot_pose}")
+            
+            pic_name = self.file_address + "/"+str(len(self.image_points)) + ".jpg"
+            print(f"save path as: {pic_name}")
+            # 绘制检测到的角点 (用于可视化，可选)
+            cv2.drawChessboardCorners(rgb, self.board_size, corners, ret)
+            cv2.imwrite(pic_name, rgb)
+
+            return True
+        return False 
+
+        
+    def add_sample(self, gray, corners,robot_pose):
+        """
+        添加标定样本。
+        image: 捕获的图像
+        robot_pose: 机械臂的位姿，格式为 [x, y, z, roll, pitch, yaw]
+        """
+        # 检测棋盘角点
+
+        self.obj_points.append(self.objp)
+        corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)  # 在原角点的基础上寻找亚像素角点
+        if [corners2]:
+            self.image_points.append(corners2)
+        else:
+            self.image_points.append(corners)
+        
+        self.robot_poses.append(robot_pose)
+        
+    def get_pose_homomat(self):
+        pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose()
+        x, y, z, rx, ry, rz = pose
+        r = R.from_euler('xyz', [rx, ry, rz])
+        rotation_matrix = r.as_matrix()
+        translation_vector = np.array([x, y, z]).reshape(3, 1)
+        homo_mat = np.eye(4) # 创建 4x4 单位矩阵
+        homo_mat[:3, :3] = rotation_matrix
+        homo_mat[:3, 3] = translation_vector.flatten() 
+        return homo_mat
+    
+    def camera_calib(self):
+        # print(f"\n\n\n\n\nsize is {self.size}")
+        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.obj_points, self.image_points, (self.size[1], self.size[0]), None, None)
+        # print("rvecs,tvecs:",rvecs,tvecs)
+        if ret:
+            print("内参矩阵:\n", mtx)  # 内参数矩阵
+            print("畸变系数:\n", dist)  # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
+            print("++++++++++相机标定完成++++++++++++++")
+            # 从mtx中提取内参
+            fx = mtx[0, 0]  # 焦距 fx
+            fy = mtx[1, 1]  # 焦距 fy
+            cx = mtx[0, 2]  # 主点 cx
+            cy = mtx[1, 2]  # 主点 cy
+
+            # 从 distortion_array 提取畸变系数
+            k1 = dist[0][0]  # 畸变系数 k1
+            k2 = dist[0][1]  # 畸变系数 k2
+            p1 = dist[0][2]  # 畸变系数 p1
+            p2 = dist[0][3]  # 畸变系数 p2
+            k3 = dist[0][4]  # 畸变系数 k3
+            # self.intrinsics = mtx
+            self.intrinsics = {
+                'fx': fx,
+                'fy': fy,
+                'cx': cx,
+                'cy': cy,
+                'distortion_coeffs': {
+                    'k1': k1,
+                    'k2': k2,
+                    'p1': p1,
+                    'p2': p2,
+                    'k3': k3
+                }
+            }
+            self.distortion_cofficients = dist
+        else:
+            print("-----------相机标定失败--------------")
+        return rvecs, tvecs
+    
+    def split_robot_pose(self):
+        """
+        将包含 4x4 齐次矩阵的 robot_pose 列表拆分成 R_arm 和 t_arm。
+        Args:
+            robot_pose: 包含 4x4 齐次矩阵的 Python list。
+        Returns:
+            R_arm: 旋转矩阵的 Python list, 每个元素是 3x3 numpy 数组。
+            t_arm: 平移向量的 Python list, 每个元素是 3x1 numpy 数组。
+        """
+        R_arm = []
+        t_arm = []
+
+        for pose_matrix in self.robot_poses:
+            # # 1. 提取旋转矩阵 (3x3)
+            rotation_matrix = pose_matrix[:3, :3]
+            R_arm.append(rotation_matrix)
+            # R_arm.append(pose_matrix[:3])
+
+
+            # 2. 提取平移向量 (3x1)
+            translation_vector = pose_matrix[:3, 3]  # 获取齐次矩阵的平移部分 (1x3 numpy 数组)
+            translation_vector = translation_vector.reshape(3, 1) # 转换为 3x1 的列向量
+            t_arm.append(translation_vector)
+
+        return R_arm, t_arm
+
+    def calibrate(self):
+        rvecs, tvecs = self.camera_calib()
+        r_arm, t_arm= self.split_robot_pose()
+        print(rvecs)
+        print(tvecs)
+        print(r_arm)
+        print(t_arm)
+        rvecs_array = np.array(rvecs)
+        tvecs_array = np.array(tvecs)
+        t_arma = np.array(t_arm)
+        r_arma = np.array(r_arm)
+        print(rvecs_array.shape)
+        print(tvecs_array.shape)
+        print(t_arma.shape)
+        print(r_arma.shape)
+        R, t = cv2.calibrateHandEye(r_arma, t_arma, rvecs_array, tvecs_array, cv2.CALIB_HAND_EYE_TSAI)
+        print("+++++++++++手眼标定完成+++++++++++++++")
+        self.arm.exit_task()
+        return R, t ,self.intrinsics
+    
+#得到图片以后 根据pose判断方向：
+    def get_next_pose_future(self):
+        return None
+    def get_next_pose_ugly(self,index):
+        poselist =[
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
+            [1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0],
+            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0],
+            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
+            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0],
+            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0],
+            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0],
+            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0],
+            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0],
+            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0]
+            ]
+        return poselist[index]
+        
+
+
+if __name__ == '__main__':
+
+    # 标定板的width 对应的角点的个数 6 
+    # 标定板的height 对应的角点的个数 3 
+    calibration_board_size = [6,3]
+    calibration_board_square_size = 0.03 #unit - meter 
+    systemPath = "/home/jsfb/Documents/"
+    
+
+    calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
+    calib.collect_data()
+    R, t,intrinsics = calib.calibrate()
+    print("旋转矩阵：")
+    print(R)
+    print("平移向量：")
+    print(t)
+    print("内参：")
+    print(intrinsics)

Massage/MassageControl/tools/auto_visual_valibration.py

@@ -10,7 +10,7 @@ from scipy.spatial.transform import Rotation as R
 sys.path.append(str(Path(__file__).resolve().parent.parent))
 sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl')
 from hardware.remote_cam import ToolCamera
-from hardware.dobot_nova5 import dobot_nova5
+from hardware.aubo_C5 import AuboC5
 
 np.set_printoptions(precision=8, suppress=True)
 # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
@@ -41,20 +41,16 @@ class Calibration:
         self.size = None
         self.intrinsics_mtx = None
         self.disorder = None
-        time.sleep(2)
         rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
-        self.save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
-       
         if rgb_image is None :
             print(f'===============相机连接失败请检查并重试==============')
         else:
+            print(f'===============相机连接成功==============')
             self.size = rgb_image.shape[:2]
-            print(f'===============相机连接成功==============  ',self.size)
-            
         self.intrinsics = camera_intrinsics
         
-        self.arm = dobot_nova5("192.168.5.1")
-        self.arm.start()
+        self.arm = AuboC5()
+        self.arm.init()
         self.obj_points = []
         print(f'===============机械臂连接成功==============')      
         
@@ -66,29 +62,24 @@ class Calibration:
   
     def collect_data(self):
         #移动到初始位置 
-        pose = [-45.0009079,55.5785141,-120.68821716,5.11103201,-90.00195312,-90.00085449]
-        code = self.arm.RunPoint_P_inJoint(pose)
+        pose = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)]
+        code = self.arm.move_joint(pose,4,wait=True)
         if code == -1:
             print("运动到拍照位置失败")
         #拍照
         self.check_corners()
         #运动到下一个位置
         # 目前只设计了44条轨迹    
-        for i in range (0,18):
+        for i in range (0,45):
             next_pose = self.get_next_pose_ugly(i)
-            code=self.arm.RunPoint_P_inJoint(next_pose)
-            time.sleep(0.5)
+            self.arm.move_joint(next_pose, 4,wait=True)
             if code == -1:
                 print("运动到拍照位置失败")
             else:
                 self.check_corners()
-                time.sleep(0.5)
     
     def check_corners(self):
         rgb, depth, intrinsics = self.cam.get_latest_frame()
-        # img_path = f'photo{i}'
-        # filename = os.path.join(self.save_directory, f'{img_path}/rgb_image.png')
-        # rgb=cv2.imread(filename)
         gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
         ret, corners = cv2.findChessboardCorners(gray, self.board_size, None)
         if ret:
@@ -124,12 +115,14 @@ class Calibration:
         self.robot_poses.append(robot_pose)
           
     def get_pose_homomat(self):
-        arm_position,arm_orientation = self.arm.get_arm_position()
-        rotation_matrix = R.from_quat(arm_orientation).as_matrix()
-        translation_vector = arm_position
+        pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose()
+        x, y, z, rx, ry, rz = pose
+        r = R.from_euler('xyz', [rx, ry, rz])
+        rotation_matrix = r.as_matrix()
+        translation_vector = np.array([x, y, z]).reshape(3, 1)
         homo_mat = np.eye(4) # 创建 4x4 单位矩阵
         homo_mat[:3, :3] = rotation_matrix
-        homo_mat[:3, 3] = translation_vector
+        homo_mat[:3, 3] = translation_vector.flatten() 
         return homo_mat
     
     def camera_calib(self):
@@ -139,7 +132,6 @@ class Calibration:
         if ret:
             print("内参矩阵:\n", mtx)  # 内参数矩阵
             print("畸变系数:\n", dist)  # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
-            print("重投影误差:\n",ret)
             print("++++++++++相机标定完成++++++++++++++")
             self.intrinsics_mtx = mtx
             self.disorder = dist
@@ -193,24 +185,52 @@ class Calibration:
         return None
     def get_next_pose_ugly(self,index):
         poselist =[
-            [-4.513801574707031250e+01,5.518265151977539062e+01,-1.237858963012695312e+02,9.085088729858398438e+00,-9.337700653076171875e+01,-9.000016784667968750e+01],
-            [-4.594435882568359375e+01,3.636156082153320312e+01,-1.064173660278320312e+02,-2.125140428543090820e+00,-9.337675476074218750e+01,-9.000016784667968750e+01],
-            [-4.595432662963867188e+01,1.059147930145263672e+01,-8.284246063232421875e+01,-1.484527587890625000e+01,-9.337491607666015625e+01,-9.000013732910156250e+01],
-            [-4.638331604003906250e+01,-6.824899196624755859e+00,-6.758448791503906250e+01,-2.286166954040527344e+01,-9.337348937988281250e+01,-9.000016784667968750e+01],
-            [-4.636801910400390625e+01,-1.807893753051757812e+01,-6.532709503173828125e+01,-2.261142921447753906e+01,-9.336659240722656250e+01,-9.000010681152343750e+01],
-            [-4.637482833862304688e+01,-4.540402412414550781e+00,-1.098018112182617188e+02,3.086268234252929688e+01,-9.305430603027343750e+01,-9.000022125244140625e+01],
-            [-4.640084075927734375e+01,8.068140029907226562e+00,-1.362033843994140625e+02,6.259086990356445312e+01,-9.334789276123046875e+01,-9.000019073486328125e+01],
-            [-4.640056610107421875e+01,8.061657905578613281e+00,-1.511797027587890625e+02,8.927394104003906250e+01,-9.331819915771484375e+01,-9.000019073486328125e+01],
-            [-4.638466262817382812e+01,3.287191867828369141e+00,-1.566032409667968750e+02,1.035896453857421875e+02,-9.330465698242187500e+01,-9.000022125244140625e+01],
-            [-3.178509473800659180e+00,6.977911472320556641e+00,-1.417592010498046875e+02,6.005132293701171875e+01,-1.280102996826171875e+02,-9.000010681152343750e+01],
-            [-1.424913024902343750e+01,-2.980865538120269775e-01,-1.193396759033203125e+02,3.752756500244140625e+01,-1.278784713745117188e+02,-8.999975585937500000e+01],
-            [-2.393187522888183594e+01,-2.973175048828125000e-01,-1.121088027954101562e+02,3.928178024291992188e+01,-1.183067703247070312e+02,-8.999983215332031250e+01],
-            [-3.699544525146484375e+01,-2.981689572334289551e-01,-1.078408889770507812e+02,3.178039932250976562e+01,-1.024006271362304688e+02,-8.999983215332031250e+01],
-            [-6.734904479980468750e+01,-4.368630886077880859e+00,-1.035898971557617188e+02,2.342595481872558594e+01,-7.085527801513671875e+01,-8.999980926513671875e+01],
-            [-8.632910919189453125e+01,-1.039798259735107422e+01,-1.038105545043945312e+02,2.330961036682128906e+01,-5.476583099365234375e+01,-8.999980926513671875e+01],
-            [-8.156823730468750000e+01,5.878097534179687500e+00,-1.439221954345703125e+02,6.846669006347656250e+01,-4.482905197143554688e+01,-8.999922943115234375e+01],
-            [-8.601764678955078125e+01,3.570293045043945312e+01,-1.308228912353515625e+02,2.318510818481445312e+01,-5.378013610839843750e+01,-8.999922943115234375e+01],
-            [-7.132132053375244141e+00,3.722338104248046875e+01,-1.065378265380859375e+02,-3.337371826171875000e+00,-1.185582199096679688e+02,-8.999931335449218750e+01]
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
+            [1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0],
+            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0],
+            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
+            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0],
+            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0],
+            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0],
+            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0],
+            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0],
+            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0]
             ]
         return poselist[index]
         
@@ -221,7 +241,7 @@ if __name__ == '__main__':
     # 标定板的width 对应的角点的个数 6 
     # 标定板的height 对应的角点的个数 3 
     calibration_board_size = [6,3]
-    calibration_board_square_size = 0.03 #unit - meter 
+    calibration_board_square_size = 0.027 #unit - meter 
     systemPath = "/home/jsfb/Documents/"
     
     now = datetime.now()
@@ -231,22 +251,10 @@ if __name__ == '__main__':
     os.makedirs(systemPath, exist_ok=True)
 
     calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
-    
-    time.sleep(0.5)
-    calib.arm.chooseBaseFrame(i=1)
-    calib.arm.chooseEndEffector(i=0)
-    time.sleep(0.5)
-    
     calib.collect_data()
-    print("=================数据采集完成===================")
     R, t,intrin = calib.calibrate()
-    
     print("旋转矩阵：")
     print(R)
     print("平移向量：")
     print(t)
     print(f"内参： {intrin}")
-    time.sleep(0.5)
-    calib.cam.stop()
-    time.sleep(0.5)
-    print('camera stopped')

Massage/MassageControl/tools/identifier_dobot.py

@@ -94,13 +94,21 @@ class Identifier:
             x y z : mm
             r p y : deg
         '''
-        pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]])
+        # pose_command = np.concatenate([[x,y,z]/1000, [roll,pitch,yaw]*np.pi/180])
+        pose_command = np.concatenate([
+            np.array([x, y, z]) / 1000.0,  # mm ➝ m
+            np.array([roll, pitch, yaw]) * np.pi / 180.0  # deg ➝ rad
+        ])
+
         cur = self.arm.getAngel() # 接口调用: 获取机器人的关节位置
         cur_str = "{" + ",".join(map(str, cur)) + "}"
         # print("cur_str",cur_str)
-        target_J = self.arm.getInverseKin(poses_list=pose_command,useJointNear=1,JointNear=cur_str)
-        print(target_J)
-        self.arm.RunPoint_P_inJoint(target_J)
+        print("pose_command:",pose_command)
+        self.arm.RunPoint_P_inPose_M_RAD(pose = pose_command)
+        
+        # target_J = self.arm.getInverseKin(poses_list=pose_command,useJointNear=1,JointNear=cur_str)
+        # print(target_J)
+        # self.arm.RunPoint_P_inJoint(target_J)
 
     def identify_param_auto(self,ready_pose,cnt,sample_num = 5):
         if cnt < 15:
@@ -170,19 +178,20 @@ class Identifier:
         
         
 if __name__ == '__main__':
-    arm = dobot_nova5("192.168.5.1")
-    arm.start()
-    arm.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
-    arm.chooseEndEffector(i=9)
+    arm = dobot_nova5()
+    # arm.start()
+    # arm.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
+    # arm.chooseEndEffector(i=9)
     arm.init()
-    sensor = XjcSensor("192.168.5.1", 60000)
+    sensor = XjcSensor()
     sensor.connect()
     sensor.disable_active_transmission()
     sensor.disable_active_transmission()
     sensor.disable_active_transmission()
     atexit.register(sensor.disconnect)
-    # identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/massage_head/finger_playload.yaml")
+    identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb_ws/global_config/massage_head/roller_playload.yaml")
+    ready_pose = [0.403467*1000,-0.1349880*1000,0.423604*1000,-180.0000,-30.0000,0.0042]
     # ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
-    # time.sleep(1)
-    # identifier.identify_param_auto(ready_pose,45)
+    time.sleep(1)
+    identifier.identify_param_auto(ready_pose,45)
     arm.disableRobot()

Massage/MassageControl/tools/replay_trajectory_from_csv.py

@@ -0,0 +1,328 @@
+# cython: language_level=3
+import time
+import csv
+import atexit
+from pathlib import Path
+import sys
+import ctypes
+import threading
+import pygame  # 添加pygame导入
+
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+from hardware.aubo_C5 import AuboC5
+from hardware.force_sensor_aubo import XjcSensor
+
+# 初始化libc
+try:
+    libc = ctypes.CDLL("libc.so.6")
+except Exception as e:
+    print(f"Failed to load libc: {e}")
+    libc = None
+
+class JointTrajectoryPlayer:
+    def __init__(self, csv_path: str, arm: AuboC5 = None, sensor: XjcSensor = None):
+        """
+        初始化轨迹回放器
+        :param csv_path: 轨迹CSV文件路径
+        :param arm: 可选，机械臂控制器
+        :param sensor: 可选，力传感器
+        """
+        self.csv_path = csv_path
+        self.arm = arm
+        self.sensor = sensor
+        self.file = None
+        self.reader = None
+        self.header = None
+        self.keep_playing = True
+        self.play_thread = None
+        
+    def __enter__(self):
+        """上下文管理器入口，打开文件"""
+        self.file = open(self.csv_path, 'r')
+        self.reader = csv.reader(self.file)
+        self.header = next(self.reader)  # 读取表头
+        return self
+    
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        """上下文管理器退出，关闭文件"""
+        if self.file:
+            self.file.close()
+    
+    def read_next_joint_angles(self):
+        """
+        读取下一行关节角度
+        :return: (timestamp, [j1,j2,j3,j4,j5,j6]) 或 None(文件结束)
+        """
+        try:
+            row = next(self.reader)
+            timestamp = float(row[0])
+            angles = [float(x) for x in row[1:7]]  # 读取6个关节角度
+            return timestamp, angles
+        except (StopIteration, ValueError, IndexError):
+            return None
+        
+    def playback(self, speed_factor: float = 1.0):
+        """
+        回放轨迹
+        :param speed_factor: 回放速度因子(1.0为原速)
+        """
+        while self.keep_playing:  # 外部可以通过设置keep_playing为False来停止播放
+            # 每次循环开始时重新打开文件和读取器
+            self.file = open(self.csv_path, 'r')
+            self.reader = csv.reader(self.file)
+            self.header = next(self.reader)  # 读取表头
+            
+            start_time = time.time()
+            first_timestamp = None
+            
+            while self.keep_playing:  # 内部循环，播放单次轨迹
+                data = self.read_next_joint_angles()
+                if data is None:
+                    break
+                    
+                timestamp, angles = data
+                
+                # 初始化第一个时间戳
+                if first_timestamp is None:
+                    first_timestamp = timestamp
+                    continue
+                
+                usleep(15000)
+                
+                # 执行运动
+                if self.arm:
+                    self.arm.mc.servoJoint(angles, 0, 0, 0.15, 0.1, 200)
+                    
+                print(f"时间: {timestamp:.3f}s, 角度: {angles}")
+            
+            # 关闭当前文件
+            self.file.close()
+
+        return 0
+    
+    def start(self, speed_factor: float = 1.0):
+        """启动轨迹播放，返回是否成功启动"""
+        if self.play_thread and self.play_thread.is_alive():
+            return False  # 已经在运行中
+            
+        self.keep_playing = True
+        self.play_thread = threading.Thread(target=self.playback, kwargs={'speed_factor': speed_factor})
+        self.play_thread.daemon = True
+        self.play_thread.start()
+        return True
+        
+    def stop(self):
+        """停止轨迹播放，返回是否成功停止"""
+        if not self.play_thread or not self.play_thread.is_alive():
+            return False  # 未在运行或已停止
+            
+        self.keep_playing = False
+        return True
+
+
+class RobotDanceController:
+    """机器人舞蹈控制器，单例模式"""
+    _instance = None
+    
+    def __new__(cls, *args, **kwargs):
+        if cls._instance is None:
+            cls._instance = super(RobotDanceController, cls).__new__(cls)
+            cls._instance._initialized = False
+        return cls._instance
+        
+    def __init__(self):
+        if self._initialized:
+            return
+            
+        self.arm = None
+        self.player = None
+        self.is_running = False
+        self._initialized = True
+        self.music_playing = False
+        
+    def initialize(self):
+        """初始化机械臂和轨迹播放器"""
+        try:
+            # 初始化机械臂
+            self.arm = AuboC5()
+            self.arm.init()
+            
+            # 设置CSV路径
+            script_dir = Path(__file__).resolve().parent
+            csv_path = script_dir / "joint_processed_0.025s_6.csv"
+            
+            # 创建轨迹播放器
+            self.player = JointTrajectoryPlayer(str(csv_path), self.arm)
+            
+            return True
+        except Exception as e:
+            print(f"初始化失败: {e}")
+            return False
+            
+    def play_music(self):
+        """播放背景音乐，从头开始，循环播放"""
+        try:
+            # 初始化pygame混音器
+            pygame.mixer.init()
+            
+            # 获取音乐文件路径
+            script_dir = Path(__file__).resolve().parent
+            # music_path = script_dir / "demo_music.mp3"
+            music_path = script_dir / "demo_music1.mp3"
+            
+        # 加载并播放音乐从头开始，设置循环播放（loops=-1表示无限循环）
+            pygame.mixer.music.load(str(music_path))
+            pygame.mixer.music.play(loops=-1)
+            self.music_playing = True
+            print("背景音乐已开始播放（循环模式）")
+            
+            return True
+        except Exception as e:
+            print(f"播放音乐失败: {e}")
+            return False
+            
+    def stop_music(self):
+        """停止背景音乐"""
+        if self.music_playing:
+            try:
+                pygame.mixer.music.stop()
+                pygame.mixer.quit()
+                self.music_playing = False
+                print("背景音乐已停止")
+            except Exception as e:
+                print(f"停止音乐失败: {e}")
+            
+    def start_demo(self):
+        """开始演示，返回是否成功启动"""
+        if self.is_running:
+            return False
+            
+        # 如果未初始化，先初始化
+        if not self.arm or not self.player:
+            if not self.initialize():
+                return False
+                
+        try:
+            # 移动到舞蹈准备姿势
+            self.arm.move_joint([1.571,0.578,2.35,0.205,1.569,0.0]) 
+            time.sleep(2)
+            
+            # 先播放背景音乐
+            self.play_music()
+            
+            # 创建一个线程等待14秒后启动机器人运动
+            def start_robot_movement_at_music_mark():
+                try:
+                    start_time = time.time()
+                    # wait_time = 8.0  # 8秒
+                    wait_time = 4.0  # 4秒
+                    print(f"等待音乐播放到{wait_time}秒...")
+                    time.sleep(wait_time)
+                    
+                    # 检查是否仍在运行状态（可能用户已经停止了演示）
+                    if self.is_running:
+                        print(f"音乐已播放{wait_time}秒，开始机器人运动")
+                        # 启动轨迹播放器
+                        # 启动伺服
+                        self.arm.enable_servo()
+                        time.sleep(1)
+                        self.arm.enable_servo()
+                        time.sleep(1)
+                        self.arm.enable_servo()
+                        time.sleep(1)
+                        self.player.start()
+                except Exception as e:
+                    print(f"启动机器人运动失败: {e}")
+                    
+            # 设置为正在运行状态，这样线程中可以检查状态
+            self.is_running = True
+            
+            # 启动等待线程
+            threading.Thread(target=start_robot_movement_at_music_mark, daemon=True).start()
+            
+            return True
+        except Exception as e:
+            # 发生错误时，确保停止音乐
+            self.stop_music()
+            self.is_running = False
+            print(f"启动演示失败: {e}")
+            return False
+            
+    def stop_demo(self):
+        """停止演示，返回是否成功停止"""
+        if not self.is_running:
+            return False
+            
+        try:
+            # 停止背景音乐
+            self.stop_music()
+            
+            # 停止播放器
+            if self.player:
+                self.player.stop()
+                
+            # 关闭伺服
+            if self.arm:
+                self.arm.disable_servo()
+                time.sleep(1)
+                self.arm.disable_servo()
+                time.sleep(1)
+                self.arm.disable_servo()
+                time.sleep(1)
+                
+                # 回到初始位置
+                self.arm.move_joint(self.arm.off_pos)
+            
+            self.is_running = False
+            return True
+        except Exception as e:
+            print(f"停止演示失败: {e}")
+            # 即使发生异常，也将状态设为未运行
+            self.is_running = False
+            return False
+            
+    def cleanup(self):
+        """清理资源"""
+        try:
+            if self.is_running:
+                self.stop_demo()
+            else:
+                # 确保音乐停止
+                self.stop_music()
+                
+            if self.arm:
+                self.arm.power_off()
+                
+            return True
+        except Exception as e:
+            print(f"清理资源失败: {e}")
+            return False
+
+
+def usleep(microseconds):
+    """Linux 下的微秒级休眠（比 time.sleep 更精确）"""
+    if libc and hasattr(libc, 'usleep'):
+        libc.usleep(microseconds)
+    else:
+        # 如果libc不可用，使用time.sleep作为备选
+        time.sleep(microseconds / 1000000.0)
+
+
+if __name__ == '__main__':
+    # 作为独立脚本运行时，提供简单的CLI接口
+    controller = RobotDanceController()
+    
+    try:
+        print("开始机器人舞蹈演示...")
+        if controller.start_demo():
+            print("演示已开始，按Ctrl+C停止...")
+            
+            # 等待用户按Ctrl+C
+            while True:
+                time.sleep(1)
+    except KeyboardInterrupt:
+        print("\n接收到停止信号，正在停止演示...")
+    finally:
+        controller.stop_demo()
+        controller.cleanup()
+        print("演示已停止，资源已清理。")