From ba241c1f554fc5efae1a798a59e2f4ecc9668f69 Mon Sep 17 00:00:00 2001 From: tangger Date: Mon, 5 May 2025 18:16:30 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BE=AE=E8=B0=83=E6=89=8B=E6=9F=84=E6=8E=A7?= =?UTF-8?q?=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lerobot/common/robot_devices/motors/piper.py | 4 +- lerobot/common/robot_devices/robots/piper.py | 2 +- .../common/robot_devices/teleop/gamepad.py | 17 +- piper_scripts/check_controller_xbox.py | 146 ++++++++++++++++++ 4 files changed, 162 insertions(+), 7 deletions(-) create mode 100644 piper_scripts/check_controller_xbox.py diff --git a/lerobot/common/robot_devices/motors/piper.py b/lerobot/common/robot_devices/motors/piper.py index bd335af..b8fdd5c 100644 --- a/lerobot/common/robot_devices/motors/piper.py +++ b/lerobot/common/robot_devices/motors/piper.py @@ -113,7 +113,9 @@ class PiperMotorsBus: joint_5 = round(target_joint[5]*self.joint_factor) gripper_range = round(target_joint[6]*1000*1000) - self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) # joint control + self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) # joint control # 0x00 标准模式 + # self.piper.MotionCtrl_2(0x01, 0x01, 10, 0xAD) # joint control # mit模式 + self.piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5) self.piper.GripperCtrl(abs(gripper_range), 1000, 0x01, 0) # 单位 0.001° diff --git a/lerobot/common/robot_devices/robots/piper.py b/lerobot/common/robot_devices/robots/piper.py index 133e43e..2beb492 100644 --- a/lerobot/common/robot_devices/robots/piper.py +++ b/lerobot/common/robot_devices/robots/piper.py @@ -136,7 +136,7 @@ class PiperRobot: ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: if not self.is_connected: raise ConnectionError() - + if self.teleop is None and self.inference_time: self.teleop = SixAxisArmController() diff --git a/lerobot/common/robot_devices/teleop/gamepad.py b/lerobot/common/robot_devices/teleop/gamepad.py index 5e6ec6f..f21743b 100755 --- a/lerobot/common/robot_devices/teleop/gamepad.py +++ b/lerobot/common/robot_devices/teleop/gamepad.py @@ -26,10 +26,10 @@ class SixAxisArmController: # 定义关节弧度限制（计算好的范围） self.joint_limits = [ (-92000 / 57324.840764, 92000 / 57324.840764), # joint1 - (-1300 / 57324.840764, 90000 / 57324.840764), # joint2 + ( 0 / 57324.840764, 120000 / 57324.840764), # joint2 (-80000 / 57324.840764, 0 / 57324.840764), # joint3 (-90000 / 57324.840764, 90000 / 57324.840764), # joint4 - (-77000 / 57324.840764, 19000 / 57324.840764), # joint5 + (-65000 / 57324.840764, 65000 / 57324.840764), # joint5 (-90000 / 57324.840764, 90000 / 57324.840764) # joint6 ] @@ -56,9 +56,13 @@ class SixAxisArmController: if abs(left_y) < 0.5: left_y = 0.0 - right_x = -self.joystick.get_axis(3) # 右摇杆x轴（取反，因为y轴向下为正） + right_x = self.joystick.get_axis(3) # 右摇杆x轴（取反，因为y轴向下为正） if abs(right_x) < 0.5: right_x = 0.0 + + right_y = self.joystick.get_axis(4) # 右摇杆y轴 + if abs(right_y) < 0.5: + right_y = 0.0 # 获取方向键输入 hat = self.joystick.get_hat(0) @@ -72,13 +76,16 @@ class SixAxisArmController: cross = self.joystick.get_button(0) # 叉按钮 triangle = self.joystick.get_button(2) square = self.joystick.get_button(3) + + LB = self.joystick.get_button(4) # LB按钮 + RB = self.joystick.get_button(5) # RB按钮 # 映射输入到速度 self.speeds[0] = left_x * 0.01 # joint1速度 self.speeds[1] = left_y * 0.01 # joint2速度 - self.speeds[2] = 0.01 if triangle else (-0.01 if square else 0.0) # joint3速度 + self.speeds[2] = right_y * 0.01 #0.01 if triangle else (-0.01 if square else 0.0) # joint3速度 self.speeds[3] = right_x * 0.01 # joint4速度 - self.speeds[4] = 0.01 if up else (-0.01 if down else 0.0) # joint5速度 + self.speeds[4] = -0.01 if up else (0.01 if down else 0.0) # joint5速度 self.speeds[5] = 0.01 if right else (-0.01 if left else 0.0) # joint6速度 self.gripper_speed = 0.01 if circle else (-0.01 if cross else 0.0) # 夹爪速度 diff --git a/piper_scripts/check_controller_xbox.py b/piper_scripts/check_controller_xbox.py new file mode 100644 index 0000000..0012b69 --- /dev/null +++ b/piper_scripts/check_controller_xbox.py @@ -0,0 +1,146 @@ +import pygame +import time + +pygame.init() +pygame.joystick.init() + +if pygame.joystick.get_count() == 0: + print("没有检测到手柄，请插入手柄后重试。") + exit() + +joystick = pygame.joystick.Joystick(0) +joystick.init() +print(f"检测到手柄: {joystick.get_name()}\n") + +# 手柄按键和功能列表 +button_names = [ + ("A 键", "button"), + ("B 键", "button"), + ("X 键", "button"), + ("Y 键", "button"), + ("左摇杆（按下）", "button"), + ("右摇杆（按下）", "button"), + ("LB（左肩键）", "button"), + ("RB（右肩键）", "button"), + ("BACK/SELECT", "button"), + ("START", "button"), + ("方向键 ↑", "hat_up"), + ("方向键 ↓", "hat_down"), + ("方向键 ←", "hat_left"), + ("方向键 →", "hat_right"), + ("左摇杆（上推）", "axis", 1, -1), + ("左摇杆（下推）", "axis", 1, 1), + ("左摇杆（左推）", "axis", 0, -1), + ("左摇杆（右推）", "axis", 0, 1), + ("右摇杆（上推）", "axis_guess", "up"), + ("右摇杆（下推）", "axis_guess", "down"), + ("右摇杆（左推）", "axis_guess", "left"), + ("右摇杆（右推）", "axis_guess", "right"), +] + +results = {} + +print("请根据提示依次操作，操作后立即松开。\n") + +for item in button_names: + name = item[0] + print(f"请操作：{name}，操作后松开，等待检测...") + + detected = False + start_time = time.time() + + while not detected: + pygame.event.pump() + + # 检查按钮 + if item[1] == "button": + for i in range(joystick.get_numbuttons()): + if joystick.get_button(i): + print(f"检测到：{name} -> 按钮编号: {i}") + results[name] = f"按钮 {i}" + detected = True + while joystick.get_button(i): # 等待松开 + pygame.event.pump() + break + + # 检查方向键 + elif item[1].startswith("hat"): + for i in range(joystick.get_numhats()): + hat = joystick.get_hat(i) + if item[1] == "hat_up" and hat[1] == 1: + print(f"检测到：{name} -> hat编号: {i}, 数值: {hat}") + results[name] = f"hat {i}, {hat}" + detected = True + elif item[1] == "hat_down" and hat[1] == -1: + print(f"检测到：{name} -> hat编号: {i}, 数值: {hat}") + results[name] = f"hat {i}, {hat}" + detected = True + elif item[1] == "hat_left" and hat[0] == -1: + print(f"检测到：{name} -> hat编号: {i}, 数值: {hat}") + results[name] = f"hat {i}, {hat}" + detected = True + elif item[1] == "hat_right" and hat[0] == 1: + print(f"检测到：{name} -> hat编号: {i}, 数值: {hat}") + results[name] = f"hat {i}, {hat}" + detected = True + if detected: + while joystick.get_hat(i) == hat: + pygame.event.pump() + break + + # 检查左摇杆 + elif item[1] == "axis": + axis_id = item[2] + direction = item[3] + val = joystick.get_axis(axis_id) + if direction == -1 and val < -0.7: + print(f"检测到：{name} -> 轴编号: {axis_id}, 数值: {val:.2f}") + results[name] = f"axis {axis_id}, {val:.2f}" + detected = True + elif direction == 1 and val > 0.7: + print(f"检测到：{name} -> 轴编号: {axis_id}, 数值: {val:.2f}") + results[name] = f"axis {axis_id}, {val:.2f}" + detected = True + if detected: + # 不强制等待回中，立即进入下一步 + break + + # 猜测右摇杆的轴编号 + elif item[1] == "axis_guess": + print("当前所有轴数值如下（请推动右摇杆到指定方向）：") + for axis_id in range(joystick.get_numaxes()): + print(f" axis {axis_id}: {joystick.get_axis(axis_id):.2f}", end="; ") + print() + # 下面等待你推动，检测哪个轴发生较大变化 + # 只检测一次 + max_diff = 0 + target_axis = None + target_val = None + base = [joystick.get_axis(i) for i in range(joystick.get_numaxes())] + time.sleep(0.1) + for _ in range(15): + pygame.event.pump() + for axis_id in range(joystick.get_numaxes()): + now = joystick.get_axis(axis_id) + diff = abs(now - base[axis_id]) + if diff > 0.6 and diff > max_diff: + max_diff = diff + target_axis = axis_id + target_val = now + time.sleep(0.02) + if target_axis is not None: + print(f"检测到：{name} -> 轴编号: {target_axis}, 数值: {target_val:.2f}") + results[name] = f"axis {target_axis}, {target_val:.2f}" + detected = True + else: + print("未检测到明显变化，请重试。") + time.sleep(0.5) + continue + + time.sleep(0.25) + +print("\n=== 检测结果（请复制以下内容发给我） ===") +for k, v in results.items(): + print(f"{k}: {v}") + +print("全部完成！")