微调手柄控制

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