#!/usr/bin/env python3 # -*-coding:utf8-*- from typing import Optional import time from Robotic_Arm.rm_robot_interface import * import pygame import threading from typing import Dict def enable_fun(arm: RoboticArm): ''' 使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序 ''' enable_flag = False # 设置超时时间（秒） timeout = 5 # 记录进入循环前的时间 start_time = time.time() elapsed_time_flag = False while not enable_flag: elapsed_time = time.time() - start_time print("--------------------") # 获取机械臂状态 ret = arm.rm_get_current_arm_state() if ret[0] == 0: # 成功获取状态 arm_state = ret[1] enable_flag = True print("使能状态:", enable_flag) print("--------------------") # 检查是否超过超时时间 if elapsed_time > timeout: print("超时....") elapsed_time_flag = True enable_flag = True break time.sleep(1) if elapsed_time_flag: print("程序自动使能超时,退出程序") exit(0) class EndPoseController: def __init__(self, init_joint, init_pose): # 初始化pygame和手柄 pygame.init() pygame.joystick.init() # 检查是否有连接的手柄 if pygame.joystick.get_count() == 0: raise Exception("未检测到手柄") # 初始化手柄 self.joystick = pygame.joystick.Joystick(0) self.joystick.init() # 摇杆死区 self.deadzone = 0.15 # 精细控制模式 self.fine_control_mode = False # 初始化末端姿态 [X, Y, Z, RX, RY, RZ] XYZ meter RX RY RZ rad self.init_joint = init_joint self.init_pose = init_pose self.joint = self.init_joint self.pose = self.init_pose self.pose_speeds = [0.0] * 6 # 末端位姿限制 self.pose_limits = [ (-0.850, 0.850), # X (m) (-0.850, 0.850), # Y (m) (0.850, 0.850), # Z (m) - 设置最小高度防止碰撞 (-3.14, 3.14), # RX (rad) (-3.14, 3.14), # RY (rad) (-3.14, 3.14) # RZ (rad) ] # 控制参数 self.linear_step = 0.0015 # 线性移动步长(m) self.angular_step = 0.001 # 角度步长(rad) - 从度转换为弧度 # 夹爪状态和速度 self.gripper_open = False self.gripper_speed = 10 # 启动更新线程 self.running = True self.thread = threading.Thread(target=self.update_controller) self.thread.start() print("机械臂末端位姿控制器已启动") def _apply_nonlinear_mapping(self, value): """应用非线性映射以提高控制精度""" # 保持符号，但对数值应用平方映射以提高精度 sign = 1 if value >= 0 else -1 return sign * (abs(value) ** 2) def _normalize_angle(self, angle): """将角度归一化到[-π, π]范围内""" import math while angle > math.pi: angle -= 2 * math.pi while angle < -math.pi: angle += 2 * math.pi return angle def update_controller(self): while self.running: try: pygame.event.pump() except Exception as e: print(f"控制器错误: {e}") self.stop() continue # 检查精细控制模式切换 (使用L3按钮) if self.joystick.get_button(10): # L3按钮 self.fine_control_mode = not self.fine_control_mode print(f"切换到{'精细' if self.fine_control_mode else '普通'}控制模式") time.sleep(0.3) # 防止多次触发 # 检查重置按钮 (7号按钮，通常是Start按钮) if self.joystick.get_button(7): # Start按钮 print("重置机械臂到初始位置...") # 重置位姿 self.joint = self.init_joint self.pose = self.init_pose self.pose_speeds = [0.0] * 6 self.gripper_open = False self.gripper_speed = 10 print("机械臂已重置到初始位置") time.sleep(0.3) # 防止多次触发 # 更新末端位姿 self.update_end_pose() # 夹爪控制（圈/叉） circle = self.joystick.get_button(1) # 夹爪开 cross = self.joystick.get_button(0) # 夹爪关 self.gripper_speed = 10 if circle else (10 if cross else 0) self.gripper_open = True if circle else False # 更新夹爪 # self.gripper += self.gripper_speed # self.gripper = max(0.0, min(0.1, self.gripper)) time.sleep(0.02) def update_end_pose(self): print("1", self.pose, "griper", self.gripper_open) """更新末端位姿控制""" # 根据控制模式调整步长 current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0) current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0) # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}") print(f"精细控制模式: {self.fine_control_mode}") # 方向键控制XY hat = self.joystick.get_hat(0) hat_up = hat[1] == 1 # Y+ hat_down = hat[1] == -1 # Y- hat_left = hat[0] == -1 # X- hat_right = hat[0] == 1 # X+ # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}") # 右摇杆控制Z right_y_raw = -self.joystick.get_axis(4) # print(f"右摇杆原始值(axis 4): {self.joystick.get_axis(4)}") # print(f"右摇杆处理值: {right_y_raw}") # 左摇杆控制RZ left_y_raw = -self.joystick.get_axis(1) # print(f"左摇杆原始值(axis 1): {self.joystick.get_axis(1)}") # print(f"左摇杆处理值: {left_y_raw}") # 应用死区 right_y = 0.0 if abs(right_y_raw) < self.deadzone else right_y_raw left_y = 0.0 if abs(left_y_raw) < self.deadzone else left_y_raw # print(f"死区处理后 - 右摇杆: {right_y}, 左摇杆: {left_y}") # 计算各轴速度 self.pose_speeds[1] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # Y self.pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X # 设置Z速度（右摇杆Y轴控制） z_mapping = self._apply_nonlinear_mapping(right_y) # print(f"Z轴非线性映射: {right_y} -> {z_mapping}") self.pose_speeds[2] = z_mapping * current_linear_step # Z # L1/R1控制RX旋转 LB = self.joystick.get_button(4) # RX- RB = self.joystick.get_button(5) # RX+ self.pose_speeds[3] = (-current_angular_step if LB else (current_angular_step if RB else 0.0)) # △/□控制RY旋转 triangle = self.joystick.get_button(2) # RY+ square = self.joystick.get_button(3) # RY- self.pose_speeds[4] = (current_angular_step if triangle else (-current_angular_step if square else 0.0)) # 左摇杆Y轴控制RZ旋转 rz_mapping = self._apply_nonlinear_mapping(left_y) # print(f"RZ轴非线性映射: {left_y} -> {rz_mapping}") self.pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ # print(f"计算出的速度: {self.pose_speeds}") # 更新末端位姿 old_pose = self.pose.copy() for i in range(6): self.pose[i] += self.pose_speeds[i] # print(f"位姿更新: {old_pose} -> {self.pose}") # 位置限制 # pose_before_limit = self.pose.copy() # for i in range(3): # min_val, max_val = self.pose_limits[i] # self.pose[i] = max(min_val, min(max_val, self.pose[i])) # if pose_before_limit != self.pose: # print(f"位置限制生效: {pose_before_limit} -> {self.pose}") # 角度归一化处理 pose_before_normalize = self.pose.copy() for i in range(3, 6): self.pose[i] = self._normalize_angle(self.pose[i]) # if pose_before_normalize != self.pose: # print(f"角度归一化生效: {pose_before_normalize} -> {self.pose}") # print("2", self.pose) # print("=" * 50) def update_state(self, end_pose, joint_state): """更新状态信息（从机械臂获取当前状态）""" # 这里可以选择是否要同步机械臂的实际位置到控制器 # 如果需要严格同步，可以取消下面的注释 # self.pose = end_pose.copy() pass def get_action(self) -> Dict: """获取当前控制命令""" return { 'X': self.pose[0], 'Y': self.pose[1], 'Z': self.pose[2], 'RX': self.pose[3], 'RY': self.pose[4], 'RZ': self.pose[5], 'gripper_speed': self.gripper_speed, 'gripper_open': self.gripper_open } def stop(self): """停止控制器""" self.running = False if self.thread.is_alive(): self.thread.join() pygame.quit() print("控制器已退出") def reset(self): """重置到初始状态""" self.joint = self.init_joint self.pose = self.init_pose self.pose_speeds = [0.0] * 6 self.gripper_open = False self.gripper_speed = 10 print("已重置到初始状态") if __name__ == "__main__": # 初始化睿尔曼机械臂 arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) # 创建机械臂连接 handle = arm.rm_create_robot_arm("192.168.3.18", 8080) print(f"机械臂连接ID: {handle.id}") # 使能机械臂 enable_fun(arm=arm) # init joint init_joint = [-90, 90, 90, -90, -90, 90] arm.rm_movej(init_joint, 50, 0, 0, 1) init_pose = arm.rm_get_current_arm_state()[1]['pose'] teleop = EndPoseController(init_joint, init_pose) try: while True: # 获取当前控制命令 action = teleop.get_action() # 构建目标位姿列表 [X, Y, Z, RX, RY, RZ] target_pose = [ action['X'], # X (m) action['Y'], # Y (m) action['Z'], # Z (m) action['RX'], # RX (rad) action['RY'], # RY (rad) action['RZ'] # RZ (rad) ] # 使用笛卡尔空间直线运动控制末端位姿 # 参数: 目标位姿, 速度比例(20%), 交融半径(0), 连接标志(0), 阻塞模式(0-非阻塞) result = arm.rm_movej_p(target_pose, 50, 0, 0, 0) if result != 0: print(f"运动控制错误，错误码: {result}") if action['gripper_open']: # arm.rm_set_gripper_release(action['gripper_speed'], block=True) arm.rm_set_gripper_position(1000, True, 1) else: # arm.rm_set_gripper_pick(action['gripper_speed'], force=50, block=True) arm.rm_set_gripper_position(1, True, 1) # 获取当前机械臂状态 ret = arm.rm_get_current_arm_state() if ret[0] == 0: # 成功获取状态 current_pose = ret[1].get('pose', target_pose) current_joint = ret[1].get('joint', [0]*7) teleop.update_state(current_pose, current_joint) print("控制模式: 末端控制") print(f"目标位姿: {target_pose}") print(f"当前位姿: {current_pose}") print(f"关节位置: {current_joint}") else: print(f"获取机械臂状态失败，错误码: {ret[0]}") time.sleep(0.1) except KeyboardInterrupt: print("程序被用户中断") finally: # 清理资源 teleop.stop() arm.rm_delete_robot_arm() print("程序退出完成")