diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 1c899a1..24e0b38 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -578,7 +578,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_traj) + self.arm_control_thread = threading.Thread(target=self.arm_command_loop) # 线程启动 self.arm_measure_thread.start() ## 测量线程 position,quat_rot = self.arm.get_arm_position() @@ -629,8 +629,8 @@ class MassageRobot: self.controller_manager.switch_controller('position') - else: - self.logger.log_error(f"未找到{name}按摩头") + # else: + # self.logger.log_error(f"未找到{name}按摩头") def log_thread(self): while True: @@ -1617,35 +1617,35 @@ class MassageRobot: if __name__ == "__main__": - waypoints = [ - {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0], - "acceleration": [0, 0, 0], - "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], - "acceleration": [0, 0, 0], - "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()}, - ] ## 单位 m deg - myInterpolate = TrajectoryInterpolator(waypoints) - 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 = [], [], [] - for t in ts: - xr, vr, ar = myInterpolate.interpolate(t) - xr_list.append(xr) - vr_list.append(vr) - ar_list.append(ar) - xr_array = np.array(xr_list) - vr_array = np.array(vr_list) - ar_array = np.array(ar_list) + # waypoints = [ + # {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0], + # "acceleration": [0, 0, 0], + # "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], + # "acceleration": [0, 0, 0], + # "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()}, + # ] ## 单位 m deg + # myInterpolate = TrajectoryInterpolator(waypoints) + # ts = np.linspace(0,5,100) + robot = MassageRobot(arm_config_path="/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml") + # xr_list, vr_list, ar_list = [], [], [] + # for t in ts: + # xr, vr, ar = myInterpolate.interpolate(t) + # xr_list.append(xr) + # vr_list.append(vr) + # ar_list.append(ar) + # xr_array = np.array(xr_list) + # vr_array = np.array(vr_list) + # ar_array = np.array(ar_list) - robot.xr = xr_array - robot.vr = vr_array - robot.ar = ar_array - robot.ts = ts - robot.dt = ts[1] - ts[0] + # robot.xr = xr_array + # robot.vr = vr_array + # robot.ar = ar_array + # robot.ts = ts + # robot.dt = ts[1] - ts[0] - - ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0] + + ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] robot.current_head = 'finger_head' robot.force_sensor.disable_active_transmission() @@ -1666,15 +1666,18 @@ if __name__ == "__main__": atexit.register(robot.force_sensor.disconnect) - robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0]) + robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0]) try: robot_thread = threading.Thread(target=robot.start) robot_thread.start() except KeyboardInterrupt: + robot_thread.join() print("用户中断操作。") except Exception as e: + robot_thread.join() print("Exception occurred at line:", e.__traceback__.tb_lineno) print("发生异常：", e) + # robot.arm.disableRobot() \ No newline at end of file diff --git a/Massage/MassageControl/config/hybridAdmit.yaml b/Massage/MassageControl/config/hybridAdmit.yaml index e6c1d5d..8c7489f 100755 --- a/Massage/MassageControl/config/hybridAdmit.yaml +++ b/Massage/MassageControl/config/hybridAdmit.yaml @@ -3,6 +3,7 @@ name: 'hybridAdmit' mass_z: [3.0] stiff_z: [0] damp_z: [30] +desired_xi: 1.0 # 位控参数 Kp: [20,20] diff --git a/Massage/MassageControl/config/robot_config.yaml b/Massage/MassageControl/config/robot_config.yaml index 7f78d12..705813a 100644 --- a/Massage/MassageControl/config/robot_config.yaml +++ b/Massage/MassageControl/config/robot_config.yaml @@ -2,11 +2,17 @@ arm_ip: '192.168.5.1' # controller -controller: ['Massage/MassageControl/config/admittance.yaml', - 'Massage/MassageControl/config/position.yaml'] +controller: ['MassageControl/config/admittance.yaml', + 'MassageControl/config/hybrid.yaml', + 'MassageControl/config/position.yaml', + 'MassageControl/config/admithybrid.yaml', + 'MassageControl/config/hybridPid.yaml', + 'MassageControl/config/hybridAdmit.yaml', + 'MassageControl/config/positionerSensor.yaml', + 'MassageControl/config/admittanceZ.yaml',] # massage heads diretory -massage_head_dir: 'Massage/MassageControl/config/massage_head' +massage_head_dir: '/home/jsfb/jsfb_ws/global_config/massage_head' control_rate: 55 sensor_rate: 55 diff --git a/Massage/MassageControl/hardware/dobot_api.py b/Massage/MassageControl/hardware/dobot_api.py index 27bde60..07e1616 100644 --- a/Massage/MassageControl/hardware/dobot_api.py +++ b/Massage/MassageControl/hardware/dobot_api.py @@ -127,6 +127,7 @@ class DobotApi: if self.port == 29999 or self.port == 30004 or self.port == 30005: try: self.socket_dobot = socket.socket() + self.socket_dobot.settimeout(5) # set time out 5s self.socket_dobot.connect((self.ip, self.port)) self.socket_dobot.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 144000) except socket.error: @@ -174,14 +175,32 @@ class DobotApi: def close(self): """ - Close the port + Close the socket connection safely """ - if (self.socket_dobot != 0): + if isinstance(self.socket_dobot, socket.socket): try: self.socket_dobot.shutdown(socket.SHUT_RDWR) - self.socket_dobot.close() except socket.error as e: - print(f"Error while closing socket: {e}") + print(f"[WARN] Error during socket shutdown: {e}") + try: + self.socket_dobot.close() + print("[INFO] Socket closed successfully") + except socket.error as e: + print(f"[ERROR] Error while closing socket: {e}") + finally: + self.socket_dobot = None # 清理引用 + + + # def close(self): + # """ + # Close the port + # """ + # if (self.socket_dobot != 0): + # try: + # self.socket_dobot.shutdown(socket.SHUT_RDWR) + # self.socket_dobot.close() + # except socket.error as e: + # print(f"Error while closing socket: {e}") def sendRecvMsg(self, string): """ @@ -3050,7 +3069,13 @@ class DobotApiDashboard(DobotApi): class DobotApiFeedBack(DobotApi): def __init__(self, ip, port, *args): - super().__init__(ip, port, *args) + try: + super().__init__(ip, port, *args) + except Exception as e: + print(f"[错误] 无法连接 Dobot（IP: {ip}, Port: {port}）：{e}") + # 可根据需要抛出异常或安全退出 + raise # 或者：self.connected = False; return + # super().__init__(ip, port, *args) self.__MyType = [] self.last_recv_time = time.perf_counter() diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index 70f89a1..b5b72dc 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -149,6 +149,7 @@ class dobot_nova5: atexit.register(self.exit_task) self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180] + self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180] # 为状态管理而封装的初始化函数 def init(self): @@ -207,7 +208,7 @@ class dobot_nova5: "http://127.0.0.1:5000/update_massage_status", data=instruction ) time.sleep(1) - # self.move_joint(self.off_pos) + self.move_joint(self.off_pos) self.stop_feedback.set() self.feedback_thread.join() self.dashboard.Disconnect() diff --git a/Massage/MassageControl/hardware/force_sensor.py b/Massage/MassageControl/hardware/force_sensor.py index b62ca4c..9b26acc 100644 --- a/Massage/MassageControl/hardware/force_sensor.py +++ b/Massage/MassageControl/hardware/force_sensor.py @@ -15,7 +15,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): + def __init__(self, port='/dev/ttyS0', baudrate=115200, rate=250): self.port = port self.baudrate = baudrate self.rate = rate