diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py
index 24e0b38..6deced9 100644
--- a/Massage/MassageControl/MassageRobot_nova5.py
+++ b/Massage/MassageControl/MassageRobot_nova5.py
@@ -247,7 +247,9 @@ class MassageRobot:
         return x_update, P_update
     def init_hardwares(self,ready_pose):
         self.ready_pose = np.array(ready_pose)
-        self.switch_payload(self.current_head)
+        self.arm_state.desired_position = self.ready_pose[:3]
+        euler_angles = self.ready_pose[3:]
+        self.arm_state.desired_orientation = R.from_euler('xyz',euler_angles).as_quat()
         print(self.arm.get_arm_position())
         time.sleep(0.5)
         
@@ -551,12 +553,15 @@ class MassageRobot:
                     if sleep_duration > 0:
                         time.sleep(sleep_duration)
                     # print(f"real sleep:{time.time()-b2}")
-
+                    
+                    # print("self111:",self.arm.feedbackData.robotMode)
                     self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                    # print("self111222:",self.arm.feedbackData.robotMode)
                     if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState: 
                         self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode}")
                         self.arm.dashboard.Stop()
                         self.arm.dashboard.EmergencyStop(mode=1)
+                        self.stop()
                         return -1 
                     if self.arm.feedbackData.EnableState and self.arm.feedbackData.robotMode == 10:
                         if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
@@ -591,11 +596,12 @@ class MassageRobot:
             self.arm_state.arm_orientation = quat_rot
             for i in range(20):
                 self.controller_manager.step(self.control_rate.to_sec())
-                self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
-                self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
+                # self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
+                # self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
                 position, quat_rot = self.arm.get_arm_position()
-                self.logger.log_blue(f"position current:{position}")
-                self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
+                # self.logger.log_blue(f"position current:{position}")
+                # self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
+                print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode)
                 self.command_rate.precise_sleep()
                 
             position ,quat_rot = self.arm.get_arm_position()
@@ -1613,6 +1619,26 @@ class MassageRobot:
             'wrench': wrench_traj
         }
         return traj
+
+    # 工具函数
+    ############################################################################################################
+    def convert_to_7d(self,matrix):
+        matrix = np.array(matrix)  
+        positions = matrix[:, :3]  
+        rpy_angles = matrix[:, 3:]  
+        # 将RPY角度转换为四元数
+        quaternions = R.from_euler('xyz', rpy_angles).as_quat()
+        # 将位置和四元数组合成一个新的矩阵
+        result_matrix = np.hstack([positions, quaternions])
+        # 加入当前位置
+        current_quat = self.arm_state.arm_orientation
+        current_position  = self.arm_state.arm_position
+        if quaternions[0].dot(current_quat) < 0:
+            current_quat = -current_quat
+        current_pose = np.hstack([current_position,current_quat])
+        result_matrix = np.vstack([current_pose,result_matrix])
+        return result_matrix
+
     
 
 
@@ -1645,7 +1671,7 @@ if __name__ == "__main__":
     # robot.dt = ts[1] - ts[0]
 
     
-    ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
+    ready_pose = [0.1543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
     robot.current_head = 'finger_head'
 
     robot.force_sensor.disable_active_transmission()
@@ -1660,7 +1686,13 @@ if __name__ == "__main__":
     
     robot.init_hardwares(ready_pose)
     time.sleep(3)
+    robot.switch_payload("none")
     robot.controller_manager.switch_controller('admittance')
+    position,quat_rot = robot.arm.get_arm_position()
+    euler_rot = R.from_quat(quat_rot).as_euler('xyz')
+    target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]]
+    robot.move_to_point(pose=target_point,t=60)
+
     # robot.controller_manager.switch_controller('position')
     
     
diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py
index 05952f7..f6a218c 100644
--- a/Massage/MassageControl/algorithms/admittance_controller.py
+++ b/Massage/MassageControl/algorithms/admittance_controller.py
@@ -40,42 +40,100 @@ class AdmittanceController(BaseController):
 
         self.laset_print_time = 0
         
