Merge remote-tracking branch 'refs/remotes/origin/LWH' into LWH

合并

.gitignore

@@ -1,26 +0,0 @@
-# 忽略 Python 缓存文件和编译文件
-*.pyc
-__pycache__/
-
-# 忽略虚拟环境
-venv/
-.env/
-.venv/
-env/
-
-# 忽略日志、临时文件
-*.log
-*.tmp
-
-# 忽略 Jupyter 缓存
-.ipynb_checkpoints/
-
-# 忽略 VS Code 或 PyCharm 等 IDE 配置
-.vscode/
-.idea/
-
-# 忽略构建目录
-build/
-dist/
-*.egg-info/
-.eggs/

Massage/MassageControl/MassageRobot_nova5.py

@@ -108,20 +108,12 @@ class MassageRobot:
 
         self.vtxdb = VTXClient()
         
-        # 当前执行的函数
-        self.current_function = None
         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.arm.init()
-
-
         self.thermotherapy = None
         self.shockwave = None
         self.stone = None
@@ -135,16 +127,25 @@ 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'][1])
+        self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2])
-        # 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')
-
+        # 按摩头参数暂时使用本地数据
+        massage_head_dir = self.arm_config['massage_head_dir']
+        all_items = os.listdir(massage_head_dir)
+        head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
+        self.playload_dict = {}
+        for file in head_config_files:
+            file_address = massage_head_dir + '/' + file
+            play_load = read_yaml(file_address)
+            self.playload_dict[play_load['name']] = play_load
+        self.current_head = 'none'
 
         # 读取数据
         self.gravity_base = None
@@ -153,7 +154,7 @@ class MassageRobot:
         self.tool_position = None
         self.mass_center_position = None
         self.s_tf_matrix_t = None
-        arm_orientation_set0 = np.array([-180,-30,0])
+        arm_orientation_set0 = np.array([-180,0,-90])
         self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
         
         # 频率数据赋值
@@ -176,30 +177,15 @@ class MassageRobot:
         # 按摩调整
         self.adjust_wrench_envent = threading.Event()
         self.adjust_wrench_envent.clear() # 调整初始化为False
-        self.is_execute = False   
         self.pos_increment = np.zeros(3,dtype=np.float64)
         self.adjust_wrench = np.zeros(6,dtype=np.float64)
-        self.skip_pos = np.zeros(6,dtype=np.float64)
 
-        # 按摩头参数暂时使用本地数据
-        massage_head_dir = self.arm_config['massage_head_dir']
-        all_items = os.listdir(massage_head_dir)
-        head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
-        self.playload_dict = {}
-        for file in head_config_files:
-            file_address = massage_head_dir + '/' + file
-            play_load = read_yaml(file_address)
-            self.playload_dict[play_load['name']] = play_load
-        # self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
-        # print(self.playload_dict)
-        self.current_head = 'none'
         self.is_waitting = False
 
         self.last_print_time = 0
         self.last_record_time = 0
         self.last_command_time = 0
         self.move_to_point_count = 0
-        self.width_default = 5		
         # 卡尔曼滤波相关初始化
         self.x_base = np.zeros(6)
         self.P_base  = np.eye(6)
@@ -217,16 +203,7 @@ class MassageRobot:
         # 传感器故障计数器
         self.sensor_fail_count = 0 
         # 机械臂初始化,适配中间层
-        # 读取数据
+        self.arm.init()
-        self.gravity_base = None
-        self.force_zero = None
-        self.torque_zero = None
-        self.tool_position = None
-        self.mass_center_position = None
-        self.s_tf_matrix_t = None
-        arm_orientation_set0 = np.array([-180,0,-90])
-        self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
-
         
         # REF TRAJ
         self.xr = []
@@ -238,36 +215,6 @@ class MassageRobot:
         self.last_time = -1
         self.cur_time = -1
 
