添加关节控制和末端控制两种控制模式的统一类；添加测试脚本；融合到piper.py

lerobot/common/robot_devices/robots/piper.py

@@ -7,7 +7,7 @@ import torch
 import numpy as np
 from dataclasses import dataclass, field, replace
 
-from lerobot.common.robot_devices.teleop.gamepad import SixAxisArmController
+from lerobot.common.robot_devices.teleop.gamepad import SixAxisArmController, UnifiedArmController
 from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
 from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
 from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
@@ -31,7 +31,8 @@ class PiperRobot:
         
         # build gamepad teleop
         if not self.inference_time:
-            self.teleop = SixAxisArmController()
+            # self.teleop = SixAxisArmController()
+            self.teleop = UnifiedArmController()
         else:
             self.teleop = None
         
@@ -138,24 +139,31 @@ class PiperRobot:
             raise ConnectionError()
 
         if self.teleop is None and self.inference_time:
-            self.teleop = SixAxisArmController()
+            # self.teleop = SixAxisArmController()
+            self.teleop = UnifiedArmController()
 
         # read target pose state as 
         before_read_t = time.perf_counter()
         state = self.arm.read() # read current joint position from robot
-        action = self.teleop.get_action() # target joint position from gamepad
+        action = self.teleop.get_action() # target joint position/ end pose from gamepad
+        control_mode = self.teleop.get_control_mode()
         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
 
         # do action
         before_write_t = time.perf_counter()
         target_joints = list(action.values())
-        self.arm.write(target_joints)
+        self.arm.write(control_mode, target_joints)  # execute joint pos / end pose
+        # update teleop state
+        new_end_pose = self.arm.piper.GetArmEndPoseMsgs().end_pose
+        new_joint_state = self.arm.piper.GetArmJointMsgs().joint_state
+        self.teleop.update_state(control_mode, new_end_pose, new_joint_state)   # new arm state update teleop state
         self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
 
         if not record_data:
             return
         
         state = torch.as_tensor(list(state.values()), dtype=torch.float32)
+        action = self.teleop.get_joints() # 取最新的关节状态, 取joints，不用前面的action是因为action同时包含endpose或joints
         action = torch.as_tensor(list(action.values()), dtype=torch.float32)
 
         # Capture images from cameras