微调手柄控制

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