Massage/MassageControl/MassageRobot_nova5.py

@@ -1,24 +1,24 @@
 from functools import wraps
-	import sys
-	import numpy as np
-	import threading
-	import time
-	from typing import List, Literal, Union
+import sys
+import numpy as np
+import threading
+import time
+from typing import List, Literal, Union
 
-	from scipy.interpolate import interp1d
+from scipy.interpolate import interp1d
 
-	import requests
-	import os
-	from scipy.spatial.transform import Rotation as R
-	import ctypes
-	libc = ctypes.CDLL("libc.so.6")
-	import atexit
-	import copy
+import requests
+import os
+from scipy.spatial.transform import Rotation as R
+import ctypes
+libc = ctypes.CDLL("libc.so.6")
+import atexit
+import copy
 
-	from scipy.spatial.transform import Slerp
+from scipy.spatial.transform import Slerp
 
 
-	try:
+try:
     # 导入算法
     from .algorithms.arm_state import ArmState
     from .algorithms.controller_manager import ControllerManager
@@ -44,7 +44,7 @@ from functools import wraps
     from .tools.decorator_tools import custom_decorator,apply_decorators
     from .tools.serial_tools import find_serial_by_location
     from .tools.Trajectory import TrajectoryInterpolator
-	except:
+except:
     #导入算法
     from algorithms.arm_state import ArmState
     from algorithms.controller_manager import ControllerManager
@@ -71,22 +71,22 @@ from functools import wraps
     from tools.serial_tools import find_serial_by_location
     from tools.Trajectory import TrajectoryInterpolator
 
-	import matplotlib.pyplot as plt
-	from matplotlib.animation import FuncAnimation 
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation 
 
-	current_file_path = os.path.abspath(__file__)
-	MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
-	print(MassageRobot_Dobot_Path)
-	# 添加目标文件夹到系统路径
-	sys.path.append(MassageRobot_Dobot_Path)
-	from VortXDB.client import VTXClient
+current_file_path = os.path.abspath(__file__)
+MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
+print(MassageRobot_Dobot_Path)
+# 添加目标文件夹到系统路径
+sys.path.append(MassageRobot_Dobot_Path)
+from VortXDB.client import VTXClient
 
-	"""
-	在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。
+"""
+在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。
 当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。
-	"""
+"""
 
-	def track_function(function_name,log = False):
+def track_function(function_name,log = False):
     def before(self, *args, **kwargs):
         self.current_function = function_name
         if log:
@@ -97,8 +97,8 @@ from functools import wraps
             self.logger.log_info(f"Exiting function: {function_name},code: {result}")
     return custom_decorator(before=before, after=after)
 
-	@apply_decorators
-	class MassageRobot:
+@apply_decorators
+class MassageRobot:
     def __init__(self,arm_config_path,is_log=False):
         self.logger = CustomLogger()
         if is_log:
@@ -1567,7 +1567,7 @@ from functools import wraps
     
 
 
