暂存修改

Massage/MassageControl/MassageRobot_nova5.py

@@ -111,8 +111,10 @@ class MassageRobot:
         self.arm_state = ArmState()
         self.arm_config = read_yaml(arm_config_path)
         # arm 实例化时机械臂类内部进行通讯连接
+        # 初始化坐标系 TOOL=0 BASE=1
         self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
         self.arm.start()
+        self.arm.chooseRightFrame()
 
         self.thermotherapy = None
         self.shockwave = None
@@ -127,13 +129,13 @@ class MassageRobot:
         # 控制器,初始为导纳控制
         self.controller_manager = ControllerManager(self.arm_state)
         self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
-        self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
+        # self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
-        self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2])
+        self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1])
-        self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
+        # self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
-        self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
+        # self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
-        self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
+        # self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
-        self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
+        # self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
-        self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
+        # self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
         
         self.controller_manager.switch_controller('admittance')
         # 按摩头参数暂时使用本地数据
@@ -154,7 +156,7 @@ class MassageRobot:
         self.tool_position = None
         self.mass_center_position = None
         self.s_tf_matrix_t = None
-        arm_orientation_set0 = np.array([-180,0,-90])
+        arm_orientation_set0 = np.array([-180,-30,0])
         self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
         
         # 频率数据赋值
@@ -499,9 +501,10 @@ class MassageRobot:
                     status, pose_processed = self.arm.process_command(
                         command_pose[:3],
                         command_pose[3:]
-                    )  
+                    )
-                    # print(f"pose_processed:{pose_processed}")  
+                    # pose_processed = command_pose
-                    
+                    print(f"pose_processed:{pose_processed}")  
+                    print(self.arm.feedbackData.robotMode)
                     tcp_command = (
                     f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
                     f"{pose_processed[2]:.3f},{pose_processed[3]:.3f},"
@@ -1555,35 +1558,35 @@ class MassageRobot:
 
 
 if __name__ == "__main__":
-    waypoints = [
+    # waypoints = [
-        {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
+    #     {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
-         "acceleration": [0, 0, 0],
+    #      "acceleration": [0, 0, 0],
-         "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
+    #      "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
-        {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
+    #     {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
-         "acceleration": [0, 0, 0],
+    #      "acceleration": [0, 0, 0],
-         "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
+    #      "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
-    ] ## 单位 m deg
+    # ] ## 单位 m deg
-    myInterpolate = TrajectoryInterpolator(waypoints)
+    # myInterpolate = TrajectoryInterpolator(waypoints)
-    ts = np.linspace(0,5,100)
+    # ts = np.linspace(0,5,100)
     robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
-    xr_list, vr_list, ar_list = [], [], []
+    # xr_list, vr_list, ar_list = [], [], []
-    for t in ts:
+    # for t in ts:
-        xr, vr, ar = myInterpolate.interpolate(t)  
+    #     xr, vr, ar = myInterpolate.interpolate(t)  
-        xr_list.append(xr)
+    #     xr_list.append(xr)
-        vr_list.append(vr)
+    #     vr_list.append(vr)
-        ar_list.append(ar)  
+    #     ar_list.append(ar)  
-    xr_array = np.array(xr_list)
+    # xr_array = np.array(xr_list)
-    vr_array = np.array(vr_list)
+    # vr_array = np.array(vr_list)
-    ar_array = np.array(ar_list)
+    # ar_array = np.array(ar_list)
 
-    robot.xr = xr_array
+    # robot.xr = xr_array
-    robot.vr = vr_array
+    # robot.vr = vr_array
-    robot.ar = ar_array
+    # robot.ar = ar_array
-    robot.ts = ts
+    # robot.ts = ts
-    robot.dt = ts[1] - ts[0]
+    # robot.dt = ts[1] - ts[0]
 
-
+    
-    ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
+    ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
     robot.current_head = 'finger_head'
 
     robot.force_sensor.disable_active_transmission()

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,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,19 +107,24 @@ 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(
             target=self.GetFeedback # 机器状态反馈线程
         )
         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)
@@ -843,20 +856,39 @@ class dobot_nova5:
             return 0, result
         
         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]
+    # posJ_home = [-45,87,-147,0,90,-90]
-    dobot.RunPoint_P_inJoint(posJ)
+    posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90] 
-    sleep(1)
+    dobot.RunPoint_P_inJoint(posJ_ready)
-    dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
+    
-    dobot.chooseEndEffector(i=9)
+    # sleep(1)
-    print("Arm pose:",dobot.getPose())
+    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)