+    # def step(self,dt):
+    #     # 方向统一
+    #     if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+    #         self.state.arm_orientation = -self.state.arm_orientation
+    #     # 缓存常用计算
+    #     arm_ori_quat = R.from_quat(self.state.arm_orientation)
+    #     arm_ori_mat = arm_ori_quat.as_matrix()
+    #     # 位置误差
+    #     temp_pose_error = self.state.arm_position - self.state.desired_position
+    #     self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+    #     # 姿态误差(四元数)
+    #     rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+    #     self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+    #     # 期望加速度
+    #     wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+    #     D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist)
+    #     K_pose = self.K @ self.state.pose_error
+    #     self.state.arm_desired_acc = self.M_inv @ (wrench_err - D_vel - K_pose)
+    #     self.clip_command(self.state.arm_desired_acc, "acc")
+    #     ## 更新速度和位姿
+    #     self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+    #     self.clip_command(self.state.arm_desired_twist, "vel")
+    #     # 计算位姿变化
+    #     delta_pose = np.concatenate([
+    #         arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+    #         self.state.arm_desired_twist[3:] * dt
+    #     ])
+    #     self.clip_command(delta_pose, "pose")
+    #     # 更新四元数
+    #     delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+    #     arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+    #     self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+    #     # 归一化四元数
+    #     self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+    #     # 更新位置
+    #     self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
     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
-        # 缓存常用计算
-        arm_ori_quat = R.from_quat(self.state.arm_orientation)
-        arm_ori_mat = arm_ori_quat.as_matrix()
-        # 位置误差
-        temp_pose_error = self.state.arm_position - self.state.desired_position
-        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
-        # 姿态误差(四元数)
-        rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
-        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
-        # 期望加速度
+
+        self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error
+        # if time.time() - self.laset_print_time > 5:
+        #     print("pose_error:",self.state.pose_error[:3])
+        # 计算误差 位置直接作差,姿态误差以旋转向量表示
+        #rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T
+        rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
+        # print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}')
+        rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
+        self.state.pose_error[3:] = -rot_err_rotvex
+
+        #wrench_err = self.state.external_wrench_base - self.state.desired_wrench
         wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
-        D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist)
-        K_pose = self.K @ self.state.pose_error
-        self.state.arm_desired_acc = self.M_inv @ (wrench_err - D_vel - K_pose)
-        self.clip_command(self.state.arm_desired_acc, "acc")
-        ## 更新速度和位姿
-        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
-        self.clip_command(self.state.arm_desired_twist, "vel")
-        # 计算位姿变化
-        delta_pose = np.concatenate([
-            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
-            self.state.arm_desired_twist[3:] * dt
-        ])
-        self.clip_command(delta_pose, "pose")
-        # 更新四元数
-        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
-        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
-        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
-        # 归一化四元数
-        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
-        # 更新位置
+        if time.time() - self.laset_print_time > 5:
+            print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}')
+        self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
+        # if time.time() - self.laset_print_time > 5:
+        #     print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
+        self.clip_command(self.state.arm_desired_acc,"acc")
+        self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist
+        self.clip_command(self.state.arm_desired_twist,"vel")
+        delta_pose = self.state.arm_desired_twist * dt
+
+        delta_pose[:3] = self.pos_scale_factor * delta_pose[:3]
+        delta_pose[3:] = self.rot_scale_factor * delta_pose[3:]
+        # if time.time() - self.laset_print_time > 5:
+        #     print("delta_pose:",delta_pose)
+        
+        delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3]
+
+        # if time.time() - self.laset_print_time > 5:
+        #     print("tf_delta_pose:",delta_pose)
+        self.clip_command(delta_pose,"pose")
+
+        # testlsy
+        delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
+
+        #arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
+        arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat 
+        self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
+
+        # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:]
+        # self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat()
+        
+        # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
         self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+
