single arm

lerobot/common/robot_devices/robots/configs.py

@@ -683,7 +683,8 @@ 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"
+    axis: int = 6
+    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 +702,27 @@ 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]}
+            )
+        }
+    )
+
+    right_follower_arm: dict[str, MotorsBusConfig] = field(
+        default_factory=lambda: {
+            "main": RealmanMotorsBusConfig(
+                ip = "192.168.3.19",
+                port = 8080,
+                motors={
+                    # name: (index, model)
+                    "joint_1": [1, "realman"],
+                    "joint_2": [2, "realman"],
+                    "joint_3": [3, "realman"],
+                    "joint_4": [4, "realman"],
+                    "joint_5": [5, "realman"],
+                    "joint_6": [6, "realman"],
+                    "gripper": [7, "realman"],
+                },
+                init_joint = {'joint': [-90, 90, 90, 90, 90, -90, 10]}
             )
         }
     )

lerobot/common/robot_devices/robots/realman.py

@@ -13,6 +13,46 @@ 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
 
+
+def smooth_movep_canfd(arm_controller, current_pose, target_pose, fps=100):
+    """
+    可变帧率版本
+    
+    Args:
+        fps: 目标帧率，决定每次移动的时长
+    """
+    total_duration = 1.0 / fps
+    control_dt = 0.002  # 2ms控制周期
+    num_points = max(1, int(total_duration / control_dt))
+    
+    print(f"{fps}fps移动: {total_duration*1000:.1f}ms, {num_points}个插值点")
+    
+    # 生成轨迹
+    trajectory = []
+    for i in range(num_points):
+        t = i / (num_points - 1) if num_points > 1 else 1.0
+        s = 6 * t**5 - 15 * t**4 + 10 * t**3  # S曲线
+        
+        pose = []
+        for axis in range(6):
+            value = current_pose[axis] + s * (target_pose[axis] - current_pose[axis])
+            pose.append(value)
+        trajectory.append(pose)
+    
+    # 执行
+    start_time = time.perf_counter()
+    for i, pose in enumerate(trajectory):
+        target_time = start_time + i * control_dt
+        current_time = time.perf_counter()
+        
+        sleep_time = target_time - current_time
+        # if sleep_time > 0:
+            # time.sleep(sleep_time)
+        
+        arm_controller.rmarm.rm_movep_canfd(pose, True)
+    
+    return True
+
 class RealmanRobot:
     def __init__(self, config: RealmanRobotConfig | None = None, **kwargs):
         if config is None:
@@ -43,6 +83,7 @@ class RealmanRobot:
         
         # build state-action cache
         self.joint_queue = deque(maxlen=2)
+        self.last_endpose = init_pose
 
         # build gamepad teleop
         if not self.inference_time:
@@ -165,8 +206,13 @@ class RealmanRobot:
             ret = self.arm.rmarm.rm_get_current_arm_state()
             current_pose = ret[1]['pose']
             self.teleop.update_endpose_state(current_pose)
-            target_joints = action['joint_angles']
-
+            target_joints = action['joint_angles'][:-1]
+            if action['master_controller_status']['infrared'] == 1:
+                if action['master_controller_status']['button'] == 1:
+                    self.arm.rmarm.rm_movej_canfd(target_joints, False)
+                else:
+                    self.arm.rmarm.rm_movej(target_joints, 5, 0, 0, 0)
+                
             # do action
             before_write_t = time.perf_counter()
             self.joint_queue.append(list(self.arm.read().values()))
@@ -174,22 +220,27 @@ class RealmanRobot:
 
         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 = [
+            #     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 = action['end_pose']
 
             # do action
             before_write_t = time.perf_counter()
             
             # result = self.arm.rmarm.rm_movej_p(target_pose, 100, 0, 0, 0)
-            self.arm.rmarm.rm_movep_follow(target_pose)
+            if self.last_endpose != target_pose:
+                # self.arm.rmarm.rm_movep_follow(target_pose)
+                # self.arm.rmarm.rm_movep_canfd(target_pose, True)
+                self.arm.rmarm.rm_movep_canfd(target_pose, False)
+                self.last_endpose = 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)
             self.joint_queue.append(list(self.arm.read().values()))

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}")
                     
