cast robot data to int/float

src/lerobot/robots/unitree_g1/run_g1_server.py

@@ -54,25 +54,25 @@ def lowstate_to_dict(msg: hg_LowState) -> dict[str, Any]:
     for i in range(NUM_MOTORS):
         motor_states.append(
             {
-                "q": msg.motor_state[i].q,
+                "q": float(msg.motor_state[i].q),
-                "dq": msg.motor_state[i].dq,
+                "dq": float(msg.motor_state[i].dq),
-                "tau_est": msg.motor_state[i].tau_est,
+                "tau_est": float(msg.motor_state[i].tau_est),
-                "temperature": msg.motor_state[i].temperature,
+                "temperature": float(msg.motor_state[i].temperature),
             }
         )
 
     return {
         "motor_state": motor_states,
         "imu_state": {
-            "quaternion": list(msg.imu_state.quaternion),
+            "quaternion": [float(x) for x in msg.imu_state.quaternion],
-            "gyroscope": list(msg.imu_state.gyroscope),
+            "gyroscope": [float(x) for x in msg.imu_state.gyroscope],
-            "accelerometer": list(msg.imu_state.accelerometer),
+            "accelerometer": [float(x) for x in msg.imu_state.accelerometer],
-            "rpy": list(msg.imu_state.rpy),
+            "rpy": [float(x) for x in msg.imu_state.rpy],
-            "temperature": msg.imu_state.temperature,
+            "temperature": float(msg.imu_state.temperature),
         },
         # Encode bytes as base64 for JSON compatibility
         "wireless_remote": base64.b64encode(bytes(msg.wireless_remote)).decode("ascii"),
-        "mode_machine": msg.mode_machine,
+        "mode_machine": int(msg.mode_machine),
     }
 
 

src/lerobot/robots/unitree_g1/unitree_g1.py

@@ -18,6 +18,7 @@ import logging
 import struct
 import threading
 import time
+from dataclasses import dataclass, field
 from functools import cached_property
 from typing import Any
 
@@ -53,29 +54,29 @@ H1_2_Num_Motors = 35
 H1_Num_Motors = 20
 
 
+@dataclass
 class MotorState:
-    def __init__(self):
+    q: float | None = None  # position
-        self.q = None  # position
+    dq: float | None = None  # velocity
-        self.dq = None  # velocity
+    tau_est: float | None = None  # estimated torque
-        self.tau_est = None  # estimated torque
+    temperature: float | None = None  # motor temperature
-        self.temperature = None  # motor temperature
 
 
+@dataclass
 class IMUState:
-    def __init__(self):
+    quaternion: np.ndarray | None = None  # [w, x, y, z]
-        self.quaternion = None  # [w, x, y, z]
+    gyroscope: np.ndarray | None = None  # [x, y, z] angular velocity (rad/s)
-        self.gyroscope = None  # [x, y, z] angular velocity (rad/s)
+    accelerometer: np.ndarray | None = None  # [x, y, z] linear acceleration (m/s²)
-        self.accelerometer = None  # [x, y, z] linear acceleration (m/s²)
+    rpy: np.ndarray | None = None  # [roll, pitch, yaw] (rad)
-        self.rpy = None  # [roll, pitch, yaw] (rad)
+    temperature: float | None = None  # IMU temperature
-        self.temperature = None  # IMU temperature
 
 
 # g1 observation class
+@dataclass
 class G1_29_LowState:  # noqa: N801
-    def __init__(self):
+    motor_state: list[MotorState] = field(default_factory=lambda: [MotorState() for _ in range(G1_29_Num_Motors)])
-        self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)]
+    imu_state: IMUState = field(default_factory=IMUState)
-        self.imu_state = IMUState()
+    wireless_remote: Any = None  # Raw wireless remote data
-        self.wireless_remote = None  # Raw wireless remote data
 
 
 class DataBuffer:
@@ -148,11 +149,17 @@ class UnitreeG1(Robot):
         self.crc = CRC()
         self.msg = unitree_hg_msg_dds__LowCmd_()
         self.msg.mode_pr = 0
-        self.msg.mode_machine = self.lowstate_subscriber.Read().mode_machine
+        
+        # Wait for first state message to arrive
+        lowstate = None
+        while lowstate is None:
+            lowstate = self.lowstate_buffer.get_data()
+            if lowstate is None:
+                time.sleep(0.01)
+        
+        self.msg.mode_machine = lowstate.mode_machine if hasattr(lowstate, 'mode_machine') else 0
 
         # initialize all motors with unified kp/kd from config
-        lowstate = self.lowstate_buffer.get_data()
-
         self.kp = np.array(config.kp, dtype=np.float32)
         self.kd = np.array(config.kd, dtype=np.float32)
 
@@ -221,7 +228,7 @@ class UnitreeG1(Robot):
 
     @property
     def is_connected(self) -> bool:
-        return self.lowstate_buffer.get_data() is None
+        return self.lowstate_buffer.get_data() is not None
 
     @property
     def _motors_ft(self) -> dict[str, type]:

src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py

@@ -72,20 +72,20 @@ def lowcmd_to_dict(topic: str, msg: Any) -> dict[str, Any]:
     for i in range(len(msg.motor_cmd)):
         motor_cmds.append(
             {
-                "mode": msg.motor_cmd[i].mode,
+                "mode": int(msg.motor_cmd[i].mode),
-                "q": msg.motor_cmd[i].q,
+                "q": float(msg.motor_cmd[i].q),
-                "dq": msg.motor_cmd[i].dq,
+                "dq": float(msg.motor_cmd[i].dq),
-                "kp": msg.motor_cmd[i].kp,
+                "kp": float(msg.motor_cmd[i].kp),
-                "kd": msg.motor_cmd[i].kd,
+                "kd": float(msg.motor_cmd[i].kd),
-                "tau": msg.motor_cmd[i].tau,
+                "tau": float(msg.motor_cmd[i].tau),
             }
         )
 
     return {
         "topic": topic,
         "data": {
-            "mode_pr": msg.mode_pr,
+            "mode_pr": int(msg.mode_pr),
-            "mode_machine": msg.mode_machine,
+            "mode_machine": int(msg.mode_machine),
             "motor_cmd": motor_cmds,
         },
     }