From 2064831435535c901841f0f7bde5ce27dc4ecff1 Mon Sep 17 00:00:00 2001
From: "Ziwei.He" <hellohezw@gmail.com>
Date: Thu, 15 May 2025 16:39:33 +0800
Subject: [PATCH] =?UTF-8?q?=E5=8A=9B=E6=8E=A7=E8=B0=83=E8=AF=95=E7=AC=AC?=
 =?UTF-8?q?=E4=B8=80=E9=98=B6=E6=AE=B5=E7=BB=93=E6=9D=9F=EF=BC=8C=E9=9C=80?=
 =?UTF-8?q?=E8=A6=81=E9=92=88=E5=AF=B9=E8=B6=8A=E7=96=86=E5=BA=95=E5=B1=82?=
 =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E6=B6=88=E6=81=AF=E9=98=9F=E5=88=97/?=
 =?UTF-8?q?=E9=AA=8C=E8=AF=81=E7=9A=84=E5=BC=80=E5=8F=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Massage/MassageControl/MassageRobot_nova5.py  | 189 +++++++++++++-----
 .../admittance_controller.cpython-39.pyc      | Bin 0 -> 3705 bytes
 .../__pycache__/arm_state.cpython-39.pyc      | Bin 0 -> 1541 bytes
 .../base_controller.cpython-39.pyc            | Bin 0 -> 3382 bytes
 .../controller_manager.cpython-39.pyc         | Bin 0 -> 1826 bytes
 .../position_controller.cpython-39.pyc        | Bin 0 -> 2439 bytes
 .../algorithms/admittance_controller.py       |  23 +--
 .../config/massage_head/finger_playload.yaml  |  28 +++
 .../config/massage_head/none_playload.yaml    |  26 +--
 Massage/MassageControl/config/position.yaml   |   9 +
 .../MassageControl/config/robot_config.yaml   |   7 +-
 .../__pycache__/dobot_api.cpython-39.pyc      | Bin 136445 -> 136445 bytes
 .../__pycache__/dobot_nova5.cpython-39.pyc    | Bin 0 -> 15043 bytes
 .../__pycache__/force_sensor.cpython-39.pyc   | Bin 0 -> 15294 bytes
 Massage/MassageControl/hardware/dobot_api.py  |   2 +-
 .../MassageControl/hardware/dobot_nova5.py    |  99 ++++++---
 .../MassageControl/hardware/force_sensor.py   |   7 +-
 .../tools/__pycache__/Rate.cpython-39.pyc     | Bin 0 -> 2573 bytes
 .../tools/__pycache__/log.cpython-39.pyc      | Bin 0 -> 4288 bytes
 .../__pycache__/yaml_operator.cpython-39.pyc  | Bin 0 -> 1678 bytes
 .../MassageControl/tools/identifier_dobot.py  |  39 ++--
 logs/MassageRobot_nova5_test.log              |   0
 22 files changed, 299 insertions(+), 130 deletions(-)
 create mode 100644 Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc
 create mode 100644 Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc
 create mode 100644 Massage/MassageControl/algorithms/__pycache__/base_controller.cpython-39.pyc
 create mode 100644 Massage/MassageControl/algorithms/__pycache__/controller_manager.cpython-39.pyc
 create mode 100644 Massage/MassageControl/algorithms/__pycache__/position_controller.cpython-39.pyc
 create mode 100644 Massage/MassageControl/config/massage_head/finger_playload.yaml
 create mode 100755 Massage/MassageControl/config/position.yaml
 create mode 100644 Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc
 create mode 100644 Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc
 create mode 100644 Massage/MassageControl/tools/__pycache__/Rate.cpython-39.pyc
 create mode 100644 Massage/MassageControl/tools/__pycache__/log.cpython-39.pyc
 create mode 100644 Massage/MassageControl/tools/__pycache__/yaml_operator.cpython-39.pyc
 create mode 100644 logs/MassageRobot_nova5_test.log

diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py
index 20f10aa..1c8b31a 100644
--- a/Massage/MassageControl/MassageRobot_nova5.py
+++ b/Massage/MassageControl/MassageRobot_nova5.py
@@ -14,6 +14,9 @@ import threading
 import time
 import os
 from scipy.spatial.transform import Rotation as R
+import ctypes
+libc = ctypes.CDLL("libc.so.6")
+import atexit
 class MassageRobot:
     def __init__(self,arm_config_path,is_log=False):
         self.logger = CustomLogger(
@@ -30,9 +33,11 @@ class MassageRobot:
         self.arm_state = ArmState()
         self.arm_config = read_yaml(arm_config_path)
         # arm 实例化时机械臂类内部进行通讯连接
-        self.arm = dobot_nova5(arm_ip=self.arm_config['arm_ip'])
-        self.force_sensor = XjcSensor(host=self.arm_config['arm_ip'],port=60000)
-
+        self.arm = dobot_nova5(self.arm_config['arm_ip'])
+        self.arm.start()
+        # 传感器
+        self.force_sensor = XjcSensor(self.arm_config['arm_ip'],60000)
+        self.force_sensor.connect()
         # 控制器初始化(初始化为导纳控制)
         self.controller_manager = ControllerManager(self.arm_state)
         self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
@@ -49,6 +54,16 @@ class MassageRobot:
             self.playload_dict[play_load['name']] = play_load
         self.current_head = 'none'
 
+        # 读取数据
+        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,0,-90])
+        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'])
@@ -94,7 +109,6 @@ class MassageRobot:
         self.R_tcp = np.eye(6) * 0.1
         # 传感器故障计数器
         self.sensor_fail_count = 0 
-
         # 机械臂初始化,适配中间层
         self.arm.init()
     
@@ -117,13 +131,6 @@ class MassageRobot:
         return x_update, P_update
     
     def update_wrench(self):
-        compensation_config = self.playload_dict[self.current_head]
-        # 读取数据
-        gravity_base = np.array(compensation_config['gravity_base'])
-        force_zero = np.array(compensation_config['force_zero'])
-        torque_zero = np.array(compensation_config['torque_zero'])
-        tool_position = np.array(compensation_config['tcp_offset'])
-        mass_center_position = np.array(compensation_config['mass_center_position'])
         # 当前的机械臂到末端转换 (实时)
         b_rotation_s = R.from_quat(self.arm_state.arm_orientation).as_matrix()
         # 读取数据
@@ -132,31 +139,34 @@ class MassageRobot:
             self.force_sensor.stop_background_reading()
             self.logger.log_error("传感器数据读取失败")
             return -1
+        # 反重力补偿
+        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)
         # 传感器数据通过矫正计算得到外来施加力 传感器坐标系下
-        # 重力和零位
-        gravity_in_sensor = b_rotation_s.T @ gravity_base
-        s_force = sensor_data[:3] - force_zero - gravity_in_sensor
+        # 重力
+        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:] - torque_zero - np.cross(mass_center_position,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])
-        # 传感器工具转换 
-        s_tf_matrix_t = self.get_tf_matrix(tool_position[:3], R.from_euler('xyz',tool_position[3:],degrees=False).as_quat())
         # 传感器到TCP
-        wrench = self.wrench_coordinate_conversion(s_tf_matrix_t,wrench)
+        wrench = self.wrench_coordinate_conversion(self.s_tf_matrix_t,wrench)
         # 交给ARM STATE集中管理
         self.arm_state.external_wrench_tcp = wrench
-        self.arm_state.external_wrench_base = np.concatenate([b_rotation_s @ self.arm_state.external_wrench_tcp[:3],
-                                                          b_rotation_s @ self.arm_state.external_wrench_tcp[3:]])
+        # self.arm_state.external_wrench_base = np.concatenate([b_rotation_s @ self.arm_state.external_wrench_tcp[:3],
+        #                                                   b_rotation_s @ self.arm_state.external_wrench_tcp[3:]])
+        
         # 卡尔曼滤波
-        x_base_predict, P_base_predict = self.kalman_predict(x = self.x_base, 
-                                                             P = self.P_base, 
-                                                             Q = self.Q_base)
-        self.x_base, self.P_base = self.kalman_update(x_predict = x_base_predict, 
-                                                      P_predict = P_base_predict, 
-                                                      z = self.arm_state.external_wrench_base, 
-                                                      R = self.R_base)
-        self.arm_state.external_wrench_base = self.x_base
-        self.arm_state.last_external_wrench_base = self.arm_state.external_wrench_base
+        
+        # x_base_predict, P_base_predict = self.kalman_predict(x = self.x_base, 
+        #                                                      P = self.P_base, 
+        #                                                      Q = self.Q_base)
+        # self.x_base, self.P_base = self.kalman_update(x_predict = x_base_predict, 
+        #                                               P_predict = P_base_predict, 
+        #                                               z = self.arm_state.external_wrench_base, 
+        #                                               R = self.R_base)
+        # self.arm_state.external_wrench_base = self.x_base
+        # self.arm_state.last_external_wrench_base = self.arm_state.external_wrench_base
         # 对tcp坐标系下的外力外矩进行平滑
         x_tcp_predict, P_tcp_predict = self.kalman_predict(x = self.x_tcp, 
                                                            P = self.P_tcp, 
@@ -174,9 +184,11 @@ class MassageRobot:
         while (not self.arm.is_exit) and (not self.exit_event.is_set()):
             try:
                 if not self.is_waitting:
-                    self.arm_state.arm_position,
-                    self.arm_state.arm_orientation = self.arm.get_arm_position()
-                    code = self.update_wrench() 
+                    self.arm_state.arm_position,self.arm_state.arm_orientation = self.arm.get_arm_position()
+                    code = self.update_wrench()
+                    self.logger.log_blue(f"measure position: {self.arm_state.arm_position}")
+                    self.logger.log_blue(f"measure orientation: {self.arm_state.arm_orientation}") 
+                    self.logger.log_blue(f"measure update wrench:{self.arm_state.external_wrench_tcp}")
                     if code == -1:
                         self.sensor_fail_count += 1
                         self.logger.log_error(f"传感器线程数据读取失败-{self.sensor_fail_count}")
@@ -193,13 +205,34 @@ class MassageRobot:
 
     def arm_command_loop(self):
         self.logger.log_info("机械臂控制线程启动")
+        self.arm.filter_matirx = np.zeros((8,7)) # 滑动窗口
         while (not self.arm.is_exit) and (not self.exit_event.is_set()):
             try:
                 if not self.is_waitting:
+                    process_start_time = time.time()
                     self.controller_manager.step(self.control_rate.to_sec())
-                    self.last_command_time += 1
-                    code = self.arm.ServoPose(self.arm_state.arm_position,
-                                               self.arm_state.arm_orientation_command)
+                    status, joints_processed = self.arm.process_command(self.arm_state.arm_position_command,
+                                                                self.arm_state.arm_orientation_command)
+                    self.logger.log_blue(f"处理后的关节角度: {joints_processed}")                    
+                    run_time = time.time() - process_start_time
+                    sleep_duration = self.control_rate.to_sec() - run_time
+                    delta_command = np.linalg.norm(self.arm_state.arm_position_command - self.arm_state.arm_position * 1000)
+                    self.logger.log_blue(f"期望位置：{self.arm_state.desired_position}")
+                    self.logger.log_blue(f"现在的位置：{self.arm_state.arm_position}")
+                    self.logger.log_blue(f"指令位置：{self.arm_state.arm_position_command}")
+                    self.logger.log_blue(f"位置变化量: {delta_command}")
+                    if delta_command > 20: # 20 mm
+                        self.logger.log_error("机械臂位置变化过大，可能是数据异常")
+                        break
+                    if sleep_duration > 0:
+                        libc.usleep(int(sleep_duration * 1e6))
+                    if status == 0:
+                        # self.logger.log_blue("不进行伺服运动，只计算")
+                        self.logger.log_info("进行伺服运动")
+                        code = self.arm.ServoJoint(joints_processed,t=0.008,aheadtime=20,gain=200)
+                    else:
+                        self.logger.log_error("滑动窗口数据处理失败")
+                        self.exit_event.set()
                     if code == -1:
                         self.logger.log_error("机械臂急停")
                         # self.stop() # 底层已经做了急停处理
@@ -207,20 +240,39 @@ class MassageRobot:
             except Exception as e:
                 self.logger.log_error(f"机械臂控制失败:{e}")
                 self.exit_event.set()
-            self.control_rate.precise_sleep()
     def start(self):
         if self.exit_event.is_set():
             # 线程
             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.arm_measure_thread.start()
+            # 线程启动
+            self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
             # 初始值
             self.arm_state.desired_position = position
             self.arm_state.arm_position_command = position
             self.arm_state.desired_orientation = quat_rot
             self.arm_state.arm_orientation_command = quat_rot
+            self.arm_state.arm_position = position
+            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)}")
+                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.command_rate.precise_sleep()
+                
+            poistion ,quat_rot  = self.arm.get_arm_position()
+            self.arm_state.desired_position = poistion
+            self.arm_state.arm_position_command = poistion
+            self.arm_state.desired_orientation = quat_rot
+            self.arm_state.arm_orientation_command = quat_rot
+            
+            # 线程启动
+            self.arm_control_thread.start() ## 赋初始值后控制线程
             self.logger.log_info("MassageRobot启动")
             time.sleep(0.5)
             
@@ -239,27 +291,31 @@ class MassageRobot:
     def init_hardwares(self,ready_pose):
         self.ready_pose = np.array(ready_pose)
         self.switch_payload(self.current_head)
-        self.arm_state.desired_orientation = self.ready_pose[:3]
-        euler_angles = self.ready_pose
-        self.arm_state.desired_orientation = R.from_euler('xyz',euler_angles).as_quat()
+        print(self.arm.get_arm_position())
         time.sleep(0.5)
         
     def switch_payload(self,name):
         if name in self.playload_dict:
             self.stop()
             self.current_head = name
+            
+            compensation_config = self.playload_dict[self.current_head]
+
+            # 读取数据
+            self.gravity_base = np.array(compensation_config['gravity_base'])
+            self.force_zero = np.array(compensation_config['force_zero'])
+            self.torque_zero = np.array(compensation_config['torque_zero'])
+            self.tool_position = np.array(compensation_config['tcp_offset'])
+            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)
-            self.arm.setEndEffector(i=1,tool_i=tcp_offset_str)
-            self.arm.chooseEndEffector(i=1)
+            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}按摩头")
-            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)
-            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')
         else:
             self.logger.log_error(f"未找到{name}按摩头")
@@ -274,13 +330,13 @@ class MassageRobot:
             time.sleep(1)
 
     # 工具函数
-    def get_tf_matrix(position,orientation):
+    def get_tf_matrix(self,position,orientation):
         tf_matrix = np.eye(4)
         rotation_matrix = R.from_quat(orientation).as_matrix()
         tf_matrix[:3,3] = position
         tf_matrix[:3,:3] = rotation_matrix
         return tf_matrix
-    def wrench_coordinate_conversion(tf_matrix, wrench):
+    def wrench_coordinate_conversion(self,tf_matrix, wrench):
         rot_matrix = tf_matrix[:3, :3]
         vector_p = tf_matrix[:3, 3]
         temp_force = wrench[:3]
