From 24cbba8253c2be6787103ac790a8413639b23d16 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 28 Apr 2026 22:45:53 -0700 Subject: [PATCH 01/28] cherrry picked old low level adapter implementatio for the g1 --- dimos/hardware/whole_body/spec.py | 113 +++++++ .../hardware/whole_body/unitree/g1/adapter.py | 293 ++++++++++++++++++ 2 files changed, 406 insertions(+) create mode 100644 dimos/hardware/whole_body/spec.py create mode 100644 dimos/hardware/whole_body/unitree/g1/adapter.py diff --git a/dimos/hardware/whole_body/spec.py b/dimos/hardware/whole_body/spec.py new file mode 100644 index 0000000000..8a5bd0f697 --- /dev/null +++ b/dimos/hardware/whole_body/spec.py @@ -0,0 +1,113 @@ +# 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. + +"""WholeBodyAdapter protocol for joint-level motor control. + +Lightweight protocol for robots that expose per-motor +position/velocity/torque control (as opposed to TwistBaseAdapter which +only exposes velocity commands). + +Supports any number of motors — quadrupeds (12 DOF), humanoids (29 DOF), etc. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Protocol, runtime_checkable + +# Sentinel values from Unitree SDK — used to signal "no command" for a DOF. +POS_STOP: float = 2.146e9 +VEL_STOP: float = 16000.0 + + +@dataclass(frozen=True) +class MotorCommand: + """Command for a single motor.""" + + q: float = POS_STOP # target position (rad) + dq: float = VEL_STOP # target velocity (rad/s) + kp: float = 0.0 # position gain + kd: float = 0.0 # velocity gain + tau: float = 0.0 # feedforward torque (Nm) + + +@dataclass(frozen=True) +class MotorState: + """Feedback from a single motor.""" + + q: float = 0.0 # position (rad) + dq: float = 0.0 # velocity (rad/s) + tau: float = 0.0 # estimated torque (Nm) + + +@dataclass(frozen=True) +class IMUState: + """IMU feedback.""" + + quaternion: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + gyroscope: tuple[float, float, float] = (0.0, 0.0, 0.0) + accelerometer: tuple[float, float, float] = (0.0, 0.0, 0.0) + rpy: tuple[float, float, float] = (0.0, 0.0, 0.0) + + +@runtime_checkable +class WholeBodyAdapter(Protocol): + """Protocol for joint-level whole-body motor IO. + + Implement this per vendor SDK. All methods use SI units: + - Position: radians + - Velocity: rad/s + - Torque: Nm + - Force: N + """ + + # --- Connection --- + + def connect(self) -> bool: + """Connect to hardware. Returns True on success.""" + ... + + def disconnect(self) -> None: + """Disconnect from hardware.""" + ... + + def is_connected(self) -> bool: + """Check if connected.""" + ... + + # --- State Reading --- + + def read_motor_states(self) -> list[MotorState]: + """Read motor states for all joints.""" + ... + + def read_imu(self) -> IMUState: + """Read IMU state.""" + ... + + # --- Control --- + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + """Write motor commands for all joints. Returns success.""" + ... + + +__all__ = [ + "POS_STOP", + "VEL_STOP", + "IMUState", + "MotorCommand", + "MotorState", + "WholeBodyAdapter", +] diff --git a/dimos/hardware/whole_body/unitree/g1/adapter.py b/dimos/hardware/whole_body/unitree/g1/adapter.py new file mode 100644 index 0000000000..5dda4cb800 --- /dev/null +++ b/dimos/hardware/whole_body/unitree/g1/adapter.py @@ -0,0 +1,293 @@ +# 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 low-level adapter -- direct 29-DOF joint control over DDS. + +Uses ``rt/lowcmd`` / ``rt/lowstate`` DDS topics for motor-level +position/velocity/torque control, bypassing the high-level LocoClient. + +Important: The G1 must first exit sport mode (via MotionSwitcherClient) +before low-level commands are accepted. + +Motor ordering (29 joints): + 0-5: Left leg (HipPitch, HipRoll, HipYaw, Knee, AnklePitch, AnkleRoll) + 6-11: Right leg + 12: WaistYaw + 13-14: WaistRoll, WaistPitch (may be invalid on some variants) + 15-21: Left arm (ShoulderPitch, ShoulderRoll, ShoulderYaw, Elbow, + WristRoll, WristPitch, WristYaw) + 22-28: Right arm + +G1-specific protocol details: + - Uses ``unitree_hg`` IDL types (not ``unitree_go`` like the Go2) + - LowCmd has ``mode_pr`` and ``mode_machine`` fields instead of head/level_flag + - ``mode_machine`` must be read from LowState and echoed back in every LowCmd + - Motor array has 35 slots (only 29 are used) +""" + +from __future__ import annotations + +import logging +import threading +import time +from typing import TYPE_CHECKING + +from dimos.hardware.whole_body.spec import ( + POS_STOP, + VEL_STOP, + IMUState, + MotorCommand, + MotorState, +) + +if TYPE_CHECKING: + from dimos.hardware.whole_body.registry import WholeBodyAdapterRegistry + +logger = logging.getLogger(__name__) + +_NUM_MOTORS = 29 +_NUM_MOTOR_SLOTS = 35 + + +class UnitreeG1LowLevelAdapter: + """WholeBodyAdapter implementation for Unitree G1 -- low-level DDS. + + The coordinator's tick loop drives the publish cadence. Each call to + ``write_motor_commands()`` updates the ``LowCmd_`` buffer, computes + CRC, and publishes immediately -- no background thread needed. + + Args: + network_interface: DDS network interface name (e.g., "eth0"). Empty string + means let CycloneDDS pick the default interface — matches the Go2 + adapter's convention. The cyclonedds Python binding requires a string; + int values like 0 trigger AttributeError on participant init. + release_sport_mode: If True (default), call MotionSwitcher to exit sport mode + during connect(). Set False for mock/sim backends that don't run MotionSwitcher. + """ + + def __init__( + self, + network_interface: str = "", + release_sport_mode: bool = True, + **_: object, + ) -> None: + self._network_interface = network_interface + self._release_sport_mode_flag = release_sport_mode + + self._connected = False + self._lock = threading.Lock() + + # SDK objects (lazy-imported on connect) + self._low_cmd = None + self._publisher = None + self._subscriber = None + self._crc = None + + # Latest feedback + self._low_state = None + + # mode_machine must be read from first LowState and echoed back + self._mode_machine: int | None = None + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + """Connect to G1 and release sport mode for low-level control.""" + try: + from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, + ) + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ + from unitree_sdk2py.utils.crc import CRC + + # 1. Initialise DDS transport. Pass NIC only if non-empty — passing + # an int or empty arg trips cyclonedds' participant init. + logger.info(f"Initializing DDS (G1 low-level) interface={self._network_interface!r}...") + try: + if self._network_interface: + ChannelFactoryInitialize(0, self._network_interface) + else: + ChannelFactoryInitialize(0) + except Exception as e: + logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") + + # 2. Create publisher / subscriber + self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self._publisher.Init() + + self._subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self._subscriber.Init(self._on_low_state, 10) + + # 3. Initialise LowCmd with safe defaults + self._low_cmd = unitree_hg_msg_dds__LowCmd_() + self._low_cmd.mode_pr = 0 # PR mode (Pitch/Roll) + for i in range(_NUM_MOTOR_SLOTS): + self._low_cmd.motor_cmd[i].mode = 0x01 # Enable + self._low_cmd.motor_cmd[i].q = POS_STOP + self._low_cmd.motor_cmd[i].kp = 0 + self._low_cmd.motor_cmd[i].dq = VEL_STOP + self._low_cmd.motor_cmd[i].kd = 0 + self._low_cmd.motor_cmd[i].tau = 0 + + self._crc = CRC() + + # 4. Release sport mode so low-level commands are accepted + if self._release_sport_mode_flag: + logger.info("Releasing sport mode...") + self._release_sport_mode() + else: + logger.info("Skipping sport mode release (release_sport_mode=False)") + + # 5. Wait for first LowState to get mode_machine + logger.info("Waiting for first LowState to capture mode_machine...") + deadline = time.time() + 10.0 + while self._mode_machine is None and time.time() < deadline: + time.sleep(0.1) + + if self._mode_machine is None: + logger.error("Timed out waiting for LowState — mode_machine not captured") + self._connected = False + return False + + self._connected = True + logger.info(f"G1 low-level adapter connected (mode_machine={self._mode_machine})") + return True + + except Exception as e: + logger.error(f"Failed to connect G1 low-level adapter: {e}") + self._connected = False + return False + + def disconnect(self) -> None: + """Disconnect from the robot.""" + self._connected = False + self._publisher = None + self._subscriber = None + self._low_cmd = None + self._low_state = None + self._mode_machine = None + logger.info("G1 low-level adapter disconnected") + + def is_connected(self) -> bool: + return self._connected + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_motor_states(self) -> list[MotorState]: + """Read motor states for all 29 joints.""" + with self._lock: + if self._low_state is None: + return [MotorState()] * _NUM_MOTORS + return [ + MotorState( + q=self._low_state.motor_state[i].q, + dq=self._low_state.motor_state[i].dq, + tau=self._low_state.motor_state[i].tau_est, + ) + for i in range(_NUM_MOTORS) + ] + + def read_imu(self) -> IMUState: + """Read IMU state.""" + with self._lock: + if self._low_state is None: + return IMUState() + imu = self._low_state.imu_state + return IMUState( + quaternion=tuple(imu.quaternion), + gyroscope=tuple(imu.gyroscope), + accelerometer=tuple(imu.accelerometer), + rpy=tuple(imu.rpy), + ) + + # ========================================================================= + # Control + # ========================================================================= + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + """Update command buffer, compute CRC, and publish immediately. + + Called by the coordinator tick loop on every tick -- no background + thread needed. + """ + if len(commands) != _NUM_MOTORS: + logger.error(f"Expected {_NUM_MOTORS} commands, got {len(commands)}") + return False + + with self._lock: + if self._low_cmd is None or self._crc is None or self._publisher is None: + return False + if self._mode_machine is None: + return False + + # Echo mode_machine from latest LowState + self._low_cmd.mode_machine = self._mode_machine + + for i, cmd in enumerate(commands): + self._low_cmd.motor_cmd[i].q = cmd.q + self._low_cmd.motor_cmd[i].dq = cmd.dq + self._low_cmd.motor_cmd[i].kp = cmd.kp + self._low_cmd.motor_cmd[i].kd = cmd.kd + self._low_cmd.motor_cmd[i].tau = cmd.tau + self._low_cmd.crc = self._crc.Crc(self._low_cmd) + self._publisher.Write(self._low_cmd) + return True + + # ========================================================================= + # Internal + # ========================================================================= + + def _on_low_state(self, msg: object) -> None: + """DDS callback for rt/lowstate.""" + with self._lock: + self._low_state = msg + # Capture mode_machine from first LowState + if self._mode_machine is None: + self._mode_machine = msg.mode_machine # type: ignore[attr-defined] + + def _release_sport_mode(self) -> None: + """Exit sport mode so that low-level commands are accepted. + + Loops ReleaseMode until CheckMode returns empty. + """ + from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( + MotionSwitcherClient, + ) + + msc = MotionSwitcherClient() + msc.SetTimeout(5.0) + msc.Init() + + _status, result = msc.CheckMode() + while result["name"]: + msc.ReleaseMode() + _status, result = msc.CheckMode() + time.sleep(1) + + logger.info("Sport mode released -- low-level control active") + + +def register(registry: WholeBodyAdapterRegistry) -> None: + """Register this adapter with the whole-body registry.""" + registry.register("unitree_g1", UnitreeG1LowLevelAdapter) + + +__all__ = ["UnitreeG1LowLevelAdapter"] From d8e82dcbce346caef03d27bce0b48ea3b2c27f70 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 28 Apr 2026 22:46:21 -0700 Subject: [PATCH 02/28] MotorCommand LCM data type added --- dimos/msgs/sensor_msgs/MotorCommandArray.py | 146 ++++++++++++++++++++ 1 file changed, 146 insertions(+) create mode 100644 dimos/msgs/sensor_msgs/MotorCommandArray.py diff --git a/dimos/msgs/sensor_msgs/MotorCommandArray.py b/dimos/msgs/sensor_msgs/MotorCommandArray.py new file mode 100644 index 0000000000..9091d80d04 --- /dev/null +++ b/dimos/msgs/sensor_msgs/MotorCommandArray.py @@ -0,0 +1,146 @@ +# 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. + +"""LCM type for low-level motor command arrays. + +Hand-rolled struct.pack serialization, modeled on JointCommand.py. +Carries the full per-motor PD+feedforward command tuple needed for +low-level joint control: q, dq, kp, kd, tau. +""" + +from io import BytesIO +import struct +import time + + +class MotorCommandArray: + """Low-level motor command array. + + Variable-length per-motor commands with target position, target velocity, + PD gains (kp, kd), and feedforward torque. Joint order is implicit — + callers must agree on a fixed ordering (e.g., G1's 29-motor sequence). + """ + + msg_name = "sensor_msgs.MotorCommandArray" + + __slots__ = ["dq", "kd", "kp", "num_joints", "q", "tau", "timestamp"] + + def __init__( + self, + q: list[float] | None = None, + dq: list[float] | None = None, + kp: list[float] | None = None, + kd: list[float] | None = None, + tau: list[float] | None = None, + timestamp: float | None = None, + ) -> None: + if q is None: + q = [] + if timestamp is None: + timestamp = time.time() + + n = len(q) + if dq is None: + dq = [0.0] * n + if kp is None: + kp = [0.0] * n + if kd is None: + kd = [0.0] * n + if tau is None: + tau = [0.0] * n + + if not (len(dq) == len(kp) == len(kd) == len(tau) == n): + raise ValueError( + f"All arrays must have length {n}; got " + f"dq={len(dq)} kp={len(kp)} kd={len(kd)} tau={len(tau)}" + ) + + self.timestamp = timestamp + self.num_joints = n + self.q = list(q) + self.dq = list(dq) + self.kp = list(kp) + self.kd = list(kd) + self.tau = list(tau) + + def lcm_encode(self) -> bytes: + return self.encode() + + def encode(self) -> bytes: + buf = BytesIO() + buf.write(MotorCommandArray._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf: BytesIO) -> None: + buf.write(struct.pack(">d", self.timestamp)) + buf.write(struct.pack(">i", self.num_joints)) + for arr in (self.q, self.dq, self.kp, self.kd, self.tau): + for v in arr: + buf.write(struct.pack(">d", v)) + + @classmethod + def lcm_decode(cls, data: bytes) -> "MotorCommandArray": + return cls.decode(data) + + @classmethod + def decode(cls, data: bytes) -> "MotorCommandArray": + if hasattr(data, "read"): + buf = data # type: ignore[assignment] + else: + buf = BytesIO(data) + if buf.read(8) != cls._get_packed_fingerprint(): + raise ValueError("Decode error") + return cls._decode_one(buf) + + @classmethod + def _decode_one(cls, buf: BytesIO) -> "MotorCommandArray": + self = MotorCommandArray.__new__(MotorCommandArray) + self.timestamp = struct.unpack(">d", buf.read(8))[0] + self.num_joints = struct.unpack(">i", buf.read(4))[0] + n = self.num_joints + arrays = [] + for _ in range(5): + arrays.append([struct.unpack(">d", buf.read(8))[0] for _ in range(n)]) + self.q, self.dq, self.kp, self.kd, self.tau = arrays + return self + + @classmethod + def _get_hash_recursive(cls, parents: list) -> int: + if cls in parents: + return 0 + # Distinct fingerprint from JointCommand (0x8A3D2E1C5F4B6A9D) + tmphash = (0x9B4E3F2D6A5C7B8E) & 0xFFFFFFFFFFFFFFFF + tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF + return tmphash + + _packed_fingerprint: bytes | None = None + + @classmethod + def _get_packed_fingerprint(cls) -> bytes: + if cls._packed_fingerprint is None: + cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) + return cls._packed_fingerprint + + def get_hash(self) -> int: + return struct.unpack(">Q", MotorCommandArray._get_packed_fingerprint())[0] + + def __str__(self) -> str: + return f"MotorCommandArray(timestamp={self.timestamp:.6f}, num_joints={self.num_joints})" + + def __repr__(self) -> str: + return ( + f"MotorCommandArray(q={self.q}, dq={self.dq}, kp={self.kp}, " + f"kd={self.kd}, tau={self.tau}, timestamp={self.timestamp})" + ) From 446838d905918534b59f3212c3608450a425adc9 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 28 Apr 2026 22:46:47 -0700 Subject: [PATCH 03/28] g1 low level control module --- .../robot/unitree/g1/wholebody_connection.py | 219 +++++++++++++ .../test_g1_module/mock_lowstate_publisher.py | 152 +++++++++ scripts/test_g1_module/test_g1_module.py | 308 ++++++++++++++++++ 3 files changed, 679 insertions(+) create mode 100644 dimos/robot/unitree/g1/wholebody_connection.py create mode 100644 scripts/test_g1_module/mock_lowstate_publisher.py create mode 100644 scripts/test_g1_module/test_g1_module.py diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py new file mode 100644 index 0000000000..b9523200cd --- /dev/null +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -0,0 +1,219 @@ +# 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. + +"""G1 wholebody (Arch B) Module. + +Wraps UnitreeG1LowLevelAdapter as a worker-isolated dimos Module. Owns the +DDS connection; the coordinator stays out of the DDS process. + +Stream interface: + - motor_states: Out[JointState] 29 motors, q/dq/tau in position/velocity/effort + - imu: Out[Imu] quaternion + gyro + accel + - motor_command: In[MotorCommandArray] 29-DOF q/dq/kp/kd/tau + +mode_machine handling stays inside the adapter — never appears on the wire. +""" + +from __future__ import annotations + +import logging +import threading +from threading import Thread +import time +from typing import Any + +from pydantic import Field +from reactivex.disposable import Disposable + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.hardware.whole_body.spec import MotorCommand +from dimos.hardware.whole_body.unitree.g1.adapter import UnitreeG1LowLevelAdapter +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray + +logger = logging.getLogger(__name__) + +_NUM_MOTORS = 29 + +# Motor index → joint name. Order matches UnitreeG1LowLevelAdapter docstring: +# 0-5: left leg (hip pitch/roll/yaw, knee, ankle pitch/roll) +# 6-11: right leg +# 12-14: waist (yaw, roll, pitch — roll/pitch may be invalid on some variants) +# 15-21: left arm (shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw) +# 22-28: right arm +G1_JOINT_NAMES: list[str] = [ + "g1/left_hip_pitch", + "g1/left_hip_roll", + "g1/left_hip_yaw", + "g1/left_knee", + "g1/left_ankle_pitch", + "g1/left_ankle_roll", + "g1/right_hip_pitch", + "g1/right_hip_roll", + "g1/right_hip_yaw", + "g1/right_knee", + "g1/right_ankle_pitch", + "g1/right_ankle_roll", + "g1/waist_yaw", + "g1/waist_roll", + "g1/waist_pitch", + "g1/left_shoulder_pitch", + "g1/left_shoulder_roll", + "g1/left_shoulder_yaw", + "g1/left_elbow", + "g1/left_wrist_roll", + "g1/left_wrist_pitch", + "g1/left_wrist_yaw", + "g1/right_shoulder_pitch", + "g1/right_shoulder_roll", + "g1/right_shoulder_yaw", + "g1/right_elbow", + "g1/right_wrist_roll", + "g1/right_wrist_pitch", + "g1/right_wrist_yaw", +] +assert len(G1_JOINT_NAMES) == _NUM_MOTORS + + +class G1WholeBodyConnectionConfig(ModuleConfig): + network_interface: str = Field(default="") + release_sport_mode: bool = True + publish_rate_hz: float = 500.0 + frame_id: str = "g1_pelvis" + + +class G1WholeBodyConnection(Module): + """G1 humanoid Module — owns the DDS connection in its own worker. + + Mirrors GO2Connection's shape (sensors + actuation in one Module) but at + the low-level joint layer instead of high-level Twist. + """ + + config: G1WholeBodyConnectionConfig + + motor_command: In[MotorCommandArray] + motor_states: Out[JointState] + imu: Out[Imu] + + connection: UnitreeG1LowLevelAdapter + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self.connection = UnitreeG1LowLevelAdapter( + network_interface=self.config.network_interface, + release_sport_mode=self.config.release_sport_mode, + ) + self._stop_event = threading.Event() + self._publish_thread: Thread | None = None + + @rpc + def start(self) -> None: + super().start() + + if not self.connection.connect(): + raise RuntimeError("G1 low-level adapter failed to connect") + + self.register_disposable(Disposable(self.motor_command.subscribe(self._on_motor_command))) + + self._publish_thread = Thread( + target=self._publish_loop, + name="g1-wholebody-pump", + daemon=True, + ) + self._publish_thread.start() + + logger.info( + f"G1WholeBodyConnection started (rate={self.config.publish_rate_hz}Hz, " + f"release_sport_mode={self.config.release_sport_mode})" + ) + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._publish_thread is not None and self._publish_thread.is_alive(): + self._publish_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + self._publish_thread = None + + if self.connection.is_connected(): + self.connection.disconnect() + + super().stop() + + def _publish_loop(self) -> None: + period = 1.0 / float(self.config.publish_rate_hz) + next_tick = time.perf_counter() + frame_id = self.config.frame_id + + while not self._stop_event.is_set(): + states = self.connection.read_motor_states() + imu_state = self.connection.read_imu() + now = time.time() + + self.motor_states.publish( + JointState( + ts=now, + frame_id=frame_id, + name=G1_JOINT_NAMES, + position=[s.q for s in states], + velocity=[s.dq for s in states], + effort=[s.tau for s in states], + ) + ) + + # Unitree IMU quaternion is (w, x, y, z); dimos Quaternion is (x, y, z, w). + q = imu_state.quaternion + g = imu_state.gyroscope + a = imu_state.accelerometer + self.imu.publish( + Imu( + ts=now, + frame_id=frame_id, + orientation=Quaternion(q[1], q[2], q[3], q[0]), + angular_velocity=Vector3(g[0], g[1], g[2]), + linear_acceleration=Vector3(a[0], a[1], a[2]), + ) + ) + + next_tick += period + sleep_for = next_tick - time.perf_counter() + if sleep_for > 0: + time.sleep(sleep_for) + else: + next_tick = time.perf_counter() + + def _on_motor_command(self, msg: MotorCommandArray) -> None: + if msg.num_joints != _NUM_MOTORS: + logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") + return + + commands = [ + MotorCommand( + q=msg.q[i], + dq=msg.dq[i], + kp=msg.kp[i], + kd=msg.kd[i], + tau=msg.tau[i], + ) + for i in range(_NUM_MOTORS) + ] + self.connection.write_motor_commands(commands) + + +__all__ = ["G1_JOINT_NAMES", "G1WholeBodyConnection", "G1WholeBodyConnectionConfig"] diff --git a/scripts/test_g1_module/mock_lowstate_publisher.py b/scripts/test_g1_module/mock_lowstate_publisher.py new file mode 100644 index 0000000000..cd51a8ea43 --- /dev/null +++ b/scripts/test_g1_module/mock_lowstate_publisher.py @@ -0,0 +1,152 @@ +# 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. + +"""Mock G1 LowState publisher — provides synthetic rt/lowstate at a target rate. + +Stands in for either real G1 hardware or Unitree's mujoco bridge so the +G1WholeBodyConnection Module + transport seam can be validated end-to-end +without hardware. + +Usage (typically from test_g1_module.py): + pub = MockG1LowStatePublisher(rate_hz=500, mode_machine=42) + pub.start() + ... + pub.stop() + +Note: ChannelFactoryInitialize is a per-process singleton. This publisher +calls it on construction; if the same process needs to do other DDS work +(e.g., subscribe to rt/lowcmd to verify command round-trip), instantiate +this publisher BEFORE creating other DDS participants. +""" + +from __future__ import annotations + +import logging +import threading +from threading import Thread +import time + +logger = logging.getLogger(__name__) + +_NUM_MOTORS = 29 +_NUM_MOTOR_SLOTS = 35 + + +class MockG1LowStatePublisher: + """Publishes synthetic LowState_ messages on rt/lowstate at a target rate. + + Args: + rate_hz: Publish rate (Hz). + mode_machine: Value to put in LowState.mode_machine. MUST be non-zero + so UnitreeG1LowLevelAdapter.connect() completes (it captures the + first non-None mode_machine and exits the wait loop). + network_interface: DDS NIC, default 0. + """ + + def __init__( + self, + rate_hz: float = 500.0, + mode_machine: int = 42, + network_interface: str = "", + ) -> None: + self._rate_hz = rate_hz + self._mode_machine = mode_machine + self._network_interface = network_interface + self._stop = threading.Event() + self._thread: Thread | None = None + self._pub = None # type: ignore[var-annotated] + self._sequence = 0 + + def start(self) -> None: + from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ) + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ + + # cyclonedds requires a string NIC; passing an int trips AttributeError on + # participant init. Only pass when explicitly set. Wrap in try/except since + # the call also raises if the factory was already initialized in this process. + try: + if self._network_interface: + ChannelFactoryInitialize(0, self._network_interface) + else: + ChannelFactoryInitialize(0) + except Exception as e: + logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") + + self._pub = ChannelPublisher("rt/lowstate", LowState_) + self._pub.Init() + + self._thread = Thread(target=self._loop, name="mock-g1-lowstate", daemon=True) + self._thread.start() + logger.info(f"MockG1LowStatePublisher started @ {self._rate_hz} Hz") + + def stop(self, timeout: float = 2.0) -> None: + self._stop.set() + if self._thread is not None: + self._thread.join(timeout=timeout) + self._thread = None + logger.info(f"MockG1LowStatePublisher stopped after {self._sequence} messages") + + @property + def messages_published(self) -> int: + return self._sequence + + def _loop(self) -> None: + # Use the default factory (mirrors how the adapter constructs LowCmd_) — + # LowState_() directly would require all 9 positional fields. + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ + + msg = unitree_hg_msg_dds__LowState_() + msg.mode_machine = self._mode_machine + + # IMU defaults (identity quaternion, zero everything else) + msg.imu_state.quaternion[0] = 1.0 + msg.imu_state.quaternion[1] = 0.0 + msg.imu_state.quaternion[2] = 0.0 + msg.imu_state.quaternion[3] = 0.0 + for j in range(3): + msg.imu_state.gyroscope[j] = 0.0 + msg.imu_state.accelerometer[j] = 0.0 + msg.imu_state.rpy[j] = 0.0 + + period = 1.0 / self._rate_hz + next_tick = time.perf_counter() + + while not self._stop.is_set(): + seq = self._sequence + # Sweep value per motor — lets downstream verify ordering + freshness + for i in range(_NUM_MOTORS): + msg.motor_state[i].q = float(i) + 0.001 * seq + msg.motor_state[i].dq = 0.0 + msg.motor_state[i].tau_est = 0.0 + + try: + self._pub.Write(msg) + self._sequence += 1 + except Exception as e: + logger.error(f"Mock publish failed: {e}") + return + + next_tick += period + sleep_for = next_tick - time.perf_counter() + if sleep_for > 0: + time.sleep(sleep_for) + else: + # We fell behind — reset cadence to avoid runaway catch-up + next_tick = time.perf_counter() + + +__all__ = ["MockG1LowStatePublisher"] diff --git a/scripts/test_g1_module/test_g1_module.py b/scripts/test_g1_module/test_g1_module.py new file mode 100644 index 0000000000..aec3b6fbfa --- /dev/null +++ b/scripts/test_g1_module/test_g1_module.py @@ -0,0 +1,308 @@ +#!/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. + +"""Disposable validation harness for the G1 wholebody Module. + +NOT a reusable adapter — Session B will write the real TransportWholeBodyAdapter +against the same topic interface. This script's job is to keep the Module/transport +seam honest with mock DDS traffic. + +Lifecycle: + 1. Spawn MockG1LowStatePublisher in this process (DDS rt/lowstate at 500 Hz). + 2. Spawn ChannelSubscriber("rt/lowcmd", LowCmd_) in this process to count + motor commands round-tripped through the adapter. + 3. Build a blueprint with G1WholeBodyConnection.blueprint(release_sport_mode=False) + and LCM transports on motor_states / imu / motor_command. Run with n_workers=2 + so the Module lands in its own worker process. + 4. From this process, subscribe to the same LCM topics + publish synthetic + MotorCommandArray commands at 100 Hz. + 5. After 60 s, print rate / drop counts and assert thresholds: + - motor_states rate == 500 Hz, drops == 0 + - imu rate == 500 Hz, drops == 0 + - motor_command round-trip count >= 99% of sent + - mode_machine NOT exposed on the wire (true by JointState construction) + +Run inside the nix dev shell with [unitree-dds] extras installed: + python scripts/test_g1_module/test_g1_module.py +""" + +from __future__ import annotations + +import argparse +from collections import deque +import logging +import os +import sys +import threading +import time + +logger = logging.getLogger(__name__) + + +# --------------------------------------------------------------------------- +# RateMonitor — counts arrivals + tracks instantaneous rate over a sliding window +# --------------------------------------------------------------------------- + + +class RateMonitor: + """Counts callback invocations and reports rate over a fixed window.""" + + def __init__(self, name: str, window_s: float = 1.0) -> None: + self.name = name + self.window_s = window_s + self._lock = threading.Lock() + self._times: deque[float] = deque() + self._total: int = 0 + self._first_ts: float | None = None + self._last_ts: float | None = None + + def tick(self) -> None: + now = time.perf_counter() + with self._lock: + self._total += 1 + if self._first_ts is None: + self._first_ts = now + self._last_ts = now + self._times.append(now) + cutoff = now - self.window_s + while self._times and self._times[0] < cutoff: + self._times.popleft() + + @property + def total(self) -> int: + with self._lock: + return self._total + + @property + def instantaneous_hz(self) -> float: + with self._lock: + return len(self._times) / self.window_s + + def average_hz(self) -> float: + with self._lock: + if self._first_ts is None or self._last_ts is None: + return 0.0 + elapsed = self._last_ts - self._first_ts + if elapsed <= 0: + return 0.0 + return self._total / elapsed + + +# --------------------------------------------------------------------------- +# Test harness +# --------------------------------------------------------------------------- + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--duration", type=float, default=60.0, help="Run time (s)") + parser.add_argument("--mock-rate", type=float, default=500.0, help="Mock LowState Hz") + parser.add_argument("--cmd-rate", type=float, default=100.0, help="Synthetic command Hz") + parser.add_argument( + "--rate-tolerance", + type=float, + default=2.0, + help="Hz tolerance below target to allow before flagging (default 2.0)", + ) + parser.add_argument( + "--cmd-roundtrip-min", type=float, default=0.99, help="Min round-trip ratio" + ) + parser.add_argument("--verbose", "-v", action="store_true") + args = parser.parse_args() + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s %(name)s %(levelname)s %(message)s", + ) + logger.info(f"test_g1_module pid={os.getpid()} duration={args.duration}s") + + # Heavy imports deferred so --help works even outside the nix env + from unitree_sdk2py.core.channel import ChannelSubscriber + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ + + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.core.global_config import global_config + from dimos.core.transport import LCMTransport + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.JointState import JointState + from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray + from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection + from scripts.test_g1_module.mock_lowstate_publisher import MockG1LowStatePublisher + + # ---------------- Step 1: mock LowState publisher ---------------- + logger.info("Starting mock LowState publisher...") + mock = MockG1LowStatePublisher(rate_hz=args.mock_rate, mode_machine=42) + mock.start() + + # ---------------- Step 2: rt/lowcmd subscriber for round-trip ---------------- + cmd_roundtrip_count = [0] + cmd_roundtrip_lock = threading.Lock() + + def on_lowcmd(_msg: LowCmd_) -> None: + with cmd_roundtrip_lock: + cmd_roundtrip_count[0] += 1 + + cmd_sub = ChannelSubscriber("rt/lowcmd", LowCmd_) + cmd_sub.Init(on_lowcmd, 10) + + # ---------------- Step 3: build blueprint with LCM transports ---------------- + logger.info("Building blueprint (n_workers=2)...") + global_config.update(viewer="none", n_workers=2) + + blueprint = autoconnect( + G1WholeBodyConnection.blueprint(release_sport_mode=False), + ).transports( + { + ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), + ("imu", Imu): LCMTransport("/g1/imu", Imu), + ("motor_command", MotorCommandArray): LCMTransport( + "/g1/motor_command", MotorCommandArray + ), + } + ) + + coord = ModuleCoordinator.build(blueprint) + + # ---------------- Step 4: external LCM subscribers + command publisher ---------------- + motor_states_monitor = RateMonitor("motor_states") + imu_monitor = RateMonitor("imu") + + # mode_machine leak detection — JointState has no such attr by construction. + mode_machine_leaked = [False] + + def on_motor_states(msg: JointState) -> None: + motor_states_monitor.tick() + if getattr(msg, "mode_machine", None) is not None: + mode_machine_leaked[0] = True + + def on_imu(msg: Imu) -> None: + imu_monitor.tick() + if getattr(msg, "mode_machine", None) is not None: + mode_machine_leaked[0] = True + + motor_states_sub = LCMTransport("/g1/motor_states", JointState) + motor_states_unsub = motor_states_sub.subscribe(on_motor_states) + + imu_sub = LCMTransport("/g1/imu", Imu) + imu_unsub = imu_sub.subscribe(on_imu) + + motor_command_pub = LCMTransport("/g1/motor_command", MotorCommandArray) + + # Wait for the Module to come up — adapter.connect() waits for first LowState + logger.info("Waiting 3s for Module to start in worker...") + time.sleep(3.0) + + # ---------------- Step 5: 60 s sustained run ---------------- + logger.info(f"Running for {args.duration}s — driving cmd at {args.cmd_rate} Hz...") + cmd_sent = 0 + cmd_period = 1.0 / args.cmd_rate + next_cmd = time.perf_counter() + end_at = time.perf_counter() + args.duration + + last_log = time.perf_counter() + while time.perf_counter() < end_at: + cmd = MotorCommandArray( + q=[0.0] * 29, + dq=[0.0] * 29, + kp=[10.0] * 29, + kd=[1.0] * 29, + tau=[0.0] * 29, + ) + motor_command_pub.publish(cmd) + cmd_sent += 1 + + next_cmd += cmd_period + sleep_for = next_cmd - time.perf_counter() + if sleep_for > 0: + time.sleep(sleep_for) + else: + next_cmd = time.perf_counter() + + # 5s heartbeat so the operator knows we're not stuck + now = time.perf_counter() + if now - last_log >= 5.0: + last_log = now + logger.info( + f" motor_states={motor_states_monitor.instantaneous_hz:.1f}Hz " + f"imu={imu_monitor.instantaneous_hz:.1f}Hz " + f"cmd_sent={cmd_sent} cmd_round_trip={cmd_roundtrip_count[0]}" + ) + + # ---------------- Step 6: tear down + assertions ---------------- + logger.info("Stopping...") + motor_states_unsub() + imu_unsub() + motor_states_sub.stop() + imu_sub.stop() + motor_command_pub.stop() + + coord.stop() + cmd_sub.Close() + mock.stop() + + # Drain a tiny bit to let any in-flight messages land + time.sleep(0.5) + + avg_motor_states_hz = motor_states_monitor.average_hz() + avg_imu_hz = imu_monitor.average_hz() + cmd_round_trip_ratio = cmd_roundtrip_count[0] / cmd_sent if cmd_sent else 0.0 + + print() + print("=" * 60) + print("RESULTS") + print("=" * 60) + print(f" duration: {args.duration:.1f}s") + print(f" mock published: {mock.messages_published}") + print( + f" motor_states: total={motor_states_monitor.total} " + f"avg={avg_motor_states_hz:.2f}Hz target={args.mock_rate}Hz" + ) + print( + f" imu: total={imu_monitor.total} " + f"avg={avg_imu_hz:.2f}Hz target={args.mock_rate}Hz" + ) + print( + f" motor_command: sent={cmd_sent} round_trip={cmd_roundtrip_count[0]} " + f"ratio={cmd_round_trip_ratio:.3f} min={args.cmd_roundtrip_min}" + ) + print(f" mode_machine leaked: {mode_machine_leaked[0]}") + print("=" * 60) + + failures: list[str] = [] + rate_floor = args.mock_rate - args.rate_tolerance + if avg_motor_states_hz < rate_floor: + failures.append(f"motor_states {avg_motor_states_hz:.2f}Hz < {rate_floor}Hz floor") + if avg_imu_hz < rate_floor: + failures.append(f"imu {avg_imu_hz:.2f}Hz < {rate_floor}Hz floor") + if cmd_round_trip_ratio < args.cmd_roundtrip_min: + failures.append(f"cmd round-trip {cmd_round_trip_ratio:.3f} < {args.cmd_roundtrip_min}") + if mode_machine_leaked[0]: + failures.append("mode_machine leaked onto output stream") + + if failures: + print() + print("FAIL:") + for f in failures: + print(f" - {f}") + return 1 + + print() + print("PASS") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) From 752a1312320104f39ab47a0dfffa643425809600 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 29 Apr 2026 17:01:43 -0700 Subject: [PATCH 04/28] absorbed adapter into wholebody_connection module --- .../hardware/whole_body/unitree/g1/adapter.py | 293 ----------------- .../robot/unitree/g1/wholebody_connection.py | 310 ++++++++++++------ .../test_g1_module/mock_lowstate_publisher.py | 4 +- 3 files changed, 219 insertions(+), 388 deletions(-) delete mode 100644 dimos/hardware/whole_body/unitree/g1/adapter.py diff --git a/dimos/hardware/whole_body/unitree/g1/adapter.py b/dimos/hardware/whole_body/unitree/g1/adapter.py deleted file mode 100644 index 5dda4cb800..0000000000 --- a/dimos/hardware/whole_body/unitree/g1/adapter.py +++ /dev/null @@ -1,293 +0,0 @@ -# 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 low-level adapter -- direct 29-DOF joint control over DDS. - -Uses ``rt/lowcmd`` / ``rt/lowstate`` DDS topics for motor-level -position/velocity/torque control, bypassing the high-level LocoClient. - -Important: The G1 must first exit sport mode (via MotionSwitcherClient) -before low-level commands are accepted. - -Motor ordering (29 joints): - 0-5: Left leg (HipPitch, HipRoll, HipYaw, Knee, AnklePitch, AnkleRoll) - 6-11: Right leg - 12: WaistYaw - 13-14: WaistRoll, WaistPitch (may be invalid on some variants) - 15-21: Left arm (ShoulderPitch, ShoulderRoll, ShoulderYaw, Elbow, - WristRoll, WristPitch, WristYaw) - 22-28: Right arm - -G1-specific protocol details: - - Uses ``unitree_hg`` IDL types (not ``unitree_go`` like the Go2) - - LowCmd has ``mode_pr`` and ``mode_machine`` fields instead of head/level_flag - - ``mode_machine`` must be read from LowState and echoed back in every LowCmd - - Motor array has 35 slots (only 29 are used) -""" - -from __future__ import annotations - -import logging -import threading -import time -from typing import TYPE_CHECKING - -from dimos.hardware.whole_body.spec import ( - POS_STOP, - VEL_STOP, - IMUState, - MotorCommand, - MotorState, -) - -if TYPE_CHECKING: - from dimos.hardware.whole_body.registry import WholeBodyAdapterRegistry - -logger = logging.getLogger(__name__) - -_NUM_MOTORS = 29 -_NUM_MOTOR_SLOTS = 35 - - -class UnitreeG1LowLevelAdapter: - """WholeBodyAdapter implementation for Unitree G1 -- low-level DDS. - - The coordinator's tick loop drives the publish cadence. Each call to - ``write_motor_commands()`` updates the ``LowCmd_`` buffer, computes - CRC, and publishes immediately -- no background thread needed. - - Args: - network_interface: DDS network interface name (e.g., "eth0"). Empty string - means let CycloneDDS pick the default interface — matches the Go2 - adapter's convention. The cyclonedds Python binding requires a string; - int values like 0 trigger AttributeError on participant init. - release_sport_mode: If True (default), call MotionSwitcher to exit sport mode - during connect(). Set False for mock/sim backends that don't run MotionSwitcher. - """ - - def __init__( - self, - network_interface: str = "", - release_sport_mode: bool = True, - **_: object, - ) -> None: - self._network_interface = network_interface - self._release_sport_mode_flag = release_sport_mode - - self._connected = False - self._lock = threading.Lock() - - # SDK objects (lazy-imported on connect) - self._low_cmd = None - self._publisher = None - self._subscriber = None - self._crc = None - - # Latest feedback - self._low_state = None - - # mode_machine must be read from first LowState and echoed back - self._mode_machine: int | None = None - - # ========================================================================= - # Connection - # ========================================================================= - - def connect(self) -> bool: - """Connect to G1 and release sport mode for low-level control.""" - try: - from unitree_sdk2py.core.channel import ( - ChannelFactoryInitialize, - ChannelPublisher, - ChannelSubscriber, - ) - from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ - from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ - from unitree_sdk2py.utils.crc import CRC - - # 1. Initialise DDS transport. Pass NIC only if non-empty — passing - # an int or empty arg trips cyclonedds' participant init. - logger.info(f"Initializing DDS (G1 low-level) interface={self._network_interface!r}...") - try: - if self._network_interface: - ChannelFactoryInitialize(0, self._network_interface) - else: - ChannelFactoryInitialize(0) - except Exception as e: - logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") - - # 2. Create publisher / subscriber - self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) - self._publisher.Init() - - self._subscriber = ChannelSubscriber("rt/lowstate", LowState_) - self._subscriber.Init(self._on_low_state, 10) - - # 3. Initialise LowCmd with safe defaults - self._low_cmd = unitree_hg_msg_dds__LowCmd_() - self._low_cmd.mode_pr = 0 # PR mode (Pitch/Roll) - for i in range(_NUM_MOTOR_SLOTS): - self._low_cmd.motor_cmd[i].mode = 0x01 # Enable - self._low_cmd.motor_cmd[i].q = POS_STOP - self._low_cmd.motor_cmd[i].kp = 0 - self._low_cmd.motor_cmd[i].dq = VEL_STOP - self._low_cmd.motor_cmd[i].kd = 0 - self._low_cmd.motor_cmd[i].tau = 0 - - self._crc = CRC() - - # 4. Release sport mode so low-level commands are accepted - if self._release_sport_mode_flag: - logger.info("Releasing sport mode...") - self._release_sport_mode() - else: - logger.info("Skipping sport mode release (release_sport_mode=False)") - - # 5. Wait for first LowState to get mode_machine - logger.info("Waiting for first LowState to capture mode_machine...") - deadline = time.time() + 10.0 - while self._mode_machine is None and time.time() < deadline: - time.sleep(0.1) - - if self._mode_machine is None: - logger.error("Timed out waiting for LowState — mode_machine not captured") - self._connected = False - return False - - self._connected = True - logger.info(f"G1 low-level adapter connected (mode_machine={self._mode_machine})") - return True - - except Exception as e: - logger.error(f"Failed to connect G1 low-level adapter: {e}") - self._connected = False - return False - - def disconnect(self) -> None: - """Disconnect from the robot.""" - self._connected = False - self._publisher = None - self._subscriber = None - self._low_cmd = None - self._low_state = None - self._mode_machine = None - logger.info("G1 low-level adapter disconnected") - - def is_connected(self) -> bool: - return self._connected - - # ========================================================================= - # State Reading - # ========================================================================= - - def read_motor_states(self) -> list[MotorState]: - """Read motor states for all 29 joints.""" - with self._lock: - if self._low_state is None: - return [MotorState()] * _NUM_MOTORS - return [ - MotorState( - q=self._low_state.motor_state[i].q, - dq=self._low_state.motor_state[i].dq, - tau=self._low_state.motor_state[i].tau_est, - ) - for i in range(_NUM_MOTORS) - ] - - def read_imu(self) -> IMUState: - """Read IMU state.""" - with self._lock: - if self._low_state is None: - return IMUState() - imu = self._low_state.imu_state - return IMUState( - quaternion=tuple(imu.quaternion), - gyroscope=tuple(imu.gyroscope), - accelerometer=tuple(imu.accelerometer), - rpy=tuple(imu.rpy), - ) - - # ========================================================================= - # Control - # ========================================================================= - - def write_motor_commands(self, commands: list[MotorCommand]) -> bool: - """Update command buffer, compute CRC, and publish immediately. - - Called by the coordinator tick loop on every tick -- no background - thread needed. - """ - if len(commands) != _NUM_MOTORS: - logger.error(f"Expected {_NUM_MOTORS} commands, got {len(commands)}") - return False - - with self._lock: - if self._low_cmd is None or self._crc is None or self._publisher is None: - return False - if self._mode_machine is None: - return False - - # Echo mode_machine from latest LowState - self._low_cmd.mode_machine = self._mode_machine - - for i, cmd in enumerate(commands): - self._low_cmd.motor_cmd[i].q = cmd.q - self._low_cmd.motor_cmd[i].dq = cmd.dq - self._low_cmd.motor_cmd[i].kp = cmd.kp - self._low_cmd.motor_cmd[i].kd = cmd.kd - self._low_cmd.motor_cmd[i].tau = cmd.tau - self._low_cmd.crc = self._crc.Crc(self._low_cmd) - self._publisher.Write(self._low_cmd) - return True - - # ========================================================================= - # Internal - # ========================================================================= - - def _on_low_state(self, msg: object) -> None: - """DDS callback for rt/lowstate.""" - with self._lock: - self._low_state = msg - # Capture mode_machine from first LowState - if self._mode_machine is None: - self._mode_machine = msg.mode_machine # type: ignore[attr-defined] - - def _release_sport_mode(self) -> None: - """Exit sport mode so that low-level commands are accepted. - - Loops ReleaseMode until CheckMode returns empty. - """ - from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( - MotionSwitcherClient, - ) - - msc = MotionSwitcherClient() - msc.SetTimeout(5.0) - msc.Init() - - _status, result = msc.CheckMode() - while result["name"]: - msc.ReleaseMode() - _status, result = msc.CheckMode() - time.sleep(1) - - logger.info("Sport mode released -- low-level control active") - - -def register(registry: WholeBodyAdapterRegistry) -> None: - """Register this adapter with the whole-body registry.""" - registry.register("unitree_g1", UnitreeG1LowLevelAdapter) - - -__all__ = ["UnitreeG1LowLevelAdapter"] diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index b9523200cd..651d70ba6f 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -12,17 +12,32 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""G1 wholebody (Arch B) Module. +"""G1 wholebody Module (Arch B). -Wraps UnitreeG1LowLevelAdapter as a worker-isolated dimos Module. Owns the -DDS connection; the coordinator stays out of the DDS process. +Owns the G1 low-level DDS connection directly — no separate adapter file. +Sits in its own worker; the coordinator never imports unitree_sdk2py and +talks to this Module exclusively through LCM streams. Stream interface: - - motor_states: Out[JointState] 29 motors, q/dq/tau in position/velocity/effort - - imu: Out[Imu] quaternion + gyro + accel - - motor_command: In[MotorCommandArray] 29-DOF q/dq/kp/kd/tau - -mode_machine handling stays inside the adapter — never appears on the wire. + - motor_states: Out[JointState] 29 motors, q/dq/tau in position/velocity/effort + - imu: Out[Imu] quaternion + gyro + accel + - motor_command: In[MotorCommandArray] 29-DOF q/dq/kp/kd/tau + +Hardware protocol: + - DDS topics: rt/lowstate (subscribe) + rt/lowcmd (publish) + - IDL: unitree_hg (G1 specific; Go2 uses unitree_go) + - mode_machine: read from first LowState, echoed back in every LowCmd + - CRC: computed on every LowCmd via unitree_sdk2py.utils.crc.CRC + - Sport-mode release: gated by release_sport_mode config (set False for sim/mock) + +mode_machine never appears on the published JointState — it stays internal. + +Motor ordering (29 joints): + 0-5: left leg (hip pitch/roll/yaw, knee, ankle pitch/roll) + 6-11: right leg + 12-14: waist (yaw, roll, pitch — roll/pitch may be invalid on some variants) + 15-21: left arm (shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw) + 22-28: right arm """ from __future__ import annotations @@ -37,11 +52,11 @@ from reactivex.disposable import Disposable from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.control.components import make_humanoid_joints from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.hardware.whole_body.spec import MotorCommand -from dimos.hardware.whole_body.unitree.g1.adapter import UnitreeG1LowLevelAdapter +from dimos.hardware.whole_body.spec import POS_STOP, VEL_STOP from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.Imu import Imu @@ -51,44 +66,13 @@ logger = logging.getLogger(__name__) _NUM_MOTORS = 29 +_NUM_MOTOR_SLOTS = 35 # G1 hg LowCmd has 35 slots; only 29 are used +_MODE_MACHINE_WAIT_S = 10.0 -# Motor index → joint name. Order matches UnitreeG1LowLevelAdapter docstring: -# 0-5: left leg (hip pitch/roll/yaw, knee, ankle pitch/roll) -# 6-11: right leg -# 12-14: waist (yaw, roll, pitch — roll/pitch may be invalid on some variants) -# 15-21: left arm (shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw) -# 22-28: right arm -G1_JOINT_NAMES: list[str] = [ - "g1/left_hip_pitch", - "g1/left_hip_roll", - "g1/left_hip_yaw", - "g1/left_knee", - "g1/left_ankle_pitch", - "g1/left_ankle_roll", - "g1/right_hip_pitch", - "g1/right_hip_roll", - "g1/right_hip_yaw", - "g1/right_knee", - "g1/right_ankle_pitch", - "g1/right_ankle_roll", - "g1/waist_yaw", - "g1/waist_roll", - "g1/waist_pitch", - "g1/left_shoulder_pitch", - "g1/left_shoulder_roll", - "g1/left_shoulder_yaw", - "g1/left_elbow", - "g1/left_wrist_roll", - "g1/left_wrist_pitch", - "g1/left_wrist_yaw", - "g1/right_shoulder_pitch", - "g1/right_shoulder_roll", - "g1/right_shoulder_yaw", - "g1/right_elbow", - "g1/right_wrist_roll", - "g1/right_wrist_pitch", - "g1/right_wrist_yaw", -] +# Joint names sourced from the canonical helper. Order matches the motor index +# convention above. Single-source-of-truth so any coordinator-side adapter built +# from make_humanoid_joints("g1") agrees on the wire-level name → motor-index mapping. +G1_JOINT_NAMES: list[str] = make_humanoid_joints("g1") assert len(G1_JOINT_NAMES) == _NUM_MOTORS @@ -100,11 +84,7 @@ class G1WholeBodyConnectionConfig(ModuleConfig): class G1WholeBodyConnection(Module): - """G1 humanoid Module — owns the DDS connection in its own worker. - - Mirrors GO2Connection's shape (sensors + actuation in one Module) but at - the low-level joint layer instead of high-level Twist. - """ + """G1 humanoid Module — owns the DDS connection in its own worker.""" config: G1WholeBodyConnectionConfig @@ -112,38 +92,108 @@ class G1WholeBodyConnection(Module): motor_states: Out[JointState] imu: Out[Imu] - connection: UnitreeG1LowLevelAdapter - def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self.connection = UnitreeG1LowLevelAdapter( - network_interface=self.config.network_interface, - release_sport_mode=self.config.release_sport_mode, - ) + + # DDS / SDK objects — populated in start(), torn down in stop(). + self._publisher: Any = None + self._subscriber: Any = None + self._low_cmd: Any = None + self._low_state: Any = None + self._crc: Any = None + + # mode_machine must be read from first LowState and echoed back in every LowCmd. + self._mode_machine: int | None = None + + # Guards _low_cmd, _low_state, _mode_machine across the DDS callback thread, + # the publish loop thread, and the motor_command (LCM) callback thread. + self._lock = threading.Lock() + self._stop_event = threading.Event() self._publish_thread: Thread | None = None + # ========================================================================= + # Lifecycle + # ========================================================================= + @rpc def start(self) -> None: super().start() - if not self.connection.connect(): - raise RuntimeError("G1 low-level adapter failed to connect") + # Lazy SDK imports so the Module module file imports cleanly outside the + # nix env / [unitree-dds] extra. Connect path: + # 1. ChannelFactoryInitialize(0[, nic]) + # 2. publisher rt/lowcmd, subscriber rt/lowstate + # 3. LowCmd buffer initialised with safe defaults (POS_STOP, kp=0) + # 4. CRC computer + # 5. (optional) MotionSwitcher release sport mode + # 6. wait up to _MODE_MACHINE_WAIT_S for first LowState + from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, + ) + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ + from unitree_sdk2py.utils.crc import CRC + + nic = self.config.network_interface + logger.info(f"Initializing DDS (G1 wholebody) interface={nic!r}...") + try: + if nic: + ChannelFactoryInitialize(0, nic) + else: + ChannelFactoryInitialize(0) + except Exception as e: + # Idempotent: if the factory was already initialised in this process + # (e.g. by a sibling DDS publisher in the same worker), continue. + logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") + + self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self._publisher.Init() + + self._subscriber = ChannelSubscriber("rt/lowstate", LowState_) + self._subscriber.Init(self._on_low_state, 10) + + # LowCmd safe defaults — POS_STOP/VEL_STOP sentinels with zero gains so a + # robot doesn't twitch on the first publish if commands haven't started. + self._low_cmd = unitree_hg_msg_dds__LowCmd_() + self._low_cmd.mode_pr = 0 # PR (pitch/roll) mode + for i in range(_NUM_MOTOR_SLOTS): + self._low_cmd.motor_cmd[i].mode = 0x01 # enable + self._low_cmd.motor_cmd[i].q = POS_STOP + self._low_cmd.motor_cmd[i].kp = 0 + self._low_cmd.motor_cmd[i].dq = VEL_STOP + self._low_cmd.motor_cmd[i].kd = 0 + self._low_cmd.motor_cmd[i].tau = 0 + + self._crc = CRC() + + if self.config.release_sport_mode: + logger.info("Releasing sport mode...") + self._release_sport_mode() + else: + logger.info("Skipping sport mode release (release_sport_mode=False)") + + logger.info("Waiting for first LowState to capture mode_machine...") + deadline = time.time() + _MODE_MACHINE_WAIT_S + while self._mode_machine is None and time.time() < deadline: + time.sleep(0.1) + if self._mode_machine is None: + raise RuntimeError( + f"Timed out after {_MODE_MACHINE_WAIT_S:.1f}s waiting for " + f"first LowState — mode_machine never captured" + ) + + logger.info(f"G1WholeBodyConnection connected (mode_machine={self._mode_machine})") self.register_disposable(Disposable(self.motor_command.subscribe(self._on_motor_command))) self._publish_thread = Thread( - target=self._publish_loop, - name="g1-wholebody-pump", - daemon=True, + target=self._publish_loop, name="g1-wholebody-pump", daemon=True ) self._publish_thread.start() - logger.info( - f"G1WholeBodyConnection started (rate={self.config.publish_rate_hz}Hz, " - f"release_sport_mode={self.config.release_sport_mode})" - ) - @rpc def stop(self) -> None: self._stop_event.set() @@ -151,43 +201,69 @@ def stop(self) -> None: self._publish_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) self._publish_thread = None - if self.connection.is_connected(): - self.connection.disconnect() + # Drop SDK references so the underlying DDS participants can be GC'd. + self._publisher = None + self._subscriber = None + self._low_cmd = None + self._low_state = None + self._mode_machine = None + self._crc = None + logger.info("G1WholeBodyConnection disconnected") super().stop() + # ========================================================================= + # Publish loop (state out) + # ========================================================================= + def _publish_loop(self) -> None: period = 1.0 / float(self.config.publish_rate_hz) next_tick = time.perf_counter() frame_id = self.config.frame_id + # Identity quaternion + zeros while LowState hasn't arrived (start() blocks + # for it, but the publish loop may also see _low_state cleared during stop()). + zero_quat = (1.0, 0.0, 0.0, 0.0) + zero_vec3 = (0.0, 0.0, 0.0) + while not self._stop_event.is_set(): - states = self.connection.read_motor_states() - imu_state = self.connection.read_imu() - now = time.time() + with self._lock: + ls = self._low_state + if ls is None: + positions: list[float] = [0.0] * _NUM_MOTORS + velocities: list[float] = [0.0] * _NUM_MOTORS + efforts: list[float] = [0.0] * _NUM_MOTORS + quat = zero_quat + gyro = zero_vec3 + accel = zero_vec3 + else: + positions = [ls.motor_state[i].q for i in range(_NUM_MOTORS)] + velocities = [ls.motor_state[i].dq for i in range(_NUM_MOTORS)] + efforts = [ls.motor_state[i].tau_est for i in range(_NUM_MOTORS)] + quat = tuple(ls.imu_state.quaternion) + gyro = tuple(ls.imu_state.gyroscope) + accel = tuple(ls.imu_state.accelerometer) + now = time.time() self.motor_states.publish( JointState( ts=now, frame_id=frame_id, name=G1_JOINT_NAMES, - position=[s.q for s in states], - velocity=[s.dq for s in states], - effort=[s.tau for s in states], + position=positions, + velocity=velocities, + effort=efforts, ) ) # Unitree IMU quaternion is (w, x, y, z); dimos Quaternion is (x, y, z, w). - q = imu_state.quaternion - g = imu_state.gyroscope - a = imu_state.accelerometer self.imu.publish( Imu( ts=now, frame_id=frame_id, - orientation=Quaternion(q[1], q[2], q[3], q[0]), - angular_velocity=Vector3(g[0], g[1], g[2]), - linear_acceleration=Vector3(a[0], a[1], a[2]), + orientation=Quaternion(quat[1], quat[2], quat[3], quat[0]), + angular_velocity=Vector3(gyro[0], gyro[1], gyro[2]), + linear_acceleration=Vector3(accel[0], accel[1], accel[2]), ) ) @@ -198,22 +274,70 @@ def _publish_loop(self) -> None: else: next_tick = time.perf_counter() + # ========================================================================= + # Motor command in (MotorCommandArray → LowCmd → DDS rt/lowcmd) + # ========================================================================= + def _on_motor_command(self, msg: MotorCommandArray) -> None: if msg.num_joints != _NUM_MOTORS: logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") return - commands = [ - MotorCommand( - q=msg.q[i], - dq=msg.dq[i], - kp=msg.kp[i], - kd=msg.kd[i], - tau=msg.tau[i], - ) - for i in range(_NUM_MOTORS) - ] - self.connection.write_motor_commands(commands) + with self._lock: + if ( + self._low_cmd is None + or self._crc is None + or self._publisher is None + or self._mode_machine is None + ): + # Pre-start or post-stop — drop silently. + return + + # Echo mode_machine from the latest LowState — required by G1 firmware. + self._low_cmd.mode_machine = self._mode_machine + + for i in range(_NUM_MOTORS): + self._low_cmd.motor_cmd[i].q = msg.q[i] + self._low_cmd.motor_cmd[i].dq = msg.dq[i] + self._low_cmd.motor_cmd[i].kp = msg.kp[i] + self._low_cmd.motor_cmd[i].kd = msg.kd[i] + self._low_cmd.motor_cmd[i].tau = msg.tau[i] + + self._low_cmd.crc = self._crc.Crc(self._low_cmd) + self._publisher.Write(self._low_cmd) + + # ========================================================================= + # DDS subscriber callback (LowState in) + # ========================================================================= + + def _on_low_state(self, msg: Any) -> None: + """rt/lowstate callback — captures mode_machine and the latest snapshot.""" + with self._lock: + self._low_state = msg + if self._mode_machine is None: + self._mode_machine = msg.mode_machine + + # ========================================================================= + # Sport-mode release (real hardware only — gated by release_sport_mode) + # ========================================================================= + + def _release_sport_mode(self) -> None: + """Loop ReleaseMode until MotionSwitcher reports no active controller.""" + from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( + MotionSwitcherClient, + ) + + msc = MotionSwitcherClient() + msc.SetTimeout(5.0) + msc.Init() + + _status, result = msc.CheckMode() + while result["name"]: + msc.ReleaseMode() + _status, result = msc.CheckMode() + time.sleep(1) + + logger.info("Sport mode released — low-level control active") __all__ = ["G1_JOINT_NAMES", "G1WholeBodyConnection", "G1WholeBodyConnectionConfig"] diff --git a/scripts/test_g1_module/mock_lowstate_publisher.py b/scripts/test_g1_module/mock_lowstate_publisher.py index cd51a8ea43..659e46cf89 100644 --- a/scripts/test_g1_module/mock_lowstate_publisher.py +++ b/scripts/test_g1_module/mock_lowstate_publisher.py @@ -49,8 +49,8 @@ class MockG1LowStatePublisher: Args: rate_hz: Publish rate (Hz). mode_machine: Value to put in LowState.mode_machine. MUST be non-zero - so UnitreeG1LowLevelAdapter.connect() completes (it captures the - first non-None mode_machine and exits the wait loop). + so G1WholeBodyConnection.start() completes (it captures the first + non-None mode_machine from rt/lowstate and exits the wait loop). network_interface: DDS NIC, default 0. """ From 8bfd47e08114f6611a9837e07153f726aa3fdafd Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 29 Apr 2026 17:02:22 -0700 Subject: [PATCH 05/28] Updated control coordinator to support WholeBodyAdapter --- dimos/control/components.py | 54 +++++++++++++ dimos/control/coordinator.py | 57 +++++++++++-- dimos/control/hardware_interface.py | 110 ++++++++++++++++++++++++++ dimos/hardware/whole_body/registry.py | 84 ++++++++++++++++++++ 4 files changed, 298 insertions(+), 7 deletions(-) create mode 100644 dimos/hardware/whole_body/registry.py diff --git a/dimos/control/components.py b/dimos/control/components.py index be12aa4fb1..04ca6f03a7 100644 --- a/dimos/control/components.py +++ b/dimos/control/components.py @@ -37,6 +37,7 @@ def split_joint_name(joint_name: str) -> tuple[str, str]: class HardwareType(Enum): MANIPULATOR = "manipulator" BASE = "base" + WHOLE_BODY = "whole_body" @dataclass(frozen=True) @@ -135,6 +136,58 @@ def make_twist_base_joints( return [f"{hardware_id}/{s}" for s in suffixes] +_HUMANOID_29DOF_JOINTS = [ + # Left leg (0-5) + "left_hip_pitch", + "left_hip_roll", + "left_hip_yaw", + "left_knee", + "left_ankle_pitch", + "left_ankle_roll", + # Right leg (6-11) + "right_hip_pitch", + "right_hip_roll", + "right_hip_yaw", + "right_knee", + "right_ankle_pitch", + "right_ankle_roll", + # Waist (12-14) + "waist_yaw", + "waist_roll", + "waist_pitch", + # Left arm (15-21) + "left_shoulder_pitch", + "left_shoulder_roll", + "left_shoulder_yaw", + "left_elbow", + "left_wrist_roll", + "left_wrist_pitch", + "left_wrist_yaw", + # Right arm (22-28) + "right_shoulder_pitch", + "right_shoulder_roll", + "right_shoulder_yaw", + "right_elbow", + "right_wrist_roll", + "right_wrist_pitch", + "right_wrist_yaw", +] + + +def make_humanoid_joints(hardware_id: HardwareId) -> list[JointName]: + """Create joint names for a 29-DOF humanoid. + + Covers 6-DOF legs, 3-DOF waist, and 7-DOF arms. + + Args: + hardware_id: The hardware identifier (e.g., "g1") + + Returns: + List of 29 joint names like ["g1/left_hip_pitch", ..., "g1/right_wrist_yaw"] + """ + return [f"{hardware_id}/{j}" for j in _HUMANOID_29DOF_JOINTS] + + __all__ = [ "TWIST_SUFFIX_MAP", "HardwareComponent", @@ -144,6 +197,7 @@ def make_twist_base_joints( "JointState", "TaskName", "make_gripper_joints", + "make_humanoid_joints", "make_joints", "make_twist_base_joints", "split_joint_name", diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 555db13522..fb21c64c89 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -40,7 +40,11 @@ TaskName, split_joint_name, ) -from dimos.control.hardware_interface import ConnectedHardware, ConnectedTwistBase +from dimos.control.hardware_interface import ( + ConnectedHardware, + ConnectedTwistBase, + ConnectedWholeBody, +) from dimos.control.task import ControlTask from dimos.control.tick_loop import TickLoop from dimos.core.core import rpc @@ -210,8 +214,10 @@ def _setup_from_config(self) -> None: def _setup_hardware(self, component: HardwareComponent) -> None: """Connect and add a single hardware adapter.""" - adapter: ManipulatorAdapter | TwistBaseAdapter - if component.hardware_type == HardwareType.BASE: + adapter: ManipulatorAdapter | TwistBaseAdapter | object + if component.hardware_type == HardwareType.WHOLE_BODY: + adapter = self._create_whole_body_adapter(component) + elif component.hardware_type == HardwareType.BASE: adapter = self._create_twist_base_adapter(component) else: adapter = self._create_adapter(component) @@ -252,6 +258,28 @@ def _create_twist_base_adapter(self, component: HardwareComponent) -> TwistBaseA **component.adapter_kwargs, ) + def _create_whole_body_adapter(self, component: HardwareComponent) -> object: + """Create a whole-body adapter from component config. + + ``component.address`` carries the DDS network interface — int (CAN port) + or str ("enp60s0"); cyclonedds requires the right type, so try int() first + and fall back to keeping the original string. + """ + from dimos.hardware.whole_body.registry import whole_body_adapter_registry + + addr: int | str | None = component.address + if addr is not None: + try: + addr = int(addr) + except ValueError: + pass # keep as string (e.g. "enp60s0") + + return whole_body_adapter_registry.create( + component.adapter_type, + network_interface=addr if addr is not None else "", + **component.adapter_kwargs, + ) + def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: """Create a control task from config.""" task_type = cfg.type.lower() @@ -334,13 +362,23 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: @rpc def add_hardware( self, - adapter: ManipulatorAdapter | TwistBaseAdapter, + adapter: ManipulatorAdapter | TwistBaseAdapter | object, component: HardwareComponent, ) -> bool: """Register a hardware adapter with the coordinator.""" + from dimos.hardware.whole_body.spec import WholeBodyAdapter + is_base = component.hardware_type == HardwareType.BASE + is_whole_body = component.hardware_type == HardwareType.WHOLE_BODY + + if is_base and not isinstance(adapter, TwistBaseAdapter): + raise TypeError( + f"Hardware type / adapter mismatch for '{component.hardware_id}': " + f"hardware_type={component.hardware_type.value} but got " + f"{type(adapter).__name__}" + ) - if is_base != isinstance(adapter, TwistBaseAdapter): + if is_whole_body and not isinstance(adapter, WholeBodyAdapter): raise TypeError( f"Hardware type / adapter mismatch for '{component.hardware_id}': " f"hardware_type={component.hardware_type.value} but got " @@ -352,8 +390,13 @@ def add_hardware( logger.warning(f"Hardware {component.hardware_id} already registered") return False - if isinstance(adapter, TwistBaseAdapter): - connected: ConnectedHardware = ConnectedTwistBase( + if isinstance(adapter, WholeBodyAdapter): + connected: ConnectedHardware = ConnectedWholeBody( + adapter=adapter, + component=component, + ) + elif isinstance(adapter, TwistBaseAdapter): + connected = ConnectedTwistBase( adapter=adapter, component=component, ) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 3925116f97..73c411beac 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -33,6 +33,7 @@ if TYPE_CHECKING: from dimos.control.components import HardwareComponent, HardwareId, JointName, JointState from dimos.hardware.drive_trains.spec import TwistBaseAdapter + from dimos.hardware.whole_body.spec import WholeBodyAdapter logger = setup_logger() @@ -319,7 +320,116 @@ def write_command(self, commands: dict[str, float], _mode: ControlMode) -> bool: return self._twist_adapter.write_velocities(ordered) +_DEFAULT_KP: float = 40.0 +_DEFAULT_KD: float = 3.0 + + +class ConnectedWholeBody(ConnectedHardware): + """Runtime wrapper for a whole-body motor platform connected to the coordinator. + + Wraps a WholeBodyAdapter for joint-level motor control (any DOF count). + + Key differences from ConnectedHardware: + - Reads joint state from MotorState (q, dq, tau) + - write_command converts position commands to MotorCommand with PD gains + - write_motor_commands provides direct pass-through to adapter + """ + + _wb_adapter: WholeBodyAdapter + + def __init__( + self, + adapter: WholeBodyAdapter, + component: HardwareComponent, + ) -> None: + from dimos.hardware.whole_body.spec import WholeBodyAdapter as WholeBodyAdapterProto + + if not isinstance(adapter, WholeBodyAdapterProto): + raise TypeError("adapter must implement WholeBodyAdapter") + + self._wb_adapter = adapter + self._component = component + self._joint_names = component.joints + + self._last_commanded: dict[str, float] = {} + self._initialized = False + self._warned_unknown_joints: set[str] = set() + self._current_mode: ControlMode | None = None + + @property + def adapter(self) -> WholeBodyAdapter: # type: ignore[override] + """The underlying whole-body adapter.""" + return self._wb_adapter + + def disconnect(self) -> None: + """Disconnect the underlying adapter.""" + self._wb_adapter.disconnect() + + def read_state(self) -> dict[JointName, JointState]: + """Read motor states as {joint_name: JointState}.""" + from dimos.control.components import JointState + + motor_states = self._wb_adapter.read_motor_states() + return { + name: JointState( + position=motor_states[i].q, + velocity=motor_states[i].dq, + effort=motor_states[i].tau, + ) + for i, name in enumerate(self._joint_names) + } + + def write_command(self, commands: dict[str, float], _mode: ControlMode) -> bool: + """Write position commands — converts to MotorCommand with PD gains. + + Args: + commands: {joint_name: target_position} - can be partial + _mode: Control mode (uses position PD regardless) + + Returns: + True if command was sent successfully + """ + from dimos.hardware.whole_body.spec import MotorCommand + + if not self._initialized: + self._initialize_last_commanded() + + for joint_name, value in commands.items(): + if joint_name in self._joint_names: + self._last_commanded[joint_name] = value + elif joint_name not in self._warned_unknown_joints: + logger.warning( + f"WholeBody {self.hardware_id} received command for unknown joint " + f"{joint_name}. Valid joints: {self._joint_names}" + ) + self._warned_unknown_joints.add(joint_name) + + motor_cmds = [ + MotorCommand( + q=self._last_commanded[name], + dq=0.0, + kp=_DEFAULT_KP, + kd=_DEFAULT_KD, + tau=0.0, + ) + for name in self._joint_names + ] + return self._wb_adapter.write_motor_commands(motor_cmds) + + def write_motor_commands(self, commands: list) -> bool: + """Direct pass-through to adapter for full MotorCommand control.""" + return self._wb_adapter.write_motor_commands(commands) + + def _initialize_last_commanded(self) -> None: + """Initialize last_commanded with current motor positions.""" + motor_states = self._wb_adapter.read_motor_states() + for i, name in enumerate(self._joint_names): + self._last_commanded[name] = motor_states[i].q + self._initialized = True + + __all__ = [ "ConnectedHardware", "ConnectedTwistBase", + "ConnectedWholeBody", ] diff --git a/dimos/hardware/whole_body/registry.py b/dimos/hardware/whole_body/registry.py new file mode 100644 index 0000000000..ca39193656 --- /dev/null +++ b/dimos/hardware/whole_body/registry.py @@ -0,0 +1,84 @@ +# 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. + +"""WholeBodyAdapter registry with auto-discovery. + +Mirrors the TwistBaseAdapterRegistry pattern: each subpackage provides a +``register(registry)`` function in its ``adapter.py`` module. + +Usage: + from dimos.hardware.whole_body.registry import whole_body_adapter_registry + + adapter = whole_body_adapter_registry.create("unitree_go2") + print(whole_body_adapter_registry.available()) # ["unitree_go2"] +""" + +from __future__ import annotations + +import importlib +import logging +import pkgutil +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from dimos.hardware.whole_body.spec import WholeBodyAdapter + +logger = logging.getLogger(__name__) + + +class WholeBodyAdapterRegistry: + """Registry for whole-body motor adapters with auto-discovery.""" + + def __init__(self) -> None: + self._adapters: dict[str, type[WholeBodyAdapter]] = {} + + def register(self, name: str, cls: type[WholeBodyAdapter]) -> None: + """Register an adapter class.""" + self._adapters[name.lower()] = cls + + def create(self, name: str, **kwargs: Any) -> WholeBodyAdapter: + """Create an adapter instance by name.""" + key = name.lower() + if key not in self._adapters: + raise KeyError(f"Unknown whole-body adapter: {name}. Available: {self.available()}") + return self._adapters[key](**kwargs) + + def available(self) -> list[str]: + """List available adapter names.""" + return sorted(self._adapters.keys()) + + def discover(self) -> None: + """Discover and register adapters from subpackages. + + Mirrors ``dimos.hardware.drive_trains.registry``'s one-level walk: + each direct subpackage of ``dimos.hardware.whole_body`` is expected + to expose ``adapter.py`` with a ``register(registry)`` function. + """ + import dimos.hardware.whole_body as pkg + + for _, name, ispkg in pkgutil.iter_modules(pkg.__path__): + if not ispkg: + continue + try: + mod = importlib.import_module(f"dimos.hardware.whole_body.{name}.adapter") + if hasattr(mod, "register"): + mod.register(self) + except ImportError as e: + logger.warning(f"Skipping whole-body adapter {name}: {e}") + + +whole_body_adapter_registry = WholeBodyAdapterRegistry() +whole_body_adapter_registry.discover() + +__all__ = ["WholeBodyAdapterRegistry", "whole_body_adapter_registry"] From f0b90e23a95e6f3d6b4169ae0142d02e98873500 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 29 Apr 2026 17:50:15 -0700 Subject: [PATCH 06/28] TransportWholeBody adapter --- dimos/hardware/whole_body/registry.py | 21 +- .../hardware/whole_body/transport/adapter.py | 223 ++++++++++++++++++ 2 files changed, 237 insertions(+), 7 deletions(-) create mode 100644 dimos/hardware/whole_body/transport/adapter.py diff --git a/dimos/hardware/whole_body/registry.py b/dimos/hardware/whole_body/registry.py index ca39193656..55d2547854 100644 --- a/dimos/hardware/whole_body/registry.py +++ b/dimos/hardware/whole_body/registry.py @@ -28,7 +28,7 @@ import importlib import logging -import pkgutil +import os from typing import TYPE_CHECKING, Any if TYPE_CHECKING: @@ -61,21 +61,28 @@ def available(self) -> list[str]: def discover(self) -> None: """Discover and register adapters from subpackages. - Mirrors ``dimos.hardware.drive_trains.registry``'s one-level walk: - each direct subpackage of ``dimos.hardware.whole_body`` is expected + Mirrors ``dimos.hardware.drive_trains.registry``'s walk pattern: raw + filesystem listing + ``os.path.isdir`` check. We do NOT use + ``pkgutil.iter_modules`` because dimos uses PEP 420 namespace packages + (no ``__init__.py``) and ``iter_modules`` doesn't reliably enumerate + subpackage directories under namespace packages. + + Each direct subpackage of ``dimos.hardware.whole_body`` is expected to expose ``adapter.py`` with a ``register(registry)`` function. """ import dimos.hardware.whole_body as pkg - for _, name, ispkg in pkgutil.iter_modules(pkg.__path__): - if not ispkg: + pkg_dir = pkg.__path__[0] + for entry in sorted(os.listdir(pkg_dir)): + entry_path = os.path.join(pkg_dir, entry) + if not os.path.isdir(entry_path) or entry.startswith(("_", ".")): continue try: - mod = importlib.import_module(f"dimos.hardware.whole_body.{name}.adapter") + mod = importlib.import_module(f"dimos.hardware.whole_body.{entry}.adapter") if hasattr(mod, "register"): mod.register(self) except ImportError as e: - logger.warning(f"Skipping whole-body adapter {name}: {e}") + logger.warning(f"Skipping whole-body adapter {entry}: {e}") whole_body_adapter_registry = WholeBodyAdapterRegistry() diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py new file mode 100644 index 0000000000..126ad6d8b6 --- /dev/null +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -0,0 +1,223 @@ +# 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. + +"""Transport-based whole-body adapter — connects coordinator to a Module via pub/sub. + +One generic adapter for every humanoid/quadruped that publishes the same +JointState/Imu/MotorCommandArray topic interface. Per-robot logic stays in +the Module, not here. + +Topics derived from ``hardware_id``: + - subscribes /{hardware_id}/motor_states (JointState) + - subscribes /{hardware_id}/imu (Imu) + - publishes /{hardware_id}/motor_command (MotorCommandArray) + +The ``network_interface`` kwarg is accepted (the coordinator forwards +``component.address``) and ignored — LCM transport doesn't use a NIC. +""" + +from __future__ import annotations + +from functools import partial +import threading +from typing import TYPE_CHECKING, Any + +from dimos.core.transport import LCMTransport +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.hardware.whole_body.registry import WholeBodyAdapterRegistry + +logger = setup_logger() + + +class TransportWholeBodyAdapter: + """WholeBodyAdapter that bridges to a robot-side Module via pub/sub.""" + + def __init__( + self, + dof: int = 29, + hardware_id: str = "wholebody", + transport_cls: type = LCMTransport, + network_interface: int | str = "", # accepted-and-ignored — see module docstring + **_: object, + ) -> None: + self._dof = dof + self._prefix = hardware_id + self._transport_cls = transport_cls + + self._lock = threading.Lock() + self._latest_motor_states: list[MotorState] | None = None + self._latest_imu: IMUState | None = None + + self._motor_states_transport: Any = None + self._imu_transport: Any = None + self._motor_command_transport: Any = None + self._motor_states_unsub: Any = None + self._imu_unsub: Any = None + self._connected = False + + # ------------------------------------------------------------------ + # Connection + # ------------------------------------------------------------------ + + def connect(self) -> bool: + ms_topic = f"/{self._prefix}/motor_states" + imu_topic = f"/{self._prefix}/imu" + cmd_topic = f"/{self._prefix}/motor_command" + + self._motor_states_transport = self._transport_cls(ms_topic, JointState) + self._imu_transport = self._transport_cls(imu_topic, Imu) + self._motor_command_transport = self._transport_cls(cmd_topic, MotorCommandArray) + + self._motor_states_unsub = self._motor_states_transport.subscribe(self._on_motor_states) + self._imu_unsub = self._imu_transport.subscribe(self._on_imu) + + self._connected = True + logger.info( + f"TransportWholeBodyAdapter connected: motor_states={ms_topic}, " + f"imu={imu_topic}, motor_command={cmd_topic}" + ) + return True + + def disconnect(self) -> None: + if self._motor_states_unsub is not None: + self._motor_states_unsub() + self._motor_states_unsub = None + if self._imu_unsub is not None: + self._imu_unsub() + self._imu_unsub = None + + for t in ( + self._motor_states_transport, + self._imu_transport, + self._motor_command_transport, + ): + if t is not None: + t.stop() + self._motor_states_transport = None + self._imu_transport = None + self._motor_command_transport = None + + with self._lock: + self._latest_motor_states = None + self._latest_imu = None + + self._connected = False + logger.info("TransportWholeBodyAdapter disconnected") + + def is_connected(self) -> bool: + return self._connected + + # ------------------------------------------------------------------ + # State reading (latest-frame cached, non-blocking) + # ------------------------------------------------------------------ + + def read_motor_states(self) -> list[MotorState]: + """Return latest cached motor states. Returns defaults if no frame yet.""" + with self._lock: + if self._latest_motor_states is None: + return [MotorState() for _ in range(self._dof)] + return list(self._latest_motor_states) + + def read_imu(self) -> IMUState: + """Return latest cached IMU state. Returns defaults if no frame yet.""" + with self._lock: + if self._latest_imu is None: + return IMUState() + return self._latest_imu + + # ------------------------------------------------------------------ + # Control + # ------------------------------------------------------------------ + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + """Publish a MotorCommandArray to /{hardware_id}/motor_command. + + Per-joint q/dq/kp/kd/tau survive the wire — that's the whole point + of being at the low-level layer. + """ + if self._motor_command_transport is None: + logger.warning("write_motor_commands called before connect()") + return False + + msg = MotorCommandArray( + q=[c.q for c in commands], + dq=[c.dq for c in commands], + kp=[c.kp for c in commands], + kd=[c.kd for c in commands], + tau=[c.tau for c in commands], + ) + self._motor_command_transport.publish(msg) + return True + + # ------------------------------------------------------------------ + # Subscriber callbacks (LCM dispatcher thread) + # ------------------------------------------------------------------ + + def _on_motor_states(self, msg: JointState) -> None: + # JointState.position / .velocity / .effort -> MotorState.q / .dq / .tau. + # Ignore msg.name — order is implicit (set by the producing Module's + # G1_JOINT_NAMES which mirrors make_humanoid_joints("g1")). + n = min(len(msg.position), self._dof) + states = [ + MotorState(q=msg.position[i], dq=msg.velocity[i], tau=msg.effort[i]) for i in range(n) + ] + with self._lock: + self._latest_motor_states = states + + def _on_imu(self, msg: Imu) -> None: + # dimos Imu.orientation Quaternion is (x, y, z, w); + # IMUState.quaternion is (w, x, y, z) — translate. + with self._lock: + self._latest_imu = IMUState( + quaternion=( + msg.orientation.w, + msg.orientation.x, + msg.orientation.y, + msg.orientation.z, + ), + gyroscope=( + msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z, + ), + accelerometer=( + msg.linear_acceleration.x, + msg.linear_acceleration.y, + msg.linear_acceleration.z, + ), + rpy=(0.0, 0.0, 0.0), # dimos Imu doesn't carry rpy; downstream typically ignores + ) + + +def register(registry: WholeBodyAdapterRegistry) -> None: + """Auto-discovered by ``whole_body_adapter_registry.discover()``.""" + from dimos.core.transport import ROSTransport + + registry.register( + "transport_lcm", + partial(TransportWholeBodyAdapter, transport_cls=LCMTransport), + ) + registry.register( + "transport_ros", + partial(TransportWholeBodyAdapter, transport_cls=ROSTransport), + ) + + +__all__ = ["TransportWholeBodyAdapter"] From 7c152f0e3cfdf5b05f461fe5bc05f9f7fadcca7c Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 29 Apr 2026 17:56:41 -0700 Subject: [PATCH 07/28] low level g1 coordinator blueprint --- .../basic/unitree_g1_coordinator.py | 99 +++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py new file mode 100644 index 0000000000..7c248920b3 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -0,0 +1,99 @@ +#!/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 — G1WholeBodyConnection + coordinator via LCM transport adapter. + +Pattern mirrors `unitree_go2_coordinator.py`. The Module owns the DDS connection +in its own worker; the coordinator constructs a `TransportWholeBodyAdapter` +(registered as ``transport_lcm``) that pub/subs the same LCM topics the Module +publishes to / subscribes from. + +No tasks are configured by default — Session B2 drives damping commands from +an external bring-up script (see ``scripts/g1_damping/``). When a real-time +control task lands, add a TaskConfig here. + +Usage: + dimos run unitree-g1-coordinator # real hardware (default) + python scripts/test_g1_coordinator/test_g1_coordinator.py # mock — uses build_blueprint(release_sport_mode=False) +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from dimos.control.components import HardwareComponent, HardwareType, make_humanoid_joints +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection + +if TYPE_CHECKING: + from dimos.core.coordination.blueprints import Blueprint + + +def build_unitree_g1_coordinator(release_sport_mode: bool = True) -> Blueprint: + """Build the G1 wholebody coordinator blueprint. + + Args: + release_sport_mode: Forwarded to ``G1WholeBodyConnection`` config. + Default ``True`` for real hardware (the Module's start() will call + ``MotionSwitcherClient`` to exit Unitree's sport mode before taking + low-level control). Tests against ``MockG1LowStatePublisher`` should + pass ``False`` since there's no MotionSwitcher endpoint to talk to. + """ + g1_joints = make_humanoid_joints("g1") + + return ( + autoconnect( + G1WholeBodyConnection.blueprint(release_sport_mode=release_sport_mode), + ControlCoordinator.blueprint( + hardware=[ + HardwareComponent( + hardware_id="g1", + hardware_type=HardwareType.WHOLE_BODY, + joints=g1_joints, + adapter_type="transport_lcm", + ), + ], + ), + ) + # No remappings needed: G1WholeBodyConnection's stream names (motor_states, + # imu, motor_command) don't collide with any ControlCoordinator stream + # names (joint_state, joint_command, cartesian_command, twist_command, + # buttons). Compare unitree_go2_coordinator, which DOES need to remap + # GO2Connection.cmd_vel because it collides with the coordinator's + # twist_command path bound to /cmd_vel. + .transports( + { + ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), + ("imu", Imu): LCMTransport("/g1/imu", Imu), + ("motor_command", MotorCommandArray): LCMTransport( + "/g1/motor_command", MotorCommandArray + ), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } + ) + ) + + +# Top-level constant for `dimos run unitree-g1-coordinator` (see all_blueprints.py). +# Defaults to release_sport_mode=True (production / real hardware). +unitree_g1_coordinator = build_unitree_g1_coordinator() + + +__all__ = ["build_unitree_g1_coordinator", "unitree_g1_coordinator"] From cdaded12975a45eeea9e4ee87b4a5f12cb34c270 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 29 Apr 2026 19:13:59 -0700 Subject: [PATCH 08/28] Close dds endpoints to clean exit --- dimos/robot/unitree/g1/wholebody_connection.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 651d70ba6f..437990a6ae 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -201,7 +201,23 @@ def stop(self) -> None: self._publish_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) self._publish_thread = None - # Drop SDK references so the underlying DDS participants can be GC'd. + # Close DDS endpoints explicitly before dropping refs — relying on GC + # leaves the cyclonedds participant in a state where in-flight callbacks + # race with cleanup and segfault on process exit. Verified against + # unitree_sdk2py/core/channel.py (Close exists at lines 267 + 288); + # mirrors the Go2 SDK adapter's pattern in + # dimos/hardware/drive_trains/unitree_go2/adapter.py. + if self._subscriber is not None: + try: + self._subscriber.Close() + except (OSError, RuntimeError) as e: + logger.warning(f"ChannelSubscriber Close raised: {e}") + if self._publisher is not None: + try: + self._publisher.Close() + except (OSError, RuntimeError) as e: + logger.warning(f"ChannelPublisher Close raised: {e}") + self._publisher = None self._subscriber = None self._low_cmd = None From 72bea7962e0fd09ee7df984d53cbc8fbb435e77e Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Wed, 29 Apr 2026 20:52:14 -0700 Subject: [PATCH 09/28] fixed network adapter field missing --- .../g1/blueprints/basic/unitree_g1_coordinator.py | 14 ++++++++++++-- dimos/robot/unitree/g1/wholebody_connection.py | 6 +++++- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py index 7c248920b3..94d8aa9842 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -46,7 +46,10 @@ from dimos.core.coordination.blueprints import Blueprint -def build_unitree_g1_coordinator(release_sport_mode: bool = True) -> Blueprint: +def build_unitree_g1_coordinator( + release_sport_mode: bool = True, + network_interface: str = "", +) -> Blueprint: """Build the G1 wholebody coordinator blueprint. Args: @@ -55,12 +58,19 @@ def build_unitree_g1_coordinator(release_sport_mode: bool = True) -> Blueprint: ``MotionSwitcherClient`` to exit Unitree's sport mode before taking low-level control). Tests against ``MockG1LowStatePublisher`` should pass ``False`` since there's no MotionSwitcher endpoint to talk to. + network_interface: NIC name (e.g. ``"enp60s0"``) to bind cyclonedds to. + Empty string lets cyclonedds pick the default interface — silently + picks the wrong NIC on a multi-interface host (wifi + ethernet + both up) and stalls discovery. Set this when LowState never arrives. """ g1_joints = make_humanoid_joints("g1") return ( autoconnect( - G1WholeBodyConnection.blueprint(release_sport_mode=release_sport_mode), + G1WholeBodyConnection.blueprint( + release_sport_mode=release_sport_mode, + network_interface=network_interface, + ), ControlCoordinator.blueprint( hardware=[ HardwareComponent( diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 437990a6ae..1020858e75 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -347,8 +347,12 @@ def _release_sport_mode(self) -> None: msc.SetTimeout(5.0) msc.Init() + # MotionSwitcher.CheckMode() returns (status, dict) while a sport + # mode is active and (status, None) once nothing is active. Use a + # null-tolerant check so the loop exits cleanly after the release + # rather than crashing on `None["name"]`. _status, result = msc.CheckMode() - while result["name"]: + while result and result.get("name"): msc.ReleaseMode() _status, result = msc.CheckMode() time.sleep(1) From 88c26a37345ebb695c8a8dbe382dd215f1a0c36d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 16:59:15 -0700 Subject: [PATCH 10/28] hardware id added to whole body adapter --- dimos/control/coordinator.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index fb21c64c89..435c224f29 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -276,6 +276,7 @@ def _create_whole_body_adapter(self, component: HardwareComponent) -> object: return whole_body_adapter_registry.create( component.adapter_type, + hardware_id=component.hardware_id, network_interface=addr if addr is not None else "", **component.adapter_kwargs, ) From b3eb8ecff96da55adc6d123bd471ad8bf86c790d Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 16:59:50 -0700 Subject: [PATCH 11/28] coordinator blueprint added with TransportWholeBodyAdapter --- dimos/robot/all_blueprints.py | 2 + .../basic/unitree_g1_coordinator.py | 121 ++++++++---------- 2 files changed, 55 insertions(+), 68 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 96ce1c6784..f1f23da4da 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -70,6 +70,7 @@ "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-coordinator": "dimos.robot.unitree.g1.blueprints.basic.unitree_g1_coordinator:unitree_g1_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", @@ -125,6 +126,7 @@ "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", "g1-sim-connection": "dimos.robot.unitree.g1.sim.G1SimConnection", + "g1-whole-body-connection": "dimos.robot.unitree.g1.wholebody_connection.G1WholeBodyConnection", "go2-connection": "dimos.robot.unitree.go2.connection.GO2Connection", "go2-fleet-connection": "dimos.robot.unitree.go2.fleet_connection.Go2FleetConnection", "go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2.Go2Memory", diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py index 94d8aa9842..465e1b9152 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -20,21 +20,20 @@ (registered as ``transport_lcm``) that pub/subs the same LCM topics the Module publishes to / subscribes from. -No tasks are configured by default — Session B2 drives damping commands from -an external bring-up script (see ``scripts/g1_damping/``). When a real-time -control task lands, add a TaskConfig here. +A single ``servo_g1`` task converts incoming JointState commands on +``/g1/joint_command`` into per-joint position commands routed to the +WHOLE_BODY hardware (PD gains baked into ``ConnectedWholeBody``). Usage: - dimos run unitree-g1-coordinator # real hardware (default) - python scripts/test_g1_coordinator/test_g1_coordinator.py # mock — uses build_blueprint(release_sport_mode=False) + ROBOT_INTERFACE=enp60s0 dimos run unitree-g1-coordinator """ from __future__ import annotations -from typing import TYPE_CHECKING +import os from dimos.control.components import HardwareComponent, HardwareType, make_humanoid_joints -from dimos.control.coordinator import ControlCoordinator +from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs.Imu import Imu @@ -42,68 +41,54 @@ from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection -if TYPE_CHECKING: - from dimos.core.coordination.blueprints import Blueprint - - -def build_unitree_g1_coordinator( - release_sport_mode: bool = True, - network_interface: str = "", -) -> Blueprint: - """Build the G1 wholebody coordinator blueprint. - - Args: - release_sport_mode: Forwarded to ``G1WholeBodyConnection`` config. - Default ``True`` for real hardware (the Module's start() will call - ``MotionSwitcherClient`` to exit Unitree's sport mode before taking - low-level control). Tests against ``MockG1LowStatePublisher`` should - pass ``False`` since there's no MotionSwitcher endpoint to talk to. - network_interface: NIC name (e.g. ``"enp60s0"``) to bind cyclonedds to. - Empty string lets cyclonedds pick the default interface — silently - picks the wrong NIC on a multi-interface host (wifi + ethernet - both up) and stalls discovery. Set this when LowState never arrives. - """ - g1_joints = make_humanoid_joints("g1") - - return ( - autoconnect( - G1WholeBodyConnection.blueprint( - release_sport_mode=release_sport_mode, - network_interface=network_interface, - ), - ControlCoordinator.blueprint( - hardware=[ - HardwareComponent( - hardware_id="g1", - hardware_type=HardwareType.WHOLE_BODY, - joints=g1_joints, - adapter_type="transport_lcm", - ), - ], - ), - ) - # No remappings needed: G1WholeBodyConnection's stream names (motor_states, - # imu, motor_command) don't collide with any ControlCoordinator stream - # names (joint_state, joint_command, cartesian_command, twist_command, - # buttons). Compare unitree_go2_coordinator, which DOES need to remap - # GO2Connection.cmd_vel because it collides with the coordinator's - # twist_command path bound to /cmd_vel. - .transports( - { - ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), - ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport( - "/g1/motor_command", MotorCommandArray +_g1_joints = make_humanoid_joints("g1") + +# ROBOT_INTERFACE pins cyclonedds to a specific NIC — required on multi-NIC hosts +# where the default pick stalls discovery (no LowState ever arrives). +unitree_g1_coordinator = ( + autoconnect( + G1WholeBodyConnection.blueprint( + release_sport_mode=True, + network_interface=os.getenv("ROBOT_INTERFACE", ""), + ), + ControlCoordinator.blueprint( + tick_rate=500, + hardware=[ + HardwareComponent( + hardware_id="g1", + hardware_type=HardwareType.WHOLE_BODY, + joints=_g1_joints, + adapter_type="transport_lcm", ), - ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), - } - ) + ], + tasks=[ + TaskConfig( + name="servo_g1", + type="servo", + joint_names=_g1_joints, + priority=10, + ), + ], + ), ) + # No remappings needed: G1WholeBodyConnection's stream names (motor_states, + # imu, motor_command) don't collide with any ControlCoordinator stream + # names (joint_state, joint_command, cartesian_command, twist_command, + # buttons). Compare unitree_go2_coordinator, which DOES need to remap + # GO2Connection.cmd_vel because it collides with the coordinator's + # twist_command path bound to /cmd_vel. + .transports( + { + ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), + ("imu", Imu): LCMTransport("/g1/imu", Imu), + ("motor_command", MotorCommandArray): LCMTransport( + "/g1/motor_command", MotorCommandArray + ), + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), + } + ) +) -# Top-level constant for `dimos run unitree-g1-coordinator` (see all_blueprints.py). -# Defaults to release_sport_mode=True (production / real hardware). -unitree_g1_coordinator = build_unitree_g1_coordinator() - - -__all__ = ["build_unitree_g1_coordinator", "unitree_g1_coordinator"] +__all__ = ["unitree_g1_coordinator"] From 2ab8e4214b0cf202879ea7d3541fa46b92f5621c Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 17:00:32 -0700 Subject: [PATCH 12/28] replay script added to test wholebodyadapter --- scripts/g1_replay.py | 217 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 217 insertions(+) create mode 100644 scripts/g1_replay.py diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py new file mode 100644 index 0000000000..e4f5285e10 --- /dev/null +++ b/scripts/g1_replay.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python3 +# Copyright 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. + +# 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. +"""Replay a recorded 29-DOF G1 joint trajectory through the coordinator. + +Publishes JointState to /g1/joint_command. Flow: + + g1_replay.py + | JointState (29 names + 29 positions) + v /g1/joint_command (LCM) + ControlCoordinator (servo_g1 task, tick_rate=500) + | per-joint position commands → MotorCommand with built-in PD gains + v TransportWholeBodyAdapter + | MotorCommandArray + v /g1/motor_command (LCM) + G1WholeBodyConnection + v DDS to robot + +Joint name remapping: the trajectory file (recorded on the playback branch) uses +``g1_LeftHipPitch`` style names, but this branch's coordinator expects +``g1/left_hip_pitch``. Both lists share the same positional ordering — defined +once in ``_HUMANOID_29DOF_JOINTS`` — so we ignore the file's joint_names entirely +and zip its position arrays onto the canonical list from +``make_humanoid_joints("g1")``. + +Trajectory file format: + { + "joint_names": [...29 names...], # ignored — only used for length check + "samples": [{"ts": float, "position": [...29 floats...]}, ...] + } + +Usage: + # Terminal 1: + ROBOT_INTERFACE=enp194s0 dimos run unitree-g1-coordinator + # Terminal 2: + python scripts/g1_replay.py --file /tmp/g1_traj.json + python scripts/g1_replay.py --file /tmp/g1_traj.json --loop + python scripts/g1_replay.py --file /tmp/g1_traj.json --dry-run +""" + +from __future__ import annotations + +import argparse +import json +import logging +from pathlib import Path +import threading +import time + +from dimos.control.components import make_humanoid_joints +from dimos.core.transport import LCMTransport +from dimos.msgs.sensor_msgs.JointState import JointState + +NUM_DOF = 29 +CANONICAL_JOINTS = make_humanoid_joints("g1") +assert len(CANONICAL_JOINTS) == NUM_DOF + +logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s") +logger = logging.getLogger("g1_replay") + + +def load_trajectory(path: Path) -> tuple[list[float], list[list[float]]]: + data = json.loads(path.read_text()) + joint_names = data.get("joint_names", []) + samples = data.get("samples", []) + if len(joint_names) != NUM_DOF: + raise ValueError(f"trajectory has {len(joint_names)} joint_names, expected {NUM_DOF}") + if not samples: + raise ValueError("trajectory has zero samples") + for i, s in enumerate(samples): + if len(s["position"]) != NUM_DOF: + raise ValueError(f"sample {i} has {len(s['position'])} positions, expected {NUM_DOF}") + t0 = samples[0]["ts"] + rel_ts = [s["ts"] - t0 for s in samples] + positions = [list(s["position"]) for s in samples] + return rel_ts, positions + + +def make_joint_state(positions: list[float]) -> JointState: + return JointState( + name=list(CANONICAL_JOINTS), + position=list(positions), + velocity=[0.0] * NUM_DOF, + effort=[0.0] * NUM_DOF, + ) + + +def main() -> None: + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--file", default="/tmp/g1_traj.json", help="trajectory JSON path") + p.add_argument( + "--ramp", + type=float, + default=3.0, + help="seconds to interpolate from current pose to first sample", + ) + p.add_argument("--loop", action="store_true", help="repeat trajectory until Ctrl+C") + p.add_argument( + "--dry-run", action="store_true", help="subscribe + log but do NOT publish commands" + ) + args = p.parse_args() + + rel_ts, positions = load_trajectory(Path(args.file)) + native_rate = len(rel_ts) / rel_ts[-1] if rel_ts[-1] > 0 else 0.0 + logger.info( + f"loaded {len(rel_ts)} samples, duration={rel_ts[-1]:.2f}s, native_rate={native_rate:.1f}Hz" + ) + logger.info(f"first sample[0:3]={positions[0][:3]} last sample[0:3]={positions[-1][:3]}") + logger.info( + f"will publish to /g1/joint_command using canonical joint names ({CANONICAL_JOINTS[0]} ...)" + ) + + # Subscribe to coordinator's joint_state output to capture the current pose. + # Coordinator joint state names are {hardware_id}/{joint} so they align with + # CANONICAL_JOINTS, lookup is straightforward. + state_lock = threading.Lock() + current_q: dict[str, float] = {} + state_event = threading.Event() + + def on_state(msg: JointState) -> None: + with state_lock: + current_q.clear() + for n, q in zip(msg.name, msg.position, strict=True): + current_q[n] = q + state_event.set() + + state_sub = LCMTransport("/coordinator/joint_state", JointState) + state_unsub = state_sub.subscribe(on_state) + cmd_pub = LCMTransport("/g1/joint_command", JointState) + + try: + logger.info("waiting up to 10s for /coordinator/joint_state ...") + if not state_event.wait(timeout=10.0): + logger.error("no joint_state received — is `dimos run unitree-g1-coordinator` running?") + return + with state_lock: + start_q = [current_q.get(n, 0.0) for n in CANONICAL_JOINTS] + missing = [n for n in CANONICAL_JOINTS if n not in current_q] + if missing: + logger.warning( + f"coordinator joint_state missing {len(missing)} of {NUM_DOF} canonical joints: {missing[:3]}..." + ) + logger.warning("will treat missing joints as 0.0 — check joint name conventions") + logger.info(f"current pose[0:3]={start_q[0:3]}") + + if args.dry_run: + logger.info("--dry-run set — exiting before publish phase") + return + + # ---------------- Ramp ---------------- + logger.info(f"ramping current → trajectory[0] over {args.ramp:.2f}s") + ramp_period = 1.0 / 100.0 # 100 Hz during ramp + ramp_start = time.perf_counter() + target_q = positions[0] + while True: + elapsed = time.perf_counter() - ramp_start + a = min(elapsed / args.ramp, 1.0) + interp = [start_q[i] + a * (target_q[i] - start_q[i]) for i in range(NUM_DOF)] + cmd_pub.publish(make_joint_state(interp)) + if a >= 1.0: + break + time.sleep(ramp_period) + + # ---------------- Replay ---------------- + passes = 0 + while True: + passes += 1 + logger.info(f"playback pass {passes} (Ctrl+C to stop)") + replay_start = time.perf_counter() + last_log = replay_start + for i, (sample_t, q) in enumerate(zip(rel_ts, positions, strict=True)): + wait = sample_t - (time.perf_counter() - replay_start) + if wait > 0: + time.sleep(wait) + cmd_pub.publish(make_joint_state(q)) + now = time.perf_counter() + if now - last_log >= 2.0: + logger.info(f" t={sample_t:6.2f}s sample={i}/{len(rel_ts)}") + last_log = now + if not args.loop: + break + + logger.info("trajectory complete (last pose held by coordinator's last command)") + + except KeyboardInterrupt: + logger.info("Ctrl+C — exiting; coordinator holds last published target") + finally: + state_unsub() + state_sub.stop() + cmd_pub.stop() + + +if __name__ == "__main__": + main() From bbb01b1b0e4276496e4bc4fc56b751f3c511f672 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 18:29:18 -0700 Subject: [PATCH 13/28] ruff and mypy fixes --- dimos/control/coordinator.py | 8 +++++--- dimos/control/hardware_interface.py | 4 ++-- dimos/hardware/whole_body/registry.py | 10 +++++++--- dimos/msgs/sensor_msgs/MotorCommandArray.py | 11 ++++------- scripts/g1_replay.py | 21 ++++++--------------- 5 files changed, 24 insertions(+), 30 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 435c224f29..b97421347a 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -65,6 +65,8 @@ if TYPE_CHECKING: from collections.abc import Callable + from dimos.hardware.whole_body.spec import WholeBodyAdapter + logger = setup_logger() @@ -214,7 +216,7 @@ def _setup_from_config(self) -> None: def _setup_hardware(self, component: HardwareComponent) -> None: """Connect and add a single hardware adapter.""" - adapter: ManipulatorAdapter | TwistBaseAdapter | object + adapter: ManipulatorAdapter | TwistBaseAdapter | WholeBodyAdapter if component.hardware_type == HardwareType.WHOLE_BODY: adapter = self._create_whole_body_adapter(component) elif component.hardware_type == HardwareType.BASE: @@ -258,7 +260,7 @@ def _create_twist_base_adapter(self, component: HardwareComponent) -> TwistBaseA **component.adapter_kwargs, ) - def _create_whole_body_adapter(self, component: HardwareComponent) -> object: + def _create_whole_body_adapter(self, component: HardwareComponent) -> "WholeBodyAdapter": """Create a whole-body adapter from component config. ``component.address`` carries the DDS network interface — int (CAN port) @@ -363,7 +365,7 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: @rpc def add_hardware( self, - adapter: ManipulatorAdapter | TwistBaseAdapter | object, + adapter: "ManipulatorAdapter | TwistBaseAdapter | WholeBodyAdapter", component: HardwareComponent, ) -> bool: """Register a hardware adapter with the coordinator.""" diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 73c411beac..91d320645f 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -33,7 +33,7 @@ if TYPE_CHECKING: from dimos.control.components import HardwareComponent, HardwareId, JointName, JointState from dimos.hardware.drive_trains.spec import TwistBaseAdapter - from dimos.hardware.whole_body.spec import WholeBodyAdapter + from dimos.hardware.whole_body.spec import MotorCommand, WholeBodyAdapter logger = setup_logger() @@ -416,7 +416,7 @@ def write_command(self, commands: dict[str, float], _mode: ControlMode) -> bool: ] return self._wb_adapter.write_motor_commands(motor_cmds) - def write_motor_commands(self, commands: list) -> bool: + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: """Direct pass-through to adapter for full MotorCommand control.""" return self._wb_adapter.write_motor_commands(commands) diff --git a/dimos/hardware/whole_body/registry.py b/dimos/hardware/whole_body/registry.py index 55d2547854..04444630ff 100644 --- a/dimos/hardware/whole_body/registry.py +++ b/dimos/hardware/whole_body/registry.py @@ -26,6 +26,7 @@ from __future__ import annotations +from collections.abc import Callable import importlib import logging import os @@ -41,10 +42,13 @@ class WholeBodyAdapterRegistry: """Registry for whole-body motor adapters with auto-discovery.""" def __init__(self) -> None: - self._adapters: dict[str, type[WholeBodyAdapter]] = {} + # Factory may be a class or any other callable (e.g. functools.partial + # binding transport_cls). Store as Callable so `register("transport_lcm", + # partial(TransportWholeBodyAdapter, ...))` typechecks. + self._adapters: dict[str, Callable[..., WholeBodyAdapter]] = {} - def register(self, name: str, cls: type[WholeBodyAdapter]) -> None: - """Register an adapter class.""" + def register(self, name: str, cls: Callable[..., WholeBodyAdapter]) -> None: + """Register an adapter factory (class or callable).""" self._adapters[name.lower()] = cls def create(self, name: str, **kwargs: Any) -> WholeBodyAdapter: diff --git a/dimos/msgs/sensor_msgs/MotorCommandArray.py b/dimos/msgs/sensor_msgs/MotorCommandArray.py index 9091d80d04..b8a3a6c206 100644 --- a/dimos/msgs/sensor_msgs/MotorCommandArray.py +++ b/dimos/msgs/sensor_msgs/MotorCommandArray.py @@ -95,11 +95,8 @@ def lcm_decode(cls, data: bytes) -> "MotorCommandArray": return cls.decode(data) @classmethod - def decode(cls, data: bytes) -> "MotorCommandArray": - if hasattr(data, "read"): - buf = data # type: ignore[assignment] - else: - buf = BytesIO(data) + def decode(cls, data: bytes | BytesIO) -> "MotorCommandArray": + buf: BytesIO = data if isinstance(data, BytesIO) else BytesIO(data) if buf.read(8) != cls._get_packed_fingerprint(): raise ValueError("Decode error") return cls._decode_one(buf) @@ -117,7 +114,7 @@ def _decode_one(cls, buf: BytesIO) -> "MotorCommandArray": return self @classmethod - def _get_hash_recursive(cls, parents: list) -> int: + def _get_hash_recursive(cls, parents: list[type]) -> int: if cls in parents: return 0 # Distinct fingerprint from JointCommand (0x8A3D2E1C5F4B6A9D) @@ -134,7 +131,7 @@ def _get_packed_fingerprint(cls) -> bytes: return cls._packed_fingerprint def get_hash(self) -> int: - return struct.unpack(">Q", MotorCommandArray._get_packed_fingerprint())[0] + return int(struct.unpack(">Q", MotorCommandArray._get_packed_fingerprint())[0]) def __str__(self) -> str: return f"MotorCommandArray(timestamp={self.timestamp:.6f}, num_joints={self.num_joints})" diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index e4f5285e10..4af69e9a80 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# Copyright 2026 Dimensional Inc. +# 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. @@ -13,17 +13,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -# 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. """Replay a recorded 29-DOF G1 joint trajectory through the coordinator. Publishes JointState to /g1/joint_command. Flow: @@ -147,9 +136,11 @@ def on_state(msg: JointState) -> None: current_q[n] = q state_event.set() - state_sub = LCMTransport("/coordinator/joint_state", JointState) - state_unsub = state_sub.subscribe(on_state) - cmd_pub = LCMTransport("/g1/joint_command", JointState) + state_sub: LCMTransport[JointState] = LCMTransport("/coordinator/joint_state", JointState) + # LCMTransport.subscribe() actually returns an unsub callable but its + # signature claims None — match the pattern in scripts/test_g1_module. + state_unsub = state_sub.subscribe(on_state) # type: ignore[func-returns-value] + cmd_pub: LCMTransport[JointState] = LCMTransport("/g1/joint_command", JointState) try: logger.info("waiting up to 10s for /coordinator/joint_state ...") From 726bdc58ef05a05efa9f093da3182421a97c3da6 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 20:01:58 -0700 Subject: [PATCH 14/28] delete mock scripts --- .../test_g1_module/mock_lowstate_publisher.py | 152 --------- scripts/test_g1_module/test_g1_module.py | 308 ------------------ 2 files changed, 460 deletions(-) delete mode 100644 scripts/test_g1_module/mock_lowstate_publisher.py delete mode 100644 scripts/test_g1_module/test_g1_module.py diff --git a/scripts/test_g1_module/mock_lowstate_publisher.py b/scripts/test_g1_module/mock_lowstate_publisher.py deleted file mode 100644 index 659e46cf89..0000000000 --- a/scripts/test_g1_module/mock_lowstate_publisher.py +++ /dev/null @@ -1,152 +0,0 @@ -# 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. - -"""Mock G1 LowState publisher — provides synthetic rt/lowstate at a target rate. - -Stands in for either real G1 hardware or Unitree's mujoco bridge so the -G1WholeBodyConnection Module + transport seam can be validated end-to-end -without hardware. - -Usage (typically from test_g1_module.py): - pub = MockG1LowStatePublisher(rate_hz=500, mode_machine=42) - pub.start() - ... - pub.stop() - -Note: ChannelFactoryInitialize is a per-process singleton. This publisher -calls it on construction; if the same process needs to do other DDS work -(e.g., subscribe to rt/lowcmd to verify command round-trip), instantiate -this publisher BEFORE creating other DDS participants. -""" - -from __future__ import annotations - -import logging -import threading -from threading import Thread -import time - -logger = logging.getLogger(__name__) - -_NUM_MOTORS = 29 -_NUM_MOTOR_SLOTS = 35 - - -class MockG1LowStatePublisher: - """Publishes synthetic LowState_ messages on rt/lowstate at a target rate. - - Args: - rate_hz: Publish rate (Hz). - mode_machine: Value to put in LowState.mode_machine. MUST be non-zero - so G1WholeBodyConnection.start() completes (it captures the first - non-None mode_machine from rt/lowstate and exits the wait loop). - network_interface: DDS NIC, default 0. - """ - - def __init__( - self, - rate_hz: float = 500.0, - mode_machine: int = 42, - network_interface: str = "", - ) -> None: - self._rate_hz = rate_hz - self._mode_machine = mode_machine - self._network_interface = network_interface - self._stop = threading.Event() - self._thread: Thread | None = None - self._pub = None # type: ignore[var-annotated] - self._sequence = 0 - - def start(self) -> None: - from unitree_sdk2py.core.channel import ( - ChannelFactoryInitialize, - ChannelPublisher, - ) - from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ - - # cyclonedds requires a string NIC; passing an int trips AttributeError on - # participant init. Only pass when explicitly set. Wrap in try/except since - # the call also raises if the factory was already initialized in this process. - try: - if self._network_interface: - ChannelFactoryInitialize(0, self._network_interface) - else: - ChannelFactoryInitialize(0) - except Exception as e: - logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") - - self._pub = ChannelPublisher("rt/lowstate", LowState_) - self._pub.Init() - - self._thread = Thread(target=self._loop, name="mock-g1-lowstate", daemon=True) - self._thread.start() - logger.info(f"MockG1LowStatePublisher started @ {self._rate_hz} Hz") - - def stop(self, timeout: float = 2.0) -> None: - self._stop.set() - if self._thread is not None: - self._thread.join(timeout=timeout) - self._thread = None - logger.info(f"MockG1LowStatePublisher stopped after {self._sequence} messages") - - @property - def messages_published(self) -> int: - return self._sequence - - def _loop(self) -> None: - # Use the default factory (mirrors how the adapter constructs LowCmd_) — - # LowState_() directly would require all 9 positional fields. - from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ - - msg = unitree_hg_msg_dds__LowState_() - msg.mode_machine = self._mode_machine - - # IMU defaults (identity quaternion, zero everything else) - msg.imu_state.quaternion[0] = 1.0 - msg.imu_state.quaternion[1] = 0.0 - msg.imu_state.quaternion[2] = 0.0 - msg.imu_state.quaternion[3] = 0.0 - for j in range(3): - msg.imu_state.gyroscope[j] = 0.0 - msg.imu_state.accelerometer[j] = 0.0 - msg.imu_state.rpy[j] = 0.0 - - period = 1.0 / self._rate_hz - next_tick = time.perf_counter() - - while not self._stop.is_set(): - seq = self._sequence - # Sweep value per motor — lets downstream verify ordering + freshness - for i in range(_NUM_MOTORS): - msg.motor_state[i].q = float(i) + 0.001 * seq - msg.motor_state[i].dq = 0.0 - msg.motor_state[i].tau_est = 0.0 - - try: - self._pub.Write(msg) - self._sequence += 1 - except Exception as e: - logger.error(f"Mock publish failed: {e}") - return - - next_tick += period - sleep_for = next_tick - time.perf_counter() - if sleep_for > 0: - time.sleep(sleep_for) - else: - # We fell behind — reset cadence to avoid runaway catch-up - next_tick = time.perf_counter() - - -__all__ = ["MockG1LowStatePublisher"] diff --git a/scripts/test_g1_module/test_g1_module.py b/scripts/test_g1_module/test_g1_module.py deleted file mode 100644 index aec3b6fbfa..0000000000 --- a/scripts/test_g1_module/test_g1_module.py +++ /dev/null @@ -1,308 +0,0 @@ -#!/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. - -"""Disposable validation harness for the G1 wholebody Module. - -NOT a reusable adapter — Session B will write the real TransportWholeBodyAdapter -against the same topic interface. This script's job is to keep the Module/transport -seam honest with mock DDS traffic. - -Lifecycle: - 1. Spawn MockG1LowStatePublisher in this process (DDS rt/lowstate at 500 Hz). - 2. Spawn ChannelSubscriber("rt/lowcmd", LowCmd_) in this process to count - motor commands round-tripped through the adapter. - 3. Build a blueprint with G1WholeBodyConnection.blueprint(release_sport_mode=False) - and LCM transports on motor_states / imu / motor_command. Run with n_workers=2 - so the Module lands in its own worker process. - 4. From this process, subscribe to the same LCM topics + publish synthetic - MotorCommandArray commands at 100 Hz. - 5. After 60 s, print rate / drop counts and assert thresholds: - - motor_states rate == 500 Hz, drops == 0 - - imu rate == 500 Hz, drops == 0 - - motor_command round-trip count >= 99% of sent - - mode_machine NOT exposed on the wire (true by JointState construction) - -Run inside the nix dev shell with [unitree-dds] extras installed: - python scripts/test_g1_module/test_g1_module.py -""" - -from __future__ import annotations - -import argparse -from collections import deque -import logging -import os -import sys -import threading -import time - -logger = logging.getLogger(__name__) - - -# --------------------------------------------------------------------------- -# RateMonitor — counts arrivals + tracks instantaneous rate over a sliding window -# --------------------------------------------------------------------------- - - -class RateMonitor: - """Counts callback invocations and reports rate over a fixed window.""" - - def __init__(self, name: str, window_s: float = 1.0) -> None: - self.name = name - self.window_s = window_s - self._lock = threading.Lock() - self._times: deque[float] = deque() - self._total: int = 0 - self._first_ts: float | None = None - self._last_ts: float | None = None - - def tick(self) -> None: - now = time.perf_counter() - with self._lock: - self._total += 1 - if self._first_ts is None: - self._first_ts = now - self._last_ts = now - self._times.append(now) - cutoff = now - self.window_s - while self._times and self._times[0] < cutoff: - self._times.popleft() - - @property - def total(self) -> int: - with self._lock: - return self._total - - @property - def instantaneous_hz(self) -> float: - with self._lock: - return len(self._times) / self.window_s - - def average_hz(self) -> float: - with self._lock: - if self._first_ts is None or self._last_ts is None: - return 0.0 - elapsed = self._last_ts - self._first_ts - if elapsed <= 0: - return 0.0 - return self._total / elapsed - - -# --------------------------------------------------------------------------- -# Test harness -# --------------------------------------------------------------------------- - - -def main() -> int: - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--duration", type=float, default=60.0, help="Run time (s)") - parser.add_argument("--mock-rate", type=float, default=500.0, help="Mock LowState Hz") - parser.add_argument("--cmd-rate", type=float, default=100.0, help="Synthetic command Hz") - parser.add_argument( - "--rate-tolerance", - type=float, - default=2.0, - help="Hz tolerance below target to allow before flagging (default 2.0)", - ) - parser.add_argument( - "--cmd-roundtrip-min", type=float, default=0.99, help="Min round-trip ratio" - ) - parser.add_argument("--verbose", "-v", action="store_true") - args = parser.parse_args() - - logging.basicConfig( - level=logging.DEBUG if args.verbose else logging.INFO, - format="%(asctime)s %(name)s %(levelname)s %(message)s", - ) - logger.info(f"test_g1_module pid={os.getpid()} duration={args.duration}s") - - # Heavy imports deferred so --help works even outside the nix env - from unitree_sdk2py.core.channel import ChannelSubscriber - from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ - - from dimos.core.coordination.blueprints import autoconnect - from dimos.core.coordination.module_coordinator import ModuleCoordinator - from dimos.core.global_config import global_config - from dimos.core.transport import LCMTransport - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.JointState import JointState - from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray - from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection - from scripts.test_g1_module.mock_lowstate_publisher import MockG1LowStatePublisher - - # ---------------- Step 1: mock LowState publisher ---------------- - logger.info("Starting mock LowState publisher...") - mock = MockG1LowStatePublisher(rate_hz=args.mock_rate, mode_machine=42) - mock.start() - - # ---------------- Step 2: rt/lowcmd subscriber for round-trip ---------------- - cmd_roundtrip_count = [0] - cmd_roundtrip_lock = threading.Lock() - - def on_lowcmd(_msg: LowCmd_) -> None: - with cmd_roundtrip_lock: - cmd_roundtrip_count[0] += 1 - - cmd_sub = ChannelSubscriber("rt/lowcmd", LowCmd_) - cmd_sub.Init(on_lowcmd, 10) - - # ---------------- Step 3: build blueprint with LCM transports ---------------- - logger.info("Building blueprint (n_workers=2)...") - global_config.update(viewer="none", n_workers=2) - - blueprint = autoconnect( - G1WholeBodyConnection.blueprint(release_sport_mode=False), - ).transports( - { - ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), - ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport( - "/g1/motor_command", MotorCommandArray - ), - } - ) - - coord = ModuleCoordinator.build(blueprint) - - # ---------------- Step 4: external LCM subscribers + command publisher ---------------- - motor_states_monitor = RateMonitor("motor_states") - imu_monitor = RateMonitor("imu") - - # mode_machine leak detection — JointState has no such attr by construction. - mode_machine_leaked = [False] - - def on_motor_states(msg: JointState) -> None: - motor_states_monitor.tick() - if getattr(msg, "mode_machine", None) is not None: - mode_machine_leaked[0] = True - - def on_imu(msg: Imu) -> None: - imu_monitor.tick() - if getattr(msg, "mode_machine", None) is not None: - mode_machine_leaked[0] = True - - motor_states_sub = LCMTransport("/g1/motor_states", JointState) - motor_states_unsub = motor_states_sub.subscribe(on_motor_states) - - imu_sub = LCMTransport("/g1/imu", Imu) - imu_unsub = imu_sub.subscribe(on_imu) - - motor_command_pub = LCMTransport("/g1/motor_command", MotorCommandArray) - - # Wait for the Module to come up — adapter.connect() waits for first LowState - logger.info("Waiting 3s for Module to start in worker...") - time.sleep(3.0) - - # ---------------- Step 5: 60 s sustained run ---------------- - logger.info(f"Running for {args.duration}s — driving cmd at {args.cmd_rate} Hz...") - cmd_sent = 0 - cmd_period = 1.0 / args.cmd_rate - next_cmd = time.perf_counter() - end_at = time.perf_counter() + args.duration - - last_log = time.perf_counter() - while time.perf_counter() < end_at: - cmd = MotorCommandArray( - q=[0.0] * 29, - dq=[0.0] * 29, - kp=[10.0] * 29, - kd=[1.0] * 29, - tau=[0.0] * 29, - ) - motor_command_pub.publish(cmd) - cmd_sent += 1 - - next_cmd += cmd_period - sleep_for = next_cmd - time.perf_counter() - if sleep_for > 0: - time.sleep(sleep_for) - else: - next_cmd = time.perf_counter() - - # 5s heartbeat so the operator knows we're not stuck - now = time.perf_counter() - if now - last_log >= 5.0: - last_log = now - logger.info( - f" motor_states={motor_states_monitor.instantaneous_hz:.1f}Hz " - f"imu={imu_monitor.instantaneous_hz:.1f}Hz " - f"cmd_sent={cmd_sent} cmd_round_trip={cmd_roundtrip_count[0]}" - ) - - # ---------------- Step 6: tear down + assertions ---------------- - logger.info("Stopping...") - motor_states_unsub() - imu_unsub() - motor_states_sub.stop() - imu_sub.stop() - motor_command_pub.stop() - - coord.stop() - cmd_sub.Close() - mock.stop() - - # Drain a tiny bit to let any in-flight messages land - time.sleep(0.5) - - avg_motor_states_hz = motor_states_monitor.average_hz() - avg_imu_hz = imu_monitor.average_hz() - cmd_round_trip_ratio = cmd_roundtrip_count[0] / cmd_sent if cmd_sent else 0.0 - - print() - print("=" * 60) - print("RESULTS") - print("=" * 60) - print(f" duration: {args.duration:.1f}s") - print(f" mock published: {mock.messages_published}") - print( - f" motor_states: total={motor_states_monitor.total} " - f"avg={avg_motor_states_hz:.2f}Hz target={args.mock_rate}Hz" - ) - print( - f" imu: total={imu_monitor.total} " - f"avg={avg_imu_hz:.2f}Hz target={args.mock_rate}Hz" - ) - print( - f" motor_command: sent={cmd_sent} round_trip={cmd_roundtrip_count[0]} " - f"ratio={cmd_round_trip_ratio:.3f} min={args.cmd_roundtrip_min}" - ) - print(f" mode_machine leaked: {mode_machine_leaked[0]}") - print("=" * 60) - - failures: list[str] = [] - rate_floor = args.mock_rate - args.rate_tolerance - if avg_motor_states_hz < rate_floor: - failures.append(f"motor_states {avg_motor_states_hz:.2f}Hz < {rate_floor}Hz floor") - if avg_imu_hz < rate_floor: - failures.append(f"imu {avg_imu_hz:.2f}Hz < {rate_floor}Hz floor") - if cmd_round_trip_ratio < args.cmd_roundtrip_min: - failures.append(f"cmd round-trip {cmd_round_trip_ratio:.3f} < {args.cmd_roundtrip_min}") - if mode_machine_leaked[0]: - failures.append("mode_machine leaked onto output stream") - - if failures: - print() - print("FAIL:") - for f in failures: - print(f" - {f}") - return 1 - - print() - print("PASS") - return 0 - - -if __name__ == "__main__": - sys.exit(main()) From 33de772dfb1b67b242b6da880aa7125a7a943407 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 20:02:49 -0700 Subject: [PATCH 15/28] changed to use dimos logger --- dimos/control/coordinator.py | 9 +++------ dimos/hardware/whole_body/registry.py | 5 +++-- dimos/robot/unitree/g1/wholebody_connection.py | 4 ++-- scripts/g1_replay.py | 5 ++--- 4 files changed, 10 insertions(+), 13 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index b97421347a..a77d42dff1 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -54,6 +54,7 @@ TwistBaseAdapter, ) from dimos.hardware.manipulators.spec import ManipulatorAdapter +from dimos.hardware.whole_body.spec import WholeBodyAdapter from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.JointState import JointState @@ -65,8 +66,6 @@ if TYPE_CHECKING: from collections.abc import Callable - from dimos.hardware.whole_body.spec import WholeBodyAdapter - logger = setup_logger() @@ -260,7 +259,7 @@ def _create_twist_base_adapter(self, component: HardwareComponent) -> TwistBaseA **component.adapter_kwargs, ) - def _create_whole_body_adapter(self, component: HardwareComponent) -> "WholeBodyAdapter": + def _create_whole_body_adapter(self, component: HardwareComponent) -> WholeBodyAdapter: """Create a whole-body adapter from component config. ``component.address`` carries the DDS network interface — int (CAN port) @@ -365,12 +364,10 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: @rpc def add_hardware( self, - adapter: "ManipulatorAdapter | TwistBaseAdapter | WholeBodyAdapter", + adapter: ManipulatorAdapter | TwistBaseAdapter | WholeBodyAdapter, component: HardwareComponent, ) -> bool: """Register a hardware adapter with the coordinator.""" - from dimos.hardware.whole_body.spec import WholeBodyAdapter - is_base = component.hardware_type == HardwareType.BASE is_whole_body = component.hardware_type == HardwareType.WHOLE_BODY diff --git a/dimos/hardware/whole_body/registry.py b/dimos/hardware/whole_body/registry.py index 04444630ff..fb8a96e56f 100644 --- a/dimos/hardware/whole_body/registry.py +++ b/dimos/hardware/whole_body/registry.py @@ -28,14 +28,15 @@ from collections.abc import Callable import importlib -import logging import os from typing import TYPE_CHECKING, Any +from dimos.utils.logging_config import setup_logger + if TYPE_CHECKING: from dimos.hardware.whole_body.spec import WholeBodyAdapter -logger = logging.getLogger(__name__) +logger = setup_logger() class WholeBodyAdapterRegistry: diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 1020858e75..4aaa5c2ddb 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -42,7 +42,6 @@ from __future__ import annotations -import logging import threading from threading import Thread import time @@ -62,8 +61,9 @@ from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.utils.logging_config import setup_logger -logger = logging.getLogger(__name__) +logger = setup_logger() _NUM_MOTORS = 29 _NUM_MOTOR_SLOTS = 35 # G1 hg LowCmd has 35 slots; only 29 are used diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index 4af69e9a80..75a7602ea1 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -54,7 +54,6 @@ import argparse import json -import logging from pathlib import Path import threading import time @@ -62,13 +61,13 @@ from dimos.control.components import make_humanoid_joints from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.utils.logging_config import setup_logger NUM_DOF = 29 CANONICAL_JOINTS = make_humanoid_joints("g1") assert len(CANONICAL_JOINTS) == NUM_DOF -logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s") -logger = logging.getLogger("g1_replay") +logger = setup_logger() def load_trajectory(path: Path) -> tuple[list[float], list[list[float]]]: From dddcdbc2dfea8714cd84fc4624b3b012cae35cf1 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 20:06:46 -0700 Subject: [PATCH 16/28] replay joint commands for the whole body controller testing --- data/.lfs/g1_wholebody_replay.json.tar.gz | 3 +++ scripts/g1_replay.py | 13 +++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) create mode 100644 data/.lfs/g1_wholebody_replay.json.tar.gz diff --git a/data/.lfs/g1_wholebody_replay.json.tar.gz b/data/.lfs/g1_wholebody_replay.json.tar.gz new file mode 100644 index 0000000000..1ad3ea8da3 --- /dev/null +++ b/data/.lfs/g1_wholebody_replay.json.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8bffca753401f78587ba743d19fc3493aac7ffdb9a42c3863299082e62da2788 +size 1147342 diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index 75a7602ea1..f3e147cf9f 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -45,9 +45,10 @@ # Terminal 1: ROBOT_INTERFACE=enp194s0 dimos run unitree-g1-coordinator # Terminal 2: - python scripts/g1_replay.py --file /tmp/g1_traj.json - python scripts/g1_replay.py --file /tmp/g1_traj.json --loop - python scripts/g1_replay.py --file /tmp/g1_traj.json --dry-run + python scripts/g1_replay.py # default LFS bundle + python scripts/g1_replay.py --loop + python scripts/g1_replay.py --dry-run + python scripts/g1_replay.py --file # any 29-DOF JSON """ from __future__ import annotations @@ -98,7 +99,11 @@ def make_joint_state(positions: list[float]) -> JointState: def main() -> None: p = argparse.ArgumentParser(description=__doc__) - p.add_argument("--file", default="/tmp/g1_traj.json", help="trajectory JSON path") + p.add_argument( + "--file", + default="data/g1_wholebody_replay.json", + help="trajectory JSON path (LFS-bundled default unpacks from data/.lfs/g1_wholebody_replay.json.tar.gz)", + ) p.add_argument( "--ramp", type=float, From 63a0aef25099fa95c23ee6ee73a20e2af264fb78 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Thu, 30 Apr 2026 20:08:25 -0700 Subject: [PATCH 17/28] g1 replay now pulls trawjectory from lfs --- scripts/g1_replay.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index f3e147cf9f..1c2acc40d4 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -62,8 +62,13 @@ from dimos.control.components import make_humanoid_joints from dimos.core.transport import LCMTransport from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.utils.data import get_data from dimos.utils.logging_config import setup_logger +# LFS-bundled default trajectory. get_data() auto-pulls from +# data/.lfs/g1_wholebody_replay.json.tar.gz and decompresses on first use. +_DEFAULT_TRAJECTORY_NAME = "g1_wholebody_replay.json" + NUM_DOF = 29 CANONICAL_JOINTS = make_humanoid_joints("g1") assert len(CANONICAL_JOINTS) == NUM_DOF @@ -101,8 +106,8 @@ def main() -> None: p = argparse.ArgumentParser(description=__doc__) p.add_argument( "--file", - default="data/g1_wholebody_replay.json", - help="trajectory JSON path (LFS-bundled default unpacks from data/.lfs/g1_wholebody_replay.json.tar.gz)", + default=None, + help=f"trajectory JSON path (defaults to LFS-bundled {_DEFAULT_TRAJECTORY_NAME})", ) p.add_argument( "--ramp", @@ -116,7 +121,8 @@ def main() -> None: ) args = p.parse_args() - rel_ts, positions = load_trajectory(Path(args.file)) + traj_path = Path(args.file) if args.file else get_data(_DEFAULT_TRAJECTORY_NAME) + rel_ts, positions = load_trajectory(traj_path) native_rate = len(rel_ts) / rel_ts[-1] if rel_ts[-1] > 0 else 0.0 logger.info( f"loaded {len(rel_ts)} samples, duration={rel_ts[-1]:.2f}s, native_rate={native_rate:.1f}Hz" From 39ab10451915b447584fc3b2d2dcc63b0e0cb287 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 10:01:50 -0700 Subject: [PATCH 18/28] skipo zero division when ramp is set to 0 --- scripts/g1_replay.py | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index 1c2acc40d4..ac2ebfaf21 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -172,18 +172,27 @@ def on_state(msg: JointState) -> None: return # ---------------- Ramp ---------------- - logger.info(f"ramping current → trajectory[0] over {args.ramp:.2f}s") - ramp_period = 1.0 / 100.0 # 100 Hz during ramp - ramp_start = time.perf_counter() + # ramp <= 0 means "skip ramp" — publish trajectory[0] once and proceed. + # Snaps to the recorded start pose under PD; only safe when the robot + # already sits there (e.g. dry-run had us close, or replay loop resume). target_q = positions[0] - while True: - elapsed = time.perf_counter() - ramp_start - a = min(elapsed / args.ramp, 1.0) - interp = [start_q[i] + a * (target_q[i] - start_q[i]) for i in range(NUM_DOF)] - cmd_pub.publish(make_joint_state(interp)) - if a >= 1.0: - break - time.sleep(ramp_period) + if args.ramp <= 0: + logger.warning( + f"--ramp={args.ramp} ≤ 0; snapping directly to trajectory[0] (no interpolation)" + ) + cmd_pub.publish(make_joint_state(target_q)) + else: + logger.info(f"ramping current → trajectory[0] over {args.ramp:.2f}s") + ramp_period = 1.0 / 100.0 # 100 Hz during ramp + ramp_start = time.perf_counter() + while True: + elapsed = time.perf_counter() - ramp_start + a = min(elapsed / args.ramp, 1.0) + interp = [start_q[i] + a * (target_q[i] - start_q[i]) for i in range(NUM_DOF)] + cmd_pub.publish(make_joint_state(interp)) + if a >= 1.0: + break + time.sleep(ramp_period) # ---------------- Replay ---------------- passes = 0 From e8bea3f4ee10f97ef0bcb98d7c905e752d1deb79 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 10:23:10 -0700 Subject: [PATCH 19/28] states now publish only for smallest message type to prevent crash in case of malformed publisher --- dimos/hardware/whole_body/transport/adapter.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 126ad6d8b6..814ef0fa62 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -172,9 +172,8 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: def _on_motor_states(self, msg: JointState) -> None: # JointState.position / .velocity / .effort -> MotorState.q / .dq / .tau. - # Ignore msg.name — order is implicit (set by the producing Module's - # G1_JOINT_NAMES which mirrors make_humanoid_joints("g1")). - n = min(len(msg.position), self._dof) + + n = min(len(msg.position), len(msg.velocity), len(msg.effort), self._dof) states = [ MotorState(q=msg.position[i], dq=msg.velocity[i], tau=msg.effort[i]) for i in range(n) ] From bfbd2866ac121ee9e5a667ce51b3568c14a93fe2 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 11:06:52 -0700 Subject: [PATCH 20/28] removed section headers --- dimos/hardware/whole_body/spec.py | 6 ----- .../hardware/whole_body/transport/adapter.py | 19 ++------------ .../robot/unitree/g1/wholebody_connection.py | 26 ++----------------- scripts/g1_replay.py | 2 -- 4 files changed, 4 insertions(+), 49 deletions(-) diff --git a/dimos/hardware/whole_body/spec.py b/dimos/hardware/whole_body/spec.py index 8a5bd0f697..9800b45c4c 100644 --- a/dimos/hardware/whole_body/spec.py +++ b/dimos/hardware/whole_body/spec.py @@ -72,8 +72,6 @@ class WholeBodyAdapter(Protocol): - Force: N """ - # --- Connection --- - def connect(self) -> bool: """Connect to hardware. Returns True on success.""" ... @@ -86,8 +84,6 @@ def is_connected(self) -> bool: """Check if connected.""" ... - # --- State Reading --- - def read_motor_states(self) -> list[MotorState]: """Read motor states for all joints.""" ... @@ -96,8 +92,6 @@ def read_imu(self) -> IMUState: """Read IMU state.""" ... - # --- Control --- - def write_motor_commands(self, commands: list[MotorCommand]) -> bool: """Write motor commands for all joints. Returns success.""" ... diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 814ef0fa62..4929236455 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -72,10 +72,6 @@ def __init__( self._imu_unsub: Any = None self._connected = False - # ------------------------------------------------------------------ - # Connection - # ------------------------------------------------------------------ - def connect(self) -> bool: ms_topic = f"/{self._prefix}/motor_states" imu_topic = f"/{self._prefix}/imu" @@ -124,10 +120,6 @@ def disconnect(self) -> None: def is_connected(self) -> bool: return self._connected - # ------------------------------------------------------------------ - # State reading (latest-frame cached, non-blocking) - # ------------------------------------------------------------------ - def read_motor_states(self) -> list[MotorState]: """Return latest cached motor states. Returns defaults if no frame yet.""" with self._lock: @@ -142,10 +134,6 @@ def read_imu(self) -> IMUState: return IMUState() return self._latest_imu - # ------------------------------------------------------------------ - # Control - # ------------------------------------------------------------------ - def write_motor_commands(self, commands: list[MotorCommand]) -> bool: """Publish a MotorCommandArray to /{hardware_id}/motor_command. @@ -166,13 +154,10 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: self._motor_command_transport.publish(msg) return True - # ------------------------------------------------------------------ - # Subscriber callbacks (LCM dispatcher thread) - # ------------------------------------------------------------------ - def _on_motor_states(self, msg: JointState) -> None: # JointState.position / .velocity / .effort -> MotorState.q / .dq / .tau. - + # Bound by the shortest array so a malformed publisher (e.g. empty + # velocity) can't IndexError-kill the LCM dispatcher thread. n = min(len(msg.position), len(msg.velocity), len(msg.effort), self._dof) states = [ MotorState(q=msg.position[i], dq=msg.velocity[i], tau=msg.effort[i]) for i in range(n) diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 4aaa5c2ddb..a64cbe91c7 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -12,11 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""G1 wholebody Module (Arch B). +"""G1 wholebody Module. -Owns the G1 low-level DDS connection directly — no separate adapter file. -Sits in its own worker; the coordinator never imports unitree_sdk2py and -talks to this Module exclusively through LCM streams. +Owns the G1 low-level DDS connection directly. Stream interface: - motor_states: Out[JointState] 29 motors, q/dq/tau in position/velocity/effort @@ -112,10 +110,6 @@ def __init__(self, **kwargs: Any) -> None: self._stop_event = threading.Event() self._publish_thread: Thread | None = None - # ========================================================================= - # Lifecycle - # ========================================================================= - @rpc def start(self) -> None: super().start() @@ -228,10 +222,6 @@ def stop(self) -> None: logger.info("G1WholeBodyConnection disconnected") super().stop() - # ========================================================================= - # Publish loop (state out) - # ========================================================================= - def _publish_loop(self) -> None: period = 1.0 / float(self.config.publish_rate_hz) next_tick = time.perf_counter() @@ -290,10 +280,6 @@ def _publish_loop(self) -> None: else: next_tick = time.perf_counter() - # ========================================================================= - # Motor command in (MotorCommandArray → LowCmd → DDS rt/lowcmd) - # ========================================================================= - def _on_motor_command(self, msg: MotorCommandArray) -> None: if msg.num_joints != _NUM_MOTORS: logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") @@ -322,10 +308,6 @@ def _on_motor_command(self, msg: MotorCommandArray) -> None: self._low_cmd.crc = self._crc.Crc(self._low_cmd) self._publisher.Write(self._low_cmd) - # ========================================================================= - # DDS subscriber callback (LowState in) - # ========================================================================= - def _on_low_state(self, msg: Any) -> None: """rt/lowstate callback — captures mode_machine and the latest snapshot.""" with self._lock: @@ -333,10 +315,6 @@ def _on_low_state(self, msg: Any) -> None: if self._mode_machine is None: self._mode_machine = msg.mode_machine - # ========================================================================= - # Sport-mode release (real hardware only — gated by release_sport_mode) - # ========================================================================= - def _release_sport_mode(self) -> None: """Loop ReleaseMode until MotionSwitcher reports no active controller.""" from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index ac2ebfaf21..09e23fbe69 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -171,7 +171,6 @@ def on_state(msg: JointState) -> None: logger.info("--dry-run set — exiting before publish phase") return - # ---------------- Ramp ---------------- # ramp <= 0 means "skip ramp" — publish trajectory[0] once and proceed. # Snaps to the recorded start pose under PD; only safe when the robot # already sits there (e.g. dry-run had us close, or replay loop resume). @@ -194,7 +193,6 @@ def on_state(msg: JointState) -> None: break time.sleep(ramp_period) - # ---------------- Replay ---------------- passes = 0 while True: passes += 1 From 26e5df25228ea8a1db99539a8e4e5f68a7f40425 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 11:22:35 -0700 Subject: [PATCH 21/28] command current position to prevent robot jerking when starting --- dimos/control/hardware_interface.py | 20 ++++++++++++++----- .../hardware/whole_body/transport/adapter.py | 5 +++++ 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 91d320645f..a4318fce43 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -421,11 +421,21 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: return self._wb_adapter.write_motor_commands(commands) def _initialize_last_commanded(self) -> None: - """Initialize last_commanded with current motor positions.""" - motor_states = self._wb_adapter.read_motor_states() - for i, name in enumerate(self._joint_names): - self._last_commanded[name] = motor_states[i].q - self._initialized = True + """Initialize last_commanded from current motor positions.""" + is_ready = getattr(self._wb_adapter, "has_motor_states", lambda: True) + for _ in range(10): + if is_ready(): + motor_states = self._wb_adapter.read_motor_states() + for i, name in enumerate(self._joint_names): + self._last_commanded[name] = motor_states[i].q + self._initialized = True + return + time.sleep(0.1) + + raise RuntimeError( + f"WholeBody {self._component.hardware_id} did not receive motor_states " + f"within 1s — refusing to initialize last_commanded with adapter defaults" + ) __all__ = [ diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 4929236455..ffdad57b2b 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -127,6 +127,11 @@ def read_motor_states(self) -> list[MotorState]: return [MotorState() for _ in range(self._dof)] return list(self._latest_motor_states) + def has_motor_states(self) -> bool: + """True once the first motor_states frame has been received.""" + with self._lock: + return self._latest_motor_states is not None + def read_imu(self) -> IMUState: """Return latest cached IMU state. Returns defaults if no frame yet.""" with self._lock: From cc864321b93460921780ede83b2eacc0b6fb3486 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 11:51:51 -0700 Subject: [PATCH 22/28] drop state messages for buggy frame and use previous good frame --- dimos/hardware/whole_body/transport/adapter.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index ffdad57b2b..94479f7831 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -161,11 +161,20 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: def _on_motor_states(self, msg: JointState) -> None: # JointState.position / .velocity / .effort -> MotorState.q / .dq / .tau. - # Bound by the shortest array so a malformed publisher (e.g. empty - # velocity) can't IndexError-kill the LCM dispatcher thread. - n = min(len(msg.position), len(msg.velocity), len(msg.effort), self._dof) + # Drop the frame if any array is shorter than _dof — keeps the last + # valid cached frame and guarantees _latest_motor_states is either + # None (no frame yet) or exactly _dof entries. Downstream callers + # iterate range(_dof) and index by joint, so a short list would + # IndexError; padding with zeros would silently substitute fake data. + if ( + len(msg.position) < self._dof + or len(msg.velocity) < self._dof + or len(msg.effort) < self._dof + ): + return states = [ - MotorState(q=msg.position[i], dq=msg.velocity[i], tau=msg.effort[i]) for i in range(n) + MotorState(q=msg.position[i], dq=msg.velocity[i], tau=msg.effort[i]) + for i in range(self._dof) ] with self._lock: self._latest_motor_states = states From 6679a85a42825a8b49015196fc818d767d4e2ebc Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 13:30:30 -0700 Subject: [PATCH 23/28] forward dof for the wholebody adapter object creation --- dimos/control/coordinator.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index a77d42dff1..13c959cd89 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -277,6 +277,7 @@ def _create_whole_body_adapter(self, component: HardwareComponent) -> WholeBodyA return whole_body_adapter_registry.create( component.adapter_type, + dof=len(component.joints), hardware_id=component.hardware_id, network_interface=addr if addr is not None else "", **component.adapter_kwargs, From 4cb34e934955577852a058d961d3270e735019ef Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 19:23:47 -0700 Subject: [PATCH 24/28] replaced Any annotations on the DDS/SDK fields --- dimos/robot/unitree/g1/wholebody_connection.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index a64cbe91c7..1ff20273b7 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -43,11 +43,16 @@ import threading from threading import Thread import time -from typing import Any +from typing import TYPE_CHECKING, Any from pydantic import Field from reactivex.disposable import Disposable +if TYPE_CHECKING: + from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ + from unitree_sdk2py.utils.crc import CRC + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.control.components import make_humanoid_joints from dimos.core.core import rpc @@ -94,11 +99,11 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) # DDS / SDK objects — populated in start(), torn down in stop(). - self._publisher: Any = None - self._subscriber: Any = None - self._low_cmd: Any = None - self._low_state: Any = None - self._crc: Any = None + self._publisher: ChannelPublisher | None = None + self._subscriber: ChannelSubscriber | None = None + self._low_cmd: LowCmd_ | None = None + self._low_state: LowState_ | None = None + self._crc: CRC | None = None # mode_machine must be read from first LowState and echoed back in every LowCmd. self._mode_machine: int | None = None From 47b25db614226046beae0765a0d7b179d5c51283 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 19:30:47 -0700 Subject: [PATCH 25/28] reduced verbose docstring --- dimos/hardware/whole_body/registry.py | 12 +--- dimos/hardware/whole_body/spec.py | 51 +++----------- .../hardware/whole_body/transport/adapter.py | 34 ++-------- dimos/msgs/sensor_msgs/MotorCommandArray.py | 14 +--- .../basic/unitree_g1_coordinator.py | 25 ++----- .../robot/unitree/g1/wholebody_connection.py | 68 ++++--------------- scripts/g1_replay.py | 50 ++------------ 7 files changed, 45 insertions(+), 209 deletions(-) diff --git a/dimos/hardware/whole_body/registry.py b/dimos/hardware/whole_body/registry.py index fb8a96e56f..ddab15b0fc 100644 --- a/dimos/hardware/whole_body/registry.py +++ b/dimos/hardware/whole_body/registry.py @@ -64,17 +64,7 @@ def available(self) -> list[str]: return sorted(self._adapters.keys()) def discover(self) -> None: - """Discover and register adapters from subpackages. - - Mirrors ``dimos.hardware.drive_trains.registry``'s walk pattern: raw - filesystem listing + ``os.path.isdir`` check. We do NOT use - ``pkgutil.iter_modules`` because dimos uses PEP 420 namespace packages - (no ``__init__.py``) and ``iter_modules`` doesn't reliably enumerate - subpackage directories under namespace packages. - - Each direct subpackage of ``dimos.hardware.whole_body`` is expected - to expose ``adapter.py`` with a ``register(registry)`` function. - """ + """Discover and register adapters from subpackages.""" import dimos.hardware.whole_body as pkg pkg_dir = pkg.__path__[0] diff --git a/dimos/hardware/whole_body/spec.py b/dimos/hardware/whole_body/spec.py index 9800b45c4c..5bb524ba8a 100644 --- a/dimos/hardware/whole_body/spec.py +++ b/dimos/hardware/whole_body/spec.py @@ -12,21 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""WholeBodyAdapter protocol for joint-level motor control. - -Lightweight protocol for robots that expose per-motor -position/velocity/torque control (as opposed to TwistBaseAdapter which -only exposes velocity commands). - -Supports any number of motors — quadrupeds (12 DOF), humanoids (29 DOF), etc. -""" +"""WholeBodyAdapter protocol for joint-level (q/dq/kp/kd/tau) motor control.""" from __future__ import annotations from dataclasses import dataclass from typing import Protocol, runtime_checkable -# Sentinel values from Unitree SDK — used to signal "no command" for a DOF. +# Unitree SDK sentinels meaning "no command" for that DOF. POS_STOP: float = 2.146e9 VEL_STOP: float = 16000.0 @@ -63,38 +56,14 @@ class IMUState: @runtime_checkable class WholeBodyAdapter(Protocol): - """Protocol for joint-level whole-body motor IO. - - Implement this per vendor SDK. All methods use SI units: - - Position: radians - - Velocity: rad/s - - Torque: Nm - - Force: N - """ - - def connect(self) -> bool: - """Connect to hardware. Returns True on success.""" - ... - - def disconnect(self) -> None: - """Disconnect from hardware.""" - ... - - def is_connected(self) -> bool: - """Check if connected.""" - ... - - def read_motor_states(self) -> list[MotorState]: - """Read motor states for all joints.""" - ... - - def read_imu(self) -> IMUState: - """Read IMU state.""" - ... - - def write_motor_commands(self, commands: list[MotorCommand]) -> bool: - """Write motor commands for all joints. Returns success.""" - ... + """Joint-level whole-body motor IO. SI units (rad, rad/s, Nm).""" + + def connect(self) -> bool: ... + def disconnect(self) -> None: ... + def is_connected(self) -> bool: ... + def read_motor_states(self) -> list[MotorState]: ... + def read_imu(self) -> IMUState: ... + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: ... __all__ = [ diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 94479f7831..8950803bb3 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -12,19 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Transport-based whole-body adapter — connects coordinator to a Module via pub/sub. +"""Transport-based whole-body adapter: bridges coordinator ↔ Module via pub/sub. -One generic adapter for every humanoid/quadruped that publishes the same -JointState/Imu/MotorCommandArray topic interface. Per-robot logic stays in -the Module, not here. - -Topics derived from ``hardware_id``: - - subscribes /{hardware_id}/motor_states (JointState) - - subscribes /{hardware_id}/imu (Imu) - - publishes /{hardware_id}/motor_command (MotorCommandArray) - -The ``network_interface`` kwarg is accepted (the coordinator forwards -``component.address``) and ignored — LCM transport doesn't use a NIC. +Subscribes /{hardware_id}/motor_states + /{hardware_id}/imu, publishes +/{hardware_id}/motor_command. ``network_interface`` is accepted but ignored. """ from __future__ import annotations @@ -121,7 +112,6 @@ def is_connected(self) -> bool: return self._connected def read_motor_states(self) -> list[MotorState]: - """Return latest cached motor states. Returns defaults if no frame yet.""" with self._lock: if self._latest_motor_states is None: return [MotorState() for _ in range(self._dof)] @@ -133,18 +123,12 @@ def has_motor_states(self) -> bool: return self._latest_motor_states is not None def read_imu(self) -> IMUState: - """Return latest cached IMU state. Returns defaults if no frame yet.""" with self._lock: if self._latest_imu is None: return IMUState() return self._latest_imu def write_motor_commands(self, commands: list[MotorCommand]) -> bool: - """Publish a MotorCommandArray to /{hardware_id}/motor_command. - - Per-joint q/dq/kp/kd/tau survive the wire — that's the whole point - of being at the low-level layer. - """ if self._motor_command_transport is None: logger.warning("write_motor_commands called before connect()") return False @@ -160,12 +144,7 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: return True def _on_motor_states(self, msg: JointState) -> None: - # JointState.position / .velocity / .effort -> MotorState.q / .dq / .tau. - # Drop the frame if any array is shorter than _dof — keeps the last - # valid cached frame and guarantees _latest_motor_states is either - # None (no frame yet) or exactly _dof entries. Downstream callers - # iterate range(_dof) and index by joint, so a short list would - # IndexError; padding with zeros would silently substitute fake data. + # Drop short frames; downstream code indexes range(_dof) directly. if ( len(msg.position) < self._dof or len(msg.velocity) < self._dof @@ -180,8 +159,7 @@ def _on_motor_states(self, msg: JointState) -> None: self._latest_motor_states = states def _on_imu(self, msg: Imu) -> None: - # dimos Imu.orientation Quaternion is (x, y, z, w); - # IMUState.quaternion is (w, x, y, z) — translate. + # dimos Imu Quaternion is (x,y,z,w); IMUState.quaternion is (w,x,y,z). with self._lock: self._latest_imu = IMUState( quaternion=( @@ -200,7 +178,7 @@ def _on_imu(self, msg: Imu) -> None: msg.linear_acceleration.y, msg.linear_acceleration.z, ), - rpy=(0.0, 0.0, 0.0), # dimos Imu doesn't carry rpy; downstream typically ignores + rpy=(0.0, 0.0, 0.0), ) diff --git a/dimos/msgs/sensor_msgs/MotorCommandArray.py b/dimos/msgs/sensor_msgs/MotorCommandArray.py index b8a3a6c206..577b03c765 100644 --- a/dimos/msgs/sensor_msgs/MotorCommandArray.py +++ b/dimos/msgs/sensor_msgs/MotorCommandArray.py @@ -12,12 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""LCM type for low-level motor command arrays. - -Hand-rolled struct.pack serialization, modeled on JointCommand.py. -Carries the full per-motor PD+feedforward command tuple needed for -low-level joint control: q, dq, kp, kd, tau. -""" +"""LCM type for low-level motor commands: q, dq, kp, kd, tau per motor.""" from io import BytesIO import struct @@ -25,12 +20,7 @@ class MotorCommandArray: - """Low-level motor command array. - - Variable-length per-motor commands with target position, target velocity, - PD gains (kp, kd), and feedforward torque. Joint order is implicit — - callers must agree on a fixed ordering (e.g., G1's 29-motor sequence). - """ + """Per-motor q/dq/kp/kd/tau. Joint order is implicit (caller-defined).""" msg_name = "sensor_msgs.MotorCommandArray" diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py index 465e1b9152..55991fa25d 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -13,19 +13,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Unitree G1 ControlCoordinator — G1WholeBodyConnection + coordinator via LCM transport adapter. +"""G1 ControlCoordinator: G1WholeBodyConnection Module + servo task via LCM bridge. -Pattern mirrors `unitree_go2_coordinator.py`. The Module owns the DDS connection -in its own worker; the coordinator constructs a `TransportWholeBodyAdapter` -(registered as ``transport_lcm``) that pub/subs the same LCM topics the Module -publishes to / subscribes from. - -A single ``servo_g1`` task converts incoming JointState commands on -``/g1/joint_command`` into per-joint position commands routed to the -WHOLE_BODY hardware (PD gains baked into ``ConnectedWholeBody``). - -Usage: - ROBOT_INTERFACE=enp60s0 dimos run unitree-g1-coordinator +Mirrors `unitree_go2_coordinator.py`. Run with `ROBOT_INTERFACE= dimos run unitree-g1-coordinator`. """ from __future__ import annotations @@ -43,8 +33,7 @@ _g1_joints = make_humanoid_joints("g1") -# ROBOT_INTERFACE pins cyclonedds to a specific NIC — required on multi-NIC hosts -# where the default pick stalls discovery (no LowState ever arrives). +# ROBOT_INTERFACE pins cyclonedds to a NIC; required on multi-NIC hosts. unitree_g1_coordinator = ( autoconnect( G1WholeBodyConnection.blueprint( @@ -71,12 +60,8 @@ ], ), ) - # No remappings needed: G1WholeBodyConnection's stream names (motor_states, - # imu, motor_command) don't collide with any ControlCoordinator stream - # names (joint_state, joint_command, cartesian_command, twist_command, - # buttons). Compare unitree_go2_coordinator, which DOES need to remap - # GO2Connection.cmd_vel because it collides with the coordinator's - # twist_command path bound to /cmd_vel. + # No remappings: Module stream names (motor_states/imu/motor_command) don't + # collide with ControlCoordinator's (joint_state/joint_command/...). .transports( { ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 1ff20273b7..f4fb762bae 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -12,30 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""G1 wholebody Module. - -Owns the G1 low-level DDS connection directly. - -Stream interface: - - motor_states: Out[JointState] 29 motors, q/dq/tau in position/velocity/effort - - imu: Out[Imu] quaternion + gyro + accel - - motor_command: In[MotorCommandArray] 29-DOF q/dq/kp/kd/tau - -Hardware protocol: - - DDS topics: rt/lowstate (subscribe) + rt/lowcmd (publish) - - IDL: unitree_hg (G1 specific; Go2 uses unitree_go) - - mode_machine: read from first LowState, echoed back in every LowCmd - - CRC: computed on every LowCmd via unitree_sdk2py.utils.crc.CRC - - Sport-mode release: gated by release_sport_mode config (set False for sim/mock) - -mode_machine never appears on the published JointState — it stays internal. - -Motor ordering (29 joints): - 0-5: left leg (hip pitch/roll/yaw, knee, ankle pitch/roll) - 6-11: right leg - 12-14: waist (yaw, roll, pitch — roll/pitch may be invalid on some variants) - 15-21: left arm (shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw) - 22-28: right arm +"""G1 low-level DDS Module: rt/lowstate (sub) + rt/lowcmd (pub), unitree_hg IDL. + +Streams: motor_states (Out[JointState]), imu (Out[Imu]), +motor_command (In[MotorCommandArray]). 29 motors, ordering from +make_humanoid_joints("g1") (left leg → right leg → waist → left arm → right arm). """ from __future__ import annotations @@ -98,20 +79,15 @@ class G1WholeBodyConnection(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - # DDS / SDK objects — populated in start(), torn down in stop(). self._publisher: ChannelPublisher | None = None self._subscriber: ChannelSubscriber | None = None self._low_cmd: LowCmd_ | None = None self._low_state: LowState_ | None = None self._crc: CRC | None = None - - # mode_machine must be read from first LowState and echoed back in every LowCmd. + # mode_machine: read from first LowState, echoed back in every LowCmd. self._mode_machine: int | None = None - - # Guards _low_cmd, _low_state, _mode_machine across the DDS callback thread, - # the publish loop thread, and the motor_command (LCM) callback thread. + # Guards _low_cmd / _low_state / _mode_machine across DDS, publish, and LCM threads. self._lock = threading.Lock() - self._stop_event = threading.Event() self._publish_thread: Thread | None = None @@ -119,14 +95,7 @@ def __init__(self, **kwargs: Any) -> None: def start(self) -> None: super().start() - # Lazy SDK imports so the Module module file imports cleanly outside the - # nix env / [unitree-dds] extra. Connect path: - # 1. ChannelFactoryInitialize(0[, nic]) - # 2. publisher rt/lowcmd, subscriber rt/lowstate - # 3. LowCmd buffer initialised with safe defaults (POS_STOP, kp=0) - # 4. CRC computer - # 5. (optional) MotionSwitcher release sport mode - # 6. wait up to _MODE_MACHINE_WAIT_S for first LowState + # Lazy SDK imports — file must import cleanly outside the [unitree-dds] extra. from unitree_sdk2py.core.channel import ( ChannelFactoryInitialize, ChannelPublisher, @@ -144,8 +113,7 @@ def start(self) -> None: else: ChannelFactoryInitialize(0) except Exception as e: - # Idempotent: if the factory was already initialised in this process - # (e.g. by a sibling DDS publisher in the same worker), continue. + # Idempotent — already initialised by a sibling participant is fine. logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) @@ -154,8 +122,7 @@ def start(self) -> None: self._subscriber = ChannelSubscriber("rt/lowstate", LowState_) self._subscriber.Init(self._on_low_state, 10) - # LowCmd safe defaults — POS_STOP/VEL_STOP sentinels with zero gains so a - # robot doesn't twitch on the first publish if commands haven't started. + # POS_STOP/VEL_STOP + zero gains so the robot can't twitch pre-command. self._low_cmd = unitree_hg_msg_dds__LowCmd_() self._low_cmd.mode_pr = 0 # PR (pitch/roll) mode for i in range(_NUM_MOTOR_SLOTS): @@ -200,12 +167,8 @@ def stop(self) -> None: self._publish_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) self._publish_thread = None - # Close DDS endpoints explicitly before dropping refs — relying on GC - # leaves the cyclonedds participant in a state where in-flight callbacks - # race with cleanup and segfault on process exit. Verified against - # unitree_sdk2py/core/channel.py (Close exists at lines 267 + 288); - # mirrors the Go2 SDK adapter's pattern in - # dimos/hardware/drive_trains/unitree_go2/adapter.py. + # Close DDS endpoints explicitly — GC-based cleanup races with in-flight + # callbacks and segfaults on process exit (mirrors the Go2 adapter). if self._subscriber is not None: try: self._subscriber.Close() @@ -267,7 +230,7 @@ def _publish_loop(self) -> None: ) ) - # Unitree IMU quaternion is (w, x, y, z); dimos Quaternion is (x, y, z, w). + # Unitree quat is (w,x,y,z); dimos Quaternion is (x,y,z,w). self.imu.publish( Imu( ts=now, @@ -330,10 +293,7 @@ def _release_sport_mode(self) -> None: msc.SetTimeout(5.0) msc.Init() - # MotionSwitcher.CheckMode() returns (status, dict) while a sport - # mode is active and (status, None) once nothing is active. Use a - # null-tolerant check so the loop exits cleanly after the release - # rather than crashing on `None["name"]`. + # CheckMode returns (status, None) once nothing is active — null-tolerant. _status, result = msc.CheckMode() while result and result.get("name"): msc.ReleaseMode() diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index 09e23fbe69..03891c938f 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -13,42 +13,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Replay a recorded 29-DOF G1 joint trajectory through the coordinator. - -Publishes JointState to /g1/joint_command. Flow: - - g1_replay.py - | JointState (29 names + 29 positions) - v /g1/joint_command (LCM) - ControlCoordinator (servo_g1 task, tick_rate=500) - | per-joint position commands → MotorCommand with built-in PD gains - v TransportWholeBodyAdapter - | MotorCommandArray - v /g1/motor_command (LCM) - G1WholeBodyConnection - v DDS to robot - -Joint name remapping: the trajectory file (recorded on the playback branch) uses -``g1_LeftHipPitch`` style names, but this branch's coordinator expects -``g1/left_hip_pitch``. Both lists share the same positional ordering — defined -once in ``_HUMANOID_29DOF_JOINTS`` — so we ignore the file's joint_names entirely -and zip its position arrays onto the canonical list from -``make_humanoid_joints("g1")``. - -Trajectory file format: - { - "joint_names": [...29 names...], # ignored — only used for length check - "samples": [{"ts": float, "position": [...29 floats...]}, ...] - } - -Usage: - # Terminal 1: - ROBOT_INTERFACE=enp194s0 dimos run unitree-g1-coordinator - # Terminal 2: - python scripts/g1_replay.py # default LFS bundle - python scripts/g1_replay.py --loop - python scripts/g1_replay.py --dry-run - python scripts/g1_replay.py --file # any 29-DOF JSON +"""Replay a 29-DOF G1 joint trajectory through unitree-g1-coordinator. + +Publishes JointState to /g1/joint_command. Trajectory file: positional 29-DOF +(joint_names ignored, zipped onto make_humanoid_joints("g1")). """ from __future__ import annotations @@ -65,8 +33,7 @@ from dimos.utils.data import get_data from dimos.utils.logging_config import setup_logger -# LFS-bundled default trajectory. get_data() auto-pulls from -# data/.lfs/g1_wholebody_replay.json.tar.gz and decompresses on first use. +# get_data() auto-pulls + decompresses data/.lfs/.tar.gz on first use. _DEFAULT_TRAJECTORY_NAME = "g1_wholebody_replay.json" NUM_DOF = 29 @@ -147,8 +114,7 @@ def on_state(msg: JointState) -> None: state_event.set() state_sub: LCMTransport[JointState] = LCMTransport("/coordinator/joint_state", JointState) - # LCMTransport.subscribe() actually returns an unsub callable but its - # signature claims None — match the pattern in scripts/test_g1_module. + # subscribe() returns an unsub callable; signature claims None (see transport.py). state_unsub = state_sub.subscribe(on_state) # type: ignore[func-returns-value] cmd_pub: LCMTransport[JointState] = LCMTransport("/g1/joint_command", JointState) @@ -171,9 +137,7 @@ def on_state(msg: JointState) -> None: logger.info("--dry-run set — exiting before publish phase") return - # ramp <= 0 means "skip ramp" — publish trajectory[0] once and proceed. - # Snaps to the recorded start pose under PD; only safe when the robot - # already sits there (e.g. dry-run had us close, or replay loop resume). + # ramp <= 0: snap to trajectory[0] (only safe when robot already there). target_q = positions[0] if args.ramp <= 0: logger.warning( From 7d0666613a78dbb51ea8aa335c6d1ab70ed978e9 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 19:38:16 -0700 Subject: [PATCH 26/28] removed getattr check for motor states and defined it as a bool in Protocol spec --- dimos/control/hardware_interface.py | 38 +++++++++++------------------ dimos/hardware/whole_body/spec.py | 1 + 2 files changed, 15 insertions(+), 24 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index a4318fce43..108058f9bf 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -382,17 +382,14 @@ def read_state(self) -> dict[JointName, JointState]: def write_command(self, commands: dict[str, float], _mode: ControlMode) -> bool: """Write position commands — converts to MotorCommand with PD gains. - Args: - commands: {joint_name: target_position} - can be partial - _mode: Control mode (uses position PD regardless) - - Returns: - True if command was sent successfully + Returns False (skip this tick) until the first motor_states frame + arrives so we don't seed last_commanded with adapter defaults and + publish a 0-rad snap on the first write. """ from dimos.hardware.whole_body.spec import MotorCommand - if not self._initialized: - self._initialize_last_commanded() + if not self._initialized and not self._try_initialize_last_commanded(): + return False for joint_name, value in commands.items(): if joint_name in self._joint_names: @@ -420,22 +417,15 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: """Direct pass-through to adapter for full MotorCommand control.""" return self._wb_adapter.write_motor_commands(commands) - def _initialize_last_commanded(self) -> None: - """Initialize last_commanded from current motor positions.""" - is_ready = getattr(self._wb_adapter, "has_motor_states", lambda: True) - for _ in range(10): - if is_ready(): - motor_states = self._wb_adapter.read_motor_states() - for i, name in enumerate(self._joint_names): - self._last_commanded[name] = motor_states[i].q - self._initialized = True - return - time.sleep(0.1) - - raise RuntimeError( - f"WholeBody {self._component.hardware_id} did not receive motor_states " - f"within 1s — refusing to initialize last_commanded with adapter defaults" - ) + def _try_initialize_last_commanded(self) -> bool: + """Non-blocking init. Returns True once motor_states is cached.""" + if not self._wb_adapter.has_motor_states(): + return False + states = self._wb_adapter.read_motor_states() + for i, name in enumerate(self._joint_names): + self._last_commanded[name] = states[i].q + self._initialized = True + return True __all__ = [ diff --git a/dimos/hardware/whole_body/spec.py b/dimos/hardware/whole_body/spec.py index 5bb524ba8a..4645e8c44e 100644 --- a/dimos/hardware/whole_body/spec.py +++ b/dimos/hardware/whole_body/spec.py @@ -62,6 +62,7 @@ def connect(self) -> bool: ... def disconnect(self) -> None: ... def is_connected(self) -> bool: ... def read_motor_states(self) -> list[MotorState]: ... + def has_motor_states(self) -> bool: ... def read_imu(self) -> IMUState: ... def write_motor_commands(self, commands: list[MotorCommand]) -> bool: ... From ffb59b9b559a68a7395bc66970796b1283a68d31 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 19:47:40 -0700 Subject: [PATCH 27/28] ControlMode validated name warns and returns --- dimos/control/hardware_interface.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 108058f9bf..3971a3ef90 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -379,15 +379,21 @@ def read_state(self) -> dict[JointName, JointState]: for i, name in enumerate(self._joint_names) } - def write_command(self, commands: dict[str, float], _mode: ControlMode) -> bool: + def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: """Write position commands — converts to MotorCommand with PD gains. - Returns False (skip this tick) until the first motor_states frame - arrives so we don't seed last_commanded with adapter defaults and - publish a 0-rad snap on the first write. + Only POSITION / SERVO_POSITION are supported; other modes are warned + and dropped (matches ConnectedHardware's warn-and-skip pattern). """ from dimos.hardware.whole_body.spec import MotorCommand + if mode not in (ControlMode.POSITION, ControlMode.SERVO_POSITION): + logger.warning( + f"WholeBody {self.hardware_id} only supports POSITION/SERVO_POSITION; " + f"got {mode.name} — skipping" + ) + return False + if not self._initialized and not self._try_initialize_last_commanded(): return False From 98eddaf1f72604298b5eb428dae5283ba704e704 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Fri, 1 May 2026 19:53:14 -0700 Subject: [PATCH 28/28] removed redundant isinstance checks --- dimos/control/hardware_interface.py | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 3971a3ef90..0169321600 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -55,15 +55,6 @@ def __init__( adapter: ManipulatorAdapter, component: HardwareComponent, ) -> None: - """Initialize hardware interface. - - Args: - adapter: ManipulatorAdapter instance (XArmAdapter, PiperAdapter, etc.) - component: Hardware component with joints config - """ - if not isinstance(adapter, ManipulatorAdapter): - raise TypeError("adapter must implement ManipulatorAdapter") - self._adapter = adapter self._component = component self._arm_joint_names: list[JointName] = list(component.joints) @@ -249,11 +240,6 @@ def __init__( adapter: TwistBaseAdapter, component: HardwareComponent, ) -> None: - from dimos.hardware.drive_trains.spec import TwistBaseAdapter as TwistBaseAdapterProto - - if not isinstance(adapter, TwistBaseAdapterProto): - raise TypeError("adapter must implement TwistBaseAdapter") - self._twist_adapter = adapter self._component = component self._joint_names = component.joints @@ -342,11 +328,6 @@ def __init__( adapter: WholeBodyAdapter, component: HardwareComponent, ) -> None: - from dimos.hardware.whole_body.spec import WholeBodyAdapter as WholeBodyAdapterProto - - if not isinstance(adapter, WholeBodyAdapterProto): - raise TypeError("adapter must implement WholeBodyAdapter") - self._wb_adapter = adapter self._component = component self._joint_names = component.joints