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_tran: [270, 270, 270]
-stiff_rot: [11, 11, 11]
-# stiff_tran: [0, 0, 0]
 # 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 = [0,30,-120,0,90,0]
-    posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90] 
+    dobot.RunPoint_P_inJoint(posJ)
-    dobot.RunPoint_P_inJoint(posJ_ready)
+    sleep(1)
-    
+    dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
-    # sleep(1)
+    dobot.chooseEndEffector(i=9)
-    dobot.chooseRightFrame()
+    print("Arm pose:",dobot.getPose())
-    # 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 = '/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)