single arm refactory

lerobot/common/robot_devices/motors/realman.py

@@ -15,6 +15,10 @@ class RealmanMotorsBus:
         self.motors = config.motors
         self.init_joint_position = config.init_joint['joint'] # [6 joints + 1 gripper]
         self.safe_disable_position = config.init_joint['joint']
+        self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
+        time.sleep(3)
+        ret = self.rmarm.rm_get_current_arm_state()
+        self.init_pose = ret[1]['pose']
 
     @property
     def motor_names(self) -> list[str]:
@@ -98,6 +102,18 @@ class RealmanMotorsBus:
         self.rmarm.rm_movej_p(target_endpose, 50, 0, 0, 1)
         self.rmarm.rm_set_gripper_position(gripper, block=False, timeout=2)
 
+    def write_joint_slow(self, target_joint: list):
+        self.rmarm.rm_movej(target_joint, 5, 0, 0, 0)
+
+    def write_joint_canfd(self, target_joint: list):
+        self.rmarm.rm_movej_canfd(target_joint, False)
+
+    def write_endpose_canfd(self, target_pose: list):
+        self.rmarm.rm_movep_canfd(target_pose, False)
+
+    def write_gripper(self, gripper: int):
+        self.rmarm.rm_set_gripper_position(gripper, False, 2)
+
     def read(self) -> Dict:
         """
             - 机械臂关节消息,单位1度;[-1, 1]
@@ -119,6 +135,12 @@ class RealmanMotorsBus:
             "gripper": (gripper_state-500)/500
         }
     
+    def read_current_arm_joint_state(self):
+        return self.rmarm.rm_get_current_arm_state()[1]['joint']
+    
+    def read_current_arm_endpose_state(self):
+        return self.rmarm.rm_get_current_arm_state()[1]['pose']
+
     def safe_disconnect(self):
         """ 
             Move to safe disconnect position

lerobot/common/robot_devices/robots/configs.py

@@ -683,7 +683,7 @@ class RealmanRobotConfig(RobotConfig):
     inference_time: bool = False
     max_gripper: int = 990
     min_gripper: int = 10
-    servo_config_file: str = "/home/maic/LYT/lerobot/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml"
+    servo_config_file: str = "/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_arm.yaml"
     
 
     left_follower_arm: dict[str, MotorsBusConfig] = field(
@@ -701,7 +701,7 @@ class RealmanRobotConfig(RobotConfig):
                     "joint_6": [6, "realman"],
                     "gripper": [7, "realman"],
                 },
-                init_joint = {'joint': [-90, 90, 90, -90, -90, 90, 1000]}
+                init_joint = {'joint': [-90, 90, 90, 90, 90, -90, 10]}
             )
         }
     )
@@ -721,13 +721,13 @@ class RealmanRobotConfig(RobotConfig):
                 height=480,
                 use_depth=False
             ),