-    # 预测步骤
-    def kalman_predict(self,x, P, Q):
-        # 预测状态（这里假设状态不变）
-        x_predict = x
-        # 预测误差协方差
-        P_predict = P + Q
-        return x_predict, P_predict
-    
-    # 更新步骤
-    def kalman_update(self,x_predict, P_predict, z, R):
-        # 卡尔曼增益
-        # K = P_predict @ np.linalg.inv(P_predict + R)
-        S = P_predict + R
-        s = np.diag(S)            # shape (6,)
-        p_diag = np.diag(P_predict)
-        K_diag = np.zeros_like(s)
-        nonzero_mask = s != 0
-        K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
-        K = np.diag(K_diag)
-        # 更新状态
-        x_update = x_predict + K @ (z - x_predict)
-        # 更新误差协方差
-        P_update = (np.eye(len(K)) - K) @ P_predict
-        return x_update, P_update
-    def init_hardwares(self,ready_pose):
-        self.ready_pose = np.array(ready_pose)
-        self.switch_payload(self.current_head)
-        print(self.arm.get_arm_position())
-        time.sleep(0.5)
-        
 
     def sensor_set_zero(self):
         self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position()
@@ -376,7 +323,30 @@ class MassageRobot:
             return -1
         return 0
     
-
+    # 预测步骤
+    def kalman_predict(self,x, P, Q):
+        # 预测状态（这里假设状态不变）
+        x_predict = x
+        # 预测误差协方差
+        P_predict = P + Q
+        return x_predict, P_predict
+    
+    # 更新步骤
+    def kalman_update(self,x_predict, P_predict, z, R):
+        # 卡尔曼增益
+        # K = P_predict @ np.linalg.inv(P_predict + R)
+        S = P_predict + R
+        s = np.diag(S)            # shape (6,)
+        p_diag = np.diag(P_predict)
+        K_diag = np.zeros_like(s)
+        nonzero_mask = s != 0
+        K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
+        K = np.diag(K_diag)
+        # 更新状态
+        x_update = x_predict + K @ (z - x_predict)
+        # 更新误差协方差
+        P_update = (np.eye(len(K)) - K) @ P_predict
+        return x_update, P_update
     
     def update_wrench(self):
         # 当前的机械臂到末端转换 (实时)
@@ -413,29 +383,6 @@ class MassageRobot:
         self.arm_state.external_wrench_tcp = self.x_tcp
         self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
         return 0
-		
-    def switch_payload(self,name):
-        if name in self.playload_dict:
-            self.stop()
-            self.current_head = name
-            
-            compensation_config = self.playload_dict[self.current_head]
-
-            # 读取数据
-            self.gravity_base = np.array(compensation_config['gravity_base'])
-            self.force_zero = np.array(compensation_config['force_zero'])
-            self.torque_zero = np.array(compensation_config['torque_zero'])
-            self.tool_position = np.array(compensation_config['tcp_offset'])
-            self.mass_center_position = np.array(compensation_config['mass_center_position'])
-            self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
-            
-            tcp_offset = self.playload_dict[name]["tcp_offset"]
-            tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
-            print("tcp_offset_str",tcp_offset_str)
-            self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
-            self.arm.chooseEndEffector(i=9)
-            print(self.arm.get_arm_position())
-            self.logger.log_info(f"切换到{name}按摩头")		
     
     # ####################### 位姿伺服 ##########################
     
@@ -552,10 +499,9 @@ class MassageRobot:
                     status, pose_processed = self.arm.process_command(
                         command_pose[:3],
                         command_pose[3:]
-                    )
+                    )  
-                    # pose_processed = command_pose
+                    # print(f"pose_processed:{pose_processed}")  
-                    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},"
@@ -570,7 +516,6 @@ class MassageRobot:
                         time.sleep(sleep_duration)
                     print(f"real sleep:{time.time()-b2}")
                     self.arm.dashboard.socket_dobot.sendall(tcp_command)
-                    print("currentCommandID",self.arm.feedbackData.robotCurrentCommandID)
                     if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                         print("机械臂为暂停状态")
                         res = self.arm.dashboard.Continue()
@@ -590,7 +535,7 @@ class MassageRobot:
             # 线程
             self.exit_event.clear()
             self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
-            self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
+            self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -639,8 +584,37 @@ class MassageRobot:
             self.arm.stop_motion()
             self.logger.log_info("MassageRobot停止")
             
+    def init_hardwares(self,ready_pose):
+        self.ready_pose = np.array(ready_pose)
+        self.switch_payload(self.current_head)
+        print(self.arm.get_arm_position())
+        time.sleep(0.5)
+        
+    def switch_payload(self,name):
+        if name in self.playload_dict:
+            self.stop()
+            self.current_head = name
+            
+            compensation_config = self.playload_dict[self.current_head]
 
