diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index d637eda438..3daefcd572 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -195,7 +195,7 @@ jobs: (needs.check-changes.outputs.dev == 'true' || (needs.ros-python.result == 'success' && (needs.check-changes.outputs.python == 'true' || needs.check-changes.outputs.ros == 'true'))) }} - from-image: ghcr.io/dimensionalos/ros-python:${{ needs.ros-python.result == 'success' && needs.check-changes.outputs.branch-tag || 'dev' }} + from-image: ghcr.io/dimensionalos/ros-python:${{ needs.ros-python.result == 'success' && (needs.check-changes.outputs.python == 'true' || needs.check-changes.outputs.ros == 'true') && needs.check-changes.outputs.branch-tag || 'dev' }} to-image: ghcr.io/dimensionalos/ros-dev:${{ needs.check-changes.outputs.branch-tag }} dockerfile: dev diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index da50491f54..777b6b05e1 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -37,7 +37,7 @@ jobs: git config --global --add safe.directory '*' - name: Install Python dependencies - run: uv sync --all-extras --no-extra dds --frozen + run: uv sync --all-extras --frozen - name: Remove pydrake stubs run: | diff --git a/dimos/hardware/drive_trains/unitree_g1/__init__.py b/dimos/hardware/drive_trains/unitree_g1/__init__.py new file mode 100644 index 0000000000..34c3e7dba4 --- /dev/null +++ b/dimos/hardware/drive_trains/unitree_g1/__init__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# 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. + +"""Unitree G1 humanoid robot adapter.""" + +from dimos.hardware.drive_trains.unitree_g1.adapter import UnitreeG1TwistAdapter + +__all__ = ["UnitreeG1TwistAdapter"] diff --git a/dimos/hardware/drive_trains/unitree_g1/adapter.py b/dimos/hardware/drive_trains/unitree_g1/adapter.py new file mode 100644 index 0000000000..b5eb272f8a --- /dev/null +++ b/dimos/hardware/drive_trains/unitree_g1/adapter.py @@ -0,0 +1,400 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# 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. + +"""Unitree G1 adapter — wraps Unitree SDK2 LocoClient for humanoid base control. + +The G1 is a humanoid robot with 3 DOF velocity control: [vx, vy, wz]. +This adapter uses the Unitree SDK2 Python bindings to communicate via DDS. + +G1 FSM states (discovered via testing): + FSM 0 = ZeroTorque + FSM 1 = Damp (robot collapses — NEVER call from adapter) + FSM 2 = Squat/Crouch + FSM 3 = Sit + FSM 4 = Lock Stand (rigid standing, no locomotion) + FSM 200 = Start (locomotion active, accepts Move commands) + FSM 702 = Lie2StandUp + FSM 706 = Squat2StandUp + +Initialization sequence: + 1. ChannelFactoryInitialize(0, interface) - Initialize DDS + 2. SetFsmId(4) - Lock stand (FSM 4) + 3. Start() - Activate locomotion (FSM 200) + 4. Move(vx, vy, vyaw) - Send velocity commands + +Shutdown: StopMove() only (robot stays standing). + +Note: Damp() and ZeroTorque() are NEVER called by the adapter — they +cause the robot to collapse and should only be invoked by the user. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import threading +import time +from typing import TYPE_CHECKING + +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from unitree_sdk2py.core.channel import ChannelSubscriber # type: ignore[import-untyped] + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient # type: ignore[import-untyped] + from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( # type: ignore[import-untyped] + SportModeState_, + ) + + from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry + +logger = setup_logger() + + +@dataclass +class _Session: + """Active connection state for a G1.""" + + client: LocoClient + lock: threading.Lock + state_sub: ChannelSubscriber | None = None + latest_state: SportModeState_ | None = None + enabled: bool = False + locomotion_ready: bool = False + + +class UnitreeG1TwistAdapter: + """TwistBaseAdapter implementation for Unitree G1 humanoid. + + Communicates with G1 via Unitree SDK2 Python over DDS. + Expects 3 DOF: [vx, vy, wz] where: + - vx: forward/backward velocity (m/s) + - vy: left/right lateral velocity (m/s) + - wz: yaw rotation velocity (rad/s) + + Args: + dof: Number of velocity DOFs (must be 3 for G1) + network_interface: DDS network interface ID or name (default: 0) + """ + + def __init__( + self, + dof: int = 3, + network_interface: int | str | None = None, + address: str | None = None, + **_: object, + ) -> None: + if dof != 3: + raise ValueError(f"G1 only supports 3 DOF (vx, vy, wz), got {dof}") + + # Accept either network_interface= or address= (coordinator passes address=) + self._network_interface = network_interface or address or "eth0" + self._session: _Session | None = None + + def _get_session(self) -> _Session: + """Return active session or raise if not connected.""" + if self._session is None: + raise RuntimeError("G1 not connected") + return self._session + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + """Connect to G1 via DDS and initialize the LocoClient.""" + try: + from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelSubscriber + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ + + # Initialize DDS + logger.info(f"Initializing DDS with network interface {self._network_interface}...") + ChannelFactoryInitialize(0, self._network_interface) + + # Create loco client + logger.info("Connecting to G1 LocoClient...") + client = LocoClient() + client.SetTimeout(10.0) + client.Init() + + # Create session — callback closes over it for state updates + session = _Session(client=client, lock=threading.Lock()) + + def state_callback(msg: SportModeState_) -> None: + with session.lock: + session.latest_state = msg + + state_sub = ChannelSubscriber("rt/sportmodestate", SportModeState_) + state_sub.Init(state_callback, 10) + session.state_sub = state_sub + + self._session = session + logger.info("Connected to G1") + + # Check if robot is freshly booted (FSM 0) — needs manual damp first + fsm = self._get_fsm_id() + if fsm == 0: + logger.warning( + "G1 is in FSM 0 (fresh boot). " + "Please put the robot in DAMP mode manually, then retry. " + "Waiting 10s for damp..." + ) + if not self._wait_for_fsm(1, timeout=30, settle=2): + logger.error("G1 did not enter damp mode (FSM 1). Cannot proceed.") + self.disconnect() + return False + + # Enter lock stand (FSM 4) so the robot is standing and ready + logger.info("Entering lock stand (FSM 4) on G1...") + session.client.SetFsmId(4) + if not self._wait_for_fsm(4): + logger.error("G1 failed to reach lock stand (FSM 4)") + self.disconnect() + return False + + return True + + except Exception as e: + logger.error(f"Failed to connect to G1: {e}") + self._session = None + return False + + def disconnect(self) -> None: + """Disconnect and safely shut down the robot. + + Stops motion but keeps the robot standing (in locomotion mode). + Does NOT call Damp/ZeroTorque/Squat — the user should manage + those transitions manually. + """ + session = self._session + if session is not None: + try: + logger.info("Stopping G1 motion...") + session.client.StopMove() + time.sleep(0.5) + except Exception as e: + logger.error(f"Error during disconnect: {e}") + + if session.state_sub is not None: + try: + session.state_sub.Close() + except Exception as e: + logger.error(f"Error closing state subscriber: {e}") + + self._session = None + + def is_connected(self) -> bool: + """Check if connected to G1.""" + return self._session is not None + + # ========================================================================= + # Info + # ========================================================================= + + def get_dof(self) -> int: + """G1 base is always 3 DOF (vx, vy, wz).""" + return 3 + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_velocities(self) -> list[float]: + """Read actual velocities from SportModeState as [vx, vy, wz].""" + session = self._get_session() + with session.lock: + if session.latest_state is None: + return [0.0, 0.0, 0.0] + try: + state = session.latest_state + return [ + float(state.velocity[0]), # vx + float(state.velocity[1]), # vy + float(state.imu_state.gyroscope[2]), # wz (yaw rate) + ] + except Exception as e: + logger.warning(f"Error reading G1 velocities: {e}") + return [0.0, 0.0, 0.0] + + def read_odometry(self) -> list[float] | None: + """Read odometry from G1 as [x, y, theta]. + + Returns position from SportModeState which provides: + - position[0]: x (meters) + - position[1]: y (meters) + - imu_state.rpy[2]: yaw (radians) + """ + session = self._get_session() + with session.lock: + if session.latest_state is None: + return None + + try: + state = session.latest_state + return [ + float(state.position[0]), + float(state.position[1]), + float(state.imu_state.rpy[2]), # yaw + ] + except Exception as e: + logger.error(f"Error reading G1 odometry: {e}") + return None + + # ========================================================================= + # Control + # ========================================================================= + + def write_velocities(self, velocities: list[float]) -> bool: + """Send velocity command to G1. + + Args: + velocities: [vx, vy, wz] in standard frame (m/s, m/s, rad/s) + """ + if len(velocities) != 3: + return False + + session = self._get_session() + + if not session.enabled: + logger.warning("G1 not enabled, ignoring velocity command") + return False + + if not session.locomotion_ready: + logger.warning("G1 locomotion not ready, ignoring velocity command") + return False + + vx, vy, wz = velocities + return self._send_velocity(vx, vy, wz) + + def write_stop(self) -> bool: + """Stop all motion.""" + session = self._get_session() + try: + session.client.StopMove() + return True + except Exception as e: + logger.error(f"Error stopping G1: {e}") + return False + + # ========================================================================= + # Enable/Disable + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable/disable the platform. + + When enabling, ensures the robot is stood up and locomotion is ready. + When disabling, stops motion but keeps standing. + """ + session = self._get_session() + + if enable: + if not session.locomotion_ready: + logger.info("Starting G1 locomotion (FSM 200)...") + session.client.Start() + if not self._wait_for_fsm(200): + logger.error("G1 failed to reach locomotion mode (FSM 200)") + return False + session.locomotion_ready = True + + session.enabled = True + logger.info("G1 enabled") + return True + else: + self.write_stop() + session.enabled = False + logger.info("G1 disabled") + return True + + def read_enabled(self) -> bool: + """Check if platform is enabled.""" + return self._session is not None and self._session.enabled + + # ========================================================================= + # Internal + # ========================================================================= + + def _get_fsm_id(self) -> int | None: + """Query the current FSM ID from the robot. Returns None on failure.""" + import json + + from unitree_sdk2py.g1.loco.g1_loco_api import ( # type: ignore[import-untyped] + ROBOT_API_ID_LOCO_GET_FSM_ID, + ) + + session = self._get_session() + try: + # LocoClient has no public GetFsmId() — _Call is the standard RPC + # dispatch used by all SDK methods (SetFsmId, Move, etc.). + code, data = session.client._Call(ROBOT_API_ID_LOCO_GET_FSM_ID, "{}") + if code == 0 and data: + # data is like '{"data":4}' — parse as JSON for exact match + parsed = json.loads(str(data)) + return int(parsed["data"]) + except Exception as e: + logger.warning(f"Error querying FSM state: {e}") + return None + + def _wait_for_fsm(self, target_fsm: int, timeout: float = 10.0, settle: float = 5.0) -> bool: + """Poll GetFsmId until the robot reports the target FSM state. + + Args: + target_fsm: Expected FSM ID (e.g. 4 for lock stand, 200 for locomotion). + timeout: Maximum seconds to wait before giving up. + settle: Seconds to wait after reaching target state, letting the robot settle. + + Returns: + True if the target state was reached, False on timeout. + """ + deadline = time.time() + timeout + + while time.time() < deadline: + fsm = self._get_fsm_id() + if fsm == target_fsm: + logger.info(f"G1 reached FSM {target_fsm}, settling for {settle}s...") + time.sleep(settle) + return True + time.sleep(1) + + logger.error(f"Timed out waiting for G1 FSM {target_fsm}") + return False + + def _send_velocity(self, vx: float, vy: float, wz: float) -> bool: + """Send raw velocity to G1 via LocoClient.Move(). + + Uses default duration (1 second) since the coordinator tick loop + calls at 100Hz, providing continuous updates. + + Args: + vx: forward/backward velocity (m/s) + vy: left/right lateral velocity (m/s) + wz: yaw rotation velocity (rad/s) + """ + session = self._get_session() + try: + with session.lock: + session.client.Move(vx, vy, wz) + + return True + + except Exception as e: + logger.error(f"Error sending G1 velocity: {e}") + return False + + +def register(registry: TwistBaseAdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("unitree_g1", UnitreeG1TwistAdapter) + + +__all__ = ["UnitreeG1TwistAdapter"] diff --git a/dimos/hardware/drive_trains/unitree_g1_webrtc/__init__.py b/dimos/hardware/drive_trains/unitree_g1_webrtc/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/drive_trains/unitree_g1_webrtc/adapter.py b/dimos/hardware/drive_trains/unitree_g1_webrtc/adapter.py new file mode 100644 index 0000000000..5c39e4eefe --- /dev/null +++ b/dimos/hardware/drive_trains/unitree_g1_webrtc/adapter.py @@ -0,0 +1,313 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# 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. + +"""Unitree G1 WebRTC adapter — wraps UnitreeWebRTCConnection for humanoid base control. + +The G1 is a humanoid robot with 3 DOF velocity control: [vx, vy, wz]. +This adapter uses WebRTC to communicate wirelessly with the robot, +wrapping the existing UnitreeWebRTCConnection which handles all +WebRTC protocol details (asyncio event loop, datachannel, RTC topics). + +Locomotion initialization is configurable via the ``locomotion_mode`` +parameter: + - ``"walk"`` (default): standup → G1 WalkMode (api_id 7101, data 500) + - ``"balance_stand"``: standup → BalanceStand (SPORT_CMD 1002) +""" + +from __future__ import annotations + +import math +import threading +import time +from typing import TYPE_CHECKING + +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from reactivex.abc import DisposableBase + + from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry + from dimos.msgs.geometry_msgs import Pose + from dimos.robot.unitree.connection import UnitreeWebRTCConnection + +logger = setup_logger() + +# G1 LocoClient API IDs and FSM states +_G1_SET_FSM_ID = 7101 # SetFsmId — also used for walk mode with data=500 +_G1_SPORT_TOPIC = "rt/api/sport/request" +_FSM_LOCK_STAND = 4 +_FSM_LOCOMOTION = 200 + + +class UnitreeG1WebRTCTwistAdapter: + """TwistBaseAdapter implementation for Unitree G1 humanoid over WebRTC. + + Communicates with G1 via WebRTC using the UnitreeWebRTCConnection driver. + Expects 3 DOF: [vx, vy, wz] where: + - vx: forward/backward velocity (m/s) + - vy: left/right lateral velocity (m/s) + - wz: yaw rotation velocity (rad/s) + + Args: + dof: Number of velocity DOFs (must be 3 for G1) + ip: Robot IP address (default: "192.168.123.164") + address: Alias for ip (used by ControlCoordinator's HardwareComponent) + locomotion_mode: Locomotion activation mode — ``"walk"`` (default) + uses G1-specific WalkMode, ``"balance_stand"`` uses the + BalanceStand command. + """ + + def __init__( + self, + dof: int = 3, + ip: str | None = None, + address: str | None = None, + locomotion_mode: str = "walk", + **_: object, + ) -> None: + if dof != 3: + raise ValueError(f"G1 only supports 3 DOF (vx, vy, wz), got {dof}") + + # Accept either ip= or address= (coordinator passes address=component.address) + self._ip = ip or address or "192.168.123.164" + self._locomotion_mode = locomotion_mode + self._conn: UnitreeWebRTCConnection | None = None + self._lock = threading.Lock() + self._enabled = False + + # Last commanded velocities (WebRTC doesn't provide velocity feedback) + self._last_velocities = [0.0, 0.0, 0.0] + + # Latest odometry from odom_stream subscription + self._latest_odom: list[float] | None = None + self._odom_disposable: DisposableBase | None = None + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + """Connect to G1 over WebRTC.""" + try: + from dimos.robot.unitree.connection import UnitreeWebRTCConnection + + logger.info(f"Connecting to G1 via WebRTC at {self._ip}...") + conn = UnitreeWebRTCConnection(self._ip) + + # Subscribe to odometry for read_odometry() + self._odom_disposable = conn.odom_stream().subscribe( + on_next=self._on_odom, + on_error=lambda e: logger.warning(f"Odom stream error: {e}"), + ) + + self._conn = conn + logger.info("Connected to G1 via WebRTC") + + # Enter lock stand (FSM 4) so the robot is standing and ready + logger.info("G1 WebRTC: entering lock stand (FSM 4)...") + conn.publish_request( + _G1_SPORT_TOPIC, + {"api_id": _G1_SET_FSM_ID, "parameter": {"data": _FSM_LOCK_STAND}}, + ) + time.sleep(6) # Wait for robot to finish standing + logger.info("G1 WebRTC: lock stand ready") + + return True + + except Exception as e: + logger.error(f"Failed to connect to G1 via WebRTC: {e}") + self._conn = None + return False + + def disconnect(self) -> None: + """Disconnect and safely shut down.""" + if self._odom_disposable is not None: + self._odom_disposable.dispose() + self._odom_disposable = None + + conn = self._conn + if conn is not None: + try: + self._send_zero_velocity(conn) + time.sleep(0.3) + logger.info("Disconnecting G1 via WebRTC...") + except Exception as e: + logger.error(f"Error during shutdown sequence: {e}") + + try: + conn.disconnect() + except Exception as e: + logger.error(f"Error during WebRTC disconnect: {e}") + + self._conn = None + self._enabled = False + with self._lock: + self._last_velocities = [0.0, 0.0, 0.0] + self._latest_odom = None + + def is_connected(self) -> bool: + """Check if connected to G1.""" + return self._conn is not None + + # ========================================================================= + # Info + # ========================================================================= + + def get_dof(self) -> int: + """G1 base is always 3 DOF (vx, vy, wz).""" + return 3 + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_velocities(self) -> list[float]: + """Return last commanded velocities (WebRTC doesn't provide velocity feedback).""" + with self._lock: + return self._last_velocities.copy() + + def read_odometry(self) -> list[float] | None: + """Read odometry as [x, y, theta] from WebRTC odom stream.""" + with self._lock: + if self._latest_odom is None: + return None + return self._latest_odom.copy() + + # ========================================================================= + # Control + # ========================================================================= + + def write_velocities(self, velocities: list[float]) -> bool: + """Send velocity command to G1 over WebRTC. + + Args: + velocities: [vx, vy, wz] in standard frame (m/s, m/s, rad/s) + """ + if len(velocities) != 3: + return False + + conn = self._conn + if conn is None: + return False + + if not self._enabled: + logger.warning("G1 WebRTC not enabled, ignoring velocity command") + return False + + vx, vy, wz = velocities + + with self._lock: + self._last_velocities = list(velocities) + + return self._send_velocity(conn, vx, vy, wz) + + def write_stop(self) -> bool: + """Stop all motion by sending zero velocity.""" + with self._lock: + self._last_velocities = [0.0, 0.0, 0.0] + + conn = self._conn + if conn is None: + return False + + return self._send_zero_velocity(conn) + + # ========================================================================= + # Enable/Disable + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable/disable the platform. + + When enabling, stands up and activates the configured locomotion mode. + When disabling, stops motion. + """ + conn = self._conn + if conn is None: + return False + + if enable: + try: + logger.info("G1 WebRTC: starting locomotion (FSM 200)...") + conn.publish_request( + _G1_SPORT_TOPIC, + {"api_id": _G1_SET_FSM_ID, "parameter": {"data": _FSM_LOCOMOTION}}, + ) + time.sleep(3) # Wait for locomotion to activate + + self._enabled = True + logger.info("G1 WebRTC: locomotion ready") + return True + except Exception as e: + logger.error(f"Failed to enable G1 via WebRTC: {e}") + return False + else: + self.write_stop() + self._enabled = False + logger.info("G1 WebRTC disabled") + return True + + def read_enabled(self) -> bool: + """Check if platform is enabled.""" + return self._enabled + + # ========================================================================= + # Internal + # ========================================================================= + + def _on_odom(self, pose: Pose) -> None: + """Callback for odom_stream subscription. Extracts [x, y, yaw] from Pose.""" + try: + x = float(pose.position.x) + y = float(pose.position.y) + + # Convert quaternion to yaw + qx = float(pose.orientation.x) + qy = float(pose.orientation.y) + qz = float(pose.orientation.z) + qw = float(pose.orientation.w) + yaw = math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz)) + + with self._lock: + self._latest_odom = [x, y, yaw] + except Exception as e: + logger.warning(f"Error processing odom message: {e}") + + def _send_velocity( + self, conn: UnitreeWebRTCConnection, vx: float, vy: float, wz: float + ) -> bool: + """Send velocity to G1 via WebRTC move() command.""" + from dimos.msgs.geometry_msgs import Twist, Vector3 + + try: + twist = Twist( + linear=Vector3(x=vx, y=vy, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=wz), + ) + return conn.move(twist) + except Exception as e: + logger.error(f"Error sending G1 WebRTC velocity: {e}") + return False + + def _send_zero_velocity(self, conn: UnitreeWebRTCConnection) -> bool: + """Send zero velocity to stop the robot.""" + return self._send_velocity(conn, 0.0, 0.0, 0.0) + + +def register(registry: TwistBaseAdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("unitree_g1_webrtc", UnitreeG1WebRTCTwistAdapter) + + +__all__ = ["UnitreeG1WebRTCTwistAdapter"] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 6026572388..5dbacd9ea4 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -65,11 +65,13 @@ "unitree-g1-agentic-sim": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_agentic_sim:unitree_g1_agentic_sim", "unitree-g1-basic": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_basic:unitree_g1_basic", "unitree-g1-basic-sim": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_basic_sim:unitree_g1_basic_sim", + "unitree-g1-dds-coordinator": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_dds_coordinator:unitree_g1_dds_coordinator", "unitree-g1-detection": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_detection:unitree_g1_detection", "unitree-g1-full": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_full:unitree_g1_full", "unitree-g1-joystick": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_joystick:unitree_g1_joystick", "unitree-g1-shm": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_shm:unitree_g1_shm", "unitree-g1-sim": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1_sim:unitree_g1_sim", + "unitree-g1-webrtc-coordinator": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_webrtc_coordinator:unitree_g1_webrtc_coordinator", "unitree-go2": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2", "unitree-go2-agentic": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic:unitree_go2_agentic", "unitree-go2-agentic-huggingface": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_huggingface:unitree_go2_agentic_huggingface", diff --git a/dimos/robot/unitree/g1/blueprints/__init__.py b/dimos/robot/unitree/g1/blueprints/__init__.py index ebc18da8d3..5b78af1978 100644 --- a/dimos/robot/unitree/g1/blueprints/__init__.py +++ b/dimos/robot/unitree/g1/blueprints/__init__.py @@ -26,7 +26,9 @@ "agentic.unitree_g1_full": ["unitree_g1_full"], "basic.unitree_g1_basic": ["unitree_g1_basic"], "basic.unitree_g1_basic_sim": ["unitree_g1_basic_sim"], + "basic.unitree_g1_dds_coordinator": ["unitree_g1_dds_coordinator"], "basic.unitree_g1_joystick": ["unitree_g1_joystick"], + "basic.unitree_g1_webrtc_coordinator": ["unitree_g1_webrtc_coordinator"], "perceptive._perception_and_memory": ["_perception_and_memory"], "perceptive.unitree_g1": ["unitree_g1"], "perceptive.unitree_g1_detection": ["unitree_g1_detection"], diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_dds_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_dds_coordinator.py new file mode 100644 index 0000000000..17eddb4171 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_dds_coordinator.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# 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. + +"""Unitree G1 ControlCoordinator over DDS (LAN ethernet). + +Twist → coordinator twist_command → UnitreeG1TwistAdapter (DDS). + +Uses Unitree SDK2 DDS for wired ethernet control of the G1 humanoid robot. + +Usage: + dimos run unitree-g1-dds-coordinator + ROBOT_INTERFACE=enp60s0 dimos run unitree-g1-dds-coordinator +""" + +from __future__ import annotations + +import os + +from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints +from dimos.control.coordinator import TaskConfig, control_coordinator +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import Twist +from dimos.msgs.sensor_msgs import JointState + +_g1_joints = make_twist_base_joints("g1") + +unitree_g1_dds_coordinator = control_coordinator( + hardware=[ + HardwareComponent( + hardware_id="g1", + hardware_type=HardwareType.BASE, + joints=_g1_joints, + adapter_type="unitree_g1", + address=os.getenv("ROBOT_INTERFACE", "enp86s0"), + ), + ], + tasks=[ + TaskConfig( + name="vel_g1", + type="velocity", + joint_names=_g1_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + +__all__ = ["unitree_g1_dds_coordinator"] diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_webrtc_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_webrtc_coordinator.py new file mode 100644 index 0000000000..259e5ddaf8 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_webrtc_coordinator.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# 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. + +"""Unitree G1 ControlCoordinator over WebRTC. + +Twist → coordinator twist_command → UnitreeG1WebRTCTwistAdapter. + +Uses WebRTC for wireless control of the G1 humanoid robot. + +Usage: + dimos run unitree-g1-webrtc-coordinator + ROBOT_IP=192.168.123.164 dimos run unitree-g1-webrtc-coordinator +""" + +from __future__ import annotations + +import os + +from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints +from dimos.control.coordinator import TaskConfig, control_coordinator +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import Twist +from dimos.msgs.sensor_msgs import JointState + +_g1_joints = make_twist_base_joints("g1") + +unitree_g1_webrtc_coordinator = control_coordinator( + hardware=[ + HardwareComponent( + hardware_id="g1", + hardware_type=HardwareType.BASE, + joints=_g1_joints, + adapter_type="unitree_g1_webrtc", + address=os.getenv("ROBOT_IP"), + ), + ], + tasks=[ + TaskConfig( + name="vel_g1", + type="velocity", + joint_names=_g1_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + +__all__ = ["unitree_g1_webrtc_coordinator"] diff --git a/docker/dev/Dockerfile b/docker/dev/Dockerfile index c6822e54cb..54249ad994 100644 --- a/docker/dev/Dockerfile +++ b/docker/dev/Dockerfile @@ -16,8 +16,21 @@ RUN apt-get update && apt-get install -y \ wget \ net-tools \ sudo \ - pre-commit + pre-commit \ + cmake \ + build-essential +# Build CycloneDDS 0.10.5 from source (Ubuntu 22.04 apt only has ~0.9.x, +# but the Python cyclonedds package requires >=0.10.5 headers) +RUN git clone --branch 0.10.5 --depth 1 https://github.com/eclipse-cyclonedds/cyclonedds.git /tmp/cyclonedds && \ + mkdir /tmp/cyclonedds/build && \ + cd /tmp/cyclonedds/build && \ + cmake -DCMAKE_INSTALL_PREFIX=/opt/cyclonedds .. && \ + cmake --build . --parallel $(nproc) && \ + cmake --install . && \ + rm -rf /tmp/cyclonedds + +ENV CYCLONEDDS_HOME=/opt/cyclonedds # Configure git to trust any directory (resolves dubious ownership issues in containers) RUN git config --global --add safe.directory '*' diff --git a/pyproject.toml b/pyproject.toml index cb4607ced5..3e8a3c0fbc 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -165,7 +165,13 @@ perception = [ unitree = [ "dimos[base]", - "unitree-webrtc-connect-leshy>=2.0.7" + "unitree-webrtc-connect-leshy>=2.0.7", +] + +unitree-dds = [ + "dimos[unitree]", + "unitree-sdk2py-dimos>=1.0.1", + "cyclonedds>=0.10.5", ] manipulation = [ @@ -409,6 +415,8 @@ concurrency = ["multiprocessing", "thread"] show_missing = true skip_empty = true +[tool.uv.sources] + [tool.largefiles] max_size_kb = 50 ignore = [ diff --git a/uv.lock b/uv.lock index 2f53ef0e6f..75ea36a963 100644 --- a/uv.lock +++ b/uv.lock @@ -1958,6 +1958,41 @@ unitree = [ { name = "unitree-webrtc-connect-leshy" }, { name = "uvicorn" }, ] +unitree-dds = [ + { name = "anthropic" }, + { name = "bitsandbytes", marker = "sys_platform == 'linux'" }, + { name = "cyclonedds" }, + { name = "fastapi" }, + { name = "ffmpeg-python" }, + { name = "filterpy" }, + { name = "hydra-core" }, + { name = "langchain" }, + { name = "langchain-chroma" }, + { name = "langchain-core" }, + { name = "langchain-huggingface" }, + { name = "langchain-ollama" }, + { name = "langchain-openai" }, + { name = "langchain-text-splitters" }, + { name = "lap" }, + { name = "moondream" }, + { name = "mujoco" }, + { name = "ollama" }, + { name = "omegaconf" }, + { name = "openai" }, + { name = "openai-whisper" }, + { name = "pillow" }, + { name = "playground" }, + { name = "pygame" }, + { name = "rerun-sdk" }, + { name = "sounddevice" }, + { name = "soundfile" }, + { name = "sse-starlette" }, + { name = "transformers", extra = ["torch"] }, + { name = "ultralytics" }, + { name = "unitree-sdk2py-dimos" }, + { name = "unitree-webrtc-connect-leshy" }, + { name = "uvicorn" }, +] visualization = [ { name = "rerun-sdk" }, ] @@ -1982,9 +2017,11 @@ requires-dist = [ { name = "ctransformers", extras = ["cuda"], marker = "extra == 'cuda'", specifier = "==0.2.27" }, { name = "cupy-cuda12x", marker = "platform_machine == 'x86_64' and extra == 'cuda'", specifier = "==13.6.0" }, { name = "cyclonedds", marker = "extra == 'dds'", specifier = ">=0.10.5" }, + { name = "cyclonedds", marker = "extra == 'unitree-dds'", specifier = ">=0.10.5" }, { name = "dimos", extras = ["agents", "web", "perception", "visualization", "sim"], marker = "extra == 'base'" }, { name = "dimos", extras = ["base"], marker = "extra == 'unitree'" }, { name = "dimos", extras = ["dev"], marker = "extra == 'dds'" }, + { name = "dimos", extras = ["unitree"], marker = "extra == 'unitree-dds'" }, { name = "dimos-lcm" }, { name = "dimos-lcm", marker = "extra == 'docker'" }, { name = "drake", marker = "platform_machine != 'aarch64' and sys_platform == 'darwin' and extra == 'manipulation'", specifier = "==1.45.0" }, @@ -2119,6 +2156,7 @@ requires-dist = [ { name = "types-tensorflow", marker = "extra == 'dev'", specifier = ">=2.18.0.20251008,<3" }, { name = "types-tqdm", marker = "extra == 'dev'", specifier = ">=4.67.0.20250809,<5" }, { name = "ultralytics", marker = "extra == 'perception'", specifier = ">=8.3.70" }, + { name = "unitree-sdk2py-dimos", marker = "extra == 'unitree-dds'", specifier = ">=1.0.1" }, { name = "unitree-webrtc-connect-leshy", marker = "extra == 'unitree'", specifier = ">=2.0.7" }, { name = "uvicorn", marker = "extra == 'web'", specifier = ">=0.34.0" }, { name = "watchdog", marker = "extra == 'dev'", specifier = ">=3.0.0" }, @@ -2128,7 +2166,7 @@ requires-dist = [ { name = "xformers", marker = "platform_machine == 'x86_64' and extra == 'cuda'", specifier = ">=0.0.20" }, { name = "yapf", marker = "extra == 'misc'", specifier = "==0.40.2" }, ] -provides-extras = ["misc", "visualization", "agents", "web", "perception", "unitree", "manipulation", "cpu", "cuda", "dev", "psql", "sim", "drone", "dds", "docker", "base"] +provides-extras = ["misc", "visualization", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "cpu", "cuda", "dev", "psql", "sim", "drone", "dds", "docker", "base"] [[package]] name = "dimos-lcm" @@ -10105,6 +10143,21 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/7f/c7/fb42228bb05473d248c110218ffb8b1ad2f76728ed8699856e5af21112ad/ultralytics_thop-2.0.18-py3-none-any.whl", hash = "sha256:2bb44851ad224b116c3995b02dd5e474a5ccf00acf237fe0edb9e1506ede04ec", size = 28941, upload-time = "2025-10-29T16:58:12.093Z" }, ] +[[package]] +name = "unitree-sdk2py-dimos" +version = "1.0.3" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cyclonedds" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "opencv-python" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/93/d2/8c927709a432e6003a7ffdb434c2a3570c1b4ed97c9a0b7b85313e32f6bb/unitree_sdk2py_dimos-1.0.3.tar.gz", hash = "sha256:d0076b9501849a8f144dd076ffb3894c5c804c87cdad7521095c2bc893049438", size = 48758, upload-time = "2026-03-03T21:19:32.8Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/39/69/76b879edbf5eab1cb200bf818b87a5943effe441722429ab940ea38a6887/unitree_sdk2py_dimos-1.0.3-py3-none-any.whl", hash = "sha256:8057cad5de5877757bc586b2ba5ddbe84522ce4c8c3d624464ef975cafa5daec", size = 110245, upload-time = "2026-03-03T21:19:31.692Z" }, +] + [[package]] name = "unitree-webrtc-connect-leshy" version = "2.0.7"