-            "right": IntelRealSenseCameraConfig(
-                serial_number="405622075165",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
+            # "right": IntelRealSenseCameraConfig(
+            #     serial_number="405622075165",
+            #     fps=30,
+            #     width=640,
+            #     height=480,
+            #     use_depth=False
+            # ),
             "front": IntelRealSenseCameraConfig(
                 serial_number="145422072751",
                 fps=30,

lerobot/common/robot_devices/robots/realman.py

@@ -13,6 +13,7 @@ from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
 from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
 from lerobot.common.robot_devices.robots.configs import RealmanRobotConfig
 
+
 class RealmanRobot:
     def __init__(self, config: RealmanRobotConfig | None = None, **kwargs):
         if config is None:
@@ -28,14 +29,11 @@ class RealmanRobot:
         # build realman motors
         self.piper_motors = make_motors_buses_from_configs(self.config.left_follower_arm)
         self.arm = self.piper_motors['main']
-        self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 1)
-        time.sleep(2)
-        ret = self.arm.rmarm.rm_get_current_arm_state()
-        init_pose = ret[1]['pose']
+        
         # build init teleop info
         self.init_info = {
             'init_joint': self.arm.init_joint_position,
-            'init_pose': init_pose,
+            'init_pose': self.arm.init_pose,
             'max_gripper': config.max_gripper,
             'min_gripper': config.min_gripper,
             'servo_config_file': config.servo_config_file
@@ -43,6 +41,7 @@ class RealmanRobot:
         
         # build state-action cache
         self.joint_queue = deque(maxlen=2)
+        self.last_endpose = self.arm.init_pose
 
         # build gamepad teleop
         if not self.inference_time:
@@ -61,7 +60,7 @@ class RealmanRobot:
             cam_ft[key] = {
                 "shape": (cam.height, cam.width, cam.channels),
                 "names": ["height", "width", "channels"],
-                "info": None,   
+                "info": None,
             }
         return cam_ft
 
@@ -162,49 +161,37 @@ class RealmanRobot:
 
         if action['control_mode'] == 'joint':
             # 关节控制模式（主模式）
-            ret = self.arm.rmarm.rm_get_current_arm_state()
-            current_pose = ret[1]['pose']
+            current_pose = self.arm.read_current_arm_endpose_state()
             self.teleop.update_endpose_state(current_pose)
-            target_joints = action['joint_angles']
 
+            target_joints = action['joint_angles'][:-1]
+            self.arm.write_gripper(action['gripper'])
+            print(action['gripper'])
+            if action['master_controller_status']['infrared'] == 1:
+                if action['master_controller_status']['button'] == 1:
+                    self.arm.write_joint_canfd(target_joints)
+                else:
+                    self.arm.write_joint_slow(target_joints)
+                
             # do action
             before_write_t = time.perf_counter()
             self.joint_queue.append(list(self.arm.read().values()))
             self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
 
         else:
-            # 末端位姿控制模式
-            target_pose = [
-                action['end_pose']['X'],    # X (m)
-                action['end_pose']['Y'],    # Y (m) 
-                action['end_pose']['Z'],    # Z (m)
-                action['end_pose']['RX'],   # RX (rad)
-                action['end_pose']['RY'],   # RY (rad)
-                action['end_pose']['RZ']    # RZ (rad)
-            ]
-
+            target_pose = list(action['end_pose'])
             # do action
             before_write_t = time.perf_counter()
+            if self.last_endpose != target_pose:
+                self.arm.write_endpose_canfd(target_pose)
+                self.last_endpose = target_pose
+            self.arm.write_gripper(action['gripper'])
             
-            # result = self.arm.rmarm.rm_movej_p(target_pose, 100, 0, 0, 0)
-            self.arm.rmarm.rm_movep_follow(target_pose)
-            self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2)
-
-            ret = self.arm.rmarm.rm_get_current_arm_state()
-            target_joints = ret[1].get('joint', self.arm.init_joint_position)
+            target_joints = self.arm.read_current_arm_joint_state()
             self.joint_queue.append(list(self.arm.read().values()))
             self.teleop.update_joint_state(target_joints)
             self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
         
-        # print('-'*80)
-        # print('mode: ', action['control_mode'])
-        # print('state: ', list(state.values()))
-        # print('action: ', target_joints)
-        # print('cache[0]: ', self.joint_queue[0])
-        # print('cache[-1]: ', self.joint_queue[-1])
-        # print('time: ', time.perf_counter() - before_write_t)
-        # print('-'*80)
-        # time.sleep(1)
         if not record_data:
             return
         

lerobot/common/robot_devices/teleop/gamepad.py

@@ -20,12 +20,13 @@ class ServoArm:
         self.config = self._load_config(config_file)
         self.port = self.config["port"]
         self.baudrate = self.config["baudrate"]
-        self.hex_data = self.config["hex_data"]
+        self.joint_hex_data = self.config["joint_hex_data"]
+        self.control_hex_data = self.config["control_hex_data"]
         self.arm_axis = self.config.get("arm_axis", 7)
 
         try:
             self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0)