@@ -324,7 +335,7 @@ class HybridController:
         
         # 如果有主臂夹爪数据，使用主臂夹爪状态
         if self.use_master_arm and "grasp" in self.master_joint_actions:
-            self.gripper = self.master_joint_actions["grasp"] * 1000
+            self.gripper = self.master_joint_actions["grasp"]# * 1000
             self.joint[-1] = self.gripper
     
 
@@ -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
@@ -37,7 +39,7 @@ bash can_activate.sh can0 1000000
 
 cd ..
 python lerobot/scripts/control_robot.py \
-    --robot.type=piper \
+    --robot.type=realman \
     --robot.inference_time=false \
     --control.type=teleoperate
 ```
@@ -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
+```

realman_src/crc_cal.py

@@ -0,0 +1,58 @@
+def crc8_maxim(data):
+    """
+    使用CRC-8/MAXIM算法计算CRC8校验值
+    多项式：X8+X5+X4+1=0x31
+    
+    Args:
+        data: 要校验的字节数据
+    
+    Returns:
+        计算出的CRC8校验值
+    """
+    # CRC8表（与您提供的表相同）
+    CRC8TAB = [
+        0x00, 0x31, 0x62, 0x53, 0xC4, 0xF5, 0xA6, 0x97,
+        0xB9, 0x88, 0xDB, 0xEA, 0x7D, 0x4C, 0x1F, 0x2E,
+        0x43, 0x72, 0x21, 0x10, 0x87, 0xB6, 0xE5, 0xD4,
+        0xFA, 0xCB, 0x98, 0xA9, 0x3E, 0x0F, 0x5C, 0x6D,
+        0x86, 0xB7, 0xE4, 0xD5, 0x42, 0x73, 0x20, 0x11,
+        0x3F, 0x0E, 0x5D, 0x6C, 0xFB, 0xCA, 0x99, 0xA8,
+        0xC5, 0xF4, 0xA7, 0x96, 0x01, 0x30, 0x63, 0x52,
+        0x7C, 0x4D, 0x1E, 0x2F, 0xB8, 0x89, 0xDA, 0xEB,
+        0x3D, 0x0C, 0x5F, 0x6E, 0xF9, 0xC8, 0x9B, 0xAA,
+        0x84, 0xB5, 0xE6, 0xD7, 0x40, 0x71, 0x22, 0x13,
+        0x7E, 0x4F, 0x1C, 0x2D, 0xBA, 0x8B, 0xD8, 0xE9,
+        0xC7, 0xF6, 0xA5, 0x94, 0x03, 0x32, 0x61, 0x50,
+        0xBB, 0x8A, 0xD9, 0xE8, 0x7F, 0x4E, 0x1D, 0x2C,
+        0x02, 0x33, 0x60, 0x51, 0xC6, 0xF7, 0xA4, 0x95,
+        0xF8, 0xC9, 0x9A, 0xAB, 0x3C, 0x0D, 0x5E, 0x6F,
+        0x41, 0x70, 0x23, 0x12, 0x85, 0xB4, 0xE7, 0xD6,
+        0x7A, 0x4B, 0x18, 0x29, 0xBE, 0x8F, 0xDC, 0xED,
+        0xC3, 0xF2, 0xA1, 0x90, 0x07, 0x36, 0x65, 0x54,
+        0x39, 0x08, 0x5B, 0x6A, 0xFD, 0xCC, 0x9F, 0xAE,
+        0x80, 0xB1, 0xE2, 0xD3, 0x44, 0x75, 0x26, 0x17,
+        0xFC, 0xCD, 0x9E, 0xAF, 0x38, 0x09, 0x5A, 0x6B,
+        0x45, 0x74, 0x27, 0x16, 0x81, 0xB0, 0xE3, 0xD2,
+        0xBF, 0x8E, 0xDD, 0xEC, 0x7B, 0x4A, 0x19, 0x28,
+        0x06, 0x37, 0x64, 0x55, 0xC2, 0xF3, 0xA0, 0x91,
+        0x47, 0x76, 0x25, 0x14, 0x83, 0xB2, 0xE1, 0xD0,
+        0xFE, 0xCF, 0x9C, 0xAD, 0x3A, 0x0B, 0x58, 0x69,
+        0x04, 0x35, 0x66, 0x57, 0xC0, 0xF1, 0xA2, 0x93,
+        0xBD, 0x8C, 0xDF, 0xFE, 0x79, 0x48, 0x1B, 0x2A,
+        0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56,
+        0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF,
+        0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15,
+        0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC
+    ]
+    
+    crc = 0
+    for byte in data:
+        crc = CRC8TAB[crc ^ byte]
+    
+    return crc
+
+
+# 数据帧：55 AA 02 00 00 67
+data = bytes([0x55, 0xAA, 0x08, 0x00, 0x02])
+crc = crc8_maxim(data)
+print(f"计算出的CRC8校验值: 0x{crc:02X}")

