mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-25 02:36:11 +00:00
feat(unitree_g1): onboard locomotion mode + teleop client + camera viewer
Run the locomotion controller onboard the G1 (real local DDS) instead of on the laptop over the ZMQ bridge, so the tight policy loop no longer crosses the network: - UnitreeG1Config: `onboard` + `dds_interface` - UnitreeG1: onboard mode uses real SDK channels, releases built-in motion services, and defers the arm soft-start to the exo's first commanded pose - run_g1_onboard.py: robot-side runner (applies laptop actions via send_action) - run_g1_teleop_client.py: laptop-side thin client (exos + IK -> action over ZMQ) - view_cameras.py: lightweight side-by-side cv2 viewer for the ZMQ camera streams - run_g1.sh: one-shot launcher (conda + CAN + server) - run_g1_server.py: exit state loop cleanly on ZMQ ContextTerminated - gr00t/holosoma: cap ONNX threads; right-stick-Y waist height + stick deadzone Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Martino Russi
@@ -59,6 +59,17 @@ class UnitreeG1Config(RobotConfig):
|
||||
# Launch mujoco simulation
|
||||
is_simulation: bool = True
|
||||
|
||||
# Run the locomotion controller ONBOARD the robot (policy on the G1 itself,
|
||||
# against local DDS) instead of on the laptop over the ZMQ bridge. In this mode
|
||||
# the robot object uses the real Unitree SDK channels and expects high-level
|
||||
# actions (arm targets + joystick axes) to be fed in via send_action (e.g. by
|
||||
# run_g1_onboard.py, which receives them from the laptop). Mutually exclusive
|
||||
# with is_simulation.
|
||||
onboard: bool = False
|
||||
# DDS network interface for onboard mode (None = SDK default, matching
|
||||
# run_g1_server.py's ChannelFactoryInitialize(0)).
|
||||
dds_interface: str | None = None
|
||||
|
||||
# Socket config for ZMQ bridge
|
||||
robot_ip: str = "192.168.123.164" # default G1 IP
|
||||
|
||||
|
||||
@@ -44,6 +44,18 @@ DOF_POS_SCALE: float = 1.0
|
||||
DOF_VEL_SCALE: float = 0.05
|
||||
CMD_SCALE: list[float] = [2.0, 2.0, 0.5]
|
||||
|
||||
# Waist-height control via the right stick Y axis (the only unmapped axis).
|
||||
# Rate control (m per control step at full deflection): a self-centering stick
|
||||
# returns to 0 = "hold current height". ~0.2 m/s at 50 Hz.
|
||||
HEIGHT_STICK_RATE: float = 0.004
|
||||
HEIGHT_MIN: float = 0.50
|
||||
HEIGHT_MAX: float = 1.00
|
||||
|
||||
# Deadzone applied to all stick axes. Resting sticks drift by ~0.03-0.05, which
|
||||
# otherwise keeps cmd_magnitude above the balance/walk threshold and makes the
|
||||
# robot march in place instead of standing still on the Balance policy.
|
||||
STICK_DEADZONE: float = 0.1
|
||||
|
||||
|
||||
DEFAULT_GROOT_REPO_ID = "nepyope/GR00T-WholeBodyControl_g1"
|
||||
|
||||
@@ -68,9 +80,16 @@ def load_groot_policies(
|
||||
filename="GR00T-WholeBodyControl-Walk.onnx",
|
||||
)
|
||||
|
||||
# Load ONNX policies
|
||||
policy_balance = ort.InferenceSession(balance_path)
|
||||
policy_walk = ort.InferenceSession(walk_path)
|
||||
# Load ONNX policies. Cap onnxruntime to a single thread: these are tiny MLP
|
||||
# policies run in the real-time control loop, and letting ORT grab one intra-op
|
||||
# thread per core oversubscribes the CPU and starves the teleop/IK loop
|
||||
# (teleop becomes abysmally slow). Sequential + 1 thread is fastest here.
|
||||
so = ort.SessionOptions()
|
||||
so.intra_op_num_threads = 1
|
||||
so.inter_op_num_threads = 1
|
||||
so.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
|
||||
policy_balance = ort.InferenceSession(balance_path, sess_options=so)
|
||||
policy_walk = ort.InferenceSession(walk_path, sess_options=so)
|
||||
|
||||
logger.info("GR00T policies loaded successfully")
|
||||
|
||||
@@ -134,12 +153,19 @@ class GrootLocomotionController:
|
||||
buttons = [int(action.get(k, 0)) for k in REMOTE_BUTTONS]
|
||||
if buttons[0]: # R1 - raise waist
|
||||
self.groot_height_cmd += 0.001
|
||||
self.groot_height_cmd = np.clip(self.groot_height_cmd, 0.50, 1.00)
|
||||
self.groot_height_cmd = np.clip(self.groot_height_cmd, HEIGHT_MIN, HEIGHT_MAX)
|
||||
if buttons[4]: # R2 - lower waist
|
||||
self.groot_height_cmd -= 0.001
|
||||
self.groot_height_cmd = np.clip(self.groot_height_cmd, 0.50, 1.00)
|
||||
self.groot_height_cmd = np.clip(self.groot_height_cmd, HEIGHT_MIN, HEIGHT_MAX)
|
||||
|
||||
lx, ly, rx, _ry = (action.get(k, 0.0) for k in REMOTE_AXES)
|
||||
lx, ly, rx, ry = (action.get(k, 0.0) for k in REMOTE_AXES)
|
||||
# Deadzone every axis so resting-stick drift doesn't leak into commands.
|
||||
lx, ly, rx, ry = (0.0 if abs(v) < STICK_DEADZONE else v for v in (lx, ly, rx, ry))
|
||||
# Right stick Y controls waist height (rate control) — the only otherwise
|
||||
# unmapped axis. Push up = raise, push down = lower, release = hold.
|
||||
if ry != 0.0:
|
||||
self.groot_height_cmd += ry * HEIGHT_STICK_RATE
|
||||
self.groot_height_cmd = np.clip(self.groot_height_cmd, HEIGHT_MIN, HEIGHT_MAX)
|
||||
self.cmd[0] = ly # Forward/backward
|
||||
self.cmd[1] = -lx # Left/right (negated)
|
||||
self.cmd[2] = -rx # Rotation rate (negated)
|
||||
|
||||
@@ -78,7 +78,14 @@ def load_policy(
|
||||
logger.info(f"Loading {policy_type.upper()} policy from: {repo_id}/{filename}")
|
||||
policy_path = hf_hub_download(repo_id=repo_id, filename=filename)
|
||||
|
||||
policy = ort.InferenceSession(policy_path)
|
||||
# Cap onnxruntime to a single thread: this is a tiny policy run in the
|
||||
# real-time control loop, and letting ORT grab one intra-op thread per core
|
||||
# oversubscribes the CPU and starves the teleop/IK loop (abysmally slow teleop).
|
||||
so = ort.SessionOptions()
|
||||
so.intra_op_num_threads = 1
|
||||
so.inter_op_num_threads = 1
|
||||
so.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
|
||||
policy = ort.InferenceSession(policy_path, sess_options=so)
|
||||
logger.info(f"Policy loaded: {policy.get_inputs()[0].shape} → {policy.get_outputs()[0].shape}")
|
||||
|
||||
# Extract KP/KD from ONNX metadata
|
||||
|
||||
Executable
+38
@@ -0,0 +1,38 @@
|
||||
#!/usr/bin/env bash
|
||||
# Launch the G1 ZMQ bridge server with grippers + cameras.
|
||||
# Handles conda activation and CAN bring-up so you only run one command on the robot.
|
||||
set -euo pipefail
|
||||
|
||||
# --- conda -------------------------------------------------------------------
|
||||
CONDA_SH="${CONDA_SH:-$HOME/miniforge3/etc/profile.d/conda.sh}"
|
||||
if [[ ! -f "$CONDA_SH" ]]; then
|
||||
echo "conda.sh not found at $CONDA_SH (set CONDA_SH=/path/to/conda.sh)" >&2
|
||||
exit 1
|
||||
fi
|
||||
# shellcheck disable=SC1090
|
||||
source "$CONDA_SH"
|
||||
conda activate lerobot
|
||||
|
||||
# --- CAN bring-up + test -----------------------------------------------------
|
||||
CAN_INTERFACES="${CAN_INTERFACES:-can0,can1}"
|
||||
echo "==> Setting up CAN interfaces: $CAN_INTERFACES"
|
||||
lerobot-setup-can --mode=setup --interfaces="$CAN_INTERFACES"
|
||||
|
||||
echo "==> Testing CAN motors on: $CAN_INTERFACES"
|
||||
lerobot-setup-can --mode=test --interfaces="$CAN_INTERFACES"
|
||||
|
||||
# --- G1 server ---------------------------------------------------------------
|
||||
CAMERAS="${CAMERAS:-head_camera:/dev/v4l/by-id/usb-Intel_R__RealSense_TM__Depth_Camera_435i_Intel_R__RealSense_TM__Depth_Camera_435i_254343063964-video-index0:640x480:YUYV,left_wrist:/dev/video2:1280x720:MJPG,right_wrist:/dev/video0:1280x720:MJPG}"
|
||||
CAMERA_FPS="${CAMERA_FPS:-30}"
|
||||
GRIPPER_PORT_LEFT="${GRIPPER_PORT_LEFT:-can1}"
|
||||
GRIPPER_PORT_RIGHT="${GRIPPER_PORT_RIGHT:-can0}"
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
|
||||
echo "==> Starting G1 server"
|
||||
exec python "$SCRIPT_DIR/run_g1_server.py" \
|
||||
--grippers \
|
||||
--gripper-port-left "$GRIPPER_PORT_LEFT" \
|
||||
--gripper-port-right "$GRIPPER_PORT_RIGHT" \
|
||||
--camera-fps "$CAMERA_FPS" \
|
||||
--cameras "$CAMERAS"
|
||||
@@ -0,0 +1,111 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 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.
|
||||
|
||||
"""Run the G1 locomotion controller ONBOARD, driven by high-level actions from a laptop.
|
||||
|
||||
The locomotion policy (GRoot / Holosoma) runs on the robot itself against local DDS, at
|
||||
full control rate. The laptop reads the exos, runs IK, and ships only the resulting
|
||||
action (arm joint targets + joystick axes + gripper flags) as JSON over ZMQ. This process
|
||||
applies each action via ``UnitreeG1.send_action`` while the onboard controller thread
|
||||
keeps the legs balanced.
|
||||
|
||||
Pair with ``run_g1_teleop_client.py`` on the laptop. Grippers/cameras are handled
|
||||
separately by ``run_g1_server.py`` and are intentionally out of scope here.
|
||||
|
||||
Example (on the robot):
|
||||
|
||||
python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import signal
|
||||
import threading
|
||||
|
||||
import zmq
|
||||
|
||||
from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
|
||||
from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
|
||||
logger = logging.getLogger("g1_onboard")
|
||||
|
||||
ACTION_PORT = 6004
|
||||
|
||||
|
||||
def main() -> None:
|
||||
p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
p.add_argument("--controller", default="GrootLocomotionController", help="Locomotion controller class")
|
||||
p.add_argument("--dds-interface", default=None, help="DDS network interface (default: SDK default)")
|
||||
p.add_argument("--action-port", type=int, default=ACTION_PORT, help="ZMQ port for laptop actions")
|
||||
p.add_argument(
|
||||
"--gravity-compensation",
|
||||
action="store_true",
|
||||
help="Enable arm gravity compensation (needs pinocchio/casadi on the robot)",
|
||||
)
|
||||
args = p.parse_args()
|
||||
|
||||
cfg = UnitreeG1Config(
|
||||
is_simulation=False,
|
||||
onboard=True,
|
||||
controller=args.controller,
|
||||
dds_interface=args.dds_interface,
|
||||
gravity_compensation=args.gravity_compensation,
|
||||
cameras={},
|
||||
)
|
||||
robot = UnitreeG1(cfg)
|
||||
logger.info("Connecting onboard robot (controller=%s)...", args.controller)
|
||||
robot.connect()
|
||||
|
||||
ctx = zmq.Context.instance()
|
||||
sock = ctx.socket(zmq.PULL)
|
||||
sock.setsockopt(zmq.CONFLATE, 1) # only ever act on the freshest command
|
||||
sock.setsockopt(zmq.RCVTIMEO, 1000)
|
||||
sock.bind(f"tcp://0.0.0.0:{args.action_port}")
|
||||
logger.info("Onboard controller live. Waiting for laptop actions on :%d ...", args.action_port)
|
||||
|
||||
stop = threading.Event()
|
||||
signal.signal(signal.SIGINT, lambda *_: stop.set())
|
||||
signal.signal(signal.SIGTERM, lambda *_: stop.set())
|
||||
|
||||
n = 0
|
||||
try:
|
||||
while not stop.is_set():
|
||||
try:
|
||||
payload = sock.recv()
|
||||
except zmq.Again:
|
||||
continue
|
||||
except zmq.ContextTerminated:
|
||||
break
|
||||
|
||||
try:
|
||||
action = json.loads(payload.decode("utf-8"))
|
||||
except (ValueError, UnicodeDecodeError) as e:
|
||||
logger.warning("Dropping malformed action: %s", e)
|
||||
continue
|
||||
|
||||
robot.send_action(action)
|
||||
n += 1
|
||||
if n % 200 == 0:
|
||||
logger.info("Applied %d actions", n)
|
||||
finally:
|
||||
logger.info("Shutting down onboard controller...")
|
||||
robot.disconnect()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -26,7 +26,6 @@ Uses JSON for secure serialization instead of pickle.
|
||||
|
||||
import argparse
|
||||
import base64
|
||||
import contextlib
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
@@ -248,9 +247,14 @@ def state_forward_loop(
|
||||
# Convert to dict and serialize with JSON
|
||||
state_dict = lowstate_to_dict(msg)
|
||||
payload = json.dumps({"topic": kTopicLowState, "data": state_dict}).encode("utf-8")
|
||||
# if no subscribers / tx buffer full, just drop
|
||||
with contextlib.suppress(zmq.Again):
|
||||
try:
|
||||
# if no subscribers / tx buffer full, just drop
|
||||
lowstate_sock.send(payload, zmq.NOBLOCK)
|
||||
except zmq.Again:
|
||||
pass
|
||||
except zmq.ContextTerminated:
|
||||
# Context torn down during shutdown; exit the loop quietly.
|
||||
break
|
||||
last_state_time = now
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,123 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 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.
|
||||
|
||||
"""Laptop-side thin client: read the exos, run IK, ship the action to the onboard G1.
|
||||
|
||||
This is the counterpart to ``run_g1_onboard.py``. The heavy IK (pinocchio/casadi) stays
|
||||
on the laptop where it's set up; only the resulting action (arm joint targets + joystick
|
||||
axes + gripper flags) is sent as JSON over ZMQ. The locomotion policy runs on the robot,
|
||||
so nothing latency-critical crosses the network.
|
||||
|
||||
Example (on the laptop):
|
||||
|
||||
export LD_PRELOAD="$HOME/Documents/miniconda3/envs/lerobot312/lib/libstdc++.so.6"
|
||||
export OMP_NUM_THREADS=1 OPENBLAS_NUM_THREADS=1
|
||||
python -m lerobot.robots.unitree_g1.run_g1_teleop_client \
|
||||
--robot-ip 172.18.130.111 \
|
||||
--left-arm-port /dev/ttyACM1 --right-arm-port /dev/ttyACM0 \
|
||||
--teleop-id asdasd
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import zmq
|
||||
|
||||
from lerobot.teleoperators.unitree_g1.config_unitree_g1 import (
|
||||
ExoskeletonArmPortConfig,
|
||||
UnitreeG1TeleoperatorConfig,
|
||||
)
|
||||
from lerobot.teleoperators.unitree_g1.unitree_g1 import UnitreeG1Teleoperator
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
|
||||
logger = logging.getLogger("g1_teleop_client")
|
||||
|
||||
ACTION_PORT = 6004
|
||||
|
||||
|
||||
def _to_jsonable(action: dict) -> dict:
|
||||
"""Cast numpy scalars to plain Python so json.dumps accepts the action."""
|
||||
out: dict = {}
|
||||
for k, v in action.items():
|
||||
if isinstance(v, (np.generic,)):
|
||||
out[k] = v.item()
|
||||
else:
|
||||
out[k] = v
|
||||
return out
|
||||
|
||||
|
||||
def main() -> None:
|
||||
p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
p.add_argument("--robot-ip", required=True, help="IP of the robot running run_g1_onboard.py")
|
||||
p.add_argument("--action-port", type=int, default=ACTION_PORT)
|
||||
p.add_argument("--left-arm-port", required=True, help="Serial port for the LEFT exo arm")
|
||||
p.add_argument("--right-arm-port", required=True, help="Serial port for the RIGHT exo arm")
|
||||
p.add_argument("--teleop-id", default="exo", help="Teleoperator id (for calibration files)")
|
||||
p.add_argument("--frozen-joints", default="", help="Comma-separated joints to freeze in IK")
|
||||
p.add_argument("--fps", type=float, default=60.0, help="Max action send rate")
|
||||
args = p.parse_args()
|
||||
|
||||
cfg = UnitreeG1TeleoperatorConfig(
|
||||
id=args.teleop_id,
|
||||
left_arm_config=ExoskeletonArmPortConfig(port=args.left_arm_port),
|
||||
right_arm_config=ExoskeletonArmPortConfig(port=args.right_arm_port),
|
||||
frozen_joints=args.frozen_joints,
|
||||
)
|
||||
teleop = UnitreeG1Teleoperator(cfg)
|
||||
logger.info("Connecting exo teleoperator (L=%s, R=%s)...", args.left_arm_port, args.right_arm_port)
|
||||
teleop.connect()
|
||||
|
||||
ctx = zmq.Context.instance()
|
||||
sock = ctx.socket(zmq.PUSH)
|
||||
sock.setsockopt(zmq.SNDHWM, 2)
|
||||
sock.setsockopt(zmq.CONFLATE, 1) # drop stale actions rather than queue them
|
||||
sock.setsockopt(zmq.LINGER, 0)
|
||||
sock.connect(f"tcp://{args.robot_ip}:{args.action_port}")
|
||||
logger.info("Sending actions to %s:%d. Ctrl-C to stop.", args.robot_ip, args.action_port)
|
||||
|
||||
period = 1.0 / args.fps if args.fps > 0 else 0.0
|
||||
n = 0
|
||||
try:
|
||||
while True:
|
||||
t0 = time.time()
|
||||
action = teleop.get_action()
|
||||
try:
|
||||
sock.send_json(_to_jsonable(action), zmq.NOBLOCK)
|
||||
except zmq.Again:
|
||||
pass
|
||||
n += 1
|
||||
if n % 200 == 0:
|
||||
logger.info("Sent %d actions", n)
|
||||
if period:
|
||||
time.sleep(max(0.0, period - (time.time() - t0)))
|
||||
except KeyboardInterrupt:
|
||||
logger.info("Stopping teleop client...")
|
||||
finally:
|
||||
with_suppress_disconnect(teleop)
|
||||
sock.close(linger=0)
|
||||
|
||||
|
||||
def with_suppress_disconnect(teleop) -> None:
|
||||
try:
|
||||
teleop.disconnect()
|
||||
except Exception as e: # noqa: BLE001
|
||||
logger.warning("teleop.disconnect() failed: %s", e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -126,8 +126,10 @@ class UnitreeG1(Robot):
|
||||
# Initialize cameras config (ZMQ-based) - actual connection in connect()
|
||||
self._cameras = make_cameras_from_configs(config.cameras)
|
||||
|
||||
# Import channel classes based on mode
|
||||
if config.is_simulation:
|
||||
# Import channel classes based on mode. Simulation and onboard both talk to a
|
||||
# real (local) DDS via the Unitree SDK; only the laptop-side bridge client uses
|
||||
# the ZMQ socket shim.
|
||||
if config.is_simulation or config.onboard:
|
||||
self._ChannelFactoryInitialize = _SDKChannelFactoryInitialize
|
||||
self._ChannelPublisher = _SDKChannelPublisher
|
||||
self._ChannelSubscriber = _SDKChannelSubscriber
|
||||
@@ -297,8 +299,28 @@ class UnitreeG1(Robot):
|
||||
def configure(self) -> None:
|
||||
pass
|
||||
|
||||
def _release_motion_control(self) -> None:
|
||||
"""Release the robot's built-in motion services so we can send raw lowcmd.
|
||||
|
||||
Onboard-only. Mirrors run_g1_server.py: on the real robot the factory
|
||||
locomotion/hand services must relinquish control before our controller can
|
||||
write to ``rt/lowcmd``, otherwise commands are ignored or fought.
|
||||
"""
|
||||
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
|
||||
|
||||
msc = MotionSwitcherClient()
|
||||
msc.SetTimeout(5.0)
|
||||
msc.Init()
|
||||
_, result = msc.CheckMode()
|
||||
while result is not None and "name" in result and result["name"]:
|
||||
logger.info("[UnitreeG1] Releasing built-in mode '%s'...", result["name"])
|
||||
msc.ReleaseMode()
|
||||
_, result = msc.CheckMode()
|
||||
time.sleep(1.0)
|
||||
|
||||
def connect(self, calibrate: bool = True) -> None: # connect to DDS
|
||||
self._is_disconnected = False
|
||||
self._pending_arm_softstart = False
|
||||
# Initialize DDS channel and simulation environment
|
||||
if self.config.is_simulation:
|
||||
from lerobot.envs import make_env
|
||||
@@ -307,6 +329,15 @@ class UnitreeG1(Robot):
|
||||
self._env_wrapper = make_env("lerobot/unitree-g1-mujoco", trust_remote_code=True)
|
||||
# Extract the actual gym env from the dict structure
|
||||
self.sim_env = self._env_wrapper["hub_env"][0].envs[0]
|
||||
elif self.config.onboard:
|
||||
# Real robot, controller running onboard against local DDS. Initialize the
|
||||
# real SDK channel factory on the robot's DDS interface and take low-level
|
||||
# control from the built-in services before we start writing lowcmd.
|
||||
if self.config.dds_interface:
|
||||
self._ChannelFactoryInitialize(0, self.config.dds_interface)
|
||||
else:
|
||||
self._ChannelFactoryInitialize(0)
|
||||
self._release_motion_control()
|
||||
else:
|
||||
self._ChannelFactoryInitialize(0, config=self.config)
|
||||
|
||||
@@ -315,7 +346,7 @@ class UnitreeG1(Robot):
|
||||
self._gripper_sock = None
|
||||
self._last_gripper_cmd = None
|
||||
self._warned_no_gripper_buttons = False
|
||||
if not self.config.is_simulation:
|
||||
if not self.config.is_simulation and not self.config.onboard:
|
||||
try:
|
||||
import zmq
|
||||
|
||||
@@ -375,12 +406,11 @@ class UnitreeG1(Robot):
|
||||
|
||||
# Start controller thread if enabled
|
||||
if self.controller is not None:
|
||||
# Soft-start: ramp the arms from their current pose to the default position
|
||||
# before the locomotion policy takes over, avoiding a jump at startup. The
|
||||
# controller owns the legs, so send_action only moves the arms here. Runs in
|
||||
# both sim and on the real robot so the ramp is visible in MuJoCo too.
|
||||
logger.info("Soft-start: ramping arms to default position...")
|
||||
self._interpolate_to_default(duration=3.0)
|
||||
# Defer the arm soft-start to the first teleop action so we ramp to the
|
||||
# exo's actual starting pose instead of the fixed default (avoids a snap
|
||||
# when the operator's arms aren't at the default). Arms hold their current
|
||||
# pose (set above) until then; the controller owns the legs meanwhile.
|
||||
self._pending_arm_softstart = True
|
||||
|
||||
self._controller_thread = threading.Thread(target=self._controller_loop, daemon=True)
|
||||
self._controller_thread.start()
|
||||
@@ -538,6 +568,13 @@ class UnitreeG1(Robot):
|
||||
return obs
|
||||
|
||||
def send_action(self, action: RobotAction) -> RobotAction:
|
||||
# One-time soft-start: ramp the arms to the exo's first commanded pose so
|
||||
# they don't snap when teleop starts. Flag is cleared before ramping so the
|
||||
# ramp's own send_action calls don't re-enter this branch.
|
||||
if self.controller is not None and getattr(self, "_pending_arm_softstart", False):
|
||||
self._pending_arm_softstart = False
|
||||
self._softstart_arms_to_action(action)
|
||||
|
||||
action_to_publish = action
|
||||
if self.controller is not None:
|
||||
# Controller thread owns legs/waist. Here we only update joystick inputs
|
||||
@@ -570,6 +607,29 @@ class UnitreeG1(Robot):
|
||||
self._send_gripper_cmd(action)
|
||||
return action
|
||||
|
||||
def _softstart_arms_to_action(self, action: RobotAction) -> None:
|
||||
"""Ramp arms from their current pose to the exo's first commanded pose.
|
||||
|
||||
Runs once, on the first teleop action after connect, so the arms move
|
||||
smoothly to wherever the operator is holding the exoskeleton instead of
|
||||
snapping. Legs stay under the controller throughout (send_action filters
|
||||
to arm joints when a controller is active).
|
||||
"""
|
||||
target = np.array(self.config.default_positions, dtype=np.float32).copy()
|
||||
have_arm_target = False
|
||||
for joint in G1_29_JointArmIndex:
|
||||
key = f"{joint.name}.q"
|
||||
if key in action:
|
||||
target[joint.value] = float(action[key])
|
||||
have_arm_target = True
|
||||
if not have_arm_target:
|
||||
return
|
||||
logger.info("Soft-start: ramping arms to exo's first commanded pose...")
|
||||
try:
|
||||
self._interpolate_to_default(duration=3.0, default_positions=target)
|
||||
except Exception as e:
|
||||
logger.warning(f"Arm soft-start to exo pose failed ({e}); continuing.")
|
||||
|
||||
def _send_gripper_cmd(self, action: RobotAction) -> None:
|
||||
"""Forward exo R3/L3 button flags to run_g1_server to open/close the grippers.
|
||||
|
||||
|
||||
@@ -0,0 +1,124 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 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.
|
||||
|
||||
"""Lightweight side-by-side viewer for the G1 robot's ZMQ camera streams.
|
||||
|
||||
A fast cv2 alternative to ``--display_data`` (rerun), for eyeballing exactly what
|
||||
the robot is streaming. Subscribes to the same ZMQ PUB server as teleop (PUB/SUB,
|
||||
so it runs concurrently), and shows head + wrists in one window.
|
||||
|
||||
Example (run alongside teleop, no ``--display_data`` needed):
|
||||
|
||||
python -m lerobot.robots.unitree_g1.view_cameras --server-address 172.18.130.111
|
||||
|
||||
Notes:
|
||||
- Needs a GUI-capable OpenCV build (``pip install opencv-python``, not the
|
||||
``-headless`` variant) since it uses ``cv2.imshow``.
|
||||
- Camera names/resolutions default to what ``run_g1.sh`` streams.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import contextlib
|
||||
import logging
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from lerobot.cameras.zmq import ZMQCamera, ZMQCameraConfig
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
|
||||
logger = logging.getLogger("g1_view_cameras")
|
||||
|
||||
# name -> (native width, native height) as served by run_g1.sh
|
||||
DEFAULT_CAMERAS: dict[str, tuple[int, int]] = {
|
||||
"head_camera": (640, 480),
|
||||
"left_wrist": (1280, 720),
|
||||
"right_wrist": (1280, 720),
|
||||
}
|
||||
|
||||
|
||||
def _placeholder(name: str, pane_h: int) -> np.ndarray:
|
||||
img = np.zeros((pane_h, int(pane_h * 4 / 3), 3), dtype=np.uint8)
|
||||
cv2.putText(img, f"{name}: no frame", (10, pane_h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||
return img
|
||||
|
||||
|
||||
def main() -> None:
|
||||
p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
p.add_argument("--server-address", required=True, help="Robot IP running the ZMQ camera server")
|
||||
p.add_argument("--port", type=int, default=5555)
|
||||
p.add_argument("--fps", type=int, default=30)
|
||||
p.add_argument("--pane-height", type=int, default=360, help="Display height per camera pane (px)")
|
||||
p.add_argument(
|
||||
"--cameras",
|
||||
default=",".join(DEFAULT_CAMERAS),
|
||||
help="Comma-separated camera names to show (must match the server).",
|
||||
)
|
||||
args = p.parse_args()
|
||||
|
||||
names = [c.strip() for c in args.cameras.split(",") if c.strip()]
|
||||
cams: dict[str, ZMQCamera] = {}
|
||||
for name in names:
|
||||
w, h = DEFAULT_CAMERAS.get(name, (None, None))
|
||||
cfg = ZMQCameraConfig(
|
||||
server_address=args.server_address,
|
||||
port=args.port,
|
||||
camera_name=name,
|
||||
width=w,
|
||||
height=h,
|
||||
fps=args.fps,
|
||||
warmup_s=5,
|
||||
)
|
||||
cam = ZMQCamera(cfg)
|
||||
logger.info("Connecting to %s @ %s:%d ...", name, args.server_address, args.port)
|
||||
cam.connect()
|
||||
cams[name] = cam
|
||||
logger.info("All cameras connected. Press 'q' or ESC in the window to quit.")
|
||||
|
||||
win = "G1 cameras (q/ESC to quit)"
|
||||
cv2.namedWindow(win, cv2.WINDOW_NORMAL)
|
||||
try:
|
||||
while True:
|
||||
panes = []
|
||||
for name, cam in cams.items():
|
||||
frame = None
|
||||
with contextlib.suppress(Exception):
|
||||
frame = cam.read_latest(max_age_ms=2000)
|
||||
if frame is None:
|
||||
panes.append(_placeholder(name, args.pane_height))
|
||||
continue
|
||||
bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
|
||||
scale = args.pane_height / bgr.shape[0]
|
||||
disp = cv2.resize(bgr, (int(bgr.shape[1] * scale), args.pane_height))
|
||||
cv2.putText(disp, name, (8, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||||
panes.append(disp)
|
||||
|
||||
if panes:
|
||||
cv2.imshow(win, cv2.hconcat(panes))
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key in (ord("q"), 27):
|
||||
break
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
cv2.destroyAllWindows()
|
||||
for cam in cams.values():
|
||||
with contextlib.suppress(Exception):
|
||||
cam.disconnect()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user