暂存修改

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)