realman_src/servo_arm.yaml

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

realman_src/servo_arm_with_io_status.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import time
+import yaml
+import serial
+import logging
+import binascii
+import numpy as np
+
+# 配置日志记录
+logging.basicConfig(
+    level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
+)
+
+
+class ServoArm:
+    def __init__(self, config_file="config.yaml"):
+        """初始化机械臂的串口连接并发送初始数据。
+
+        Args:
+            config_file (str): 配置文件的路径。
+        """
+        self.config = self._load_config(config_file)
+        self.port = self.config["port"]
+        self.baudrate = self.config["baudrate"]
+        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)
+
+        self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0)
+
+        self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", ""))
+        self.serial_conn.write(self.bytes_to_send)
+        time.sleep(1)
+
+    def _load_config(self, config_file):
+        """加载配置文件。
+
+        Args:
+            config_file (str): 配置文件的路径。
+
+        Returns:
+            dict: 配置文件内容。
+        """
+        with open(config_file, "r") as file:
+            config = yaml.safe_load(file)
+        return config
+
+    def _bytes_to_signed_int(self, byte_data):
+        """将字节数据转换为有符号整数。
+
+        Args:
+            byte_data (bytes): 字节数据。
+
+        Returns:
+            int: 有符号整数。
+        """
+        return int.from_bytes(byte_data, byteorder="little", signed=True)
+
+    def _parse_joint_data(self, hex_received):
+        """解析接收到的十六进制数据并提取关节数据。
+
+        Args:
+            hex_received (str): 接收到的十六进制字符串数据。
+
+        Returns:
+            dict: 解析后的关节数据。
+        """
+        logging.debug(f"hex_received: {hex_received}")
+        joints = {}
+        for i in range(self.arm_axis):
+            start = 14 + i * 10
+            end = start + 8
+            joint_hex = hex_received[start:end]
+            joint_byte_data = bytearray.fromhex(joint_hex)
+            joint_value = self._bytes_to_signed_int(joint_byte_data) / 10000.0
+            joints[f"joint_{i+1}"] = joint_value
+        grasp_start = 14 + self.arm_axis*10
+        grasp_hex = hex_received[grasp_start:grasp_start+8]
+        grasp_byte_data = bytearray.fromhex(grasp_hex)
+        # 夹爪进行归一化处理
+        grasp_value = self._bytes_to_signed_int(grasp_byte_data)/1000
+        # print(grasp_value)
+
+        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):
+        """从串口读取数据并解析关节动作。
+
+        Returns:
+            dict: 包含关节数据的字典。
+        """
+        self.serial_conn.write(self.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("joints:", hex_received)
+        actions = self._parse_joint_data(hex_received)
+        return actions
+    
+    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 set_gripper_action(self, action):
+        """设置夹爪动作。
+        Args:
+            action (int): 夹爪动作值。
+        """
+        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:]
+
+if __name__ == "__main__":
+    servo_arm = ServoArm("/home/maic/LYT/lerobot/realman_src/servo_arm.yaml")
+    while True:
+        joint_actions = servo_arm.get_joint_actions()
+        logging.info(joint_actions)
+        # time.sleep(0.02)
+        status = servo_arm.get_controller_status()
+        logging.info(status)
+        time.sleep(0.5)