stash

Massage/MassageControl/MassageRobot_nova5.py

@@ -536,6 +536,8 @@ class MassageRobot:
     
     def arm_command_loop(self):
         self.logger.log_info("机械臂控制线程启动")
+        self.arm.filter_matirx = np.zeros((1,7)) # 位姿伺服用
+        # self.arm.filter_matrix = np.zeros((1,6)) # 角度伺服用
         while (not self.arm.is_exit) and (not self.exit_event.is_set()):
             try:
                 if not self.is_waitting:
@@ -561,7 +563,7 @@ class MassageRobot:
                     pose_processed[:3] = temp_pose[:3] - target_matrix @ np.array(self.arm.tcp_offset[:3])*1000
                     pose_processed[3:] = R.from_matrix(target_matrix).as_euler("xyz",degrees = True)
                     
-                    # print(f"pose_processed:{pose_processed}")  
+                    print(f"pose_processed:{pose_processed},{self.arm_state.arm_position_command * 1000}")  
                     
                     tcp_command = (
                     f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
@@ -578,7 +580,30 @@ class MassageRobot:
                     # print(f"real sleep:{time.time()-b2}")
                     
                     # print("self111:",self.arm.feedbackData.robotMode,tcp_command)
-                    self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                    retry_count = 0
+                    while retry_count < 3:
+                        if self.arm.is_exit or self.exit_event.is_set():
+                            break
+                        try:
+                            self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                            break
+                        except Exception as e:
+                            print(f"发送指令错误：{e}")
+                            # 你可以选择在这里close socket，也可以让reConnect里自动处理
+                            self.arm.dashboard.socket_dobot = self.arm.dashboard.reConnectWithRetry(self.arm.dashboard.ip, self.arm.dashboard.port, 2, 0.1)
+                            retry_count += 1
+                    else:
+                        # 只有没break才会走这里，重新raise最后的异常
+                        raise Exception(f"sendall连续{retry_count}次都失败，机械臂指令发送失败！")
+
+                    # # ⬇️ 将 ToolVectorActual 写入 txt 文件
+                    # try:
+                    #     with open("tool_vector_log.txt", "a") as f:
+                    #         f.write(",".join([str(x) for x in self.arm_state.arm_position_command]) + "\n")
+                    # except Exception as e:
+                    #     print("写入 ToolVectorActual 失败:", e)
+                    # # 自定义添加所需反馈数据
+
                     # print("self111222:",self.arm.feedbackData.robotMode,tcp_command)
                     if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState: 
                         self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode}")
@@ -607,6 +632,9 @@ class MassageRobot:
             self.exit_event.clear()
             self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
             self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
+
+            self.controller_manager.switch_controller('admittance')
+
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -619,11 +647,11 @@ 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()
                 

Massage/MassageControl/algorithms/admittance_controller.py

@@ -81,6 +81,8 @@ class AdmittanceController(BaseController):
         # 更新位置
         self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
 
+        # print("currentAdmit:",self.state.arm_position_command,self.state.arm_position,self.state.arm_orientation_command,self.state.arm_orientation)
+
         # if time.time() - self.laset_print_time > 1:
         #     print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_desired_acc)
 

Massage/MassageControl/hardware/dobot_api.py

@@ -242,6 +242,20 @@ class DobotApi:
             except Exception:
                 sleep(1)
         return socket_dobot
+    
+    def reConnectWithRetry(self, ip, port, max_retries=5, retry_interval=1):
+        last_exception = None
+        for attempt in range(1, max_retries + 1):
+            try:
+                socket_dobot = socket.socket()
+                socket_dobot.connect((ip, port))
+                return socket_dobot
+            except Exception as e:
+                last_exception = e
+                print(f"第{attempt}次重连失败：{e}")
+                sleep(retry_interval)
+        # 如果走到这里，说明所有尝试都失败
+        raise Exception(f"reConnect在{max_retries}次后仍然失败，最后一次异常：{last_exception}")
 
 # 控制及运动指令接口类
 # Control and motion command interface

Massage/MassageControl/hardware/dobot_nova5.py

@@ -160,7 +160,7 @@ class dobot_nova5:
         self.is_exit = False
         self.is_standby = False
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
-        self.filter_matrix = np.zeros((1,6)) # 角度伺服用
+        # self.filter_matrix = np.zeros((1,6)) # 角度伺服用
 
 
     # 为状态管理而封装的初始化函数
@@ -173,7 +173,7 @@ class dobot_nova5:
 
 
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
-        self.filter_matrix = np.zeros((1,6)) # 角度伺服用
+        # self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
         self.is_standby = False
         self.move_joint_jog_range(q=np.zeros(6))
@@ -262,6 +262,12 @@ class dobot_nova5:
                         self.feedbackData.QActual = feedInfo['QActual'][0]
                         self.feedbackData.ActualQuaternion = feedInfo['ActualQuaternion'][0]
                         self.feedbackData.ToolVectorActual = feedInfo['ToolVectorActual'][0]
+                        # ⬇️ 将 ToolVectorActual 写入 txt 文件
+                        # try:
+                        #     with open("tool_vector_log.txt", "a") as f:
+                        #         f.write(",".join([str(x) for x in self.feedbackData.ToolVectorActual]) + "\n")
+                        # except Exception as e:
+                        #     print("写入 ToolVectorActual 失败:", e)
                         # 自定义添加所需反馈数据
                         '''
                         self.feedData.DigitalOutputs = int(feedInfo['DigitalOutputs'][0])

Massage/test.py

@@ -0,0 +1,26 @@
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+def plot_tool_vectors(filename="tool_vector_log.txt"):
+    xs, ys, zs = [], [], []
+
+    with open(filename, "r") as f:
+        for line in f:
+            if line.strip() == "":
+                continue
+            # 提取每行的6个值
+            parts = line.strip().replace("x:", "").replace("y:", "").replace("z:", "").replace("rx:", "").replace("ry:", "").replace("rz:", "").split(",")
+            values = [float(p.strip()) for p in parts]
+            xs.append(values[0])
+            ys.append(values[1])
+            zs.append(values[2])
+
+    # 绘制 3D 轨迹
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+    ax.plot(xs, ys, zs, marker='o')
+    ax.set_xlabel('X')
+    ax.set_ylabel('Y')
+    ax.set_zlabel('Z')
+    ax.set_title('Tool Vector Position Trajectory')
+    plt.show()