暂存

Massage/MassageControl/MassageRobot_nova5.py

@@ -570,6 +570,7 @@ class MassageRobot:
                         time.sleep(sleep_duration)
                     print(f"real sleep:{time.time()-b2}")
                     self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                    print("currentCommandID",self.arm.feedbackData.robotCurrentCommandID)
                     if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                         print("机械臂为暂停状态")
                         res = self.arm.dashboard.Continue()
@@ -589,7 +590,7 @@ 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_traj)
+            self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -1608,7 +1609,7 @@ if __name__ == "__main__":
     # robot.dt = ts[1] - ts[0]
 
     
-    ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
+    ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
     robot.current_head = 'finger_head'
 
     robot.force_sensor.disable_active_transmission()
@@ -1629,7 +1630,7 @@ if __name__ == "__main__":
     
     
     atexit.register(robot.force_sensor.disconnect)
-    robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0])
+    robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
     try:
         robot_thread = threading.Thread(target=robot.start)
         robot_thread.start()    

Massage/MassageControl/hardware/dobot_nova5.py

@@ -118,7 +118,7 @@ class dobot_nova5:
         self.dashboard.EmergencyStop(mode=0) # 松开急停
         self.clearError() # 清除报警
         # 关闭碰撞检测
-        self.dashboard.SetCollisionLevel(level=1)
+        self.dashboard.SetCollisionLevel(level=0)
         if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
             print("使能失败:检查29999端口是否被占用")
             return
@@ -737,11 +737,13 @@ class dobot_nova5:
         self.last_input_command = None
         self.tcp_offset = None
         self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
+        self.init_P = [654.3467,-134.9880,305.3604,-180.0000,-30.0000,0.0042]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
         self.is_standby = False
-        code = self.RunPoint_P_inJoint(self.init_J)
+        # code = self.RunPoint_P_inJoint(self.init_J)
+        code = self.RunPoint_P_inPose(self.init_P)
         if code == 0:
             print("机械臂初始化成功且到达初始位置")
         else:
@@ -881,7 +883,7 @@ if __name__ == "__main__":
     
     # print(dobot.get_arm_position())
 
-    # # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
+    dobot.RunPoint_L_inPose([179.087702,-136.993776,425.209977,179.418397,-28.9718402,0.06])
     # cur_pose = dobot.getPose(user=1,tool=8)
     # cur_pose[0] += 30
     # target_pose = cur_pose
@@ -890,5 +892,5 @@ if __name__ == "__main__":
     # dobot.ServoPose(dobot.getPose(user=1,tool=8))
     # print("new ready joint",dobot.feedbackData.QActual)
     # # dobot.start_drag()
-    dobot.stop_drag()
+    # dobot.stop_drag()
     dobot.disableRobot()