@@ -290,4 +346,33 @@ class MassageRobot:
         return np.concatenate([force, torque])
 
 if __name__ == "__main__":
-    robot = MassageRobot(arm_config_path="MassageControl/config/robot_config.yaml")
\ No newline at end of file
+    robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
+    ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
+    robot.current_head = 'finger_head'
+
+    robot.force_sensor.disable_active_transmission()
+    time.sleep(0.5)
+    robot.force_sensor.disable_active_transmission()
+    time.sleep(0.5)
+    robot.force_sensor.disable_active_transmission()
+    time.sleep(0.5)
+    robot.force_sensor.set_zero()
+    time.sleep(0.5)
+    robot.force_sensor.enable_active_transmission()
+    
+    robot.init_hardwares(ready_pose)
+    time.sleep(3)
+    robot.controller_manager.switch_controller('admittance')
+    atexit.register(robot.force_sensor.disconnect)
+    
+    robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0])
+    try:
+        robot_thread = threading.Thread(target=robot.start)
+        robot_thread.start()    
+    except KeyboardInterrupt:
+        print("用户中断操作。")
+    except Exception as e:
+        print("Exception occurred at line:", e.__traceback__.tb_lineno)
+        print("发生异常：", e)
+    
+    # robot.arm.disableRobot()
\ No newline at end of file
diff --git a/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..af78773a2c2ff364c7692281cd0ff4ec02010954
GIT binary patch
literal 3705
zcmZt}U5^_{v3tJ9U+dj<*51u-;)D<|61yB6(kY67%LUMZvO>8N5+hE-#NGCeGvjf3
zdULy$_XP*t6B4g*AbEMamwV?2fDk{SpHAW_Jn#c}qN-=?ILUf6HC5Hs)z#HqACBtv
z3W49>fA#f~f9?_TZzR@#HV{98H~%jHMi@;<P<&}%0Bs~@Y6UhW%w$&Lq;BA9+)ljI
z4}8E0bCOD04XTuIGO8`=yuqt{V@?;%OETIVZO-W>)p9i13bsJrO}5ik(4yq0gn7(=
zL730oxfxvJJFN255dY!+oCIxtja7M@)nKH<cX?;DHwQf$YmcsL+*s@n39J7VR^aXM
z`fFo2<6t%8#tOTM7^5GNUgIU~nH=}1@*Zb0l5sYENr3~5#)By5A7o=GvLxXGMcx<b
zM<B!jw|)`H6ZraqM=U&#(xi`?)_+z><nIE)NkAD13~n;Yj6WNnkpV@2zNiHjvzhaP
z1U7S-2VdtiVv)cF>wMLEpQW*s(YU{6%5l$7c8-Q9M{s#6#{K15xFq8En(*fP03?}{
z1)b5EacNL8BMW0@7UuzL(Bqy7+D<22C@&1-aV*17Rg(<1)z8L*c-XU)mGfkvtZ|fb
z;e$q1Er!Agc;O51LKyawD9^+23-Z5Dj!v?aAC2<ClL9<~k!Z*tXHPO2K16s_vX<Y{
z%|}r(%tS0t()=h|Z5+)g58-ffE^4qozPa8rZP4k#RZcow?xk;l@bXs$#D(_*c=KPw
zNlP-Lb9!OS$O!zy=q`C`itk8+Y0l?x=L_!V<S*8o{+3E}VFAb5C71NVEGb)4R!KRU
zvP;U<lv7fkrreV9HRY95MN>X#ukOMrg*IR{jXBa~^*Mb=KKac>W#-L5k~N+Z@fF*c
zxh(?oy_t>NnTfCkwzUY#-6u1DCH>`^w4<f1HEEE$Po8}Vx_o#mpvf8<mk?E8Np;1w
zXU>XSTXDUazv9+e17<WBfxFc_78~H`X|p_E2%|3Eq3(3Q%QBvK$C>P=k?fyzi~CfJ
zDnG@$Bt<k<o<>C`RVA14U{G-V0@rND^H^{ep2f;%Q93E-X%q#TlPnMOew6TV5cOpy
zR1?^*ab9Am2#ouecv*TI0`G{nT~&S=C8zwyLcm00JW+NeM074NuEYTVWrLYR4k7YP
zNM*4&8tUliLwJrcMCl_%f3E05)r1J)GMtEbEJGQmyjK+sRMP|yEF+4=eJQRXdj~-q
zK?lJu0?6M)TnC_NEVhx*poLf+z$U!;w*V08L1^s3yZ%&Yh1TG?4euSggO4%oUKu`X
z3E;H|!ti(C)e-bBa2G@l#tuTGLv$pO5k?7e>`Tfh+|+jqs;Z?!TRIDOIll0qM!>Ca
z7MdzpaeimqCvVHFIFP+?R+B&kI*Tf1vr!GR>F?=<HM5FUMimCt3pMGL>KZn{LR)T(
znu|@Kx8Qc$@ZHv9Ee)@AKyL@<>}a}zRAF1#hTPfF+%~g|Tnc+^&oJ6>x2?ARhM=jG
zYcXdf*Fw1q2PAiAPO(dj)g`X&+yhEmU5=DHSnVAAi927;*+t7rT{s_Bb(o7Q!i?*$
zbQQBM+_&_&;6qRQ&;yx$&<y(s4-i^pbf7(TL+|b2k{%GF9E0;-?V#NAP1#*^_552M
zsOe9wKN#X4@;39a*21@<wa_`*cAHf-Wt3EH-LBEs7~~K*%jM|CY821;&C#aTbhFg7
zxsnBY-;&*$Ii=k8%+*+Fb9hVVtJbkj0^$bT@1H<%cNrfF&U0l({eECR=ZUf)oABYE
z{)oT7{c!r;x7UAbHS3*q{uTJN9*HyrWij>~_okc6G`3=?4U~X#$j3!vqRg}N>Bnk&
zDY-t>TGs{zGfks0Q|&i2EC;4l#uFLg^8B8v7X+vaAs6@V6cxU<u2D6Yi!RL*?|_e#
z$ucR95TlQkKM+|OK0A%1@}nGeiTF&>k5vWN3As+8RV`GGp%~=i=e)1B`I+Qm93|nW
zf{*(rq3lmoW4UU<6Sr_BCy7C9sLXi$TzmsKqKE8Fw09{lKaF#V8!JkKCdD^VOgnYS
z$5XBKlX$Xp%vxG63@xp5!BE_o_#W!8#+gXPcQuN6TwuagwodHI15tj84WqBKyE2#z
zHYA6_^vVrE6-y9!1ichAmV{pIEU1(&fbpR6>ZAnrkwLXQQ|;f^D*)6W$tUQI`~d)B
zG|e4I|99-W#unW&o8~>UNjtPjYsMbkv+i0oa|fO+W8dx2mM$0#dIKJWSSqHwSC$67
ziih1I7IWwpD`A*s3^omTEksWx#fZTF7P@28)k!=8Qq{|5vCoqv4A-g{7B8VonFkoQ
zYB^RKkG5>DBanw{rcpe`HD3kW^g+>aPS5*Sw2g1}m_?_tCU}V5d(RZQ3AhQX==kq-
zm6@OC${wGl&~@so*UtO#<oqy)t~ZX7Lu^p;0p11d8pL}5f{iCo<kvb@fn6%Z16)qs
zv1^Yg7yITUej;#xkW+wCPBfWt2wF3jx)UG7+MllAd6qnfR(}#fWJvKLPT7`OmgL&<
zFoV`Tg1)9`TF_167y(vlRe9Asi=QDE8#3{I1h)~a0!W9)+F`8v3gz7|4)+JxAmw)f
ccyL#+jlgH0dPa>lxknwKu)To$rEkyw0rGm<!~g&Q

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..88334b51f8460393fb4e884fd19884d3b322b7cf
GIT binary patch
literal 1541
zcmZuxJ5N+W6rTIoW!VR?3kVBpVqxMoY(uP!Krot^NP=`XF_U3u*j;iT>)csch=mE5
z*l25Gp~l94Kxu9C4@~SV46&i6@|^p?3zyl<cjtU(&Nt`G%nqy7$^_cu_qTgzMM8d~
zk!=<jtI)#*08Y5!632K%xXG;}!mUHoF}bltTJ{O}$wmh;+b+Zlh~)$$xcR~07PpUV
z$BN<$y0j*}Eya}BXc@XFg&In~V`Q{&Y(rm%9=-ufkzIoufF?KJnlA~p6!~a(A+68G
zCj;z4G{er!DCU^4h{>vyE~$*?V#+zDl2fTBDupDMS}tZH$4us!sT@<!F^!1HRxq8c
zT};-V$;Hg(m}@y^j*r0}&hrvrlaB)~@G{^cuK-@>RlpmZz~R?6PVk)J+zMRn4@ArU
z17_4DTq+VlHzrP3PLOKZu?G)TBAg%qdrrio61wgH#Nn?tSGzxdU^`vu293jS-)}Wn
zyT><Q|J^)(x6*y~>!CHOd|7GT)TTesg##gjP>=1n0aJIEb(Kkv4uY_+`hl+tOiH$|
zCy@+fU-)qOke?AFC8W<>x+jI->CtT#3O#+9Rh@xTcUh=tR@Ip~r!tyOGh#1GM8fk|
zx<Fk|aS`^V;Qv#ePMM6ho=C|!U8k1XYOfzEr;h1eq-Jvz#cHCRNN2K6N6%y{qn*I>
zn9ubbvY7-@hf{{F6;wzWNT(7lo*30+BV$+9V<QUkR7vJLmDG}faw69Y;bvTDp{t1V
z_$c{GxNCMO+?}sPHsN-hZMJ)XC)&H=&UOq>SQxUdcp7X6iatPWCsg`f4}2v9x6Rxx
zRHJ%c*v862e1nz-`?^GF-|s6*@zDvF0f^Nwri_NsxZ<0z;K5PRNPh?Qwd7q}O7&Vc
zYWvI+l<G32UciSgj;oYDA2K)g$VFH%DlP-~3QH3+fga+ZWJGq$(glC$4fZ9z=Mj|&
U!!42+t4sId)$Zf}EEX{T0a0+Md;kCd

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/algorithms/__pycache__/base_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/base_controller.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9a16afda158bceccd9e8e012e7cd769d74349989
GIT binary patch
literal 3382
zcmbVPNpBp-6|U;;*@s&!n_6tS?D(LLX)*x}9SFvVt-uC?L<lGZG3MZQv#TgJ)yuf5
zM<NH~ODMxvA99NVkVpLk{|+B==#v2hMg-)PV^02Fb#pkJiDe}{=+{-R-uBh<>P@(`
z)Ma=+e)8JE+A?F`QD^q?(D@Bs^G6WLBu`k%2RwBK4yV48xT!brY}-v5gC=-h(n{Nd
z4tNdOOuA`r(BteQCR?)on8~){C+?sx-FwXMJcCu{JEi;n2Y1TlaL*JvjEYnh2f6fl
z+4_B4C><tu(QfbNMOegn_KcGb81L)!9#|?OBeM?$O^^8$M6m&vY~UzYa_KyAzGVAc
zy3%{xA9$)kAwFg&e9%PO28eb`uB!H7M|EZQ8k4;zj_k{&$BjWx_2sf$vGPk_GLH?G
z0b;FO{vb5!PM#GyPZFi?0#L(HWMxyUVx+UkrEuzp+74d&n(H71J7K5%jBz&M6BalV
z2h^499(&5~`W|eJhDw+1Ac(WL2!hhf!c^%FRDHMfj7s)3){a@uQ(kt0AWA}Gg5W9p
z&!4vr@>Fddn*F_s`~f53eYKnK<wfvYqFXiV{97a03X}V}j*ElTZ0!MH5KR$o4v+N`
zJofOKCJ5tQ-sI!;%M-DA9&to?&mU^j65hdUXsHOPCR6%nAikql@F^Wx*nUM`B9><g
zHC#i>SbOgHr8$2z{{Q&u0os0iKNyC^!MxL6l82J8L*SJe-`D4S&xn19WpTqOD&2=F
zDZOEC6t+Yw%F{H=Wa$;hLlvEGQkcX}VyuPNPRTz2ZsJJ(z|sFI*daf4CfLBz`NFxz
z&agoa-9ztzr;jeUi&8gOC3gi|s?yzI6IUM<4e2gQ{|#sM@9qib*ow73N3t1zP5ApK
z^uLCFV^IeB%}O8MYRyHdI;N(L$x7|8uXy%7u(lpJUvd5IROd3<)&mcthZ7fc8xgb@
z<q$z<8bPhSC{;($nw^!Y)MdnN_;mC)FMy{#iw7}63o&*V?IOlr6=U{qVD1Am$)3;n
zWy~Gx|C968yLPcMQ@I*VXBLh1{|WprE!u_u<>#()_k}Tc|10K|sn6+L!slw`lMcDR
zc2{4;Hh)H`Z+ru}J~N5U>0u&pUKEn2?TYam>6Q`dC{juqk%mWcI!Z;DB>5vH#ltWe
zsqs}^UkD4zZ|P9}l=_CgMnSA!4g%*jkK*E3jNkmp05--~W+J%I{P_!b*Od3?XF$eg
zn1qo^RaS`c>*@c1|FY^Eh~W9key?=0VcAUL3|Sr*O0H9TeUyDnc7U}AXkyC7P{&zO
z_Rg6ATuA_KC1HoCM?+1LlSlPe`<_kZ`Ub%2S3$~lYyxX?KFju_BpxEm+6+nZ<~9i9
zZT>3%CHJ_%m*~5=Xl~-F5#Zj$+vIOr4z)zh8Q+*o&9k;IT@MYopx5TzzoJnfZ=P_Q
zd#|F+tF{rp!>K#rf8hnPvE*m`b6n9>L{N^z8LeCEl(%hrhZXLr2N_SgRUSQUoUzYc
z%jI)i58UR&o4BjU@29OZvSRg+vmKn8M!19fvMcK6PM!`&g%ZVq5_Gi)gQKyhibqOi
zqIi^Brr8jW4&vxQ;Nmi|DU68sX&|NwO58fy5XT#0><ipS;(Qm2hbk&^EpBNHYmDo$
zzd3dLr=dheBlIXy+u|rF-u{*NHGRd~LPCDG=Ez;~3+lj1eLQh|3mqT%-KnYVVW`8@
z#<?xB;bta7t;1t+>-L5}FC<k`gaP4vj%Zb7{kXt0s!_(ioh?FqVN|3eQ(*SQ?RR91
zgJ&>3s5kX%Q1IKOo2X3RM7wN^mCjAccgx<s&Qsd%n69!DnxMj0^Xm@rD-pk84}00Y
za9X+uc+jJ8YURP2?NlO_iTM+<Qkk<;5rk51BPv5>A9jY*<-&Odzw3N_b8hp`srmdv
ze}!sEsv%KJc7q_zB^DL!eh_>%3X^I?Q$b>@JWc6Zzf5GENHrgJ$uw0i+6O87RPn}9
zRlDnViM3Q0qDbsX5X=BtB`qrHbQ^^Jff!=7`IV-_yB@t)Uw612^_Ngf?M0eOJ{w8d
zMEw+J_pR@;k&RMRqc)~>6UD>hO@lHn4wFrr+V?55(HYn8fMC6J8ra&r!bkpR0;2-d
Z23EFrs%d$TN>xK4uJS(8QJ?o4KLAqObbtT=

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/algorithms/__pycache__/controller_manager.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/controller_manager.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b0f86a2ab76534024c791688e167519b19a3b30e
GIT binary patch
literal 1826
zcmZ`)&2Hm15GE<vQS8L|X_0o@q!7?UYG4sx_LvsC-4^X-eb@j+p$nmyOsrJ0(jmRu
z80w|ox9Bd=J?52k?WKK%oH|4KN4B>VIHV}faORtDsCaj`#jySS_GI`^gR#HqrQSSz
zd5Yb9fkH9GQ<m|FbM^yMLV0hP@}?pZ%D-UU#=jglb$J;)gY3e_RuAxfr7g^Ti_4lt
zT(L-KPw}_hne!uG`Ks}TMGe(dTR1jVptf<`QZ3cSF;F{d7sqYY!6Wy|gXj6kLY}4?
zUc{sLS_3-JFz$3wJDcXzpJO*iC<<OM$r6SVy+A1xyI}Xc-}S)9?`3<iux!f1!5Bb~
ztXwX3Md=%zUb)@(47O3}A?gkDO!scg)nz4K#KxdkFZ0XX%I`?_W=r#N7C<jfuX9N3
zFf+a7Au_9iI=j6q11XbHVx=VaOcMp;O?)Pw%}gvYIGb-B(B65YIdzA@6Py&HU^na)
zhY-=AIWN4aM{9*RXQO>96t4e?seb28`5%ZAV(#@H+RFpO$zB+zpks9x>faJ$4dO?>
zRUo#0iqo<F9w3LV4*{-6`?cr>DZ*3wqd3zM;eC}{%iGuv9hj9MewLkBsT%YI^$dU(
zKN6F0>CcP=XR8W>bt)`ic#5Wo3J3#($1ZgKHPQj0z_q_b!Qb;=1e`mHaD)WOTJTj!
z?!7+jpdExl<=TX!+*Vj%$PS7v{8)O|+QKf*?u7#aIyhes^Z<00ztXi>_i&eSMao+|
z;D<aAlP@>p-O&60kRPMPMaZ94AwP0;Fw6K_c(9KO^Q=-RE($oL8D*lDrvvjkv4dgF
z{ZDYiB2}Fay1t>a;yPU!1fnDDk2oQR;oQ3X6iCUb_q!+r92eq(5n-((@RZ;9lqHud
z{;omv9R;?5_tl)pe*b}=PCh>k^UE7Ou;DPK)P=FNkX(+f4o@e?{XCpiez>G<n3&LL
z+iikN>=q`Ll*-PtxvC?4>D#+oO=U~`ma^P=IUd<0o0WsNWI@@?v>oPZ9nQ9tGuu+0
z5Rk+&D*6uhxi3O7IoV`FO=ks2Dggr10qJf)0`kz&94Y-CFLkPKz~2DtiYheUSbd8x
zRJf&3cFjQ`r@q2&+%-dY(b1+uWTW40!6P!#k}}KHICXeP`Rh1ND|CSTgEk4GJbXmr
zV-m+C=%m`|j(1cQKp|0g*aW=O2%15Fz3sl>ZC-B2kV*GPsLr6>A2L-2=ap;U(0|O3
HKRe<-Z|17U

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/algorithms/__pycache__/position_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/position_controller.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cb7c1463a41f5f94019d0fb436d112dc02af4ff6
GIT binary patch
literal 2439
zcmZt|O>Y}TbY?%i>u<+)<D^X?iV%_|lyE?BsZeS8I3S{`MI6jwwegITb!T_Y%%n7t
zb7_Kl;tx=e>|=ibKY+h6S5Em0oOo|`ow}5@W_I4k&ij7PakE(?@ci}X-pO8@kiQXF
ze|!M$!%|nUF~X=IqvB0R4#19ZQ*Y!`!d&KwN*as;d-g?`R!3Dh6IKzmv_7iaGZl?d
zgOh1<(c*1h=bcNs*jSQjciO$AOKN!n(M$WIJ|*7}7P9IUVO1Vnx}#f+J|V-}HE2Q(
zht!0Ra~<m>&#oz80MU97EB-Lgw9JLz5?Q{G=@Ve$aP}U>`UKup$zv9s#i<zMP3w<W
z0_qbua5AEdj2!MV%ADVwmt;cG%B#l6V?L`~k&({=7Q(yol6YhkfOb{WeUz&NwR)q-
z;m|R@LN!c9a(ybZ@k$a3#4|1zmfC|&lS{Iob2@jH4kdH4aOSS{4V{z7Wd!6nhteAD
z!NsRDE=?FkNtWm+GIf!Ied9cvB*&&Bz$H4G$t2T}PEtPfjHkGm7%z)cE~_BU)C=Zl
z1`c`+4vL~Ne5Ru4HTmbqgOfbv2U9gUDz@){5+C!&`BAQ;FL6I8QLA@p_(3d=bD8Lq
zR2|GppP*W0boOUwvH{YYXmtfPLR+-voZorZfBP%j#t!n854sD#dk9OtfK8J*y`&e;
zoJ_%WjBb;suKZX#%wk?aiWb;c<PYzXzM$G&c(8oHdfQ|P=?CtutYQ5dY_Nu{++ah%
zR#{-btJ$!Ghq42joOeneKo;(zpLfr9c3IBVZkFraG}hyj-NNSu)i~eIU?s5FHQF?+
z%K%>XGE@5@7N`6xDRXI@Y-T)`#K*>urHs$yCbA81+k(yb=UnE>^uXa95<>H18H<SH
zw?kJ#g%i1fe5;VNWUOT$(RRCqCyYoBmTJL9Xb5q+O*ir9oWJ)!(OJv4w*rI~VE?cI
z`VG<*0)wH!-PtN2$Yw@=aqbn62f;vUWeFAcl*s!9RJT+JO|S@8^hI?^ZeVXo5uVmM
z<iY`L9V&-T8(6Ks(hGO)+OM#VNg>OF4piXgv`;=G4v7$M6_TcHOY3Zr4ocoAI6K7Z
z+y$svo=Mb$>g-#3kNHKlg8n_c)neO&LU*{a<ZH9a4PP7iE<8)~Ad#TjLHk>Cuh0||
z4eh${=l+eY;2Xq&rRb0P9O2DnA!UFv!M1_knr<&{TMY(FdKc}pH1NJxczm0bn%yZA
z_ZsWTHkGfTNd^ouhjV4zcsw@lGcJsWIW^oVT1F$5X#@z{=}d=ll}OIsU}`o2ZQhyQ
ziVEF;XGP04F3YuP<1Oo0`q)$_GEbwQPGfDVv5KHrWb)k5?@iqjN*GBp*U$J^zK?p_
z<ln&SR>JhpiPEO|7Wk%G3ehVf+SnM2WCr~yP2-G}!U_o{0AW_4!GH8!vCBK4gQ<ul
zgPsSSHJ7RBuX||4I1FqJ8i#39hug6-(L~c)0aoO5Q)67{82!o720VkGENi_~!n)?|
z(uLNp`RpzzqdtI*xI0j_E&3tt(vGv~d`dfX3swVuSj}B$hn^1}y)&=&@s?qTGX<L$
z%#>Obr8zqlcy2`K3{g-d7P#%>wr$7<0GehwK*n5%C|YYTU1YXBCWI4qf7|&(+OLE)
ziMZjWan+eJes-F|fUtfYsBtno+gC82l346(8E0w&E|xeA<VUcX_7RM^wc#Rho<jaW
zKG?Zy)iePPIFTGlY_Z0}aR^xrI8%vdGY(;QmA0dFl2}s%$yF|%!PuU~5>nUnbe;>f
tkAEJb9L8>}b6Iq1RDR9Un{H~tdxhsd$9b*L10l3j3<3T^y5lsQe*v=wlz;#L

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py
index 3034ea0..6d93619 100644
--- a/Massage/MassageControl/algorithms/admittance_controller.py
+++ b/Massage/MassageControl/algorithms/admittance_controller.py
@@ -40,13 +40,11 @@ class AdmittanceController(BaseController):
         
     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])
@@ -97,16 +95,17 @@ class AdmittanceController(BaseController):
         
         # 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 > 0.1:
-        #     print("-------------admittance_1-------------------------------")
-        #     print("arm_position:",self.state.arm_position)
-        #     print("desired_position:",self.state.desired_position)
-        #     print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True))
-        #     print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True))
-        #     print("arm_position_command",self.state.arm_position_command)
-        #     print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True))
-        #     print("delta_pose:",delta_pose)
-        #     self.laset_print_time = time.time()
+        if time.time() - self.laset_print_time > 0.1:
+            print("-------------admittance_1-------------------------------")
+            print("arm_position:",self.state.arm_position)
+            print("desired_position:",self.state.desired_position)
+            print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True))
+            print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True))
+            print("arm_position_command",self.state.arm_position_command)
+            print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True))
+            print("delta_pose:",delta_pose)
+            print("delta_ori_mat",delta_ori_mat)
+            self.laset_print_time = time.time()
 
 
 
