mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-28 20:26:05 +00:00
feat(unitree_g1): onboard controller deployment for SONIC walk
Run the whole-body controller (SONIC decoder / GR00T) onboard the G1 against local DDS at full rate, with the laptop shipping only high-level actions over ZMQ instead of 50Hz lowcmd via the socket bridge. - config: add onboard, dds_interface, release_motion_control, physical_remote - unitree_g1: onboard connect() branch (local DDS + MotionSwitcher release + physical wireless remote), _release_motion_control, _wireless_remote_input, controller-loop wireless priority; SDK channels when sim OR onboard - run_g1_server: port Gripper/build_gripper/parse_camera_specs; add --cameras spec supporting by-path device names (survive USB re-enumeration) + FOURCC - run_g1_onboard: onboard entry point (ZMQ actions -> send_action), with a --sonic-token-action flag for the 64-D latent-token interface - infer_sonic_g1_onboard: laptop-side sender that runs nepyope/sonic_walk (pi0.5) and PUSHes 64-D tokens to the onboard controller
This commit is contained in:
Martino Russi
@@ -62,6 +62,24 @@ class UnitreeG1Config(RobotConfig):
|
||||
# Socket config for ZMQ bridge
|
||||
robot_ip: str = "192.168.123.164" # default G1 IP
|
||||
|
||||
# Run the locomotion / whole-body controller ONBOARD the robot (policy on the G1
|
||||
# itself, against local DDS at full rate) instead of on the laptop over the ZMQ
|
||||
# socket bridge. In this mode the robot object uses the real Unitree SDK channels
|
||||
# and expects high-level actions (arm targets + joystick axes, or 64-D SONIC
|
||||
# tokens) fed via send_action -- e.g. by run_g1_onboard.py, which receives them
|
||||
# from the laptop over ZMQ. 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
|
||||
# Onboard sub-flags. On a real G1 both are True: the built-in motion services
|
||||
# must be released before we can write lowcmd, and locomotion axes are read from
|
||||
# the physical wireless remote. Against a DDS sim neither applies (no
|
||||
# MotionSwitcher, no physical remote), so set both False so the controller takes
|
||||
# its locomotion axes purely from send_action (ZMQ) input.
|
||||
release_motion_control: bool = True
|
||||
physical_remote: bool = True
|
||||
|
||||
# Cameras (ZMQ-based remote cameras)
|
||||
cameras: dict[str, CameraConfig] = field(default_factory=dict)
|
||||
|
||||
|
||||
@@ -0,0 +1,187 @@
|
||||
#!/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 sender for the SONIC whole-body walk policy, onboard deployment.
|
||||
|
||||
This is the counterpart to ``run_g1_onboard.py`` (which runs the SONIC decoder on the
|
||||
robot). The heavy VLA (``nepyope/sonic_walk``, a pi0.5 token policy) runs here on the
|
||||
laptop GPU; only the resulting 64-D latent token is shipped to the robot over ZMQ:
|
||||
|
||||
laptop: camera frame (ZMQ from robot :5555) + previous token
|
||||
-> pi0.5 -> next 64-D token
|
||||
-> PUSH JSON {motion_token.i.pos: ...} to robot :6004
|
||||
robot: run_g1_onboard receives the token, SonicWholeBodyController decodes it
|
||||
into whole-body joint commands against local DDS at full rate.
|
||||
|
||||
The policy's ``observation.state`` is the token currently being executed, so we close
|
||||
the loop by feeding back the *last token we sent* (the decoder holds it until a new one
|
||||
arrives). This mirrors what ``lerobot-rollout`` does via the robot's token echo, but
|
||||
without a controller / DDS on the laptop.
|
||||
|
||||
The policy is pi0.5 with chunk_size=50, so a full diffusion inference runs only about
|
||||
once every 50 ticks; ``select_action`` pops one queued token per tick in between.
|
||||
|
||||
Run ``run_g1_onboard.py --controller SonicWholeBodyController --sonic-token-action
|
||||
--cameras ...`` on the robot first, then this on the laptop:
|
||||
|
||||
python -m lerobot.robots.unitree_g1.infer_sonic_g1_onboard \
|
||||
--policy-path nepyope/sonic_walk --robot-ip 192.168.123.164 \
|
||||
--task "walk back and forth"
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import contextlib
|
||||
import json
|
||||
import logging
|
||||
import signal
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
from lerobot.cameras.zmq import ZMQCamera, ZMQCameraConfig
|
||||
from lerobot.configs.policies import PreTrainedConfig
|
||||
from lerobot.policies.factory import get_policy_class, make_pre_post_processors
|
||||
from lerobot.policies.utils import prepare_observation_for_inference
|
||||
from lerobot.robots.unitree_g1.controllers.sonic_whole_body import TOKEN_DIM, token_action_key
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
|
||||
logger = logging.getLogger("sonic_sender")
|
||||
|
||||
ACTION_PORT = 6004 # matches run_g1_onboard.py --action-port
|
||||
IMAGE_KEY = "observation.images.ego_view" # pi05 sonic_walk VISUAL input
|
||||
STATE_KEY = "observation.state"
|
||||
|
||||
|
||||
def main() -> None:
|
||||
p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
p.add_argument("--policy-path", default="nepyope/sonic_walk", help="Policy repo id or local path")
|
||||
p.add_argument("--robot-ip", default="192.168.123.164", help="Robot IP (camera + action ports)")
|
||||
p.add_argument("--action-port", type=int, default=ACTION_PORT, help="Onboard ZMQ PULL port for actions")
|
||||
p.add_argument("--camera-port", type=int, default=5555, help="Onboard ZMQ camera PUB port")
|
||||
p.add_argument("--camera-name", default="head_camera", help="Camera name served by run_g1_onboard")
|
||||
p.add_argument("--camera-width", type=int, default=640, help="Camera width")
|
||||
p.add_argument("--camera-height", type=int, default=480, help="Camera height")
|
||||
p.add_argument("--task", default="walk back and forth", help="Language prompt for the VLA")
|
||||
p.add_argument("--fps", type=float, default=30.0, help="Token send rate (matches training inference)")
|
||||
p.add_argument("--device", default="cuda", help="Torch device")
|
||||
p.add_argument("--max-ticks", type=int, default=0, help="Stop after N ticks (0 = run forever)")
|
||||
p.add_argument("--dry-run", action="store_true", help="Run inference but do not PUSH tokens to the robot")
|
||||
args = p.parse_args()
|
||||
|
||||
device = torch.device(args.device)
|
||||
|
||||
# --- Policy + processors (normalization stats baked into the checkpoint) ---
|
||||
logger.info("Loading policy from '%s'...", args.policy_path)
|
||||
policy_cfg = PreTrainedConfig.from_pretrained(args.policy_path)
|
||||
policy_cfg.pretrained_path = args.policy_path
|
||||
policy = get_policy_class(policy_cfg.type).from_pretrained(args.policy_path, config=policy_cfg)
|
||||
policy = policy.to(device)
|
||||
policy.eval()
|
||||
policy.reset()
|
||||
|
||||
preprocessor, postprocessor = make_pre_post_processors(
|
||||
policy_cfg=policy_cfg,
|
||||
pretrained_path=args.policy_path,
|
||||
preprocessor_overrides={"device_processor": {"device": str(device)}},
|
||||
)
|
||||
logger.info("Policy loaded (type=%s, device=%s, chunk=%s)", policy_cfg.type, device,
|
||||
getattr(policy_cfg, "chunk_size", "?"))
|
||||
|
||||
# --- Camera (ZMQ from the robot's onboard image server) ---
|
||||
cam = ZMQCamera(
|
||||
ZMQCameraConfig(
|
||||
server_address=args.robot_ip,
|
||||
port=args.camera_port,
|
||||
camera_name=args.camera_name,
|
||||
width=args.camera_width,
|
||||
height=args.camera_height,
|
||||
fps=int(args.fps),
|
||||
)
|
||||
)
|
||||
logger.info("Connecting camera %s@%s:%d ...", args.camera_name, args.robot_ip, args.camera_port)
|
||||
cam.connect()
|
||||
|
||||
# --- Action PUSH socket to the onboard controller ---
|
||||
import zmq
|
||||
|
||||
ctx = zmq.Context.instance()
|
||||
sock = ctx.socket(zmq.PUSH)
|
||||
sock.setsockopt(zmq.SNDHWM, 2)
|
||||
sock.setsockopt(zmq.LINGER, 0)
|
||||
sock.connect(f"tcp://{args.robot_ip}:{args.action_port}")
|
||||
logger.info("Sending tokens to tcp://%s:%d (dry_run=%s)", args.robot_ip, args.action_port, args.dry_run)
|
||||
|
||||
stop = {"flag": False}
|
||||
signal.signal(signal.SIGINT, lambda *_: stop.__setitem__("flag", True))
|
||||
signal.signal(signal.SIGTERM, lambda *_: stop.__setitem__("flag", True))
|
||||
|
||||
# observation.state = the token currently executing on the robot (last one we sent);
|
||||
# start at zeros, matching the decoder's zero-seeded initial state.
|
||||
prev_token = np.zeros(TOKEN_DIM, dtype=np.float32)
|
||||
period = 1.0 / args.fps
|
||||
n = 0
|
||||
t_infer_total = 0.0
|
||||
logger.info("Streaming tokens at %.0f Hz. Ctrl-C to stop.", args.fps)
|
||||
try:
|
||||
while not stop["flag"]:
|
||||
t0 = time.time()
|
||||
try:
|
||||
frame = cam.read() # HxWxC uint8 RGB
|
||||
except Exception as e: # noqa: BLE001
|
||||
logger.warning("Camera read failed: %s", e)
|
||||
time.sleep(period)
|
||||
continue
|
||||
|
||||
raw_obs = {
|
||||
IMAGE_KEY: np.ascontiguousarray(frame),
|
||||
STATE_KEY: prev_token.copy(),
|
||||
}
|
||||
with torch.inference_mode():
|
||||
obs = prepare_observation_for_inference(raw_obs, device, args.task, "unitree_g1")
|
||||
obs = preprocessor(obs)
|
||||
action = policy.select_action(obs)
|
||||
action = postprocessor(action)
|
||||
token = action.squeeze(0).to("cpu").numpy().astype(np.float32)
|
||||
prev_token = token
|
||||
|
||||
if not args.dry_run:
|
||||
msg = {token_action_key(i): float(token[i]) for i in range(TOKEN_DIM)}
|
||||
with contextlib.suppress(zmq.Again):
|
||||
sock.send_string(json.dumps(msg), zmq.NOBLOCK)
|
||||
|
||||
n += 1
|
||||
t_infer_total += time.time() - t0
|
||||
if n % 30 == 0:
|
||||
logger.info(
|
||||
"tick %d | avg %.1f ms/tick | token[:3]=%s",
|
||||
n, 1000.0 * t_infer_total / 30.0, np.round(token[:3], 3).tolist(),
|
||||
)
|
||||
t_infer_total = 0.0
|
||||
|
||||
if args.max_ticks and n >= args.max_ticks:
|
||||
break
|
||||
time.sleep(max(0.0, period - (time.time() - t0)))
|
||||
finally:
|
||||
logger.info("Stopping sender after %d ticks.", n)
|
||||
with contextlib.suppress(Exception):
|
||||
cam.disconnect()
|
||||
with contextlib.suppress(Exception):
|
||||
sock.close(linger=0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,251 @@
|
||||
#!/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 / whole-body controller ONBOARD, driven by high-level actions
|
||||
from a laptop.
|
||||
|
||||
The controller (GR00T / Holosoma / SONIC whole-body) runs on the robot itself against
|
||||
local DDS, at full control rate. The laptop ships only the resulting high-level action
|
||||
(arm joint targets + joystick axes + gripper flags, or a 64-D SONIC motion token) as
|
||||
JSON over ZMQ. This process applies each action via ``UnitreeG1.send_action`` while the
|
||||
onboard controller thread keeps the legs balanced / decodes the token.
|
||||
|
||||
This is the real-deploy counterpart to running ``lerobot-rollout`` on the laptop with
|
||||
``--robot.is_simulation=false`` (the ZMQ *socket bridge*): there the 50 Hz lowcmd
|
||||
crosses the network; here only compact high-level actions do, and the control loop stays
|
||||
local to the robot. Pair with a laptop client that produces actions (exo teleop, or a
|
||||
policy such as ``nepyope/sonic_walk`` emitting ``motion_token.{i}.pos``).
|
||||
|
||||
Besides receiving actions, this process publishes ``observation.state`` (29 joint ``.q``)
|
||||
on a ZMQ PUB port so a laptop policy client has proprioception.
|
||||
|
||||
Safety: type ``e`` then Enter in this terminal to stop immediately (zero-torque + exit).
|
||||
Ctrl-C does the normal graceful shutdown (kp ramp).
|
||||
|
||||
Examples (on the robot):
|
||||
|
||||
# GR00T locomotion, arm targets from the laptop:
|
||||
python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController
|
||||
|
||||
# SONIC whole-body walk policy: laptop ships 64-D tokens, decoder runs here:
|
||||
python -m lerobot.robots.unitree_g1.run_g1_onboard \
|
||||
--controller SonicWholeBodyController --sonic-token-action \
|
||||
--cameras "head_camera:/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.3-video-index0:640x480"
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import contextlib
|
||||
import json
|
||||
import logging
|
||||
import os
|
||||
import signal
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import zmq
|
||||
|
||||
from lerobot.cameras.zmq.image_server import ImageServer
|
||||
from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
|
||||
from lerobot.robots.unitree_g1.g1_utils import G1_29_JointIndex
|
||||
from lerobot.robots.unitree_g1.run_g1_server import Gripper, build_gripper, parse_camera_specs
|
||||
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
|
||||
STATE_PORT = 6005
|
||||
|
||||
|
||||
def main() -> None:
|
||||
p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
p.add_argument("--controller", default="GrootLocomotionController", help="Controller class name")
|
||||
p.add_argument("--dds-interface", default=None, help="DDS network interface (default: SDK default)")
|
||||
p.add_argument(
|
||||
"--sim",
|
||||
action="store_true",
|
||||
help="Attach to a DDS MuJoCo sim: skip MotionSwitcher + physical remote, default dds-interface 'lo'.",
|
||||
)
|
||||
p.add_argument(
|
||||
"--sonic-token-action",
|
||||
action="store_true",
|
||||
help="SONIC token interface: actions carry a 64-D motion_token.{i}.pos that the decoder consumes.",
|
||||
)
|
||||
p.add_argument("--action-port", type=int, default=ACTION_PORT, help="ZMQ PULL port for laptop actions")
|
||||
p.add_argument("--state-port", type=int, default=STATE_PORT, help="ZMQ PUB port for observation.state")
|
||||
p.add_argument("--state-fps", type=float, default=30.0, help="observation.state publish rate; <=0 disables")
|
||||
p.add_argument("--gravity-compensation", action="store_true", help="Enable arm gravity compensation")
|
||||
# Gripper control (Damiao over CAN).
|
||||
p.add_argument("--grippers", action="store_true", help="Drive Damiao grippers from action L3/R3 flags")
|
||||
p.add_argument("--gripper-port-left", default="can1", help="CAN interface for LEFT gripper")
|
||||
p.add_argument("--gripper-port-right", default="can0", help="CAN interface for RIGHT gripper")
|
||||
p.add_argument("--gripper-send-id", type=lambda x: int(x, 0), default=0x08, help="Motor send CAN id")
|
||||
p.add_argument("--gripper-recv-id", type=lambda x: int(x, 0), default=0x18, help="Motor recv CAN id")
|
||||
p.add_argument("--gripper-motor-type", default="dm4310", help="Damiao motor type")
|
||||
p.add_argument("--gripper-open-deg", type=float, default=-65.0, help="Gripper OPEN position (deg)")
|
||||
p.add_argument("--gripper-close-deg", type=float, default=0.0, help="Gripper CLOSE position (deg)")
|
||||
p.add_argument("--gripper-kp", type=float, default=15.0, help="MIT position gain (stiffness)")
|
||||
p.add_argument("--gripper-kd", type=float, default=0.5, help="MIT damping gain")
|
||||
p.add_argument("--gripper-no-fd", dest="gripper_fd", action="store_false", help="Classic CAN (non-FD)")
|
||||
p.set_defaults(gripper_fd=True)
|
||||
# Optional camera streaming (ZMQ) so the laptop policy client / viewer can connect.
|
||||
p.add_argument("--cameras", default=None, help="Camera spec 'name:device[:WxH[:FOURCC]]', comma-sep")
|
||||
p.add_argument("--camera-fps", type=int, default=30, help="Camera FPS")
|
||||
p.add_argument("--camera-port", type=int, default=5555, help="Camera ZMQ port")
|
||||
p.add_argument("--camera-width", type=int, default=640, help="Default camera width")
|
||||
p.add_argument("--camera-height", type=int, default=480, help="Default camera height")
|
||||
args = p.parse_args()
|
||||
|
||||
dds_interface = args.dds_interface
|
||||
if args.sim and dds_interface is None:
|
||||
dds_interface = "lo"
|
||||
|
||||
cfg = UnitreeG1Config(
|
||||
is_simulation=False,
|
||||
onboard=True,
|
||||
controller=args.controller,
|
||||
dds_interface=dds_interface,
|
||||
gravity_compensation=args.gravity_compensation,
|
||||
release_motion_control=not args.sim,
|
||||
physical_remote=not args.sim,
|
||||
sonic_token_action=args.sonic_token_action,
|
||||
cameras={},
|
||||
)
|
||||
|
||||
# Optional camera server (background thread; independent of DDS/CAN).
|
||||
camera_server = None
|
||||
if args.cameras:
|
||||
cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height)
|
||||
camera_server = ImageServer({"fps": args.camera_fps, "cameras": cameras}, port=args.camera_port)
|
||||
threading.Thread(target=camera_server.run, daemon=True).start()
|
||||
cam_summary = ", ".join(f"{name}(dev {c['device_id']})" for name, c in cameras.items())
|
||||
logger.info("Camera server started on :%d: %s", args.camera_port, cam_summary)
|
||||
|
||||
robot = UnitreeG1(cfg)
|
||||
logger.info("Connecting onboard robot (controller=%s, token=%s)...", args.controller, args.sonic_token_action)
|
||||
robot.connect()
|
||||
|
||||
grippers: dict[str, Gripper] = {}
|
||||
if args.grippers:
|
||||
for side, port in (("L", args.gripper_port_left), ("R", args.gripper_port_right)):
|
||||
grippers[side] = build_gripper(
|
||||
side, port, args.gripper_send_id, args.gripper_recv_id, args.gripper_motor_type,
|
||||
args.gripper_fd, args.gripper_open_deg, args.gripper_close_deg, args.gripper_kp, args.gripper_kd,
|
||||
)
|
||||
logger.info("Grippers enabled: L3 -> left, R3 -> right")
|
||||
|
||||
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, 200) # keeps the loop responsive to the stop event
|
||||
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)
|
||||
logger.info("Type 'e' then Enter to STOP immediately (or Ctrl-C for graceful shutdown).")
|
||||
|
||||
stop = threading.Event()
|
||||
signal.signal(signal.SIGINT, lambda *_: stop.set())
|
||||
signal.signal(signal.SIGTERM, lambda *_: stop.set())
|
||||
|
||||
def estop_listener() -> None:
|
||||
for line in sys.stdin:
|
||||
if line.strip().lower() == "e":
|
||||
logger.warning("E-STOP ('e'): going passive NOW.")
|
||||
try:
|
||||
robot._shutdown_event.set() # stop the controller loop publishing
|
||||
time.sleep(0.05)
|
||||
robot._send_zero_torque() # motors limp; nothing overwrites it now
|
||||
except Exception as e: # noqa: BLE001
|
||||
logger.warning("E-stop zero-torque failed: %s", e)
|
||||
os._exit(0) # immediate hard exit, no slow cleanup
|
||||
|
||||
threading.Thread(target=estop_listener, daemon=True).start()
|
||||
|
||||
# Proprioception feedback: publish observation.state (29 joint .q) so a laptop
|
||||
# inference client can feed it to a policy. DDS stays local; only compact JSON
|
||||
# state crosses the network. (For a token policy the laptop closes the loop on the
|
||||
# token instead, but publishing joint state is harmless and useful for logging.)
|
||||
state_sock = None
|
||||
if args.state_fps > 0:
|
||||
state_sock = ctx.socket(zmq.PUB)
|
||||
state_sock.setsockopt(zmq.SNDHWM, 2)
|
||||
state_sock.setsockopt(zmq.LINGER, 0)
|
||||
state_sock.bind(f"tcp://0.0.0.0:{args.state_port}")
|
||||
logger.info("Publishing observation.state on :%d at %.0f Hz", args.state_port, args.state_fps)
|
||||
|
||||
def publish_state() -> None:
|
||||
period = 1.0 / args.state_fps
|
||||
joint_names = [j.name for j in G1_29_JointIndex]
|
||||
while not stop.is_set():
|
||||
t0 = time.time()
|
||||
obs = robot.get_observation()
|
||||
if obs:
|
||||
state = {f"{name}.q": float(obs.get(f"{name}.q", 0.0)) for name in joint_names}
|
||||
with contextlib.suppress(zmq.Again):
|
||||
state_sock.send_json(state, zmq.NOBLOCK)
|
||||
time.sleep(max(0.0, period - (time.time() - t0)))
|
||||
|
||||
threading.Thread(target=publish_state, daemon=True).start()
|
||||
else:
|
||||
logger.info("observation.state PUB disabled (--state-fps<=0)")
|
||||
|
||||
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)
|
||||
|
||||
if grippers:
|
||||
# L3 = remote.button.4 -> left, R3 = remote.button.0 -> right.
|
||||
if "L" in grippers and "remote.button.4" in action:
|
||||
grippers["L"].apply(bool(action["remote.button.4"]))
|
||||
if "R" in grippers and "remote.button.0" in action:
|
||||
grippers["R"].apply(bool(action["remote.button.0"]))
|
||||
|
||||
n += 1
|
||||
if n % 60 == 0:
|
||||
axes = {k: round(float(action.get(k, 0.0)), 3) for k in ("remote.lx", "remote.ly", "remote.rx", "remote.ry")}
|
||||
logger.info("Applied %d actions | axes=%s", n, axes)
|
||||
finally:
|
||||
logger.info("Shutting down onboard controller...")
|
||||
stop.set()
|
||||
if state_sock is not None:
|
||||
with contextlib.suppress(Exception):
|
||||
state_sock.close(linger=0)
|
||||
if camera_server is not None:
|
||||
with contextlib.suppress(Exception):
|
||||
camera_server.stop()
|
||||
for g in grippers.values():
|
||||
with contextlib.suppress(Exception):
|
||||
g.bus.disconnect()
|
||||
robot.disconnect()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -28,9 +28,11 @@ import argparse
|
||||
import base64
|
||||
import contextlib
|
||||
import json
|
||||
import re
|
||||
import threading
|
||||
import time
|
||||
from typing import Any
|
||||
from dataclasses import dataclass
|
||||
from typing import TYPE_CHECKING, Any
|
||||
|
||||
import zmq
|
||||
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
|
||||
@@ -41,6 +43,9 @@ from unitree_sdk2py.utils.crc import CRC
|
||||
|
||||
from lerobot.cameras.zmq.image_server import ImageServer
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from lerobot.motors.damiao.damiao import DamiaoMotorsBus
|
||||
|
||||
# DDS topic names follow Unitree SDK naming conventions
|
||||
# ruff: noqa: N816
|
||||
kTopicLowCommand_Debug = "rt/lowcmd" # action to robot
|
||||
@@ -51,6 +56,105 @@ LOWSTATE_PORT = 6001
|
||||
NUM_MOTORS = 35
|
||||
|
||||
|
||||
@dataclass
|
||||
class Gripper:
|
||||
"""A single Damiao gripper that only writes to CAN when the open/close state changes."""
|
||||
|
||||
name: str
|
||||
bus: "DamiaoMotorsBus"
|
||||
open_deg: float
|
||||
close_deg: float
|
||||
_last_cmd: str | None = None # "open" | "close"
|
||||
|
||||
def apply(self, want_close: bool) -> None:
|
||||
want = "close" if want_close else "open"
|
||||
if want == self._last_cmd:
|
||||
return
|
||||
target = self.close_deg if want_close else self.open_deg
|
||||
self.bus.write("Goal_Position", "gripper", target)
|
||||
self._last_cmd = want
|
||||
print(f"[gripper] {self.name} -> {want.upper()} ({target:.1f} deg)")
|
||||
|
||||
|
||||
def build_gripper(
|
||||
name: str,
|
||||
port: str,
|
||||
send_id: int,
|
||||
recv_id: int,
|
||||
motor_type: str,
|
||||
use_can_fd: bool,
|
||||
open_deg: float,
|
||||
close_deg: float,
|
||||
kp: float,
|
||||
kd: float,
|
||||
) -> Gripper:
|
||||
from lerobot.motors.damiao.damiao import DamiaoMotorsBus
|
||||
from lerobot.motors.motors_bus import Motor, MotorNormMode
|
||||
|
||||
motors = {
|
||||
"gripper": Motor(
|
||||
id=send_id,
|
||||
model=motor_type,
|
||||
norm_mode=MotorNormMode.DEGREES,
|
||||
motor_type_str=motor_type,
|
||||
recv_id=recv_id,
|
||||
)
|
||||
}
|
||||
bus = DamiaoMotorsBus(port=port, motors=motors, use_can_fd=use_can_fd)
|
||||
print(f"Connecting {name} gripper on {port} (fd={use_can_fd})...")
|
||||
bus.connect(handshake=True)
|
||||
bus.write("Kp", "gripper", kp)
|
||||
bus.write("Kd", "gripper", kd)
|
||||
bus.write("Goal_Position", "gripper", open_deg) # start open
|
||||
print(f" {name}: connected, torque enabled, opened.")
|
||||
return Gripper(name, bus, open_deg, close_deg, _last_cmd="open")
|
||||
|
||||
|
||||
def parse_camera_specs(spec: str, default_width: int, default_height: int) -> dict[str, dict]:
|
||||
"""Parse a multi-camera spec string into an ImageServer ``cameras`` dict.
|
||||
|
||||
Format: comma-separated ``name:device[:WxH[:FOURCC]]`` entries, e.g.
|
||||
``head_camera:6,left_wrist:0``. ``device`` may be an integer index or an explicit
|
||||
device path (e.g. ``/dev/video6``), including stable ``by-path`` names like
|
||||
``/dev/v4l/by-path/platform-...:2.1:1.3-video-index0`` which survive USB
|
||||
re-enumeration (unlike bare ``/dev/videoN`` indices). Because a by-path name
|
||||
itself contains colons, the optional ``WxH`` and ``FOURCC`` are parsed from the
|
||||
*right* so the device-path colons are preserved.
|
||||
"""
|
||||
wh_re = re.compile(r"\d+x\d+", re.IGNORECASE)
|
||||
fourcc_re = re.compile(r"[A-Za-z0-9]{4}")
|
||||
|
||||
cameras: dict[str, dict] = {}
|
||||
for entry in spec.split(","):
|
||||
entry = entry.strip()
|
||||
if not entry:
|
||||
continue
|
||||
if ":" not in entry:
|
||||
raise ValueError(f"Invalid camera spec '{entry}', expected 'name:device[:WxH[:FOURCC]]'")
|
||||
name, rest = entry.split(":", 1)
|
||||
name = name.strip()
|
||||
tokens = [t.strip() for t in rest.split(":")]
|
||||
|
||||
fourcc = None
|
||||
if len(tokens) >= 3 and wh_re.fullmatch(tokens[-2]) and fourcc_re.fullmatch(tokens[-1]):
|
||||
fourcc = tokens.pop().upper()
|
||||
width, height = default_width, default_height
|
||||
if len(tokens) >= 2 and wh_re.fullmatch(tokens[-1]):
|
||||
w, h = tokens.pop().lower().split("x")
|
||||
width, height = int(w), int(h)
|
||||
|
||||
raw_id = ":".join(tokens).strip()
|
||||
if not raw_id:
|
||||
raise ValueError(f"Invalid camera spec '{entry}', missing device")
|
||||
device_id: int | str = int(raw_id) if raw_id.lstrip("-").isdigit() else raw_id
|
||||
if name in cameras:
|
||||
raise ValueError(f"Duplicate camera name '{name}' in --cameras")
|
||||
cameras[name] = {"device_id": device_id, "shape": [height, width], "fourcc": fourcc}
|
||||
if not cameras:
|
||||
raise ValueError("No cameras parsed from --cameras spec")
|
||||
return cameras
|
||||
|
||||
|
||||
def lowstate_to_dict(msg: hg_LowState) -> dict[str, Any]:
|
||||
"""Convert LowState SDK message to a JSON-serializable dictionary."""
|
||||
motor_states = []
|
||||
@@ -155,7 +259,11 @@ def main() -> None:
|
||||
"""Main entry point for the robot server bridge."""
|
||||
parser = argparse.ArgumentParser(description="DDS-to-ZMQ bridge server for Unitree G1")
|
||||
parser.add_argument("--camera", action="store_true", help="Also launch camera server")
|
||||
parser.add_argument("--camera-device", type=int, default=4, help="Camera device ID (default: 4)")
|
||||
parser.add_argument("--camera-device", default="4",
|
||||
help="Camera device: index or /dev/video path or by-path name (default: 4)")
|
||||
parser.add_argument("--cameras", default=None,
|
||||
help="Multi-camera spec 'name:device[:WxH[:FOURCC]]', comma-separated. Overrides "
|
||||
"--camera-device; device may be a by-path name to survive USB re-enumeration.")
|
||||
parser.add_argument("--camera-fps", type=int, default=30, help="Camera FPS (default: 30)")
|
||||
parser.add_argument("--camera-width", type=int, default=640, help="Camera width (default: 640)")
|
||||
parser.add_argument("--camera-height", type=int, default=480, help="Camera height (default: 480)")
|
||||
@@ -164,20 +272,20 @@ def main() -> None:
|
||||
|
||||
# Optionally start camera server in background thread
|
||||
camera_thread = None
|
||||
if args.camera:
|
||||
camera_config = {
|
||||
"fps": args.camera_fps,
|
||||
"cameras": {
|
||||
"head_camera": {
|
||||
"device_id": args.camera_device,
|
||||
"shape": [args.camera_height, args.camera_width],
|
||||
}
|
||||
},
|
||||
}
|
||||
if args.camera or args.cameras:
|
||||
if args.cameras:
|
||||
cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height)
|
||||
else:
|
||||
# Single camera; accept an int index or a device/by-path string.
|
||||
dev = args.camera_device
|
||||
dev = int(dev) if str(dev).lstrip("-").isdigit() else dev
|
||||
cameras = {"head_camera": {"device_id": dev, "shape": [args.camera_height, args.camera_width]}}
|
||||
camera_config = {"fps": args.camera_fps, "cameras": cameras}
|
||||
camera_server = ImageServer(camera_config, port=args.camera_port)
|
||||
camera_thread = threading.Thread(target=camera_server.run, daemon=True)
|
||||
camera_thread.start()
|
||||
print(f"Camera server started on port {args.camera_port} (device {args.camera_device})")
|
||||
cam_summary = ", ".join(f"{n}(dev {c['device_id']})" for n, c in cameras.items())
|
||||
print(f"Camera server started on port {args.camera_port}: {cam_summary}")
|
||||
|
||||
# initialize DDS
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
@@ -87,6 +87,14 @@ class LocomotionController(Protocol):
|
||||
kTopicLowCommand_Debug = "rt/lowcmd"
|
||||
kTopicLowState = "rt/lowstate"
|
||||
|
||||
# Wireless-remote button byte layout, mapped to the positional button indices the
|
||||
# locomotion controllers expect. Used in onboard mode to read the physical Unitree
|
||||
# remote from lowstate (mirrors the exo teleoperator's RemoteController).
|
||||
_REMOTE_BUTTON_MAP: list[str] = [
|
||||
"RB", "LB", "start", "back", "RT", "LT", "", "",
|
||||
"A", "B", "X", "Y", "up", "right", "down", "left",
|
||||
]
|
||||
|
||||
|
||||
@dataclass
|
||||
class MotorState:
|
||||
@@ -130,8 +138,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
|
||||
@@ -168,6 +178,10 @@ class UnitreeG1(Robot):
|
||||
self.controller_input = default_remote_input()
|
||||
self.controller_output = {}
|
||||
|
||||
# Onboard-only: parser for the physical Unitree wireless remote (read straight
|
||||
# from local lowstate so joystick locomotion works without a laptop round-trip).
|
||||
self._joystick = None
|
||||
|
||||
# Replay-camera state: keep the encoded (raw) cells per camera and decode
|
||||
# frames lazily as the play cursor advances, with a small frame cache, so we
|
||||
# don't materialize gigabytes of decoded RGB at construction time.
|
||||
@@ -421,6 +435,13 @@ class UnitreeG1(Robot):
|
||||
with self._controller_action_lock:
|
||||
controller_input = dict(self.controller_input)
|
||||
|
||||
# Onboard: the physical Unitree remote (in local lowstate) takes
|
||||
# priority for locomotion when active; otherwise laptop/ZMQ axes stand.
|
||||
if self.config.onboard:
|
||||
wl = self._wireless_remote_input(lowstate)
|
||||
if wl is not None:
|
||||
controller_input.update(wl)
|
||||
|
||||
# Run controller step
|
||||
controller_action = self.controller.run_step(controller_input, lowstate)
|
||||
|
||||
@@ -443,6 +464,58 @@ class UnitreeG1(Robot):
|
||||
def configure(self) -> None:
|
||||
pass
|
||||
|
||||
def _wireless_remote_input(self, lowstate) -> dict | None:
|
||||
"""Parse the physical Unitree remote from lowstate into controller inputs.
|
||||
|
||||
Onboard only. Returns None when the remote is idle so the laptop-provided
|
||||
(ZMQ) axes keep control; otherwise the physical remote takes priority.
|
||||
"""
|
||||
js = self._joystick
|
||||
if js is None:
|
||||
return None
|
||||
wr = getattr(lowstate, "wireless_remote", None)
|
||||
if not wr or len(wr) < 24:
|
||||
return None
|
||||
try:
|
||||
js.extract(wr)
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
|
||||
axes = {
|
||||
"remote.lx": float(js.lx.data),
|
||||
"remote.ly": float(js.ly.data),
|
||||
"remote.rx": float(js.rx.data),
|
||||
"remote.ry": float(js.ry.data),
|
||||
}
|
||||
active = any(abs(v) > 1e-2 for v in axes.values())
|
||||
out = dict(axes)
|
||||
for i, name in enumerate(_REMOTE_BUTTON_MAP):
|
||||
if name:
|
||||
val = float(getattr(js, name).data)
|
||||
out[f"remote.button.{i}"] = val
|
||||
if val:
|
||||
active = True
|
||||
return out if active else None
|
||||
|
||||
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
|
||||
# Initialize DDS channel and simulation environment
|
||||
if self.config.is_simulation:
|
||||
@@ -468,6 +541,28 @@ class UnitreeG1(Robot):
|
||||
self._env_wrapper = _normalize_hub_result(raw)
|
||||
# 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)
|
||||
# Real robot: hand low-level control over from the built-in services.
|
||||
# A DDS sim has no MotionSwitcher, so this is skipped there.
|
||||
if self.config.release_motion_control:
|
||||
self._release_motion_control()
|
||||
# Real robot: read the physical wireless remote from lowstate for
|
||||
# locomotion. A sim has no physical remote, so leave _joystick=None and
|
||||
# let send_action (ZMQ) drive the locomotion axes instead.
|
||||
if self.config.physical_remote:
|
||||
from unitree_sdk2py.utils.joystick import Joystick
|
||||
|
||||
self._joystick = Joystick()
|
||||
for axis in (self._joystick.lx, self._joystick.ly, self._joystick.rx, self._joystick.ry):
|
||||
axis.smooth = 1.0
|
||||
axis.deadzone = 0.0
|
||||
else:
|
||||
self._ChannelFactoryInitialize(0, config=self.config)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user