+            # 读取数据
+            self.gravity_base = np.array(compensation_config['gravity_base'])
+            self.force_zero = np.array(compensation_config['force_zero'])
+            self.torque_zero = np.array(compensation_config['torque_zero'])
+            self.tool_position = np.array(compensation_config['tcp_offset'])
+            self.mass_center_position = np.array(compensation_config['mass_center_position'])
+            self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
+            
+            tcp_offset = self.playload_dict[name]["tcp_offset"]
+            tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
+            print("tcp_offset_str",tcp_offset_str)
+            self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
+            self.arm.chooseEndEffector(i=9)
+            print(self.arm.get_arm_position())
+            self.logger.log_info(f"切换到{name}按摩头")
             self.controller_manager.switch_controller('position')
+        else:
+            self.logger.log_error(f"未找到{name}按摩头")
 
     def log_thread(self):
         while True:
@@ -1581,35 +1555,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_ws/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
+    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 = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
+    ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
     robot.current_head = 'finger_head'
 
     robot.force_sensor.disable_active_transmission()
@@ -1630,7 +1604,7 @@ if __name__ == "__main__":
     
     
     atexit.register(robot.force_sensor.disconnect)
-    robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
+    robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0])
     try:
         robot_thread = threading.Thread(target=robot.start)
         robot_thread.start()    

Massage/MassageControl/config/admittance.yaml

@@ -10,8 +10,6 @@ mass_rot: [0.11, 0.11, 0.11]
 # stiff_rot: [7, 7, 7]
 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/capture.py

@@ -1,124 +0,0 @@
-import os
-import sys
-import cv2
-import threading
-import numpy as np
-import time
-
-current_file_path = os.path.abspath(__file__)
-dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path))
-sys.path.append(dobot_nova5_path)
-
-from hardware.dobot_nova5 import dobot_nova5
-from hardware.remote_cam import ToolCamera
-
-class Getpose:
-    def __init__(self):
-        self.running = True
-        self.arm = dobot_nova5()
-        self.arm.start()
-        self.arm.init()
-        self.cam = ToolCamera(host='127.0.0.1')
-        self.cam.start()
-        time.sleep(2)
-        
-        self.pose_data = []
-        self.image_data = []
-        # self.arm.start_drag()
-        self.rgb = None
-            
-    def test_show(self):
-        self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
-        if self.rgb is not None:
-            cv2.imshow("Video Feed", self.rgb)
-            cv2.waitKey(1000)
-    
-    def add_data(self):
-        angle = self.arm.getAngel()
-        self.pose_data.append(angle)
-        print(f"Current angle: {angle}")
-    
-    def save_data(self):
-        if self.pose_data:
-            data = np.array(self.pose_data)
-            np.savetxt('pose.txt', data, fmt='%.5f')
-    
-
-if __name__ == "__main__":
-    save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
-    if not os.path.exists(save_directory):
-        os.makedirs(save_directory)
-    
-    myTest = Getpose()
-    
-    myTest.arm.start_drag()
-    
-    time.sleep(0.5)
-    try:
-        count = 1
-        i = 1
-        while True:
-            # 读取摄像头的帧
-            r, d, intrinsics = myTest.cam.get_latest_frame()
-            # ret, frame = cam.read()
-            frame = r
-
-            # if not ret:
-            #     print("Error: Could not read frame from video device.")
-            #     break
-
-                        # 显示摄像头的帧
-            cv2.imshow('found', frame)
-
-            # 等待按键事件
-            key = cv2.waitKey(1) & 0xFF
-
-
-            # if key == ord('r'):
-            #     pose = increase_dof(poselist[index])
-            #     index+=1
-            #     code = cp.arm.move_joint(pose, 4,wait=True)
-            #     if code == -1:
-            #         print("运动到拍照位置失败")
-            #     else:
-
-            # 按下's'键保存照片
-            if key == ord('s'):
-                img_path = f'photo{i}'
-                if not os.path.exists(os.path.join(save_directory, img_path)):
-                    os.makedirs(os.path.join(save_directory, img_path))
-                filename = os.path.join(save_directory, f'{img_path}/rgb_image.png')
-                cv2.imwrite(filename, frame) # 保存照片
-                filename1 = os.path.join(save_directory, f'{img_path}/depth_image.png')
-                cv2.imwrite(filename1, d) # 保存照片
-                angle = myTest.arm.Get_feedInfo_angle()
-                print("angle = ",angle)
-                filename2 = os.path.join(save_directory, f'{img_path}/angle.txt')
-                np.savetxt(filename2, angle.shape(-1,6),delimiter=',')
-                print(f'Saved {filename}')
-                
-                # pose = increase_dof(poselist[index])
-                # index+=1
-                # code = cp.arm.move_joint(poselist[index], 4,wait=True)
-                # if code == -1:
-                #     print("运动到拍照位置失败")
-                count += 1 # 增加照片计数
-                i += 1
-            # 按下'q'键退出循环
-            elif key == ord('q'):
-                break
-            
-
-    except KeyboardInterrupt:
-        # 处理Ctrl+C中断
-        print("\nCamera session ended by user.")
-    finally:
-        # 释放资源
-        myTest.arm.stop_drag()
-        time.sleep(0.5)
-        myTest.arm.disableRobot()
-        myTest.cam.stop()
-        cv2.destroyAllWindows()
-
-        # 总结拍摄的照片
-        print(f'Total number of images captured: {count}')

