xiugai yidui

Massage/Massage.py

@@ -2209,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"
         

Massage/MassageControl/MassageRobot_nova5.py

@@ -198,8 +198,8 @@ 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)
@@ -422,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:
@@ -439,49 +440,59 @@ 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}")
             
@@ -665,10 +676,11 @@ class MassageRobot:
                         if self.arm.is_exit or self.exit_event.is_set():
                             break
                         try:
-                            self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                            # 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
+                            # self.arm.dashboard.socket_dobot.recv(1024)
+                            # recv_time = time.time() - recv_start_time
                             break
                         except Exception as e:
                             print(f"发送指令错误：{e}")
@@ -762,14 +774,14 @@ class MassageRobot:
             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()
             # clear_time = time.time()
             # self.clear_socket_buffer(self.arm.dashboard.socket_dobot)
             # print(f"==={time.time()-clear_time}===")
 
     def stop(self):
-        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停止")
         

Massage/MassageControl/hardware/dobot_api.py

@@ -242,13 +242,20 @@ class DobotApi:
         with self.__globalLock:
             self.send_data(string)
             send_time = time.time()
-            print(f"send_data:{string}")
+            # print(f"send_data:{string}")
             recvData = self.wait_reply()
             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()
 
@@ -2063,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):
         """
         参数名 类型 含义 是否必填 参数范围

Massage/MassageControl/hardware/force_sensor.py

@@ -15,7 +15,7 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro
 
 
 class XjcSensor:
-    def __init__(self, port='/dev/ttyS0', baudrate=115200, rate=250):
+    def __init__(self, port='/dev/ttyUSB0', baudrate=115200, rate=250):
         self.port = port
         self.baudrate = baudrate
         self.rate = rate
@@ -295,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 = [
@@ -425,14 +475,14 @@ if __name__ == "__main__":
     xjc_sensor.set_zero()
     xjc_sensor.set_zero()
     xjc_sensor.set_zero()
-    # time.sleep(1)
-    # xjc_sensor.enable_active_transmission()
-    # while True:
-    #     # sensor_data = xjc_sensor.read()
+    time.sleep(1)
+    xjc_sensor.enable_active_transmission()
+    while True:
+        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!')
-    #     print(f"{current_time}-----{sensor_data}")
-    #     rate.sleep(False)
-    #     # break
+        current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
+        if sensor_data is None:
+           print('failed to get force sensor data!')
+        print(f"{current_time}-----{sensor_data}")
+        rate.sleep(False)
+        # break

Massage/MassageControl/tools/identifier_dobot.py

@@ -180,8 +180,8 @@ class Identifier:
 if __name__ == '__main__':
     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.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()
     sensor.connect()
@@ -189,9 +189,9 @@ if __name__ == '__main__':
     sensor.disable_active_transmission()
     sensor.disable_active_transmission()
     atexit.register(sensor.disconnect)
-    identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb_ws/global_config/massage_head/shockwave_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,15)
+    identifier.identify_param_auto(ready_pose,45)
     arm.disableRobot()