-            self.bytes_to_send = binascii.unhexlify(self.hex_data.replace(" ", ""))
+            self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", ""))
             self.serial_conn.write(self.bytes_to_send)
             time.sleep(1)
             self.connected = True
@@ -53,7 +54,8 @@ class ServoArm:
             return {
                 "port": "/dev/ttyUSB0",
                 "baudrate": 460800,
-                "hex_data": "55 AA 02 00 00 67",
+                "joint_hex_data": "55 AA 02 00 00 67",
+                "control_hex_data": "55 AA 08 00 00 B9",
                 "arm_axis": 6
             }
 
@@ -94,6 +96,17 @@ class ServoArm:
 
         joints["grasp"] = grasp_value
         return joints
+    
+    def _parse_controller_data(self, hex_received):
+        status = {
+            'infrared': 0,
+            'button': 0
+        }
+        if len(hex_received) == 18:
+            status['infrared'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[12:14]))
+            status['button'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[14:16]))
+            # print(infrared)
+        return status
 
     def get_joint_actions(self):
         """从串口读取数据并解析关节动作。
@@ -106,6 +119,7 @@ class ServoArm:
         
         try:
             self.serial_conn.write(self.bytes_to_send)
+            time.sleep(0.02)
             bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
             if len(bytes_received) == 0:
                 return {}
@@ -117,20 +131,15 @@ class ServoArm:
             logging.error(f"读取串口数据错误: {e}")
             return {}
 
-    def set_gripper_action(self, action):
-        """设置夹爪动作。
-        Args:
-            action (int): 夹爪动作值。
-        """
-        if not self.connected:
-            return
-        
-        try:
-            action = int(action * 1000)
-            action_bytes = action.to_bytes(4, byteorder="little", signed=True)
-            self.bytes_to_send = self.bytes_to_send[:74] + action_bytes + self.bytes_to_send[78:]
-        except Exception as e:
-            logging.error(f"设置夹爪动作错误: {e}")
+    def get_controller_status(self):
+        bytes_to_send = binascii.unhexlify(self.control_hex_data.replace(" ", ""))
+        self.serial_conn.write(bytes_to_send)
+        time.sleep(0.02)
+        bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
+        hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper()
+        # print("control status:", hex_received)
+        status = self._parse_controller_data(hex_received)
+        return status
 
     def close(self):
         """关闭串口连接"""
@@ -174,6 +183,7 @@ class HybridController:
         
         # 主臂关节状态
         self.master_joint_actions = {}
+        self.master_controller_status = {}
         self.use_master_arm = False
         
         # 末端位姿限制
@@ -282,6 +292,7 @@ class HybridController:
             if self.servo_arm and self.servo_arm.connected:
                 try:
                     self.master_joint_actions = self.servo_arm.get_joint_actions()
+                    self.master_controller_status = self.servo_arm.get_controller_status()
                     if self.master_joint_actions:
                         logging.debug(f"主臂关节状态: {self.master_joint_actions}")
                     
@@ -406,14 +417,8 @@ class HybridController:
             'control_mode': 'joint' if self.joint_control_mode else 'end_pose',
             'use_master_arm': self.use_master_arm,
             'master_joint_actions': self.master_joint_actions,
-            'end_pose': {
-                'X': self.pose[0],
-                'Y': self.pose[1],
-                'Z': self.pose[2],
-                'RX': self.pose[3],
-                'RY': self.pose[4],
-                'RZ': self.pose[5],
-            },
+            'master_controller_status': self.master_controller_status,
+            'end_pose': self.pose,
             'joint_angles': self.joint,
             'gripper': int(self.gripper),
             'tozero': self.tozero

lerobot/common/robot_devices/teleop/servo_arm.yaml

@@ -1,5 +1,6 @@
 port: /dev/ttyUSB0
 right_port: /dev/ttyUSB1
 baudrate: 460800
-hex_data: "55 AA 02 00 00 67"
+joint_hex_data: "55 AA 02 00 00 67"
+control_hex_data: "55 AA 08 00 00 B9"
 arm_axis: 6

lerobot/common/utils/utils.py

@@ -175,7 +175,8 @@ def say(text, blocking=False):
         cmd = ["say", text]
 
     elif system == "Linux":
-        cmd = ["spd-say", text]
+        # cmd = ["spd-say", text]
+        cmd = ["edge-playback", "-t", text]
         if blocking:
             cmd.append("--wait")
 

realman.md

@@ -8,6 +8,8 @@ conda activate lerobot
 Install 🤗 LeRobot:
 ```bash
 pip install -e . -i https://pypi.tuna.tsinghua.edu.cn/simple
+pip install edge-tts
+sudo apt install mpv -y
 
 # pip uninstall numpy
 # pip install numpy==1.26.0
@@ -51,18 +53,19 @@ echo $HF_USER
 
 ```bash
 python lerobot/scripts/control_robot.py \
-    --robot.type=piper \
+    --robot.type=realman \
     --robot.inference_time=false \
     --control.type=record \
-    --control.fps=30 \
+    --control.fps=15 \
     --control.single_task="move" \
-    --control.repo_id=${HF_USER}/test \
+    --control.repo_id=maic/test \
     --control.num_episodes=2 \
     --control.warmup_time_s=2 \
     --control.episode_time_s=10 \
     --control.reset_time_s=10 \
     --control.play_sounds=true \
-    --control.push_to_hub=false
+    --control.push_to_hub=false \
+    --control.display_data=true
 ```
 
 Press right arrow -> at any time during episode recording to early stop and go to resetting. Same during resetting, to early stop and to go to the next episode recording.  
@@ -104,22 +107,50 @@ python lerobot/scripts/train.py \
   --wandb.enable=true
 ``` 
 
+# FT smolvla
+python lerobot/scripts/train.py \
+ --dataset.repo_id=maic/move_the_bottle_into_ultrasonic_device_with_realman_single  \
+ --policy.path=lerobot/smolvla_base \
+ --output_dir=outputs/train/smolvla_move_the_bottle_into_ultrasonic_device_with_realman_single \
+ --job_name=smolvla_move_the_bottle_into_ultrasonic_device_with_realman_single  \
+ --policy.device=cuda  \
+ --wandb.enable=false \
+ --steps=200000 \
+ --batch_size=16
+
 
 # Inference
 还是使用control_robot.py中的record loop，配置 **--robot.inference_time=true** 可以将手柄移出。
 ```bash
 python lerobot/scripts/control_robot.py \
-    --robot.type=piper \
+    --robot.type=realman \
     --robot.inference_time=true \
     --control.type=record \
     --control.fps=30 \
-    --control.single_task="move" \
-    --control.repo_id=$USER/eval_act_jack \
+    --control.single_task="move the bottle into ultrasonic device with realman single" \
+    --control.repo_id=maic/move_the_bottle_into_ultrasonic_device_with_realman_single \
     --control.num_episodes=1 \
     --control.warmup_time_s=2 \
     --control.episode_time_s=30 \
     --control.reset_time_s=10 \
     --control.push_to_hub=false \
-    --control.policy.path=outputs/train/act_koch_pick_place_lego/checkpoints/latest/pretrained_model
+    --control.policy.path=outputs/train/act_move_the_bottle_into_ultrasonic_device_with_realman_single/checkpoints/100000/pretrained_model
 ```
 
+
+```bash
+python lerobot/scripts/control_robot.py     \
+--robot.type=realman     \
+--robot.inference_time=true     \
+--control.type=record     \
+--control.fps=30     \
+--control.single_task="move the bottle into ultrasonic device with realman single"     \
+--control.repo_id=maic/eval_smolvla_move_the_bottle_into_ultrasonic_device_with_realman_single     \
+--control.num_episodes=1     \
+--control.warmup_time_s=2     \
+--control.episode_time_s=60     \
+--control.reset_time_s=10     \
+--control.push_to_hub=false     \
+--control.policy.path=outputs/train/smolvla_move_the_bottle_into_ultrasonic_device_with_realman_single/checkpoints/160000/pretrained_model \
+--control.display_data=true
+```