微调手柄控制

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°
     

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()
 

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)  # 夹爪速度
             

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("全部完成！")