diff --git a/Massage/MassageControl/config/massage_head/finger_playload.yaml b/Massage/MassageControl/config/massage_head/finger_playload.yaml
new file mode 100644
index 0000000..49903ba
--- /dev/null
+++ b/Massage/MassageControl/config/massage_head/finger_playload.yaml
@@ -0,0 +1,28 @@
+name: 'finger_head'
+sensor_mass: 0.334
+tool_type: Cylinder
+tcp_offset:
+- 0.0
+- 0.0
+- 154.071
+- 0.0
+- 0.0
+- 0.0
+tool_radius: 0.04
+tool_mass: 0.57
+mass_center_position:
+- 0.0023247590120227682
+- -0.0015892432698610681
+- 0.04139459803098493
+force_zero:
+- 0.07030932685649408
+- -0.329927477170834
+- 0.71291678232261
+torque_zero:
+- 0.024149745145239786
+- -0.015198458671894979
+- -0.014812358735378579
+gravity_base:
+- -0.17234130565475442
+- -0.0796927884554296
+- -6.980274002014547
\ No newline at end of file
diff --git a/Massage/MassageControl/config/massage_head/none_playload.yaml b/Massage/MassageControl/config/massage_head/none_playload.yaml
index bc5f928..d287504 100644
--- a/Massage/MassageControl/config/massage_head/none_playload.yaml
+++ b/Massage/MassageControl/config/massage_head/none_playload.yaml
@@ -4,25 +4,25 @@ tool_type: Cylinder
 tcp_offset:
 - 0
 - 0
-- 0.031
+- 0.028
 - 0
 - 0
 - 0
 tool_radius: 0.04
 tool_mass: 0
 mass_center_position:
-- 0.021919029914178913
-- -0.010820480799427892
-- 0.011034252651402962
+- 0.004926946239200975
+- -0.008545471637774995
+- 0.014413364622196482
 force_zero:
-- 0.8846671183185211
-- -0.6473878547983709
-- -2.1312346218888862
+- -1.8509176581629614
+- 0.9174475966495169
+- 0.8638814839798294
 torque_zero:
-- 0.017893715524241308
-- 0.04546799757174578
-- -0.029532236049108707
+- -0.049990966029271326
+- 0.03568758430423695
+- -0.046932565285401684
 gravity_base:
-- 0.9041730658541057
-- 1.6570854791729466
-- -1.8745612276068087
\ No newline at end of file
+- -0.2705897753073113
+- 0.1628143673462279
+- -0.10555453347781811
diff --git a/Massage/MassageControl/config/position.yaml b/Massage/MassageControl/config/position.yaml
new file mode 100755
index 0000000..eaa52da
--- /dev/null
+++ b/Massage/MassageControl/config/position.yaml
@@ -0,0 +1,9 @@
+name: position
+
+Kp: [70,70,70,7,7,7]
+Ki: [0, 0, 0, 0, 0, 0]
+Kd: [5, 5, 5, 1, 1, 1]
+
+# Kp: [60,60,60,6,6,6]
+# Ki: [10, 10, 10, 1, 1, 1]
+# Kd: [10, 10, 10,4,4,4]
\ No newline at end of file
diff --git a/Massage/MassageControl/config/robot_config.yaml b/Massage/MassageControl/config/robot_config.yaml
index f18a709..c1d2a44 100644
--- a/Massage/MassageControl/config/robot_config.yaml
+++ b/Massage/MassageControl/config/robot_config.yaml
@@ -2,11 +2,12 @@
 arm_ip: '192.168.5.1'
 
 # controller
-controller: ['Massage/MassageControl/config/admittance.yaml']
+controller: ['Massage/MassageControl/config/admittance.yaml',
+              'Massage/MassageControl/config/position.yaml']
 
 # massage heads diretory
 massage_head_dir: 'Massage/MassageControl/config/massage_head'
 
-control_rate: 50
-sensor_rate: 100
+control_rate: 120
+sensor_rate: 120
 command_rate: 120
