暂存修改

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 @@ try:
 	    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 @@ except:
 	    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 @@ def track_function(function_name,log = False):
 	            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 @@ class MassageRobot:
 	    
 	
 	
-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,4 +1628,3 @@ if __name__ == "__main__":
 	        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,3 +1,10 @@
+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:
@@ -6,6 +13,7 @@ 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
@@ -61,7 +69,7 @@ class dobot_nova5:
                 self.ToolValue = -1
                 '''
         self.feedbackData = feedbackItem() # 自定义反馈数据结构体
-        self.logger = CustomLogger('Dobot_nova5',True)
+        self.logger = CustomLogger('Dobot_nova5')
 
 
     def parseResultId(self,valueRecv):
@@ -99,12 +107,6 @@ 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(
@@ -112,6 +114,17 @@ 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):
         '''
@@ -723,7 +736,7 @@ class dobot_nova5:
         self.last_pose_command = np.zeros(6)
         self.last_input_command = None
         self.tcp_offset = None
-        self.init_J = [0,30,-120,0,90,0]
+        self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
@@ -844,19 +857,38 @@ 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 = [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())
+    # 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())
 
     # 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 = port
+        self.port = '/dev/ttyUSB0'
         self.baudrate = baudrate
         self.rate = rate
         

logs/test.py

@@ -0,0 +1,7 @@
+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)