-	if __name__ == "__main__":
+if __name__ == "__main__":
     waypoints = [
         {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
          "acceleration": [0, 0, 0],
@@ -1628,3 +1628,4 @@ from functools import wraps
         print("发生异常：", e)
     
     
+    # robot.arm.disableRobot()

Massage/MassageControl/config/admittance.yaml

@@ -8,10 +8,10 @@ mass_rot: [0.11, 0.11, 0.11]
 # P
 # stiff_tran: [300, 300, 300]
 # stiff_rot: [7, 7, 7]
-stiff_tran: [270, 270, 270]
-stiff_rot: [11, 11, 11]
-# stiff_tran: [0, 0, 0]
+# stiff_tran: [270, 270, 270]
 # stiff_rot: [11, 11, 11]
+stiff_tran: [0, 0, 0]
+stiff_rot: [11, 11, 11]
 # stiff_tran: [100, 100, 100]
 # stiff_rot: [1, 1, 1]
 # D

Massage/MassageControl/hardware/dobot_nova5.py

@@ -1,10 +1,3 @@
-import os
-import sys
-current_file_path = os.path.abspath(__file__)
-dobot_nova5_path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
-print(dobot_nova5_path)
-# 添加目标文件夹到系统路径
-sys.path.append(dobot_nova5_path)
 try:
     from dobot_api import DobotApiFeedBack,DobotApiDashboard
 except:
@@ -13,7 +6,6 @@ import threading
 from time import sleep,time
 import re
 from tools.log import CustomLogger
-# from tools.log
 import copy
 import numpy as np
 from scipy.spatial.transform import Rotation as R
@@ -69,7 +61,7 @@ class dobot_nova5:
                 self.ToolValue = -1
                 '''
         self.feedbackData = feedbackItem() # 自定义反馈数据结构体
-        self.logger = CustomLogger('Dobot_nova5')
+        self.logger = CustomLogger('Dobot_nova5',True)
 
 
     def parseResultId(self,valueRecv):
@@ -107,6 +99,12 @@ 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端口是否被占用")
+            return
+        print("机械臂使能成功")
 
         # 状态反馈线程
         feedback_thread = threading.Thread(
@@ -114,17 +112,6 @@ class dobot_nova5:
         )
         feedback_thread.daemon = True
         feedback_thread.start()
-        self.dashboard.RequestControl()
-        self.dashboard.EmergencyStop(mode=0) # 松开急停
-        self.clearError() # 清除报警
-        # 关闭碰撞检测
-        self.dashboard.SetCollisionLevel(level=1)
-        if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
-            print("使能失败:检查29999端口是否被占用")
-            return
-        print("机械臂使能成功")
-
-        
     
     def RunPoint_P_inPose(self,poses_list):
         '''
@@ -736,7 +723,7 @@ class dobot_nova5:
         self.last_pose_command = np.zeros(6)
         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_J = [0,30,-120,0,90,0]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
@@ -857,38 +844,19 @@ class dobot_nova5:
         
         return -1, None
 
-    def chooseRightFrame(self):
-        self.chooseBaseFrame(i=1)
-        # self.chooseEndEffector(i=0)
-        return
-
 
 if __name__ == "__main__":
     # sleep(5)
     dobot = dobot_nova5("192.168.5.1")
     dobot.start()
-    # posJ_home = [-45,87,-147,0,90,-90]
-    posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90] 
-    dobot.RunPoint_P_inJoint(posJ_ready)
-    
-    # sleep(1)
-    dobot.chooseRightFrame()
-    # print("Arm pose1:",dobot.getPose())
-    # print("Arm pose:",dobot.getPose(user=1,tool=0))
-    dobot.setEndEffector(i=8,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
-    dobot.chooseEndEffector(i=8)
-    print("Arm pose2:",dobot.getPose(user=1,tool=8))
-    
-    print(dobot.get_arm_position())
+    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.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
-    cur_pose = dobot.getPose(user=1,tool=8)
-    cur_pose[0] += 30
-    target_pose = cur_pose
-    print("target",target_pose)
-    dobot.RunPoint_P_inPose(target_pose)
-    dobot.ServoPose(dobot.getPose(user=1,tool=8))
-    print("new ready joint",dobot.feedbackData.QActual)
+
     # dobot.start_drag()
-    # dobot.stop_drag()
     dobot.disableRobot()

Massage/MassageControl/hardware/force_sensor.py

@@ -16,7 +16,7 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro
 
 class XjcSensor:
     def __init__(self, port=None, baudrate=115200, rate=250):
-        self.port = '/dev/ttyUSB0'
+        self.port = port
         self.baudrate = baudrate
         self.rate = rate
         

logs/test.py

@@ -1,7 +0,0 @@
-from scipy.spatial.transform import Rotation as R
-import numpy as np
-
-arm_orientation_set0 = np.array([-180,-30,0])
-b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
-tcp_offset = b_rotation_s_set0 @ np.array([0,0,154.071])
-print(tcp_offset)