xbox controller demo

realman_src/realman_xbox.py

@@ -0,0 +1,352 @@
+#!/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[0] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0)  # X
+        self.pose_speeds[1] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0)  # Y
+        
+        # 设置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)
+    
+    init_joint = [-90, 90, 90, 90, -90, -90, 90]
+    init_pose = [-0.030, 0.255, 0.161, 3.142, 0, -1.57]
+
+    # 创建机械臂连接
+    handle = arm.rm_create_robot_arm("192.168.3.18", 8080)
+    print(f"机械臂连接ID: {handle.id}")
+    
+    # 使能机械臂
+    enable_fun(arm=arm)
+    
+    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, 1)
+            
+            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("程序退出完成")