Massage/MassageControl/hardware/dobot_nova5.py

@@ -13,7 +13,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
@@ -95,7 +94,6 @@ class dobot_nova5:
                         self.feedbackData.QActual = feedInfo['QActual'][0]
                         self.feedbackData.ActualQuaternion = feedInfo['ActualQuaternion'][0]
                         self.feedbackData.ToolVectorActual = feedInfo['ToolVectorActual'][0]
-                        self.feedbackData.QDActual = feedInfo['QDActual'][0]
                         # 自定义添加所需反馈数据
                         '''
                         self.feedData.DigitalOutputs = int(feedInfo['DigitalOutputs'][0])
@@ -108,26 +106,19 @@ 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)
-        # lower the velocity
-        self.dashboard.SpeedFactor(50)
-        if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
-            print("使能失败:检查29999端口是否被占用")
-            return
-        print("机械臂使能成功")
-
-        
     
     def RunPoint_P_inPose(self,poses_list):
         '''
@@ -740,13 +731,11 @@ class dobot_nova5:
         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_P = [654.3467,-134.9880,305.3604,-180.0000,-30.0000,0.0042]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
         self.is_standby = False
-        # code = self.RunPoint_P_inJoint(self.init_J)
+        code = self.RunPoint_P_inJoint(self.init_J)
-        code = self.RunPoint_P_inPose(self.init_P)
         if code == 0:
             print("机械臂初始化成功且到达初始位置")
         else:
@@ -861,39 +850,21 @@ 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_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] 
+    pos_end=[]
-    # dobot.RunPoint_P_inJoint(posJ_ready)
+    dobot.RunPoint_P_inJoint(posJ)
-    
+    sleep(1)
-    # # sleep(1)
+    dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
-    dobot.chooseRightFrame()
+    dobot.chooseEndEffector(i=9)
-    # print("Arm pose1:",dobot.getPose())
+    print("Arm pose:",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_L_inPose([179.087702,-136.993776,425.209977,179.418397,-28.9718402,0.06])
+    # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
-    # cur_pose = dobot.getPose(user=1,tool=8)
+
-    # cur_pose[0] += 30
+    # dobot.start_drag()
-    # 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

@@ -1,11 +1,9 @@
 import struct
 import serial
-import serial.tools.list_ports
 import numpy as np
 import atexit
 import threading
 import time
-import yaml
 
 import subprocess
 import psutil
@@ -16,8 +14,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.port = '/dev/ttyS0'
         self.baudrate = baudrate
         self.rate = rate
         

Massage/MassageControl/tools/auto_visual_valibration.py

@@ -41,16 +41,12 @@ class Calibration:
         self.size = None
         self.intrinsics_mtx = None
         self.disorder = None
-        time.sleep(2)
         rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
-        self.save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
-       
         if rgb_image is None :
             print(f'===============相机连接失败请检查并重试==============')
         else:
+            print(f'===============相机连接成功==============')
             self.size = rgb_image.shape[:2]
-            print(f'===============相机连接成功==============  ',self.size)
-            
         self.intrinsics = camera_intrinsics
         
         self.arm = dobot_nova5("192.168.5.1")
@@ -66,7 +62,7 @@ class Calibration:
   
     def collect_data(self):
         #移动到初始位置 
-        pose = [-45.0009079,55.5785141,-120.68821716,5.11103201,-90.00195312,-90.00085449]
+        pose = [0,30,-120,0,90,0]
         code = self.arm.RunPoint_P_inJoint(pose)
         if code == -1:
             print("运动到拍照位置失败")
@@ -74,21 +70,16 @@ class Calibration:
         self.check_corners()
         #运动到下一个位置
         # 目前只设计了44条轨迹    
-        for i in range (0,18):
+        for i in range (0,45):
             next_pose = self.get_next_pose_ugly(i)
-            code=self.arm.RunPoint_P_inJoint(next_pose)
+            self.arm.RunPoint_P_inJoint(next_pose)
-            time.sleep(0.5)
             if code == -1:
                 print("运动到拍照位置失败")
             else:
                 self.check_corners()
-                time.sleep(0.5)
+    
-                
     def check_corners(self):
         rgb, depth, intrinsics = self.cam.get_latest_frame()
-        # img_path = f'photo{i}'
-        # filename = os.path.join(self.save_directory, f'{img_path}/rgb_image.png')
-        # rgb=cv2.imread(filename)
         gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
         ret, corners = cv2.findChessboardCorners(gray, self.board_size, None)
         if ret:
@@ -126,7 +117,7 @@ class Calibration:
     def get_pose_homomat(self):
         arm_position,arm_orientation = self.arm.get_arm_position()
         rotation_matrix = R.from_quat(arm_orientation).as_matrix()
-        translation_vector = arm_position
+        translation_vector = arm_position.reshape(3, 1)
         homo_mat = np.eye(4) # 创建 4x4 单位矩阵
         homo_mat[:3, :3] = rotation_matrix
         homo_mat[:3, 3] = translation_vector
@@ -193,24 +184,52 @@ class Calibration:
         return None
     def get_next_pose_ugly(self,index):
         poselist =[
-            [-4.513801574707031250e+01,5.518265151977539062e+01,-1.237858963012695312e+02,9.085088729858398438e+00,-9.337700653076171875e+01,-9.000016784667968750e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [-4.594435882568359375e+01,3.636156082153320312e+01,-1.064173660278320312e+02,-2.125140428543090820e+00,-9.337675476074218750e+01,-9.000016784667968750e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0],
-            [-4.595432662963867188e+01,1.059147930145263672e+01,-8.284246063232421875e+01,-1.484527587890625000e+01,-9.337491607666015625e+01,-9.000013732910156250e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0],
-            [-4.638331604003906250e+01,-6.824899196624755859e+00,-6.758448791503906250e+01,-2.286166954040527344e+01,-9.337348937988281250e+01,-9.000016784667968750e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [-4.636801910400390625e+01,-1.807893753051757812e+01,-6.532709503173828125e+01,-2.261142921447753906e+01,-9.336659240722656250e+01,-9.000010681152343750e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0],
-            [-4.637482833862304688e+01,-4.540402412414550781e+00,-1.098018112182617188e+02,3.086268234252929688e+01,-9.305430603027343750e+01,-9.000022125244140625e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0],
-            [-4.640084075927734375e+01,8.068140029907226562e+00,-1.362033843994140625e+02,6.259086990356445312e+01,-9.334789276123046875e+01,-9.000019073486328125e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0],
-            [-4.640056610107421875e+01,8.061657905578613281e+00,-1.511797027587890625e+02,8.927394104003906250e+01,-9.331819915771484375e+01,-9.000019073486328125e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [-4.638466262817382812e+01,3.287191867828369141e+00,-1.566032409667968750e+02,1.035896453857421875e+02,-9.330465698242187500e+01,-9.000022125244140625e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0],
-            [-3.178509473800659180e+00,6.977911472320556641e+00,-1.417592010498046875e+02,6.005132293701171875e+01,-1.280102996826171875e+02,-9.000010681152343750e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
-            [-1.424913024902343750e+01,-2.980865538120269775e-01,-1.193396759033203125e+02,3.752756500244140625e+01,-1.278784713745117188e+02,-8.999975585937500000e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0],
-            [-2.393187522888183594e+01,-2.973175048828125000e-01,-1.121088027954101562e+02,3.928178024291992188e+01,-1.183067703247070312e+02,-8.999983215332031250e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [-3.699544525146484375e+01,-2.981689572334289551e-01,-1.078408889770507812e+02,3.178039932250976562e+01,-1.024006271362304688e+02,-8.999983215332031250e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0],
-            [-6.734904479980468750e+01,-4.368630886077880859e+00,-1.035898971557617188e+02,2.342595481872558594e+01,-7.085527801513671875e+01,-8.999980926513671875e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0],
-            [-8.632910919189453125e+01,-1.039798259735107422e+01,-1.038105545043945312e+02,2.330961036682128906e+01,-5.476583099365234375e+01,-8.999980926513671875e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0],
-            [-8.156823730468750000e+01,5.878097534179687500e+00,-1.439221954345703125e+02,6.846669006347656250e+01,-4.482905197143554688e+01,-8.999922943115234375e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0],
-            [-8.601764678955078125e+01,3.570293045043945312e+01,-1.308228912353515625e+02,2.318510818481445312e+01,-5.378013610839843750e+01,-8.999922943115234375e+01],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [-7.132132053375244141e+00,3.722338104248046875e+01,-1.065378265380859375e+02,-3.337371826171875000e+00,-1.185582199096679688e+02,-8.999931335449218750e+01]
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
+            [1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
+            [1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
+            [1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
+            [1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0],
+            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0],
+            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
+            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0],
+            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0],
+            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
+            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0],
+            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0],
+            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0],
+            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0],
+            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0]
             ]
         return poselist[index]
         
@@ -221,7 +240,7 @@ if __name__ == '__main__':
     # 标定板的width 对应的角点的个数 6 
     # 标定板的height 对应的角点的个数 3 
     calibration_board_size = [6,3]
-    calibration_board_square_size = 0.03 #unit - meter 
+    calibration_board_square_size = 0.027 #unit - meter 
     systemPath = "/home/jsfb/Documents/"
     
     now = datetime.now()
@@ -231,22 +250,10 @@ if __name__ == '__main__':
     os.makedirs(systemPath, exist_ok=True)
 
     calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
-    
-    time.sleep(0.5)
-    calib.arm.chooseBaseFrame(i=1)
-    calib.arm.chooseEndEffector(i=0)
-    time.sleep(0.5)
-    
     calib.collect_data()
-    print("=================数据采集完成===================")
     R, t,intrin = calib.calibrate()
-    
     print("旋转矩阵：")
     print(R)
     print("平移向量：")
     print(t)
     print(f"内参： {intrin}")
-    time.sleep(0.5)
-    calib.cam.stop()
-    time.sleep(0.5)
-    print('camera stopped')

Massage/MassageControl/tools/capture.py

@@ -0,0 +1,69 @@
+import os
+import sys
+current_file_path = os.path.abspath(__file__)
+dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path))
+print(dobot_nova5_path)
+sys.path.append(dobot_nova5_path)
+
+from hardware.dobot_nova5 import dobot_nova5
+import cv2
+from hardware.remote_cam import ToolCamera
+import threading
+import numpy as np
+import time
+class Getpose:
+    def __init__(self):
+        self.arm = dobot_nova5()
+        self.arm.start()
+        self.arm.init()
+        self.cam = ToolCamera(host='127.0.0.1')
+        self.cam.start()
+        time.sleep(2)
+        self.thread1=threading.Thread(target=self.show)
+        self.thread1.start()
+        self.pose_data=[]
+        self.image_data=[]
+        self.arm.start_drag()
+    
+    def show(self):
+        while True:
+            self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
+            # print("cv:",self.rgb)
+            cv2.imshow("1111",self.rgb)
+            cv2.waitKey(1)
+            time.sleep(0.2)
+
+    def add_data(self):
+        angle=self.arm.getAngel()
+        self.pose_data=self.pose_data.append(angle)
+        print(angle)
+
+    def save_data(self):
+        data=np.array(self.pose_data)
+        np.savetxt('pose.txt', data ,fmt='%.5f')
+
+if __name__ == "__main__":
+    save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
+    if not os.path.exists(save_directory):
+        os.makedirs(save_directory)
+    sele=Getpose()
+    
+    key = cv2.waitKey(1) & 0xFF
+    while True:
+        i=1
+        if key == ord('s'):
+            img_path = f'photo{i}.png'
+            filename = os.path.join(save_directory, img_path)
+            cv2.imwrite(filename, sele.rgb) # 保存照片
+            sele.add_data()
+            
+            filename2 = os.path.join(save_directory, f'pose.txt')
+            np.savetxt(filename2, sele.pose_data)
+            # pose = increase_dof(poselist[index])
+            
+            i += 1
+            # 按下'q'键退出循环
+        elif key == ord('q'):
+            sele.arm.disableRobot()
+            break
+    

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)