diff --git a/src/lerobot/robots/unitree_g1/run_g1_server.py b/src/lerobot/robots/unitree_g1/run_g1_server.py index 50b1e1929..3932c3669 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/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, - "dq": msg.motor_state[i].dq, - "tau_est": msg.motor_state[i].tau_est, - "temperature": msg.motor_state[i].temperature, + "q": float(msg.motor_state[i].q), + "dq": float(msg.motor_state[i].dq), + "tau_est": float(msg.motor_state[i].tau_est), + "temperature": float(msg.motor_state[i].temperature), } ) return { "motor_state": motor_states, "imu_state": { - "quaternion": list(msg.imu_state.quaternion), - "gyroscope": list(msg.imu_state.gyroscope), - "accelerometer": list(msg.imu_state.accelerometer), - "rpy": list(msg.imu_state.rpy), - "temperature": msg.imu_state.temperature, + "quaternion": [float(x) for x in msg.imu_state.quaternion], + "gyroscope": [float(x) for x in msg.imu_state.gyroscope], + "accelerometer": [float(x) for x in msg.imu_state.accelerometer], + "rpy": [float(x) for x in msg.imu_state.rpy], + "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), } diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index f8b297d81..3e8c2f199 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/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): - self.q = None # position - self.dq = None # velocity - self.tau_est = None # estimated torque - self.temperature = None # motor temperature + q: float | None = None # position + dq: float | None = None # velocity + tau_est: float | None = None # estimated torque + temperature: float | None = None # motor temperature +@dataclass class IMUState: - def __init__(self): - self.quaternion = None # [w, x, y, z] - self.gyroscope = None # [x, y, z] angular velocity (rad/s) - self.accelerometer = None # [x, y, z] linear acceleration (m/s²) - self.rpy = None # [roll, pitch, yaw] (rad) - self.temperature = None # IMU temperature + quaternion: np.ndarray | None = None # [w, x, y, z] + gyroscope: np.ndarray | None = None # [x, y, z] angular velocity (rad/s) + accelerometer: np.ndarray | None = None # [x, y, z] linear acceleration (m/s²) + rpy: np.ndarray | None = None # [roll, pitch, yaw] (rad) + temperature: float | None = None # IMU temperature # g1 observation class +@dataclass class G1_29_LowState: # noqa: N801 - def __init__(self): - self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)] - self.imu_state = IMUState() - self.wireless_remote = None # Raw wireless remote data + motor_state: list[MotorState] = field(default_factory=lambda: [MotorState() for _ in range(G1_29_Num_Motors)]) + imu_state: IMUState = field(default_factory=IMUState) + wireless_remote: Any = 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]: diff --git a/src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py b/src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py index 9ba027e56..459ecbf02 100644 --- a/src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py +++ b/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, - "q": msg.motor_cmd[i].q, - "dq": msg.motor_cmd[i].dq, - "kp": msg.motor_cmd[i].kp, - "kd": msg.motor_cmd[i].kd, - "tau": msg.motor_cmd[i].tau, + "mode": int(msg.motor_cmd[i].mode), + "q": float(msg.motor_cmd[i].q), + "dq": float(msg.motor_cmd[i].dq), + "kp": float(msg.motor_cmd[i].kp), + "kd": float(msg.motor_cmd[i].kd), + "tau": float(msg.motor_cmd[i].tau), } ) return { "topic": topic, "data": { - "mode_pr": msg.mode_pr, - "mode_machine": msg.mode_machine, + "mode_pr": int(msg.mode_pr), + "mode_machine": int(msg.mode_machine), "motor_cmd": motor_cmds, }, }