add unitree_g1_robot_class

src/lerobot/robots/unitree_g1/__init__.py

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from .config_unitree_g1 import UnitreeG1Config
+from .unitree_g1 import UnitreeG1

src/lerobot/robots/unitree_g1/config_unitree_g1.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from dataclasses import dataclass, field
+
+from lerobot.cameras import CameraConfig
+
+from ..config import RobotConfig
+
+
+@RobotConfig.register_subclass("unitree_g1")
+@dataclass
+class UnitreeG1Config(RobotConfig):
+   # id: str = "unitree_g1"
+    simulation_mode: bool = True
+    kp_high = 40.0
+    kd_high = 3.0
+    kp_low = 80.0
+    kd_low = 3.0
+    kp_wrist = 40.0
+    kd_wrist = 1.5
+    all_motor_q = None
+    arm_velocity_limit = 100.0
+    control_dt = 1.0 / 250.0
+
+    all_motor_q = None
+    arm_velocity_limit = 100.0
+    control_dt = 1.0 / 250.0
+
+    gradual_start_time: float | None = None
+    gradual_time: float | None = None
+
+    freeze_body: bool = False
+    gravity_compensation: bool = True
+
+    cameras: dict[str, CameraConfig] = field(default_factory=dict)
+
+    # Socket communication configuration (REQUIRED)
+    # This robot class ONLY uses sockets to communicate with a bridge on the Orin
+    # Run 'python dds_to_socket.py' on the Orin first, then set this to the Orin's IP
+    # Example: socket_host="192.168.123.164" (Orin's wlan0 IP)
+    socket_host: str | None = None# = "172.18.129.215"
+    socket_port: int | None = None
+
+    # Locomotion control
+    locomotion_control: bool = False
+    #policy_path: str = "src/lerobot/robots/unitree_g1/assets/g1/locomotion/motion.pt"
+    policy_path: str = "src/lerobot/robots/unitree_g1/assets/g1/locomotion/GR00T-WholeBodyControl-Walk.onnx"
+    
+    # Locomotion parameters (from g1.yaml)
+    locomotion_control_dt: float = 0.02
+    
+    leg_joint2motor_idx: list = field(default_factory=lambda: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11])
+    locomotion_kps: list = field(default_factory=lambda: [150, 150, 150, 300, 40, 40, 150, 150, 150, 300, 40, 40])
+    locomotion_kds: list = field(default_factory=lambda: [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2])
+    default_leg_angles: list = field(default_factory=lambda: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0, -0.1, 0.0, 0.0, 0.3, -0.2, 0.0])
+
+    arm_waist_joint2motor_idx: list = field(default_factory=lambda: [12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28])
+    locomotion_arm_waist_kps: list = field(default_factory=lambda: [250, 250, 250, 100, 100, 50, 50, 20, 20, 20, 100, 100, 50, 50, 20, 20, 20])
+    locomotion_arm_waist_kds: list = field(default_factory=lambda: [5, 5, 5, 2, 2, 2, 2, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1])
+    locomotion_arm_waist_target: list = field(default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+    
+    ang_vel_scale: float = 0.25
+    dof_pos_scale: float = 1.0
+    dof_vel_scale: float = 0.05
+    locomotion_action_scale: float = 0.25
+    cmd_scale: list = field(default_factory=lambda: [2.0, 2.0, 0.25])
+    
+    # GR00T-specific scaling (different from regular locomotion!)
+    groot_ang_vel_scale: float = 0.25  # GR00T uses 0.5, not 0.25
+    groot_cmd_scale: list = field(default_factory=lambda: [2.0, 2.0, 0.25])  # yaw is 0.5 for GR00T
+    num_locomotion_actions: int = 12
+    num_locomotion_obs: int = 47
+    max_cmd: list = field(default_factory=lambda: [0.8, 0.5, 1.57])
+    locomotion_imu_type: str = "pelvis"  # "torso" or "pelvis"

src/lerobot/robots/unitree_g1/robot_server.py

@@ -0,0 +1,129 @@
+#!/usr/bin/env python3
+import time
+import pickle
+import threading
+
+import zmq
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as hg_LowCmd, LowState_ as hg_LowState
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+kTopicLowCommand_Debug = "rt/lowcmd"
+kTopicLowState = "rt/lowstate"
+
+LOWCMD_PORT = 6000      # laptop -> robot
+LOWSTATE_PORT = 6001    # robot -> laptop
+
+
+def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float):
+    """
+    read lowstate from dds and push to laptop at ~state_period.
+    runs in its own thread.
+    """
+    last_state_time = 0.0
+
+    while True:
+        # read from dds (blocking)
+        msg = lowstate_sub.Read()
+        if msg is None:
+            continue
+
+        now = time.time()
+        # optional downsampling (if robot dds rate > state_period)
+        if now - last_state_time >= state_period:
+            payload = pickle.dumps((kTopicLowState, msg), protocol=pickle.HIGHEST_PROTOCOL)
+            try:
+                lowstate_sock.send(payload, zmq.NOBLOCK)
+            except zmq.Again:
+                # if no subscribers / tx buffer full, just drop
+                pass
+            last_state_time = now
+
+
+def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc: CRC):
+    """
+    read lowcmd from laptop (zmq) and push to dds.
+    runs in its own thread.
+    """
+    while True:
+        # blocking wait for commands from laptop
+        payload = lowcmd_sock.recv()
+        topic, cmd = pickle.loads(payload)  # cmd is hg_LowCmd
+
+        # recompute crc just in case
+        cmd.crc = crc.Crc(cmd)
+
+        if topic == kTopicLowCommand_Debug:
+            lowcmd_pub_debug.Write(cmd)
+        else:
+            # ignore unknown topics
+            pass
+
+
+def main():
+    # dds init
+    ChannelFactoryInitialize(0)
+
+    # acquire motion mode on the robot
+    msc = MotionSwitcherClient()
+    msc.SetTimeout(5.0)
+    msc.Init()
+
+    status, result = msc.CheckMode()
+    while result is not None and "name" in result and result["name"]:
+        msc.ReleaseMode()
+        status, result = msc.CheckMode()
+        time.sleep(1.0)
+
+    crc = CRC()
+
+    # dds publishers / subscriber
+    lowcmd_pub_debug = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd)
+    lowcmd_pub_debug.Init()
+
+    lowstate_sub = ChannelSubscriber(kTopicLowState, hg_LowState)
+    lowstate_sub.Init()
+
+    # zmq setup
+    ctx = zmq.Context.instance()
+
+    # commands from laptop
+    lowcmd_sock = ctx.socket(zmq.PULL)
+    lowcmd_sock.bind(f"tcp://0.0.0.0:{LOWCMD_PORT}")
+
+    # state to laptop
+    lowstate_sock = ctx.socket(zmq.PUB)
+    lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}")
+
+    state_period = 0.002  # ~500 hz
+
+    # start threads
+    t_state = threading.Thread(
+        target=state_forward_loop,
+        args=(lowstate_sub, lowstate_sock, state_period),
+        daemon=True,
+    )
+    t_cmd = threading.Thread(
+        target=cmd_forward_loop,
+        args=(lowcmd_sock, lowcmd_pub_debug, crc),
+        daemon=True,
+    )
+
+    t_state.start()
+    t_cmd.start()
+
+    print("bridge running (lowstate -> zmq, lowcmd -> dds)")
+
+    # keep main thread alive so daemon threads don’t exit
+    try:
+        while True:
+            time.sleep(1.0)
+    except KeyboardInterrupt:
+        print("shutting down bridge...")
+        # sockets/context will be cleaned up on process exit
+
+
+if __name__ == "__main__":
+    main()