+        # if time.time() - self.laset_print_time > 1:
+        #     print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_position)
         
     def step_traj(self,dt):
         # 方向统一
diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index b5b72dc..4a2b88e 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -150,6 +150,10 @@ class dobot_nova5:
         atexit.register(self.exit_task)
         self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
         self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180]
+        self.is_exit = False
+        self.is_standby = False
+        self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
+        self.filter_matrix = np.zeros((1,6)) # 角度伺服用
 
     # 为状态管理而封装的初始化函数
     def init(self):
@@ -164,6 +168,8 @@ class dobot_nova5:
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
         self.is_standby = False
+        self.move_joint_jog(q=self.init_pos)
+        time.sleep(0.5)
         code = self.move_joint(self.init_pos)
 
         if code == 0:
@@ -275,14 +281,16 @@ class dobot_nova5:
     
     def waitArrival(self, command):
         sendCommandID = self.parseResultId(command)[1]
+        # print("self.parseResultId(command)",self.parseResultId(command))
         while True:
-            if self.feedbackData.robotMode in [3, 4, 6, 8, 9, 10, 11]: 
+            if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]: 
                 self.logger.log_error(f"机械臂异常:{self.feedbackData.robotMode}")
                 print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
                 self.dashboard.Stop()
                 self.dashboard.EmergencyStop(mode=1)
                 return -1 
             if self.feedbackData.EnableState:
+                print("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID)
                 if self.feedbackData.robotCurrentCommandID > sendCommandID:
                     break
                 else:
@@ -314,7 +322,32 @@ class dobot_nova5:
                 return -1
         print(f'After {cnt} retries, still failed to arrive at the joint position: {q}')
         return -1
-    
+
+    def move_joint_jog(self,q,**params):
+        '''
+            move_joint_jog 以JOINT jOG运动方式运动到目标点
+            MovJ flag = 1 通过关节角度走点,输入为[J1 J2 J3 J4 J5 J6]  unit: rad
+        '''
+        threshold = params.get('threshold', 10)
+        currentAngle = self.getAngel()
+        targetAngle = self.__transform_rad_2_deg(q)
+        currentAngle = np.array(currentAngle)
+        targetAngle = np.array(targetAngle)
+        self.dashboard.SpeedFactor(70)
+
+        for i in range(len(targetAngle)):
+            while np.abs(currentAngle[i] - targetAngle[i]) >threshold:
+                if currentAngle[i] < targetAngle[i]:
+                    self.dashboard.MoveJog(f"J{i+1}+")
+                else:
+                    self.dashboard.MoveJog(f"J{i+1}-")
+                time.sleep(0.5)
+
+                currentAngle = self.getAngel()
+                currentAngle = np.array(currentAngle)
+            self.dashboard.MoveJog("")
+        # self.dashboard.SpeedFactor(95)
+
     def RunPoint_P_inPose_M_RAD(self,pose,**params):
         '''
             RUNPOINT_P 以关节运动方式运动到目标点
@@ -810,16 +843,31 @@ class dobot_nova5:
 
 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("Arm pose:",dobot.getPose())
+    dobot.init()
 
-    # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
+    # dobot.move_joint_jog(q=dobot.init_pos)
 
+    # dobot.dashboard.MoveJog("J1+")
+    # time.sleep(1)
+    # dobot.dashboard.MoveJog("")
+    # print(dobot.getAngel())
+    # # while True:
+    # #     time.sleep(1)
+
+    # # dobot.start()
+    # posJ = [0,30,-120,0,90,0]
     # dobot.start_drag()
-    dobot.disableRobot()
\ No newline at end of file
+    # time.sleep(60)
+    # dobot.stop_drag()
+    # # 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("Arm pose:",dobot.getPose())
+
+    # # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
+
+    # # dobot.start_drag()
+    # dobot.disableRobot()
\ No newline at end of file