\ No newline at end of file
diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc
index 2f713281ba47f06fc11e63e66b8ebfca04fc4ac3..847b949fb9c741ed98b14bf77c0b8f33f5c744f7 100644
GIT binary patch
delta 47
zcmeynlH>164(>!=UM>b8U|guWk(+rAW7cN=IUAX$XMAOpm|Q=fv-#ot?GNWO%9H{C
DbTSbi

delta 46
zcmeynlH>164(>!=UM>b8@DEnp$jv;5(Q7mRoQ=$r6DCPauAk4@{BZvEhw~X_N&#RD
B5Q_i+

diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..0e4dece3c345e02626578e10b82530d0b560a1ea
GIT binary patch
literal 15043
zcmeHOTW}QDneOiCnd!Oc1_A*FXG~%TIkFH}8)AF|>_D(W1i83#nG8BDG4fo*nH~b;
zk%JKgVj}~IFA2u6gbg-jgPqukF$A-(*{7|2*oS`D+RZW}smisxRU3Azc-ZXspYG}D
z8I5djvfkQ<ovHr&-23$TzyCk~`OoP@Q&U*NZ~e=+@4nV2N&iF<?=OJJ1GsYk3Q}li
z5|dL>TGr*XqARlWEh)2Dmgp~XeAS%Cq>R!-sY*)KRos0ke>$KC(th19N-L?L9u)bS
zu8Dg{596+;BI&5!<dkXFn;(&wp9Ky{EHJ9*Ek-K~KCbYOj25Pi%6gm8Ha=$p&mWXn
z=!~q-MZDdZJ0Z)G5n$m_y0;C@V-Xf*O=IXWV)GGe9;2KNlwL3)iBbX9VwWCYNbNCY
zht%0RjklIM6{~eqc30MXa5(v}VX%i1{f}F1m6T11-0of31ZTY{=g*}KW7ty7WZLMI
zEv+wWCd_0u(@*Vqe<~sm;L80oh#~1RlXQj2OgSX!KIUU8?yCLXx}OD^hIoL5SQz&p
zy$$XfYhul~hgb`1#XZc{vpH<;Az6>GFSB+w51}ZV&pMFT#1^oHxHq#!>=xWx*kZN>
z_f~c<yOn(oCEHkx-G-DoYy(@$K97{S>^^ooy91$i_C>Y~dGlB&yA${M>`SZ*_YQU!
z>&ATnTh4lLU&vOlmAEfrtJrGXZ((cLTHF`2yV*UsFJbF2s`pzFM#CA;WcMc4^eY&F
z%Fl-YT^n)bz5-H|i&7k*1I6HgDog_P391fK*lz~LgA)|e02MKGOdVB5l%g_9sUare
zq5j_F^1LJ^SA)#p26f>9dSpAuOm3jMTDa5G=k9nsnMs=Qc=x@jY=0t^yRUlyz}N*~
zY%z^=zl>%oKMGU(J8<Qef{2#^zR)#Ugu!=&eS$(#@HAiV^q=tKRJYS_g*Xk@qgiHH
z^F(lCo^vB(Zp^0Bi45Da$%;H`<Z_8YV@I-JSWTOfgGn=y+L9U0n>nl5iGD3_QvAGZ
zsW~Gxa9-lAX#VEcm+#J|jpakRfn7FvG$ESm6D$%aU+yGT?i;filV?-QcLVi5OmJhl
zcW8$9TUup!dgz^nf{_Exzp%<%xKgn3IJ(2!+o@8cnMB%%$E|QYp3bs-io(%&d`~`+
zvUB(xJWsrphqNKhih9|zoO(qp(PTa!x6ZJoB!{ggR*~fGS#H`@9?s^uh=trtOQW_Z
z!wzl93}mg4xtkjamdp(5>b5M@!;-k?HRJKYRCZS)MLFM0r1D0!KbsO}qSMD)&<)-}
zggR8$d+}O#AU#L#C%HpOLmF0^HK^sPIJsk1BUkjceq2JX)*}G0$$KS!s|g^I+QZ#P
zccu=?I~;{;%Hs+P7Uc<L6IA5zJxrs#&QNb5*qb$DJNt(V9S#&?1kPA87t2t_#_jnp
z;2Gu9m&<3~nL2Xt`lYeb={K%lK30BfqV&51rQ;{dZy<2|`lV;43F0;KLR}^8DCC9Y
zk%atv(Gl5%G8mOlDtpu&_6R}2`jQzFYFFv*(S4bGx^CdC@V%*I&g{>ohwr-|E&UrV
zsc@Hfw1yKrXY?7le9GLyswk}1EBNoa|GVr<)6}g_pXDFsc%-FpLk|ulGc1uxbt-~=
zdT1}Pw$JF_Ti=bQny0KoOSVubnoR$ITv#}3pJt4WhU$WiXAme#L(;fBA&o$T3@OKp
zGRBBWd6_?H`Vf*Ag1rmDJm$lk`6N<QI|ZfHxF2!9CmujN;E99vEa-`Ah-;pB=v$cI
zRs2VNvb0B9A?1GBBWwX#dJOkH$Q5wWLpzO}DS$6Dn%>s4Z^g=0tJkc(`<``)UH!}$
zuzV>aqlbjfAt>=EUXC|`=&g0ij&+9lEtDZ-n&x7XA$}{8EZ=TpAO9RxjSvYDaa-b~
zh#Y3|#gs+*&n7}iL>@6rn%_x{KZq<b8%Ro6FsWT>m7Tv*=<u@3Q)>pZ2*`q22N3{(
zr6IwnQBwXsQ+48g5f32d8w!#UK*mW?8rR4W!97Gd<KYQu!e*)bxR2{*#F1DGGQz5m
z;^<Hl$~H6qm^=bz7Ug(LQ66s<1`RPI*ef_da{yGRk-rPwx_<TA)acdHYrmR$@9g^W
zs|U)joL#vNzsWb>E**ch{PVX<C(ccsd9yS&KKa}m`FW^O6>L$peDp-==~p0)0q|ka
zFn4ygaUY7=3T**}77-y4;tPoo|E#9XX#>SG{rh*A*<mZ(pE44BGv`^(mmyz|Y|bQh
zr3`@yPU?>@p=yDhLe=swi3m#=>1+me7ak2wU`t|4Mcc=1wOq$&z6w=$;>uCCB*E5Z
zFnIwkRn~l}to|wBgH*P8*({z=qg2T@b{P!QQb^;F41Fb^0i#aJebTeih+-<3>y=Ry
zFv0e5Us0KW9utfeVdh&XjrdSXE&4<$*nMt3$+urHUX%iJ1As{2aV1x9!ekA<fUpY}
zghIq{v<M5Co+IjnQ0{M$CTwC+9)KDF1koea2G)qb=x4H1F5;;hMcN-lJCP!3l0TxI
zCd%LQpQg&1MW~n2<j|6T1)~y<d47H0=-vLcExkMA+hf<SoGl%Gck1bbQ`Z2QH_9)3
zfLXu}myW(O`O?Mm_>swjAJkNTG`n|eY#@~wjBSYZ#HJ6Nn!0x6`qi=0>({1!^9~CC
z%kPd&efYD|;j`B-UH;CuV~@r3Sl_o}eUHWZ^zX%Dd6;SIW9jsAW(>N`3K_WRr<T|A
z82WpIR73gLk?U7pi*4Doe)5U-i;w(c;ko;#NrOz2F1bObOzG;+N<SGZy?^%l)zhV?
zPM0s7!NXl)D7|**`sHWK2hJjE^6B4}4?HzZ&E6mZa0{W6<x>aU5;wl+&78dQ`s9@h
zrT2gB6ghRs87I-RX_~-qkO?sL_F2rN&bVDX2Kk#j^$YraaR2$JkTqSUm$RjYgozVQ
zt+`NDb=q{3tuE<3EtTYulXYZiG_NN!c|$18^8q1OR(Lp@Gjeg7Q>`e+M3>I)HPWy=
zt=9f}i`2)BoTc`YF0WIwZGHL7_AF-6_;y&)+tH+fN9MXfq`-VREH~p1r5kLAM20CO
zM{!X)&6Xsk3SxyNUg@0C=IoNH{rQrL`BX@%&y`f6c7+U8B~_Giq|Wb=R7fp9B_z*L
z>_gH}2<kpa>b`-r{@uq&YXm(ZX^o&wDi`%gYZKCLdZhIx@`a3^koTyNSFz6B)5{`|
z*Vg=547)2CB)0#9^18y2S4Zey|7q#+xdu|ZbwzCJ%GlObv8}6PTi3+4u5|^sMrvc|
zDPK)w4Ux4(?j~{%5yB5A+lrHY%fC#7j7xqmkqt!dBSMBCe*mO{>c|j*(S|jDm~tN>
zLaR@{g-9KK=hR?rD^)&<Vlx4(*#IWk)&p220N*+*fJLLTBlspBM>zV+=NcEeYRw$u
z9||x+F2_?SOvrs!AQua5F{3!M4UiRdkVoY-l>2L>g`6}o6U{QmP`S!nM0E+tABpFM
z#T@ey@&&5*{5#AWd$xI)=9u%%A2Y{nqd8{srS~Q;U;95W#}sIe*=El%HfXDOt%GU<
zlzMgfHjgeR+q9~-`9l;Xt<A~O6bf6Yd%lUHb>OWk-))ZaeG8>>cY=77ukvT9-b#!A
z58TGU<-Np^uMznwkvi5?^$HE7PyxS3vHvb_8fz7|Nv@&IeljI0KT4R3ORQ7=fB^Xh
zXxaD@d;$nz$>@ZzSad>ICORQ35S<W~h6sg<3MfPvG*VPSqc6|JR7KOHT=bJ;7J}VW
z#F|Ga65kLENT`~FC_&*CsDU%0G|XN{)dU=Kww2ThYuiD}kIK$L*}q3?b6s@<^)!NY
z)VNj*z@)`Hj0B58ra0x=MSXJY&VvslXYjq*p=Gu)D0c}%kl&8?pQgpjG+9hy+9>$C
z%ngM|S7J9jJmlEjPzZDlCX$&AH|5Ska0fL5JF-r)X;eKk5s7zaNZ4jpC}(6?oYAUP
zENjGql`OD&QC>!sG}}3}4Pi?*t&rOdOQlX&5wm|d4i^V`0%xDJ!ge=lvMQsc94(h6
zc_${Ouxn+hHPcjKzIS@6R-YX^AE8N8GG#1t<WYE)umb&TkmMUz9VGcZNTP)iA?XCr
zL19ADTZEL#IvOE+VdQ55Nlv*ol$$dBY>tB>vC<YOip)R}l@n+au8nq}NDRz&8$}#D
zC{h(id<R8op=skt?TP_N(XN=w!VQIBS4=>Oe*?uTt<3;a)uItNidw3$!F}n(u;m{}
zWfP{o!u}>b>(gUuGZp_9k#7@ujEGL;D?}REJ|X*cVYB}SisTl9NU+Omaa1*S3iD=Q
zi0u7-60pjTRFS)iei9L=99o06S6q|K2Z4tg+AH&ANE2&gkp}x`98kl0T7gwA!6t`#
z5EhNkS<k9PRkSsLdT@q783kV!r83mIoub(()FNG-q5J`KqQ)Ygy7=Mb)r(m7l@7mO
zdiew`Qs2D-kLKiiXG*_&KG_{h-nOBqriPi#rsB!i^yvdr=boZf?}Zm9-#T~w(yvQb
zQRTS3V2v$(Vqe$(u0j{z*TwgD@xsVbw-KS0@-!`z^Yr-J*RNeHKlg#%g`Q=lqc4|U
zf2REI^QFV@xE<>3{gBq1w$U4-5Gk`d5Kae>HXS4)G!$vA8@GYfzuo2IrQ`2U{q}I_
z$mOYHqotSL6P}-jBax>y>Ex+HrKgThzJ6u;$Z^{tbb}scyOlas8<mTYqC;fNofseC
z<>~a>OG`Nk?~xd^*}>PGGtA8yws~N{=r^-G2<zVV;P`1>s0weYs*t%&*Bm(P3X8n>
ztF2yRr4RsJ5dHuW0wDlb3ql8Q6Ed7S5?neJxWS=Av*ZYtg-iu>w-ZbS+yV~VqDT>j
zvI{dU--E}v{Ep#QI(no$cBYOIU9p}g*SW<w^^G$S+mV+i=Ym}io`t%qf&n%!^5k)A
z*nR^BsV;ZUtM=qGKu?`jDYW(P&O)j^tXV>=A>X~E3f72;#b%0Dw{V|PcXs8l7w!Fd
z%AuiB_`a(GK8;d%6AyRUK-_953LmQ!b|rEKl!7D5C!U4Kz@u6|TGc7breFT0D@C_;
z{>A!(Am-H{b&snklF}=eJz7LyEKfS2u6XI=bV6NOo<x0afXtBnZ{!Ty!|TXCCv@on
zdvs<;Srp>?5JvD}j$OdJ2w;SLsZRD?$zM<>`EJ$O<(?+NPm+6qq<VS(%BPa|RcO8j
zLAE?P1LtAECy0Dilp*pnYm1T3lYKW^`;A22WeOic&HANDi!G(@7(PV9FE%4x`F5rI
z7Vp5<)vJ+hvUpvc`(s3G*)C!`{BZ?491Ae2j*j(C^sX$EL!2gfa!B8oAq^{>ab%el
zVIeZCHCUj7++-9BLTd+#zM{VvI4sr$Ok<%#QZYF0XW<E%b~s10q6PyX(ks?TdE!^;
z+TrPmE2R_1$}j%b7Wnef7wjnH%25HXMR5FJ`T2M3nOnGNpMAe{`~}-rZIAg)`3Yej
z-jqjXY!#6mL<XaPwb=7)wf8(}f3IfW^Sr{pkAk%6!IQ&?Ev1;eua0Z{^GHsBQ~V1E
z6u#)?3wCM95NovIDU8_8fOo=%+n7p=3ENU>+Y`?oP9;sOXlUo!QuD~gsTZ2ntJ4Gx
zpyk6_$I^yTJ&x{LDuwDgY6YF{e4SuLHUjO*U=|m1x#FX_ojk%(xm_;IZSWd3V@bA+
z3o8>5nECD=DcOsTZHPS?VH++5bbL0@1axG8h3rQa(82Q81`>cK13apLjvwV=JJJHy
zKABVW14JPQM1f)eAPRft=P7{$uMgBl1J9s$0uMZkP(g*ue0|Nk(2m)s#|0mz_BfGR
z*MdL-;nGIJvbrIBFf(YRJ`M`PY}0@OluX$$u;D?&%W<Fv8;iD~P|JM24_SIBLA8kc
zx?cqAdS_{1zvu%a{tK-RZj(as@;*%5uQ};Zsn$>IuKdUZcG+J*gxFtklopkfV574!
zM_XUYjIFPgb#PEUa0nDB2E78Wf?5m|g8~j33$P$Gi3Sj84hTZU5G^Gf5RfvqK~TQ%
zYU#vDXXoa^xvAG*Z3u<W3<L*V5Y(K4z(_uv<v7a2zM9N@A`&7{21zry*Fa&R6Y9Yb
zw!uIy3r<EG{~?hE*x>sqgSb?WiKdz^<p~(`QB0B-%>V{qQFDBvrluwJ(=w#;iz48n
z<7q@1p`)k*C4L7b0eCY4uB9Jvlm~g0Xp#C5(1Pt<$`?!00PsPZseULA_@EF&jgb(r
zq7_2|En!Dxggq)FQlzbzaGuTr)>sInEbWa6vN&ruH){$){|bj;FSgLJONvPf(^L7x
z<<g~dv8jnSCSSW8bMwnDj+dW5T2q}`cYiCK8s+g1%3~MZYNdC6HFfRe^nsrMv^AB7
z$f+bAO!HA@Ft*f9+3>{BimsuRT|=w7hE{hCt?3$CJF*lHa_ifZWmP_=5B#{HeDv1o
z%Fj$dele?nY{~33a8~Hk2uQeE))tLrU@WgH%cw0Mn<>?m4?XH-q8@SwzLDVQAu-0b
zQWe2Q0#xnBVOO&TmpbcXuWoH1Le;u8V@;kLx;Em<tp)jn+4auK+?vr4vX7Y2K@`<9
zKjIjeeFnwdC3hCBG!@Lubj?>WEk!pabJb%aVxaFpwe!xIx~kHtof54m^VB;{!$Z13
zaweWIoC$@RC4{&5XF4KeIhHRSE}uPBe){axg>%kW)F;=DLU?2C;Taot2hda<E;ua-
z&o+GB8h>x~FwSTCvc%!?GcS~%xoV#Xq|<>X-uAG6W@hb+hj!4xpG`b5=$VNcytfq=
z=Xc@^B`<#R>sJf^@4o)wcc)*xFx%^k%-Yx2=*Y&Wc>7Qe<;2Us-|*#Y{IqV_&-C4E
zPQKWuX&~!38(MSb<>#-Ij=q@h225rav4^4yPS(jcpDB&KTYhE?E!YQe;B_GNVDG57
zT7fr0Mnt@CI!ovD|El5W)ExKol&%>c)Wm+3qZvt;6r~@~h4YxucQU>XuzN(yciv2-
zP9p{5ab~3A@!wM)gh`9jXtnDdr$o(jX^r4&WHSE6IdYu6r1RybGLE&)1f4I($#PJ?
zpaFa>;X6aAK~VxG3&NTia=hyl3&T8)K)chznYMB048VXHwcVJ|=281}nRvTUFT65%
zG>%lz5KPmAo;+GP!9J}cDl#wz{S@9sBvwfkrx0D;Z~L}O$6m0-;QFQKa5QY{!w>Cr
ztft|L7jDm)H5}P!vVzH6+}M{i^^nOEL-v6sC4;?^g2A&ny)Bi<nQ^fubvDlQIU*@Z
zC*~`gX;zqa*0KWwa6IV&ao&Hc)ijVynFf!i@mUAoXCJEy<7rSB8MbRbf0SN|CLp{j
zEsY!QqNvo^-w>M#qxOOBs5sV5i(EPoEVfXTLaTSBMwMzfPfHy#ZpB<oLkf;b;w-TI
z-M|R;L`KvR|40Cf6|8iZ;3Oj4({#r=R@_xO;9W#&Q60pJcLXyuHdPgJkrac2;wzLf
z-#E5mVZg$C9iWX<fA91Z&}RAe?Jw-aG14GT=y1cx!C~+Odceu1w)_ds6Z<W>&k7Il
zY&ve>tUkAbi5#8PH*tPfj7)*b=cv4lvISk?`*7{YRj_<uj-^rQIM15(lF|wjKc3)e
zVj&hH*~}kkEJ>Usj2^iT=--O9LSeCY1naBSxT9z^No>VvE<l4OZCFEghA?_eVZJfi
zWumP|IQg*%^pSUf1=iv0HB#a3FZu>idPFs`94z7k6tQ_7JR#@WAw`;frwvHj3?C1J
z2O7DcSeQk|BWBbTTjCRP2R6(*qzUlNY#wi+ZBEEyD>x#>+whz@v?VzbL})ISAfZ7V
z&ZpSWTs)&4T$AaWpfMjxw<C2Pr6R?fHs48W#{0nGg)gpJXkmg-%R=cEx{H*1-4NVt
zA#Ae>{<BtKpskmm#6a!jKLt&0gP2dhhY&vjVnw}TB^+tCpbK9|9~C2Chpi1{T}|vu
z=7j2iylBYi?d;&sQ#&Dn972Gl0XPCe0$sY=pTgG~k$!wtl`xGAF5BAq3AOtyks^^F
zQ7bCh#{7E}^a=M<e|C63KAgfwS|ZeBo6*kcMLlXq?PJXRIjR;&3=hMF#m7Vh3feFY
zw3SagFt*ygKy-CS2c>3t5?|?v55OuJI=<yGowxZVn$&&tY?|M6a{uJ?5SosQZdd^%
z;Jbu+^tTN2Y(I)tn*4~sPbx=(hlIQ)<ST;^D0e8NRLGMuA33e^Vxd+FH4fUkwtDd`
z3bCO|W+vE-5+xj_*<rO>$qqe<hB|NMbE&;{BJ+rlA#2-ebb?!)#pYWnBvj{i3Vof(
zcZkG^>?ZO!kra_M5ut1ZpGPQ4%;rxK=^=8E$S9FRL=F=<PUJKZ+RE_^D(Ma0Muc34
zx8o8E8>o=Enue=Q`@HrAZHcxd+!k)t=4-9mJgp^+^l)pqLkqe8pf*Pfi#F(6r8LfF
z#N&m?ighcySFF9KdrkKW{$qMbj0kPJ7M48G)7{fW*NQc(yL;|lA;M1D2*!4RiVlKw
z$|6ib{Qlwo&PDj>Pki%}7*5*f>Y<eJ?Ox@JtnO+q9h&EUB4kr=GQ0R|AeJ8=%na|h
zHFig?KRLX=J2wolCQ{v)H8Z(^EKhTqUxYyFDyF>M;`Iba@l_&qK=4l|ukKcxfmLP^
zByBzk#)x6A5*lzzyVw4Z>pnnO%B=)ZfYbo+T2<qi3Kup1NTUBP&G}-6&IC6D%~8J=
F_%Cito{9hf

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..83b71af5e91c9ef3880a77d617c13c0c2bd44976
GIT binary patch
literal 15294
zcma)jdvF}bdFQ-#cXqK@fFKAyM9Cvs55l2901{~nktI@;VA&C2homigBiClRGXNG`
z?1DYBBr&&;C6N+oiWDh}l46mPKt<1UDC%NIqDWdw<*HJ3cd7iB{4x0_RpH{1KhCbY
zT;)o4k?!|>J^KP7*$Zq>Pxse7J>B2q_kG=)ZEsH~`21||E#v3Dt0@0MH_aa%H{0-a
z{|^#dvDJc-mRB{c^1YVU_^PK3zDCj!zDCngT(v@M(oCC^@pN3Jc6uQ(*_KXnUM;kz
z+fil|Qj@84N>#q4*bzH=M6siXwRDHwW5?~p5jEXux7o=fN_vI8+HSW~DCx4-*q!zY
z<W@S}cGo^l{F~EZue7_-Zk4lYw&%Q}D!8uZt1`RBUUgpmy-U_|t4?RtVYIQ;-AeD;
zMZCQ-(yImLcc;93shBMUkv-+9g43%8(W&XFLdiqUw6mV$<tLpSkvD%L;f?#R$T$j3
z9j0xQqdSIU*xEVW)@|bmrtYXRs`4b5n7){8;<aOr397hbu2t+fC=+(u5hI;IyW}~Q
zC&Wm%ImwB3zVEO*QJb<?*j;Gb5mI4lP}*s)vU|dbT!U+uz1Cib>q`3udp)k*_Ko&U
zxUO<~?626?5#_L&UhSCn&3MiR`>W_-jeU!KD=5~6{oZEZj?#7ZM!OfKH`rgZ{{YwZ
zM-@X!--wC3BZ&WSBDdQqx+SsrDc-$T4~+4W>jlPCNgx%Nv7FH~Rg@>D+kvmU2T4V(
zC>f=qAyq3nQq86k-KG+wqRd9lE67FpK4uez$@#dKm~G>_q}M*1swfj3#00j6aT(=*
zLe~i^{8W#<QhocG>WLSvJv+W<EqwCo+_P`azkH;6{Oq->vvWsYtN#3*I_=O@R%9nF
zdWj)x;k}QmZydG0{XMIC?)~a3kN@WCvFck-S{nxL9_$;~`i;JUzJdP!4Zpei)1}Sn
z?Q6~Fe{r#T>d(WbTlxo=A*X2?vgTfRf9~Xai${;n&3-ul>`}Bp@0$m=^l#qM-~W)`
z+-_0D`;83ub_x?25vPPCJ!p3e*#k}{Yumzc-N0~5xqU(Rm{WA<8JV2O4Q$PL*}VlP
zXluUd%IwYN_Kk^BxoBs+ap7d`pflrorKzThpgmK_x?YBA^Tn|sE(s}XJWLO2uT^CV
zbQ{GZ59PhU6wX-Q^&Al-?7W*R6^l;J>oqVsr!czY4c=A6I=slIyEc!PCY{X_?&w~b
z?994ucFfsT+FSB64^X-}EUI01l!~4x6*iA&g?%t9oXw*pk#jOGvuEGbbYNyO`C{J7
zWZLlrw+D$5i>kH1geG$d&F`vnbziMXj%K{}Ar7W*2fpqGBp!IwRu60LdQbJhxren`
zaO!#DWbp8W0iM*#Nf1^~zHsg1XXf5HU%h%NF!M#%%NBD^ZzPC~XWgvli6G*ZryL=o
zAP!=gOwl=*$%q?q2iekd1*~aSjJZK{A6{|HeOC?QnM@AL;)0B3PTW9=g@gv?Qa_58
zP`gxrMe|#6zb!ANfslBJMe`Uk{)nf{;?*Wpe6{l=ka(#Heb%s55;e|8kVf&1jcQcp
zv2adA&%zc^=~?9!gY@hpO7Sj3@#51@s>%%R{q>+UDk|zVs(%s`KZ1RjQ6E!_33=^T
z`cZ<Zi<VblIMpkc=YR1`vk0RQpWoi+w`1vvrySezO4g_9kiVKFJ>)bfbZa!5FF5v)
zHPV|1q9hcK7sR#?XTCl1;GUpu_jh;vQD*m^T@P;GDTp_MgzI>u=}Tphh21n5HHb`!
ze9;Ty4<5=nY%t!{#9C^nI&uvf3$Y2s8}W5Jktmvmg-EL1>UuSWzvd+gpF$cGb|xFF
zat=-LD?z5*sZ!B(G6km?M1|v(MKMS0SNo9psjr-e8~-NEy)vs-YU`xTY89-{j9$?n
zQFcRpd-{YieUAz|iZzRXb+HDRe<)AXMW>5cC4*Nff^{@%tHf)h%@~itsO<(d<#?5(
z()|Y-hZTORr=DAU{0XVksvms-l{<IlgX;4qtC!EzX+mX7?+b1D))TS~spbnWEgXMR
zHeO1-?7R$Fi?1J<JNKG=0D4$_;gjm+w;FBbv_Z*Y5XatM@(5DSZ~T{qU%k2T$(ice
zSFMp!(P>t<{)Ro08LWfj;3Ml`Ht#{}Sx_)mZFNTcwUXy1C(FfrE=xGVf*_Z~kkuQL
zY!!npva^K(45O1fAc!x5SjcDM4%~@8B)t)4CNV$-gG2)bDp^4EQyFttoyCGq>NS(a
z7ri?91J0cwa=VczF%8_7go;b4Ca2)JbqzJJv|+C*sBMc{Am*YktA)3aA=a{0fU+J>
zoz-k&E4A3or8-K##-#?8!g}i!;GOf@4A{>d_6$HjFq$3Pjb3IW70ov7_*2?iXv#;m
z8GXh;Nn$NOVV~|ki#rvls4dLLQDcPp81ni)UA&AwleIpPZ|8ghN+VVN3I<y@@bkZZ
zZ}IF0SZG?L`JWz}JNFk@VbKN}|F-@CYoOn{YpXS|!|Lz1`UkE4yEub9|3CN*>+UU!
zWM)Bp_wOLyF=!2MStP^kw?mIuz9UMkfEGW;XJqj+q=L-JBIyL*+TBb;N@~Zh9ae7K
z$?cQ;<FBYwxaC|9cKznws32<+MD|X5jvK^AMQL)}IV8S;LT02O!qz=xrL;+7$mAw%
z@pVw7&GCGOACNW+D5gg(WhxUR&rque(ZD56B`cCt6KX<Bss`A}(0jmEiO)4H@tLM4
z{zEfRrkQH9^{jQ$!cj5wRaaLPOvLZcQT|^OPZ5B}73L%4Ey+tj)Ju2?n40<q&zuQ)
zDIVs*Wz1JpPP{~XMfqgNSAXY4?e>Z`8?#gA>qZJ?9n>F;TW7_vSDZB}I;F2fi0d9v
z?Bs43z}dLng`R-Pc{JKggpg__a$XHpFR=V+bvh9$->A)w2$U~cL;0pqcQ4d$l+|z4
z?y10-N+ZeYH#!r2EK0OC=lZEl2ihD6+gKRoYOk{bN&E74Fs@O^$(r(C(|Xj@HpD|p
zhjoXwaPdm@)Qi=pF3ex~RWn$YdfD1IS}0|`I|t`xuL8~V*1M2ObjYd~P~XL=KBzt+
zveO&4Zt6wn^=`x7{$%*jrs3&L!~Uk7hc@k;-n7&IaZ3l`V4BtR9Rqa=DaflK$Ssn)
z!y1PC4vV|-ivDUyuB_c6rF(~Uf4_D2*B1$0jfi`YO0gs-Ws%^};u<RZLnQvX?}?IE
z%9RRICU3TONnHzZ{ZzI5jt4_)Z7~2J^4D&%M$5(AQ2(L+yEa*{+(U!7Lb8Vfy?^kB
ze(c^+{w$IqUnI=t_kOXFkr=jaYd-B+0&hY)<A3%0#hQ7P<FxWcw948Uvay-bI|pHm
zR<Q06_tLxFha^}50O@2!#>)Zimq$m@8!M7+L_uaL(4ky_n><m@c|o*XoPx6uXvL`@
z!b{4QOl&2}ju48$`aMX-k1blDqbZCaVWc$h)Z`?r6|17ad?-{!f%$N#ih?%zx;*A(
zEna)eEBzR~y2D76cuhlTnyDwR(@%;2r$xHeq-K1k;cMzqo$fl>G<HIl^{AbMlUFwk
zWXp3}lo+uG*#kC<0vS>av#LbK{w{wL--qV0qPiRQf5Sr<prQgj!#LJNbAKn0aW_Ob
zG7+5~kt*RwN>S^FIthD(;!u?$Ik?n6--V(ZD-(atU3zN%-OFGI39;s{K3{$MFRLdm
zkG=eJ`h0X>9x6idyLZqU10fUZ$W<P)1WnsKzVz8P65<r`2e=bkC}B|YFy+Vu@Dle*
ztSjM3D-;ob1frApx?M;VG8AkqG<6*z5Y4}#F}2t6T+8&5RFD|*^!^<(^!kiCzJ{D?
zYd{(|dGNAq{k&Q<Imeiz_@_)6LH(XO<wIN_ty9JrvutHb6O@nDDFsu;>y!zmOafW7
zF|vT(W#od$j6^l!Ds(*%jGaP1S3y^|5U=RmVg+o1VRzM9+bf}SyUR42)zcT}&%AT(
z(v|A*cYsqCK7L;Mg-b!oq77oH;4Fz$klwwWp$YUIzzSoO?e|+-xBHfaC(Bao*kNto
zF39|Xj3Li5WCQ30WM9$|p7{WX^8W_Y{Usd@T7&55{x9ih(7Fp9-S2mPzX+$7VkTfu
zic_%46+`se-=xugWqZy8uC+XoExMC=*JW&7q%ekUWj!mcJ>;|M_HP+xM~~b<hwH8E
z^-V_rp9p{~(SwrNN%QsU(w)<5h-oQ0S|>Zt$j&p(<2sA>b-dKAnx7lMO)XcQa8i<W
z4dbO-8ba~K-L^<ai>sc-1BsKx5Bwgw&Z>luJYZ+yIjEOGs281-3p?KHP@^>*Z^jgQ
z=yvm!9Xg7$!G#B}8q)cG>X(ZzU1)N?{Z&g$P_9sN9ot{eT0iuYsEqt1>2Q<xZAeEb
z!qjrY$>kB7DOsXSNV)wbzHSPMg6NC}^qGVrxkdH68*|nODYeWI0c_GDtU*YFv9zmY
zRamuI5*Ez`OD5iwV~e_HOmIlUHcoP6(xy8l6XP}}&1@Kw7(thH_yJLd5LZt>TRru@
z1S;3AK&;RI=IV*Li>Kz_{#jUf>G8S0IA8ZkmiV_L{;l`dgK%uMY3}l_l`W7Rnzlr_
zSVYk<MWsDNe;jX5Z(NWQfl!j0$rd0Za!x5;B34pGbdL<?t!awm)uVJ?)<u-g&N`E&
zA_7n7(|cEy&S&dd1Z%i*nLe)IAu_gMfXTtwQ82cNh4MEvmgtKbfANA5I0+XFu54|=
z+|~s{XpJLM2(VF5{Uq3(gU8r&0OQ8WUqdI2C3@oA+=bV#U!pp}h!4NtYJ@?XSD-fY
zd&DE?6K*nL9XMAs_k!?8W7-2_qLeR+y;Qhl5?3#Yr`FctzkyaVd3_F3@YemCn`f{k
z5?gE8GJ(YHb-(J*a6{APp7BRuv7i%XC^D&1;ODR=zJt2RsUG-I#N|==E~){8dstVM
z{ksMf_uhVRI{p0!-=c{gV#NEQHZiB4hBndQp{(=b&?Z_9&agI7XSByO1o8Mv+JyWc
zveW1r&taWHxS~Gu*DngiXWn~t{@M5IG$a0vQnLWiUcW{_GPj|}-lSm8{{iwrA`jgo
z<EsLDUx>7X4v1KVJ3$CVY@&n}5W`%9Vlfbeg5ykuHbBChexhq#O_4#_KjJGREe5G4
z6Y9xSZ-a7YCDkMW%SaBw@b12r1tgV4VFOlK^hjDjUL-)CM-;7Tk#;Lsqz%oB#1pHP
zTPAkl*$?5s&7FIE;gjdCvF^ddLKQ*rG$s~!t}!9Yv27<5O-tZ^j^3KknF5!;Ii9qX
zFh?}C4+W9xkdC%~#*`Vn5!e!u#KY<X%8&1*Fwu;Tdq5}LZ$Ll@8)w@TF+|t<z^vPq
z8GXM+cMr=PSi2%wtFwa0W^bf?JGzFSHL%rMIRDQ4nb+oyod%-1cInybv#(vd^5^rj
zPt2daR6TtH;h(%Z3+D@bjVGWLV=U|fMgTL$9Bf}qO(C*|;Cw{xm?Wl3h3R5xGLQX1
zHIHb#$OUQ`b!X-vH%^eX`k(kpzR(Q>ODJGMoRQ)Xihol>u9w@HfqJ3PUyiJN?L!k+
zkHV3sO43!KNxBI^U05+p4wwb|X!>?c-!^5ff~nH>o3f5dkV85uL$$Y{i*S<OJ3s&4
zOJ6ucpMM@s&LT3T`Do7rW}I9(DI5Ed3k(WGEtQaMsQC|kU1DlQqoC1x)$eG$J;$i7
z69IZ}7J(!(&^!uTUD6v`nK*Us)zfpY9+`XTgSi(kRIh@!X+LiSLnFYjtm4H!itQ9K
z83QZr^0aErq4%)1Wgsoe^l#9yKZM;7_U}lMO4xuY*WC?(_u$tjp-8be5|J@?P=xy)
z1Q!6n434Rj%3P1S=n|C~Bb8~J03n>?piXomHho+wVYG-7OhJ$cH0aHoz8WfE>MilR
zdV{}+kZW&S`3PR5F8T9sJwg6QZ3`5t;dlEY^~Sq+8^Rr`7g+f8v&g-186sCb{u;o~
z9Rp#B46oOTeQS95%m4jbAIQ4^_ymELf$-L3$#(ks`uv-hz)--YV#%`{D1J0{#{o%u
zm-8Ng8m0RXhEzw(g~GJ-A-WnNj9LJHL<WIwZH>@S_?*TU_J#c>4s7^cOQSFdy2Kmt
z7Ks(wiCO=clDjCWyVq=vLgSNe3%lmf?J`CvxS>X{j3;6ew{Jk_N)$0#74Z&yNex9c
z+I*tuZGBVV)`n1iNslcOLVB(aHw;MCY1N;CT7o_&k)45zKsBlEqw>C@t)Y;;A|8dD
z(8d9@HuSsU4(;zoFx7zjOgOJr6RAXOa^kxY=2StUx<$lKVk89sE06+69nO!8!E($*
z?Y5biot!aiPp+7i7@qX^OP>7qvj0k~Vy?lK!%Vyq7so46$SfRwKq1qMRv>3zS7yxp
z$HP8;i>J1uoWrShN;>s&1O$pIQEXv!)Wgn+nKqPkB8Z&Wr=LdD2X`H~!xqMARoLij
z1-${TU9!eri6An!pq%U-wcV9Au1PdX>(4YRQH(uVNrvI%_(THX-=Bq=La&FDrKV^@
z-QPl2w6o^X5l(Jz0c++=`(y2_c{s<KCy6$ng>5X1X0_K|!N$s$w}Xzs6MNVA|D!JD
zB$_{P1bgduAYhus_UyQ0$>`qgu0=6P3Q$gA_nO>$mg#!yWi`xoVdUNULw95kLF#$4
zf3sH-`^%2(nIg&-Ldq6m$`*pk7NW}5(y%gZ^;!ARI@qU;Vz)kqVTP5A3;q|RNwGK`
z+2d~-#wiiU8pk_1f;G-49i6Za=Dl&eQMjjPVJAOdvPTvzv|Svbgrr8QnXi1i2!(+|
zMxJAha&O`Go!w`B3o6WCY4PsPCaWIO9oQ<P)PIOh{1uIDBDuSLv!AL3r8&@SiEj~I
z=Z<V4S1ur|V#{KGi5hF(%?tz0v@+s5)Y@9A%V@!9VaX-k*bB{;665qvtk3E(!yry$
z=iqP<bq;yZl!1;YxY!2TpbLj=u%FE_>2UFz6d)_bKI(H967<hu<D{JH49_z*=*d~l
z&^)s*mgb$X0eKmeyIdW@natwAM{z8}6PZr&rM63IV~=dST-+yi)1wHGitkghoSMac
zZPvbx^yv>lqQr~{?|^CDGQ5dhFR53lGzuL{pwHCSz!f&&3a3!tjq)DAA5*;vnm4K^
zpoOoCJf|93m>$@I04dl^IEsLYOEb=A$q)%W0<}ly%K$?32XLlqMxz~Apglw?q++t8
zHz<$lw7-hIC|QR?d|_RT>!LF;s86cHc7G*OiIS2WgQ9^*n<FiHpYRjK5GlOCfvz!t
zv>#if_b1e2<WS=H+iLFK#_(+Yr2d??pFDCyJb*Sqw2=3_g0lxmo4KSUAVb()$d45r
z8-+TZtw_+8rd&4^X?^`-3T<)RB^vHf)B$&pxCe&bBU!q)Kubhe85ZwzzrQh^*E3w-
zJvgJ3Ex7k0Q)|xS6aNYA#Y0HO@WtnrZ8Ed%)Hc}V0!rAQ6yHM*cAb&%apZ&Ofo!4d
zxM^+p5Wds+`e|(^@;i~==}UP0Q)<tS!cUOfM*}9Dt6*O>@mbVZPY`+imkk_1m!`k>
zCOXelgcizVf<z_*<ykIJKAFkvFJ}v~hDAs`4SKPj5{nY{dU^YY)>7{EY^NN#$%0;1
zkO$4te37OcBWv>JghL6rzG4DNkc1q1`P`)AjhAe8eOSUpnJ5lW@+2knsyH1c4=UxH
z0<d_Kvp~*>oRTPVG#kuU5SmrVsM*%jm0Hu;Z>CbK&A6E|+s#hXL?|$oikU|0Ci6jg
zRw^nH&^%h>)4i?bgbX<?b*%C#<{UgqM;I*Wd-8U&N^S-a0#Hlw0qKKsxlZMtF&m+M
zo8Px&B08hPzeQXMB4<$WWCj?bqODeDV(6Rkza0zhJ=Zg0`P+(q80Z7X$xOlt71K5m
zusl&QW=+`(k04^lfl(s+PXHq&fO7)$D7PmF7sY226^wKy?zL6e2SG`a5Y9{jxpvOA
zA(!G@61fh};ecr+af33GLT`MG0@qX}QnUKF>!_sc&a>F~4#%wk#Q>*tD|R<ZSJg{<
zD#>L^S6AAXmO@u(P&TPbTP0a(hvJFbYtF@?Db|*6#3Hd7p8Kn37B0R}_fo3ouw!x(
z?z`M8xpwIp87o@+`BO5SDq|jtuRn()l-vO))l+nusNt<hJ2zz21r6aQ9L!@{3m<|a
zL)Pt*r*7XU9V7Y3UTf~yi`MOk1(0)c`^LG~-dwo&dRSNQz%FAW1KSihal|h$cjAfp
zD;MCjF23}ujR*+K?Q$j$_sbhi^En(It)6(Qdioc#@%)wN=UzQWFM8pn>Qk4PQv3`&
zUA~zZ`<vhVX84rr9XJb>Wl}dyRLvu%`cP*6`920FWl|LIt8rvjKAnQtO{2HUq<$>C
z1N^TM#zw3+%U?zD!iBf0&mG5UtfNO4&ps_rTFG^pfAS+N+`<RHWJpzJ@1gnahIpT0
z?r{oE;TSB)X~8TetbX*%>MKX(nhsgM-Zwbv>()a)S0cPseg71ub@Amhc<=%YBq;7A
zib=Ol@y6RV3Vx2fnVUUYJ^FO@!}rKR3k+IxobnD=4u@4~&pZcQha;@n17n#{;p`8R
zlldZ!ks{gwZI8R`p{6_6vm18_LMPCVlT&UGueES-W|v}gQm?XdWHrecDo+)pdV?g8
z<EVT#cH<|hoMV`VogW<yR=IS@x87yO!_K*T059NVi-C>-BFP@2U3a^;1D2Ifz={{C
zmzR*F<Ag3UbUs16LWLA87LQYnNk_>TRZ$2^yh>$BdiUDH#LHBEmXdRn{G1ZD#SMeJ
z3TKkM@Z2p=TG=V)=zxmi?ghEhZUP4353q=K3Todd$I7U@6A%i{D*kZ52-{_}uOR7g
z{D@khEQPK59{MvhL|$9|{$0F{r?j9aGIV5IcHrw;7zu$Df+w_T05=gZ1qjMGuhZQG
zcE9Wh_H?6AZ8)V*j^;_kj<^O}@J%&swHj>eCrLS@uL$;rWiCpmhqw#|fUj^C!xH&K
zJ$xdcX7`fvxrcFVvuOmKc>ap;`87Ho1=BUg(}f>Wgmf6q%KB((<BHndUpSifaC|Ac
z+N+>~dwBa~{>q<Kk9~0M(i`({pOh*NTBuoUs~10-yNE-RANOjDWVtzz+N-8xqxms6
zj{{@``J|E{W)d$TSsX+%CeOdj-S@w#S&M?j5!k&5-Q#dcVQM@ZL<-rxPJzjqHCMA7
zvs$$_PP)DFl=ace*gL>h8&rrqBO#C0vyPBDxO(jA>ZQYbd~I(Z0M!Ky!Pi7znSerF
z0aY)bs-F7ri#kB%v6n_5U;0!5hhp%)W#Ob_Zh0^LUIf2`0IEz!5AopB(P_RSlAH(h
zO-Bn3BENRp7!!FLKbAlPDc@22`T((K@fgODPI~#VaSFXp(>Y|r9WNaO7EXns9-Ij+
z7d$uU2rUu(8ZKBWjVq}442xt)@n1<2$-%-24Lyak8S7Z8Si*4Mt)@PYYBCMw4dB#N
z#U-?8kt#ykWDHoUx>>5gw{QRs>(UFM)1k!oC`Bj;r8@Z-VJRdo42EFy4gi8Bjjsti
zUOQEX@I#xdg-3(ad3HNdU;`k`%d>Z&O4X?laIrffxj31}luh?_>aIHV$|clI_jPIn
zp|~&2UbTd}={}^!8CHNe1bXSp(e@s;(vXscj}J>ps($$9wO^lW$kXF*FC6|g8+4YG
zbSzxbCd19Y7?8p?cl=G*ST^jW(&kR!2O~#){hom>e4I!8XB7JSy|{f3xpF6xW&^tM
zA-|W^wjTuzOes?|TTzlDspm;rCE1+~vYIJnGO`3^as`)IRIVfofn8G4#xh9229C`i
z@e&-ux2TG2lK6m<4=Gt<P1?z`!^tJ`a2)raiFe5zQD7qy{3VzAHnorb^n@lpLfxm`
z(&p98mfHsd*}}WzPt%uq_qWIpmmVg!0Pc<l%RszICm`rV5dD_JHo6hi1gOUufr{L)
z`VLMl!>6uL*dI`TwE{p6PO0ELDV-^`V{kDZwap^J_k3pv0SW{kiHb_?#5q3DwSRC-
zu@kQ&l!;KI3XYH4$&>o2emD!U2?R3{B5XgY&6pLOE+q|(YeH^&AuOoZc5aKE*cANl
z&6|RF55RdEEzYcV@Tm`$IX>CZ>3_sXayP&PN30mDAig894K~|L){fmjutxdFI|5r4
z+`KG-B5uaXcKimVfvOvTXUKXO>dz{THgzn$V;mcWXV1lb%^+uEZ{q|hJ(2v^A?v$k
zZ>sFohe3t>TsABIrV$GD7fa=_@y2s_-0<vW%&YM_BW!zEz<>|$8fCCMhK0wcb6WsL
zO{WMKQM{;Fp4{t*bX#pMGLwLC^|woR*CKLt{sKWdX*&n<4El*&J&0o(G5pL1UIP7&
zFfp2U3O2of>qlwIZOczJMn{v~+s1ld;zRbuXEC-Q?qE3l`z;27fl<P?Bz7O&1L?#A
z@D&~rd8}Bv?I2xclq?<R8&NvwB0@nJFEDG9noic<%B67{M8v@r`aP0ddhrQb;Gm{^
zfEoo++1V1o6z4(o?<9`5A)}ZS0W<UzKpaKA<Zr34%kX~_anqpFm8QB;<2Dqxpuk)v
z<G0m*Nw=kMApfWT3gJJ4uCOCb&W(7U@@nmJneNs}J+hIicOnTQn476-8M)}8Qqn~@
z9y2|Kbrb~q1RMe-otVseWC#oSy@Je^xJb$0P(u5v97B+biZl}|5gx2KNw=(~IBar&
zZdr|Or(8o<HQ=v#kBr6O6gMVkD-uGmwZBdd6(+URPn4f1g~U$t#>DExZ6<Hf)Q~V=
GJN`e-A#CRW

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/hardware/dobot_api.py b/Massage/MassageControl/hardware/dobot_api.py
index 0e57f05..8d598c9 100644
--- a/Massage/MassageControl/hardware/dobot_api.py
+++ b/Massage/MassageControl/hardware/dobot_api.py
@@ -860,7 +860,7 @@ class DobotApiDashboard(DobotApi):
         if useJointNear != -1:
             params.append('useJointNear={:d}'.format(useJointNear))
         if JointNear != '':
-            params.append('JointNear={:s}'.format(JointNear))
+            params.append('jointNear={:s}'.format(JointNear))
         for ii in params:
             string = string + ','+ii
         string = string + ')'
diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index a51d21d..1941d3d 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -1,9 +1,14 @@
-from dobot_api import DobotApiFeedBack,DobotApiDashboard
+try:
+    from dobot_api import DobotApiFeedBack,DobotApiDashboard
+except:
+    from hardware.dobot_api import DobotApiFeedBack,DobotApiDashboard
 import threading
 from time import sleep,time
 import re
+import copy
 import numpy as np
 from scipy.spatial.transform import Rotation as R
+import math
 '''
 
     MODE    DESCRIPTION    CN
@@ -79,6 +84,7 @@ class dobot_nova5:
     def start(self):
         self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
         self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
+        self.dashboard.EmergencyStop(mode=0) # 松开急停
         self.clearError() # 清除报警
         if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
             print("使能失败:检查29999端口是否被占用")
@@ -118,7 +124,7 @@ class dobot_nova5:
             if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                 print("机械臂为暂停状态")
                 res = self.dashboard.Continue()
-                code = int(res[0])
+                code = self.parseResultId(res)[0]
                 if code == 0:
                     print("机械臂继续已暂停的运动指令")
             if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
@@ -149,12 +155,12 @@ class dobot_nova5:
             currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
             if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                 self.dashboard.Stop()
-                self.dashboard.EmergencyStop()
+                self.dashboard.EmergencyStop(mode=1)
                 print("当前工作停止，机械臂处于急停状态")
             if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                 print("机械臂为暂停状态")
                 res = self.dashboard.Continue()
-                code = int(res[0])
+                code = self.parseResultId(res)[0]
                 if code == 0:
                     print("机械臂继续已暂停的运动指令")
             if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
@@ -183,7 +189,7 @@ class dobot_nova5:
             if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                 print("机械臂为暂停状态")
                 res = self.dashboard.Continue()
-                code = int(res[0])
+                code = self.parseResultId(res)[0]
                 if code == 0:
                     print("机械臂继续已暂停的运动指令")
             if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
@@ -212,7 +218,7 @@ class dobot_nova5:
             if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                 print("机械臂为暂停状态")
                 res = self.dashboard.Continue()
-                code = int(res[0])
+                code = self.parseResultId(res)[0]
                 if code == 0:
                     print("机械臂继续已暂停的运动指令")
             if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
@@ -233,7 +239,7 @@ class dobot_nova5:
         if self.feedbackData.robotMode == 10: # 10 当前工程暂停
             print("机械臂为暂停状态")
             res = self.dashboard.Continue()
-            code = int(res[0])
+            code = self.parseResultId(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
         return 0
@@ -250,7 +256,7 @@ class dobot_nova5:
         if self.feedbackData.robotMode == 10: # 10 当前工程暂停
             print("机械臂为暂停状态")
             res = self.dashboard.Continue()
-            code = int(res[0])
+            code = self.parseResultId(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
 
@@ -268,7 +274,8 @@ class dobot_nova5:
             return
         
         res = self.dashboard.SetTool(i,tool_i)
-        code = int(res[0])
+        print(res)
+        code = self.parseResultId(res)[0]
         if code == 0:
             print(f"设置工具坐标系{i}的偏移量成功")
         else:
@@ -283,7 +290,8 @@ class dobot_nova5:
             print("工具坐标系序号超出范围")
             return
         res = self.dashboard.Tool(i)
-        code = int(res[0])
+        print(res)
+        code = self.parseResultId(res)[0]
         if code == 0:
             print(f"切换为工具坐标系{i}成功")
         else:
@@ -304,7 +312,7 @@ class dobot_nova5:
             return
         
         res = self.dashboard.SetUser(i,base_i)
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print(f"设置用户坐标系{i}成功")
         else:
@@ -319,7 +327,7 @@ class dobot_nova5:
             print("工具坐标系序号超出范围")
             return
         res = self.dashboard.User(i)
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print(f"切换为用户基坐标系{i}成功")
         else:
@@ -330,7 +338,8 @@ class dobot_nova5:
         if (user == -1) != (tool == -1): ## XOR 必须同时设置或者同时不设置
             print("必须同时设置或同时不设置坐标系参数")
         res = self.dashboard.GetPose(user,tool) # 返回字符串
-        code = int(res[0]) # 拿到的是字符串
+        # print("get pose",res)
+        code = self.parseResultId(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -346,7 +355,7 @@ class dobot_nova5:
     
     def getAngel(self):
         res = self.dashboard.GetAngle()
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -357,12 +366,13 @@ class dobot_nova5:
         start = res.find("{")+1
         end = res.find("}")
         angle_str = res[start:end]
+        print("res:",res)
         angle = [float(x.strip()) for x in angle_str.split(",")]
         return angle
 
     def getForwardKin(self,joints_list,user=-1,tool=-1):
         res = self.dashboard.PositiveKin(*joints_list,user,tool)
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -381,10 +391,11 @@ class dobot_nova5:
             poses_list    X Y Z RX RY RZ
             useJointNear 0 或不携带 表示 JointNear无效
                          1 表示根据JointNear就近选解
-            jointNear string "{j1,j2,j3,j4,j5,j6}"    
+            jointNear string "jointNear={j1,j2,j3,j4,j5,j6}"   
         '''
         res = self.dashboard.InverseKin(*poses_list,user,tool,useJointNear,JointNear)
-        code = int(res[0])
+        print(res)
+        code = self.parseResultId(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -400,7 +411,7 @@ class dobot_nova5:
     
     def disableRobot(self):
         res = self.dashboard.DisableRobot()
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print("下使能机械臂成功")
         else:
@@ -409,7 +420,7 @@ class dobot_nova5:
     
     def clearError(self):
         res = self.dashboard.ClearError()
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print("清楚报警成功")
         else:
@@ -418,7 +429,7 @@ class dobot_nova5:
 
     def start_drag(self):
         res = self.dashboard.StartDrag()
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print("拖拽模式开启成功")
         else:
@@ -427,7 +438,7 @@ class dobot_nova5:
     
     def stop_drag(self):
         res = self.dashboard.StopDrag()
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print("拖拽模式关闭成功")
         else:
@@ -436,7 +447,7 @@ class dobot_nova5:
     
     def stop_motion(self):
         res = self.dashboard.Stop()
-        code = int(res[0])
+        code = self.parseResultId(res)[0]
         if code == 0:
             print("机械臂停止下发运动指令队列")
         else:
@@ -457,6 +468,7 @@ class dobot_nova5:
         self.last_input_command = None
         self.tcp_offset = None
         self.init_J = [0,30,-120,0,90,0]
+        self.filter_matirx = np.zeros((8,7))
         sleep(1)
         self.is_standby = False
         code = self.RunPoint_P_inJoint(self.init_J)
@@ -470,24 +482,55 @@ class dobot_nova5:
     def get_arm_position(self):
         pose = self.getPose()
         x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
+        x /= 1000
+        y /= 1000
+        z /= 1000
         pos = np.array([x,y,z])
         quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat()
         return pos,quat_rot
 
-
+    def process_command(self,arm_position_command, arm_orientation_command):
+        rot_euler = R.from_quat(arm_orientation_command).as_euler('xyz',degrees=True) 
+        rot_euler = np.clip(rot_euler,-180,180)
+        arm_position_command *= 1000
+        pose_command = np.concatenate([arm_position_command, rot_euler])
+        print("pose_command",pose_command)
+        if (not self.is_exit) and (not self.is_standby):
+            cur = self.getAngel() # 参考关节角度
+            
+            target = self.getInverseKin(pose_command,-1,-1,1,"{" + ",".join(map(str, cur)) + "}")
+            # print("target",target)
+            self.joint_current = copy.deepcopy(cur)
+            self.joint_target = target
+            ress = np.array(target)
+            ress = np.append(ress,[1])
+            self.filter_matirx = np.append(self.filter_matirx,[ress],axis=0)
+            self.filter_matirx = np.delete(self.filter_matirx,0,axis=0)
+            sum_joint = np.sum(self.filter_matirx,axis=0)
+            sum_joint[:6] = sum_joint[:6]/sum_joint[6]
+            result = sum_joint[:6]
+            return 0, result
+        return -1, None
 
 
 if __name__ == "__main__":
+    # sleep(5)
     dobot = dobot_nova5("192.168.5.1")
     dobot.start()
-    
     posJ = [0,30,-120,0,90,0]
     dobot.RunPoint_P_inJoint(posJ)
     sleep(1)
+    dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
+    dobot.chooseEndEffector(i=9)
+    print(dobot.getPose())
+    # pos,rot_quat = dobot.get_arm_position()
+    # print("pos",pos)
+    # print("rot_quat",rot_quat)
     
-    for i in range(400):
-        posJ[2] += 0.04
-        dobot.ServoJoint(posJ,t=0.008,aheadtime=20,gain=200)
-        sleep(0.007)
+    sleep(0.5)
+    # for i in range(400):
+    #     posJ[2] += 0.04
+    #     dobot.ServoJoint(posJ,t=0.008,aheadtime=20,gain=200)
+    #     sleep(0.007)
     
     dobot.disableRobot()
\ No newline at end of file
diff --git a/Massage/MassageControl/hardware/force_sensor.py b/Massage/MassageControl/hardware/force_sensor.py
index e1df33a..bbf3d79 100644
--- a/Massage/MassageControl/hardware/force_sensor.py
+++ b/Massage/MassageControl/hardware/force_sensor.py
@@ -28,9 +28,6 @@ class XjcSensor:
         self._stop_background = False
         self._last_reading = None
         self._reading_lock = threading.Lock()
-        # 建立 TCP 连接
-        self.connect()
-        
         # 注册退出时的清理函数
         atexit.register(self.disconnect)
 
@@ -261,7 +258,7 @@ class XjcSensor:
         return (crc >> 8) & 0xFF, crc & 0xFF
 
     def __del__(self):
-        """析构时自动断开连接"""
+        """析构时断开连接"""
         self.disconnect()
 
     def disable_active_transmission(self) -> int:
@@ -544,7 +541,7 @@ def save_intervals_to_csv(timestamps, filename="sensor_intervals.csv"):
 if __name__ == "__main__":
     # 替换为你的传感器 IP 和端口
     sensor = XjcSensor("192.168.5.1", 60000)
-    
+    sensor.connect()
     # 示例操作
     
     sensor.disable_active_transmission()
diff --git a/Massage/MassageControl/tools/__pycache__/Rate.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/Rate.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4aa70509b87469b64ae1f956df95eda780df9529
GIT binary patch
literal 2573
zcmbuA-D@0G6u|Gj^Rcs&-OVOS4Yrj~zgU$<K`KaT3$1B|c99lD7G;>sy-Bv+o!Q)(
zRkQ4ZxOq?!QD2HQ!7L)0Lh+?YKy4rTANV-0=EJ<{Ltcsxo^vPrkxlB0Gu(5(&fI&>
z@1A?c&1P)^&pXS<=RbkgAJ`duH0Ydz5?%(u38xiON@`l77YS!vTOwR*vl0^;*KaU+
zi5uKpA|;)txCON#^fsYH&{b0yZPK9>R=GwB=^a>+6g`xKPjAO*7!{;MIVmwtIfJz{
zKE`!!EKyk9<SAU8TRaW5$!$IYb&6+r7HW&<_$btAo`=20WBsZd36BkIgl#P6p@fql
zIBn9Sq)8kS(N)&LmewTKp*T7xunH^Aff_u2oh=x#RrVtx-*+pq9+j&iPFDjzhyuUt
z#p#M0MvfY_!^o9MyTD>S6qVVS%{SJFMD7nSPR$2ZF?Bthok_$MHw@i5aW$9;BIg3i
zslB0YeLnCb8C0gCAgF{>=+?>FVr)51*)K<qW5BYZiB?h@-bPkwbZ4jcA<#h!i$YL@
zMnqO=hqM^D&6+5+2{J)iT2tfnhAz)FsXP_woIzi^rE`5nYr}EES4LCAgH66ia0-}F
zloVF2=Rj<)bhejQw{HHtbMx-&lMl6%ug>n`-jmxud|falHYM6F@%$w^5F0fKjAJ9L
z2vIBO(t^QKP7OM|F5Rdc__5{(AH+GO(i>E46ET9T<v}3s5Dg*JqIoDf9Y;R4;-kLz
z(Ae&SwxB7B4Tr`a8we7l15^~BCbqhK-i~x2!`iH+D<*oMN$b$>7LN6ZF-1&Eh|T@8
z_V@2sX5KoxM<&kHgq(G}ppG%8=qOIHRxSI=y@H|e$T6@qPKk<J3k8?s&@0&q91O=H
zFmbjfg;x%Rqf{OP?eK9BgjpCon#C}Lu$^Iz@g&p+KiCHhVPSayB|HP72MmJ=p(Vit
zfu(}!<w%FHVIBG=<daK~A$lm1;9&`hN5n?T4Htc{93;+bedm?W!+wq^?ZnGx&_0#)
zl0aa2+Q=*429yr<OohUU|L_d125rLe59SI&;AjquZmsHmSQs$8l#`<B!X@_SRO%S^
zA>EsP2|Y-zIEHc~r>yTyAF#cb<&F*7-VbAFIdCf1$QmtzMSWvapugWlZ_%rWN8Pj7
zPNp%^UO^l2!s2zasYx57%F4dRJ~Q#=VNr<DH3%0-x$-D93sl8ARJh(S7BdQ*P<XGw
zREUle+89wgCF8{coi5mz9(}he94EFNryB4&c;Unu$62Vmm1IU{aCJ--@(79v6i=c+
zZ1Ok?44VO=k_&E%$W(H%sAbq`Ys|7N!!jk#;of+zj^jbE_uv%f80GQ1(1_p%)TS-?
z`9KDt7sz|lEgHe^wMlQ06&QgF!nocft6GO}1G2BVmwg#e!Kl7}6h@2=AYI6-ykZr1
zaKjyRpq>S>`Q_(ZcfSAg{+*3qzMI^|ytw)4+Qx6+bQ82hC5hv^==sL_XB+DuZLHtk
zymx#1i=Q^X`f+Rd+paELQRS#1U4Kr<QT(zG4vDV8uTR3CDyHQjeDK>uHh7s&gJOt>
zfFYBiIeG+M`2zG01@{2eXFwbPclrN=>*k~$K6wIml23z3@csI6AD_gO(g!A=h3*~`
z`5aCS5mAx#JWllE2}3Ei6$_Py-hlo=7%#$TA(h<FCvXAeE}ueyoFvZ2nymW+E2}-!
Y^494j^3UK+>xL`KowAsfwk(tV3xy~hNdN!<

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/tools/__pycache__/log.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/log.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4613678dbbb1b36a46cc02b3dee241300c8a972b
GIT binary patch
literal 4288
zcmb_fTWlN072Vkfxui(F{C*`u0i|YJTkg9M($=mmIR+FOkc_%spjfOqE15L8OV2Lt
zNLT?*CE0Qu)k@+=UB>{GCVn)rB_xgE*e?9n&!Qjwn%|nF^p$@DMSJfo$;guWQ+Stq
z_s-6}Gk0e0<IqYb;}TrI`EbXHL!FZJFKjHobZG2_$NMJ`Dp8V`jPOki0&O`j7ZgJ&
zsD@h542@u)l8+Q3Mnv!=A2oEL7c*kw9XI0Q-D0%Z8da$_Lx^-xq7fRMlxTE9HWGG{
z>L+FP4?96)G!8N?Gs>l38Lf6}xs4{~Kpwbunyhmjw6)H4(zZIcj<zFr6f9OHRhm$U
z<g|ZZ^8U6*BH}&<l#(GS)v*LClhP@LtNkvsxi;jF=54NKovfcCT&I?A``LnhBt?vv
zRq{c`v;7PzEx%M~?1jfW4#bvla}1eo#05?gLkX8>s8px1Ny*UYCRkVt^hD@pnxIMG
zqI3&wrES3JbSrJA9l*utHrh$o0T-v+>3X^WxE5jd4xZRw@_e^2;Es&gEZu)3qruFL
zD+&ynv>zVtGa$ZHCX@u^hICV&BY|Yffs~WWiZA6<+^Impwy6c!7YS6LQP9z8C>_Jz
zT--YW-&*FRaV^lw3HTNhx;RoY&;&2DJAP}f4W9OLM*!MXq3VoGz=B+7AXAM-X5{fm
z5Lu-kL46v9Gto<b2a_w)H!HU<R^Gi@z4l4v>9xiC3zffKsa(Gk@2X!-rnhJ@tI(Bq
zM@+{m*u7n$MrC%Uc6*^(KEF7BZl!FIxkYOP&K89KFkhLy_3TM`>FoP;#p#9G^n=wC
z*5@fQJCpTn6WyYrUYVV*UA|MD|Fm}d{o143)oTxyuKlGt{Y7(LqctPW%LFUoz|671
z?wyvG!7)-^*RHPaoelFFP5ZQ+$K}-9!`ZwYHhV!f>)4K)qC&A?d!97{a^Ry}x8KTo
zzUiJE(@OsEuAhldPR6BKXC$3k$D=qu@H{qR`vbxj9tmyYI(YT~PRrFp>Hfn!*4!no
zH>~4{<;&+W2%3WB`!+L@C^Fn=a{Ywm(7eq&o*eR-Z57@K+dKiYG_`o#qO{Q?yB^mI
z)=8UYK}{`M{s|rh9+%Bq`e6g=lBp<HJv%?l+Q3gj+i-hWD>PX<5Th;QI-Z-i&AJsv
z0tFhayAs<0QqTK)PPhfTC+7_x56PRB*vdioxa*q-koMF+HQxJO$7gQ7$9LVl*8@x5
zT^!}QX=1)GO*Fv!77!`gNm_|cA|xWKME*}i)1j3h30ajlLQjH>ZCD<_yBi~92x`|a
z<na+<jPV>u5FVto8Q5nJ-+A`%V)g3z+WooO>^TUG%G4*7OMen8OA)pWWCS0QSD?vb
zrfEBHR;KwqXnHabNgms{!jQU11}AJ>m|U6(Wj8b+_@x}A6iTM#Ez$w$x0}ZyY2>7`
z9H=4@sAXjij6c;b@|9frHYlyOdFkxg%K1kSF4c!qu!_YmC##QU(<zm;fH;GaB=ID(
z3-0O0(Q_@6cRf3$i3Os&XH0*zX!96yr>%SmSf5!L`?!@k$)ol2U%3--Vh|{BVtZf;
zF9Jl8*RORdp;SgeiTZ^+YKT+8qX?w36cC@}WM9dtkn~h6enZk-l1^z{#gJz)kYh<C
zxa)>?aImj0eOC!r5v_+X*R!5kWLa=A^v`7{C*m*b47)#t@8Q9%^dJmBl=%N_WjkSD
z*wqxy`=YTta;$G);P6rQou*B~I^j}roOaaC=iM{C%S_w!f=_)T^U{qo)cwcyrPnk~
z?AdW}h@USr?8T@<!%*TSnZ=_=_YI~Gr4NcjW*8@HTGoQ|p0St%iFofa<2J9_wYt#D
zvQO(996UT2x}Y-HcVJEHVrZSsn9GERi!4D_WQnH83$O!&7E_?m+X$HpVTXVV01?<u
z01XNy06(;xO69Ep6>q4Ur16Lo7d?<S#v{H~j?76^+aire{b)`v$AV}s4m~=&TgGF4
z0wOKxxB6{<d#+<xosil6vDf_0+&a+%-zH=)2#6C2NO?Vs1i%si2RSO8dZ0@F27lwQ
zJo@Jl1p1o-GC{`4G3eVPIeMcVsO-7Fxx5A8%Xr-18boeLREO-gO}yiR>Of;bOz_GL
zi2oaszdg4D09JAW0hN?{CHHC|2ZY8^GfNQ0LS9Hf22YkYz~)rWO)X8_sY8yBCn5b+
z%MUA0C#qM!s(x{$i5!|t9e&hvMHA(mo2uOZ>gy-d)ywnMci*nuyH@+`*0YD7_W)O!
zde~FFvQT?GS-W@P>n9gh4wc7Z-xvsBOJ({~7^rgVcZj7BCG18Vu!dsB&AY6(OT4Ut
z)wT4|hl>|Jul^oJcmUI{oTri18fd9{;e*P|LS=d()TmrqXljggb|2f-UD(x4ySm@#
z?S8YjduVrd5mK(9?D-)b>sZyhg#OtNFehO<UWwTzpsH+kOaW;0ob(EW0G*os78Hea
zs*7PI<BH>+5kLVjkI#k?h4Gd`Hp8H_0CJ!_nsvOQo$<Nm`A|pLM$lrI>$!g1@&pj$
zdIORHfD(9&$Fm;j`IeJ`e-Jba>S`X)7}xw#5&l%jPLW3)YB6Sway93<4%cX@Q1p1j
zcZFc=2$Zru3_TwIHP*6_SHMV-lz9SC8=hJkF_9M53PeE+wn#E!gMCANN6dW#1EPR(
z1<o<m5@LbxqV<|MawwvBfDBQ9k;s%7gPJe!tkK#)a8$H;B4gzNS((Ftux<8BoM|s=
z#l=7ZZ?C9TqJ;E=jQ1K4DTW|us~lGn0DB_JdLomp0EpspJ8747MI~|a6J@O9h0KQ)
zGo_~CUrmav`Xvx<AtW?S9yiT`OG|+4Or9{!Qzc74X47P^!qlt_2?7tc3rP<WEcomP
zNPdJw)VptrZL_d3tngT`Jp>^-iNB(&dNi&yZqbjH6s)XcnxzCtRlmHrIDaJ!=&^0E
z2Csn{LqO@cr>!6F#<eNJFWGib5^g9EUG@6?rL%v0_H?fL_B@1P?eSBHDF{9>_R`}?
zjIc|ywT~Z(4~rk%#19XqL0YIU-n&(K@13T4sDSF|wo8|<FV4?5J8O?V3dc^ZX9v*;
ztoVlBi080;p+_+UePPUK4!G(l>w^zm@kTxNI(CV`z`bHaNCa&ApPRkLK6Cx`@C1K>
hK-WWvfk+v#7{1|F;&Q90%Q83)-2aZKGPq25{}%$C$?yOG

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/tools/__pycache__/yaml_operator.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/yaml_operator.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..269eb5e4382449f6434ee4ce5943792186f4be8d
GIT binary patch
literal 1678
zcmb`I&rcIU6vt<Fx3Dc%k*EhFrUwo*QK~mW&}cB6gb+1RdTEx<7K+<$Gdl}uQWE4>
zJxI7931AEuH6bcsVnE`r|6{MPrThz=d^1bSR?w5v%$u2=eeX>_-#4um4u=r5C$E}^
znga;!@`b-iu($)=x(0?If(;Z$L?ZGOiesXX0E`l;BlR%KL?uBO6@nk4Xy^buP;V0R
z;i$<ls?fWjTb*DSDnLvgZ&seWJcb!wkqUSfk4cXaQ52-0Ng1PT<4t6>-9T7;`oR_?
z$Plo%Aw(zwt-SHgom+_X-|e{{DJ>NDzHPZXTjgK#?(*wMU!Dz_*@(NbQ~tcVJ3bK-
z9`4*k>E&jJn5WW)p2%rzDAH;%8tw4MDQ|DMKc}j0UQEx8XiST=l5~>Qy0|mb?)pyo
z$H&s_oBgG!lLz1@b}&5**erFueGNORo=uu0ogM6r%8p{@^sJ-gwTuxDT3Sj^7^X&G
zBUnABqwuMt@Dr>d+5i|1`(i_8Mvo0!sR7UQXqKf7>JQ8TlO^tP8#{Vb8N19ZqoxsK
zrfFC)o-hHqsm4s&mdiUqs%s>{A9LNTIxr~Ih+8oY!z{^5qta@G?S`iyEUpUMGA1!R
zEEPaDUNX?@W&;?3#Pu~2xBDd~^&qiouR$<Ru+5oTlJ3l+JN?eDRaGUwxHT%3-#<I)
z_E(LnyeoCFzr0>rDf&|$mxm|nDCH=q0;c;>RE{?hJ*C+OOQZ~QG-0v4p~u6T!54Nm
z4WjE#&@w4YJk|3Ss82a%S`X$(dtzsD@R;9H%C8O$a>Et(%o&(Dp;4M<x{&HTtSz1%
zsg0O6!kFYIsx-dC*Im%9t6=yIoWx^tK`O`v+=&>55lu=M?j;By*&pB~AwX&QOKD-#
z|K<0-egbx-7e#k{>0o}{9sdrO>{Jm>ko4WuT7<x%LL0>LazYXa&jWMFBbs6B9TER7
zWUlP?u1TUi#aE9if|(;jR3{)DJ(7R$Yp`=rCLX{moQ$aTpo&#YFTnaltydAY93zAu
zu7Wgz=p{gjZ*h=N@jj5n3P|_)%E98>e}Me22)%5z2zx0n6DTWDQB5#E70WrI8#-VC
zdZM<X@S??Ckjl~2VR{kf(SWGeW9kB#*z(n>ATEkk?aY|OHuT&4pFxCECH1V@poV_~
D-7Wy~

literal 0
HcmV?d00001

diff --git a/Massage/MassageControl/tools/identifier_dobot.py b/Massage/MassageControl/tools/identifier_dobot.py
index f5592fe..3e48fef 100644
--- a/Massage/MassageControl/tools/identifier_dobot.py
+++ b/Massage/MassageControl/tools/identifier_dobot.py
@@ -97,6 +97,7 @@ class Identifier:
         pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]])
         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)
@@ -111,7 +112,7 @@ class Identifier:
                                 roll=desired_cart_pose[3],pitch=desired_cart_pose[4],yaw=desired_cart_pose[5])
         # self.sensor.set_zero()
         time.sleep(1)
-        delta = 60/cnt*6
+        delta = 50/cnt*6
         quarter = cnt // 6
         for i in range(cnt):
             if i % 2 == 0:
@@ -120,19 +121,21 @@ class Identifier:
                 control_pose[4]+= delta *  np.where((i <= quarter) | (3* quarter< i <= 5*quarter), -1, 1)
             desired_cart_pose = control_pose.copy()
             print("desired_cart_pose:",desired_cart_pose)
-            self.set_position(x=desired_cart_pose[0],
-                                y=desired_cart_pose[1],
-                                z=desired_cart_pose[2],
-                                roll=desired_cart_pose[3],
-                                pitch=desired_cart_pose[4],
-                                yaw=desired_cart_pose[5])
+            self.set_position(
+                            x=desired_cart_pose[0],
+                            y=desired_cart_pose[1],
+                            z=desired_cart_pose[2],
+                            roll=desired_cart_pose[3],
+                            pitch=desired_cart_pose[4],
+                            yaw=desired_cart_pose[5]
+                            )
             print("move done")
             time.sleep(1)
             wrench = self.__read_data_once(sample_num)
             print(f"wrench:{wrench}")
             print("read done")
             cur_pose = self.arm.getPose()
-            b_R_e = R.from_euler('xyz', cur_pose[3:], degrees=False).as_matrix()
+            b_R_e = R.from_euler('xyz', cur_pose[3:], degrees=True).as_matrix()
             self.__add_data(wrench, b_R_e)
             print(f"=======添加了第{i+1}组数据=======")
             time.sleep(0.5)
@@ -157,7 +160,9 @@ class Identifier:
                 with open(self.config_path, 'w') as file:
                     pass
             
-            update_yaml(self.config_path, dict_data)
+            # 逐个更新键值对
+            for key, value in dict_data.items():
+                update_yaml(self.config_path, key, value)
             print("更新操作已完成。")
         else:
             print("更新操作已取消。")
@@ -165,17 +170,19 @@ class Identifier:
         
         
 if __name__ == '__main__':
-    arm = dobot_nova5()
-    arm.setEndEffector(i=9,tool_i="x,y,z,rx,ry,rz")
+    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.init()
-    sensor = XjcSensor()
+    sensor = XjcSensor("192.168.5.1", 60000)
     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_ws/global_config/massage_head/none_playload.yaml")
-    ready_pose = [-1,-1,-1,-1,-1,-1]
-    time.sleep(1)
-    identifier.identify_param_auto(ready_pose,45)
\ No newline at end of file
+    # identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/massage_head/finger_playload.yaml")
+    # ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
+    # time.sleep(1)
+    # identifier.identify_param_auto(ready_pose,45)
+    arm.disableRobot()
\ No newline at end of file
diff --git a/logs/MassageRobot_nova5_test.log b/logs/MassageRobot_nova5_test.log
new file mode 100644
index 0000000..e69de29