src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py

@@ -0,0 +1,73 @@
+# unitree_sdk2_socket.py
+import zmq
+import pickle
+import time
+
+# you can tune these or read from env
+ROBOT_IP = "172.18.129.215"
+LOWCMD_PORT = 6000      # laptop -> robot
+LOWSTATE_PORT = 6001    # robot -> laptop
+
+_ctx = None
+_lowcmd_sock = None
+_lowstate_sock = None
+
+def ChannelFactoryInitialize(*args, **kwargs):
+    global _ctx, _lowcmd_sock, _lowstate_sock
+    if _ctx is not None:
+        return
+    _ctx = zmq.Context.instance()
+
+    # lowcmd: PUSH from laptop to robot
+    _lowcmd_sock = _ctx.socket(zmq.PUSH)
+    _lowcmd_sock.setsockopt(zmq.CONFLATE, 1)      
+    _lowcmd_sock.connect(f"tcp://{ROBOT_IP}:{LOWCMD_PORT}")
+
+    # lowstate: SUB from robot
+    _lowstate_sock = _ctx.socket(zmq.SUB)      # no topic filtering
+    _lowstate_sock.setsockopt(zmq.CONFLATE, 1)          # keep only last message
+    _lowstate_sock.connect(f"tcp://{ROBOT_IP}:{LOWSTATE_PORT}")
+    _lowstate_sock.setsockopt_string(zmq.SUBSCRIBE, "")  # subscribe to all
+
+
+class ChannelPublisher:
+    # just enough api for your code: __init__, Init, Write
+    def __init__(self, topic, msg_type):
+        # we ignore topic/msg_type, the bridge only supports the topics you use
+        self.topic = topic
+        self.msg_type = msg_type
+
+    def Init(self):
+        # nothing to do, sockets are global
+        pass
+
+    def Write(self, msg):
+        # msg is hg_LowCmd_ instance – we just pickle it
+        payload = pickle.dumps((self.topic, msg))
+        _lowcmd_sock.send(payload)
+
+
+class ChannelSubscriber:
+    # api: __init__, Init, Read
+    def __init__(self, topic, msg_type):
+        self.topic = topic
+        self.msg_type = msg_type
+
+    def Init(self):
+        pass
+
+    def Read(self, timeout_ms=None):
+        """Block until we get a lowstate, optionally with timeout (ms)."""
+        if timeout_ms is None:
+            payload = _lowstate_sock.recv()
+        else:
+            poller = zmq.Poller()
+            poller.register(_lowstate_sock, zmq.POLLIN)
+            events = dict(poller.poll(timeout_ms))
+            if _lowstate_sock not in events:
+                return None
+            payload = _lowstate_sock.recv()
+
+        topic, msg = pickle.loads(payload)
+        # you can assert topic == self.topic, but not necessary if you only use one
+        return msg