diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index 81d8f90aab3..fa0ae836ec0 100644 --- a/.github/workflows/message-codegen.yml +++ b/.github/workflows/message-codegen.yml @@ -218,6 +218,11 @@ jobs: if: runner.os == 'Linux' run: | DIMOS_TRANSPORT=lcm LCM_DEFAULT_URL=memq:// .native-review/bin/python -I -c 'from dimos.core import transport; from dimos.protocol.pubsub.impl import rospubsub, rospubsub_conversion; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (transport,rospubsub,rospubsub_conversion)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/protocol/pubsub/impl/test_cdr_ros_conversion.py" "$PWD/dimos/core/test_transport_factory.py" + - name: Verify canonical whole-body default wires and fake SDK admission safety + if: runner.os == 'Linux' + run: | + uv pip install --python .native-review/bin/python 'onnxruntime==1.24.1' + DIMOS_TRANSPORT=lcm LCM_DEFAULT_URL=memq:// .native-review/bin/python -I -c 'from dimos.hardware.whole_body.transport import adapter; from dimos.robot.unitree.g1 import wholebody_connection; from dimos.robot.galaxea.r1pro import connection; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (adapter,wholebody_connection,connection)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/hardware/whole_body/transport/test_adapter.py" "$PWD/dimos/robot/unitree/g1/test_wholebody_connection.py" - name: Verify generated manipulator commands preserve built-in IK task behavior if: runner.os == 'Linux' run: | diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 2ad2b223b84..7d7c707050a 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -15,7 +15,11 @@ """Transport-based whole-body adapter: bridges coordinator ↔ Module via pub/sub. Subscribes /{hardware_id}/motor_states + /{hardware_id}/imu, publishes -/{hardware_id}/motor_command. ``network_interface`` is accepted but ignored. +/{hardware_id}/motor_command. The normal factory selects generated CDR on the +configured LCM/Zenoh backend. ``network_interface`` is accepted but ignored. +The existing ``transport_lcm`` registry key keeps its name and API; encoding +follows the canonical message type, while internal MotorState/MotorCommand +numerical values are adapted explicitly at these three wire edges. """ from __future__ import annotations @@ -24,12 +28,14 @@ import threading from typing import Any +import numpy as np + from dimos.core.transport_factory import make_transport from dimos.hardware.spec import JointLimits 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.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState +from dimos.msgs.time import header_now from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -135,11 +141,12 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: 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], + header=header_now(), + q=np.asarray([c.q for c in commands], dtype=np.float64), + dq=np.asarray([c.dq for c in commands], dtype=np.float64), + kp=np.asarray([c.kp for c in commands], dtype=np.float64), + kd=np.asarray([c.kd for c in commands], dtype=np.float64), + tau=np.asarray([c.tau for c in commands], dtype=np.float64), ) self._motor_command_transport.publish(msg) return True diff --git a/dimos/hardware/whole_body/transport/test_adapter.py b/dimos/hardware/whole_body/transport/test_adapter.py new file mode 100644 index 00000000000..fe7d1aaea5d --- /dev/null +++ b/dimos/hardware/whole_body/transport/test_adapter.py @@ -0,0 +1,301 @@ +# 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. + +"""Canonical whole-body wire bridge, with fake external DDS/ROS endpoints.""" + +from contextlib import ExitStack +from copy import deepcopy +from functools import partial +from importlib import import_module +import sys +from threading import Event, Thread +from types import ModuleType, SimpleNamespace + +import numpy as np +import pytest + +from dimos.core import cdr_transport +from dimos.core.cdr_transport import CdrTransport +from dimos.core.global_config import GlobalConfig +from dimos.core.transport_factory import make_transport +from dimos.hardware.whole_body.spec import MotorCommand +from dimos.hardware.whole_body.transport.adapter import TransportWholeBodyAdapter +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState +from dimos.messages.std_msgs.msg import Header +from dimos.protocol.service.lcmservice import LCMConfig +from dimos.protocol.service.zenohservice import ZenohConfig +from dimos.robot.galaxea.r1pro.connection import R1ProConnection, _stamp_nanoseconds +from dimos.robot.unitree.g1.wholebody_connection import G1LowStateSnapshot, G1WholeBodyConnection +from dimos.utils.testing.waiting import wait_until + + +@pytest.fixture(params=["lcm", "zenoh"]) +def factory(request, monkeypatch): + monkeypatch.setattr(cdr_transport, "LCMConfig", partial(LCMConfig, url="memq://")) + monkeypatch.setattr( + cdr_transport, + "ZenohConfig", + partial( + ZenohConfig, + scouting=False, + multicast=False, + gossip=False, + connect=[], + listen=["tcp/127.0.0.1:0"], + ), + ) + build = partial(make_transport, g=GlobalConfig(transport=request.param)) + transports = {} + + def shared(topic, kind): + # ModuleCoordinator shares one transport per logical (name, type) within a worker. + # memq is endpoint-local, so distinct memq instances cannot act as a network. + key = (topic, kind) + if key not in transports: + transports[key] = build(topic, kind) + return transports[key] + + try: + yield shared + finally: + for wire in transports.values(): + wire.stop() + + +def _command(count): + return MotorCommandArray( + Header(Time(17, 1), "coordinator"), *[np.arange(count, dtype=np.float64) for _ in range(5)] + ) + + +def test_adapter_default_factory_reuses_g1_canonical_motor_state_imu_and_command(factory, mocker): + with ExitStack() as cleanup: + connection = G1WholeBodyConnection(soft_start_seconds=0.0) + cleanup.callback(connection.stop) + publisher = mocker.Mock() + crc = mocker.Mock() + crc.Crc.return_value = 17 + low_cmd = SimpleNamespace( + mode_machine=5, + crc=0, + motor_cmd=[SimpleNamespace(mode=1, q=0, dq=0, kp=0, kd=0, tau=0) for _ in range(35)], + ) + mocker.patch.multiple( + connection, _publisher=publisher, _low_cmd=low_cmd, _crc=crc, _mode_machine=5 + ) + written = Event() + values = [] + + def write(value): + values.append(deepcopy(value)) + written.set() + + publisher.Write.side_effect = write + adapter = TransportWholeBodyAdapter(dof=29, hardware_id="g1", transport_cls=factory) + cleanup.callback(adapter.disconnect) + assert adapter.connect() + wires = {} + for name, kind in [ + ("motor_states", JointState), + ("imu", Imu), + ("motor_command", MotorCommandArray), + ]: + wire = factory("/g1/" + name, kind) + cleanup.callback(wire.stop) + wires[name] = wire + connection.set_transport(name, wire) + cleanup.callback(wires["motor_command"].subscribe(connection._on_motor_command)) + sample = G1LowStateSnapshot( + [0.25] * 29, [0.5] * 29, [0.75] * 29, (1, 0, 0, 0), (1, 2, 3), (4, 5, 6) + ) + observed = [] + cleanup.callback(wires["motor_states"].subscribe(observed.append)) + connection._publish_motor_state_and_imu( + now=17.0, frame_id="g1_pelvis", sample=sample, stamp=Time(1_789_654_321, 1) + ) + wait_until( + lambda: ( + adapter.has_motor_states() + and adapter.read_imu().gyroscope == (1, 2, 3) + and bool(observed) + ), + timeout=5, + ) + assert adapter.read_motor_states()[0].q == 0.25 + assert adapter.read_motor_states()[0].dq == 0.5 + assert adapter.read_motor_states()[0].tau == 0.75 + assert adapter.read_imu().quaternion == (1, 0, 0, 0) + assert observed[0].header.stamp == Time(1_789_654_321, 1) + commands = [MotorCommand(q=1, dq=2, kp=3, kd=4, tau=5) for _ in range(29)] + assert adapter.write_motor_commands(commands) + assert written.wait(5) + assert values[0].mode_machine == 5 and values[0].crc == 17 + assert [(v.q, v.dq, v.kp, v.kd, v.tau) for v in values[0].motor_cmd[:29]] == [ + (1, 2, 3, 4, 5) + ] * 29 + + +@pytest.mark.parametrize("field", ["q", "dq", "kp", "kd", "tau"]) +@pytest.mark.parametrize("count,kind", [(29, G1WholeBodyConnection), (18, R1ProConnection)]) +def test_inconsistent_canonical_arrays_are_dropped_before_sdk(field, count, kind, mocker): + connection = kind() + with ExitStack() as cleanup: + cleanup.callback(connection.stop) + command = _command(count) + setattr(command, field, np.zeros(count - 1, dtype=np.float64)) + publisher = mocker.Mock() + if kind is G1WholeBodyConnection: + low_cmd = SimpleNamespace( + mode_machine=5, + crc=0, + motor_cmd=[ + SimpleNamespace(mode=1, q=0, dq=0, kp=0, kd=0, tau=0) for _ in range(35) + ], + ) + mocker.patch.multiple( + connection, + _publisher=publisher, + _low_cmd=low_cmd, + _crc=mocker.Mock(), + _mode_machine=5, + ) + else: + mocker.patch.object(connection, "_ros", publisher) + cleanup.callback(mocker.patch.object, connection, "_ros", None) + # R1 imports ROS only after validation; G1 has live fake DDS state. + connection._on_motor_command(command) + publisher.Write.assert_not_called() + publisher.publish.assert_not_called() + assert connection.inputs["motor_command"].type is MotorCommandArray + + +def test_r1_segment_publication_consumes_generated_command_without_physical_ros( + factory, mocker, monkeypatch +): + ros_module = ModuleType("sensor_msgs.msg") + ros_module.JointState = lambda: SimpleNamespace(header=SimpleNamespace(stamp=None)) + monkeypatch.setitem(sys.modules, "sensor_msgs.msg", ros_module) + stamp = SimpleNamespace(sec=17, nanosec=1) + mocker.patch("dimos.robot.galaxea.r1pro.connection._ros_stamp_now", return_value=stamp) + with ExitStack() as cleanup: + connection = R1ProConnection() + cleanup.callback(connection.stop) + ros = mocker.Mock() + mocker.patch.multiple( + connection, + _ros=ros, + _cmd_torso_topic="torso", + _cmd_left_topic="left", + _cmd_right_topic="right", + ) + cleanup.callback(mocker.patch.object, connection, "_ros", None) + wire = factory("/r1pro/motor_command", MotorCommandArray) + cleanup.callback(wire.stop) + cleanup.callback(wire.subscribe(connection._on_motor_command)) + written = Event() + ros.publish.side_effect = lambda *args: ( + written.set() if ros.publish.call_count == 3 else None + ) + wire.publish(_command(18)) + assert written.wait(5) + assert [call.args[0] for call in ros.publish.call_args_list] == ["torso", "left", "right"] + assert [call.args[1].position for call in ros.publish.call_args_list] == [ + list(range(4)), + list(range(4, 11)), + list(range(11, 18)), + ] + assert all(call.args[1].header.stamp is stamp for call in ros.publish.call_args_list) + assert connection.outputs["motor_states"].type is JointState + assert connection.outputs["imu_chassis"].type is Imu + + +def test_r1_feedback_stamp_preserves_epoch_nanoseconds_and_existing_zero_fallback(mocker): + mocker.patch("dimos.robot.galaxea.r1pro.connection.time.time_ns", return_value=99) + source = SimpleNamespace( + header=SimpleNamespace(stamp=SimpleNamespace(sec=1_789_654_321, nanosec=1)) + ) + assert _stamp_nanoseconds(source) == 1_789_654_321_000_000_001 + source.header.stamp.sec = 0 + source.header.stamp.nanosec = 0 + assert _stamp_nanoseconds(source) == 99 + + +def test_r1_fused_feedback_preserves_oldest_exact_segment_stamp(factory): + with ExitStack() as cleanup: + connection = R1ProConnection() + cleanup.callback(connection.stop) + wire = factory("/r1pro/motor_states", JointState) + cleanup.callback(wire.stop) + connection.set_transport("motor_states", wire) + observed = [] + ready = Event() + + def receive(value): + observed.append(value) + connection._stop_event.set() + ready.set() + + cleanup.callback(wire.subscribe(receive)) + for callback, count, nanos in [ + (connection._on_feedback_torso, 4, 3), + (connection._on_feedback_left, 7, 1), + (connection._on_feedback_right, 7, 2), + ]: + message = SimpleNamespace( + header=SimpleNamespace(stamp=SimpleNamespace(sec=1_789_654_321, nanosec=nanos)), + position=[float(nanos)] * count, + velocity=[0.5] * count, + effort=[0.75] * count, + ) + callback(message, None) + thread = Thread(target=connection._publish_loop) + + def close_thread(): + connection._stop_event.set() + thread.join(timeout=5) + assert not thread.is_alive() + + cleanup.callback(close_thread) + thread.start() + assert ready.wait(5) + assert observed[0].header.stamp == Time(1_789_654_321, 1) + assert observed[0].position.tolist() == [3.0] * 4 + [1.0] * 7 + [2.0] * 7 + assert observed[0].velocity.tolist() == [0.5] * 18 + assert observed[0].effort.tolist() == [0.75] * 18 + + +def test_wholebody_blueprints_bind_canonical_types_without_loading_robot_assets(mocker): + download = mocker.patch( + "dimos.utils.data.get_data", + side_effect=AssertionError("blueprint import must not download assets"), + ) + simple = import_module("dimos.robot.unitree.g1.blueprints.basic.unitree_g1_coordinator") + groot = import_module("dimos.robot.unitree.g1.blueprints.basic.unitree_g1_groot_wbc") + r1 = import_module("dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator") + for blueprint in (simple.unitree_g1_coordinator, groot.unitree_g1_groot_wbc): + for name, kind in [ + ("motor_states", JointState), + ("motor_command", MotorCommandArray), + ("imu", Imu), + ("joint_command", JointState), + ("g1_joints", JointState), + ]: + spec = blueprint.transport_map[(name, kind)] + assert spec.cls is CdrTransport + assert r1._zenoh_transport("/r1pro/motor_states", JointState).cls is CdrTransport + assert r1._zenoh_transport("/r1pro/imu", Imu).cls is CdrTransport + assert r1._zenoh_transport("/r1pro/motor_command", MotorCommandArray).cls is CdrTransport + download.assert_not_called() diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py index f9f60863659..46f9d011a2c 100644 --- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py +++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright 2025-2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -31,20 +30,20 @@ from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task +from dimos.core.cdr_transport import CdrTransport from dimos.core.coordination.blueprints import Blueprint, TransportSpec, autoconnect from dimos.core.global_config import global_config from dimos.core.transport import ZenohTransport from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints import mid360_for_pointlio +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage from dimos.msgs.sensor_msgs.Image import Image -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.protocol.pubsub.impl.zenohpubsub import QOS_LATEST_WINS, Topic as ZenohTopic, Zenoh from dimos.robot.galaxea.r1pro.config import ( R1PRO_CHASSIS_LIDAR_HOST_IP, @@ -161,6 +160,13 @@ def _zenoh_transport( *, latest_wins: bool = False, ) -> TransportSpec: + if hasattr(msg_type, "__msgtype__"): + return CdrTransport.spec( + f"dimos/{topic.lstrip('/')}", + msg_type, + backend="zenoh", + qos=QOS_LATEST_WINS if latest_wins else None, + ) return ZenohTransport.spec( ZenohTopic( f"dimos/{topic.lstrip('/')}", diff --git a/dimos/robot/galaxea/r1pro/connection.py b/dimos/robot/galaxea/r1pro/connection.py index cb7d063e1f3..377a310052f 100644 --- a/dimos/robot/galaxea/r1pro/connection.py +++ b/dimos/robot/galaxea/r1pro/connection.py @@ -40,6 +40,7 @@ import time from typing import TYPE_CHECKING, Any +import numpy as np from pydantic import Field from reactivex.disposable import Disposable @@ -51,16 +52,17 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.hardware.whole_body.spec import VEL_STOP +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState +from dimos.messages.std_msgs.msg import Header from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage from dimos.msgs.sensor_msgs.Image import Image -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.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.time import time_from_nanoseconds from dimos.robot.galaxea.r1pro.joints import UPPER_BODY_JOINTS, coordinator_name from dimos.utils.logging_config import setup_logger @@ -115,10 +117,11 @@ def _make_qos() -> Any: ) -def _stamp_secs(msg: Any) -> float: - """Header stamp in seconds, falling back to wall clock if the driver left it 0.""" - ts = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 - return ts if ts > 0 else time.time() +def _stamp_nanoseconds(msg: Any) -> int: + """Exact source nanoseconds; retain the driver's existing zero/negative fallback.""" + stamp = msg.header.stamp + value = int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + return value if value > 0 else time.time_ns() def _ros_stamp_now() -> Any: @@ -206,9 +209,9 @@ def __init__(self, **kwargs: Any) -> None: self._latest_right_dq: list[float] = [0.0] * 7 self._latest_right_eff: list[float] = [0.0] * 7 self._torso_seen = False - self._ts_torso = 0.0 - self._ts_left = 0.0 - self._ts_right = 0.0 + self._ts_torso_ns = 0 + self._ts_left_ns = 0 + self._ts_right_ns = 0 self._left_seen = False self._right_seen = False self._latest_imu_chassis: Imu | None = None @@ -516,8 +519,10 @@ def sensor_stats(self) -> dict[str, Any]: # Control input handlers 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") + if any(len(values) != _NUM_MOTORS for values in (msg.q, msg.dq, msg.kp, msg.kd, msg.tau)): + logger.warning( + "Expected %d complete motor commands; ignoring inconsistent frame", _NUM_MOTORS + ) return from sensor_msgs.msg import JointState as RosJointState @@ -537,8 +542,8 @@ def _on_motor_command(self, msg: MotorCommandArray) -> None: cmd = RosJointState() cmd.header.stamp = stamp cmd.name = [""] - cmd.position = list(msg.q[sl]) - cmd.velocity = self._tracking_velocities(msg.dq[sl]) + cmd.position = msg.q[sl].tolist() + cmd.velocity = self._tracking_velocities(msg.dq[sl].tolist()) cmd.effort = [0.0] ros.publish(topic, cmd) @@ -568,7 +573,7 @@ def _on_feedback_torso(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_torso_q, self._latest_torso_dq, self._latest_torso_eff ) - self._ts_torso = _stamp_secs(msg) + self._ts_torso_ns = _stamp_nanoseconds(msg) self._torso_seen = True def _on_feedback_left(self, msg: Any, _topic: Any) -> None: @@ -576,7 +581,7 @@ def _on_feedback_left(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_left_q, self._latest_left_dq, self._latest_left_eff ) - self._ts_left = _stamp_secs(msg) + self._ts_left_ns = _stamp_nanoseconds(msg) self._left_seen = True def _on_feedback_right(self, msg: Any, _topic: Any) -> None: @@ -584,7 +589,7 @@ def _on_feedback_right(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_right_q, self._latest_right_dq, self._latest_right_eff ) - self._ts_right = _stamp_secs(msg) + self._ts_right_ns = _stamp_nanoseconds(msg) self._right_seen = True @staticmethod @@ -680,17 +685,16 @@ def _publish_loop(self) -> None: imu_torso = self._latest_imu_torso # Oldest of the three segments: the fused snapshot is only # as fresh as its stalest part. - ts = min(self._ts_torso, self._ts_left, self._ts_right) + ts = min(self._ts_torso_ns, self._ts_left_ns, self._ts_right_ns) if bootstrapped: self.motor_states.publish( JointState( - ts=ts, - frame_id=frame_id, + header=Header(time_from_nanoseconds(ts), frame_id), name=R1PRO_UPPER_BODY_JOINTS, - position=positions, # type: ignore[arg-type] - velocity=velocities, - effort=efforts, + position=np.asarray(positions, dtype=np.float64), + velocity=np.asarray(velocities, dtype=np.float64), + effort=np.asarray(efforts, dtype=np.float64), ) ) if imu_chassis is not None: 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 35408e812ba..f64770d8fd2 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright 2025-2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -25,12 +24,11 @@ from dimos.control.components import HardwareComponent, HardwareType, make_humanoid_joints from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task +from dimos.core.cdr_transport import CdrTransport from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import Out -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.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection _g1_joints = make_humanoid_joints("g1") @@ -71,13 +69,13 @@ class _G1Coordinator(ControlCoordinator): # collide with ControlCoordinator's (joint_state/joint_command/...). .transports( { - ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), - ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport( + ("motor_states", JointState): CdrTransport.spec("/g1/motor_states", JointState), + ("imu", Imu): CdrTransport.spec("/g1/imu", Imu), + ("motor_command", MotorCommandArray): CdrTransport.spec( "/g1/motor_command", MotorCommandArray ), - ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), - ("g1_joints", JointState): LCMTransport("/g1/joints", JointState), + ("joint_command", JointState): CdrTransport.spec("/g1/joint_command", JointState), + ("g1_joints", JointState): CdrTransport.spec("/g1/joints", JointState), } ) ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 0d71bb043de..7611eae0a81 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -60,20 +60,19 @@ ) from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task from dimos.control.teleop_coordinator import TeleopControlCoordinator +from dimos.core.cdr_transport import CdrTransport from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.stream import Out -from dimos.core.transport import LCMTransport from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig -from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.geometry_msgs.msg import Twist +from dimos.messages.sensor_msgs.msg import Imu, JointState from dimos.msgs.nav_msgs.Path import Path as NavPath -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.navigation.go2.replanning_a_star.module import ReplanningAStarPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.g1.config import G1 @@ -550,16 +549,18 @@ def _viewer() -> Any: ], ).transports( { - ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), - ("g1_joints", JointState): LCMTransport(_G1_JOINTS_TOPIC, JointState), - ("cmd_vel", Twist): LCMTransport(_cmd_vel_topic, Twist), + ("joint_command", JointState): CdrTransport.spec("/g1/joint_command", JointState), + ("g1_joints", JointState): CdrTransport.spec(_G1_JOINTS_TOPIC, JointState), + ("cmd_vel", Twist): CdrTransport.spec(_cmd_vel_topic, Twist), # Real-hw only: the transport_lcm adapter speaks to # G1WholeBodyConnection over these topics. autoconnect already # matches by (name, type) so sim doesn't need them; they're # harmless when the sim engine doesn't expose those ports. - ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), - ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport("/g1/motor_command", MotorCommandArray), + ("motor_states", JointState): CdrTransport.spec("/g1/motor_states", JointState), + ("imu", Imu): CdrTransport.spec("/g1/imu", Imu), + ("motor_command", MotorCommandArray): CdrTransport.spec( + "/g1/motor_command", MotorCommandArray + ), } ) diff --git a/dimos/robot/unitree/g1/test_wholebody_connection.py b/dimos/robot/unitree/g1/test_wholebody_connection.py index 35c41badabc..5fda2070e4e 100644 --- a/dimos/robot/unitree/g1/test_wholebody_connection.py +++ b/dimos/robot/unitree/g1/test_wholebody_connection.py @@ -17,10 +17,12 @@ from collections.abc import Iterator from types import SimpleNamespace +import numpy as np from pydantic import ValidationError import pytest -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.msgs.time import header_now from dimos.robot.unitree.g1.wholebody_connection import ( _NUM_MOTOR_SLOTS, _NUM_MOTORS, @@ -69,11 +71,12 @@ def _wire(connection, soft_start_seconds): def _command(): return MotorCommandArray( - q=[1.0] * _NUM_MOTORS, - dq=[0.0] * _NUM_MOTORS, - kp=[100.0] * _NUM_MOTORS, - kd=[5.0] * _NUM_MOTORS, - tau=[8.0] * _NUM_MOTORS, + header=header_now(), + q=np.full(_NUM_MOTORS, 1.0, dtype=np.float64), + dq=np.full(_NUM_MOTORS, 0.0, dtype=np.float64), + kp=np.full(_NUM_MOTORS, 100.0, dtype=np.float64), + kd=np.full(_NUM_MOTORS, 5.0, dtype=np.float64), + tau=np.full(_NUM_MOTORS, 8.0, dtype=np.float64), ) @@ -118,7 +121,9 @@ def test_soft_start_disabled_passes_through(connection: G1WholeBodyConnection): def test_wrong_joint_count_is_dropped(connection: G1WholeBodyConnection): publisher = _wire(connection, soft_start_seconds=0.0) - connection._on_motor_command(MotorCommandArray(q=[0.0] * 5)) + command = _command() + command.q = np.zeros(5, dtype=np.float64) + connection._on_motor_command(command) assert publisher.frames == [] diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 2f185d62e8b..ae850858cf3 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -21,12 +21,14 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass import threading from threading import Thread import time from typing import TYPE_CHECKING, Any +import numpy as np from pydantic import Field from reactivex.disposable import Disposable @@ -41,11 +43,12 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out 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 -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.geometry_msgs.msg import Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import Imu, JointState +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.time import time_from_nanoseconds, time_from_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -71,14 +74,17 @@ def _imu_from_unitree_wxyz( *, frame_id: str, ts: float, + stamp: Time | None = None, ) -> Imu: _w, x, y, z = quaternion return Imu( orientation=Quaternion(x, y, z, _w), angular_velocity=Vector3(*gyroscope), linear_acceleration=Vector3(*accelerometer), - frame_id=frame_id, - ts=ts, + header=Header(deepcopy(stamp) if stamp is not None else time_from_seconds(ts), frame_id), + orientation_covariance=np.zeros(9, dtype=np.float64), + angular_velocity_covariance=np.zeros(9, dtype=np.float64), + linear_acceleration_covariance=np.zeros(9, dtype=np.float64), ) @@ -170,7 +176,9 @@ def start(self) -> None: ChannelFactoryInitialize(0) except Exception as e: # Idempotent - already initialised by a sibling participant is fine. - logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") + logger.debug( + f"ChannelFactoryInitialize raised (likely already init'd): {e}", exc_info=True + ) self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) self._publisher.Init() @@ -337,16 +345,17 @@ def _snapshot_motor_imu(self) -> G1LowStateSnapshot | None: ) def _publish_motor_state_and_imu( - self, now: float, frame_id: str, sample: G1LowStateSnapshot + self, now: float, frame_id: str, sample: G1LowStateSnapshot, stamp: Time | None = None ) -> None: self.motor_states.publish( JointState( - ts=now, - frame_id=frame_id, - name=G1_JOINT_NAMES, - position=sample.positions, - velocity=sample.velocities, - effort=sample.efforts, + header=Header( + deepcopy(stamp) if stamp is not None else time_from_seconds(now), frame_id + ), + name=list(G1_JOINT_NAMES), + position=np.asarray(sample.positions, dtype=np.float64), + velocity=np.asarray(sample.velocities, dtype=np.float64), + effort=np.asarray(sample.efforts, dtype=np.float64), ) ) # Unitree reports quaternions as (w,x,y,z); Imu/Quaternion stores (x,y,z,w). @@ -357,6 +366,7 @@ def _publish_motor_state_and_imu( sample.accelerometer, frame_id=frame_id, ts=now, + stamp=stamp, ) ) @@ -369,7 +379,12 @@ def _publish_loop(self) -> None: self._drain_low_state() sample = self._snapshot_motor_imu() if sample is not None: - self._publish_motor_state_and_imu(now=time.time(), frame_id=frame_id, sample=sample) + self._publish_motor_state_and_imu( + now=time.time(), + frame_id=frame_id, + sample=sample, + stamp=time_from_nanoseconds(time.time_ns()), + ) next_tick += period sleep_for = next_tick - time.perf_counter() @@ -396,8 +411,10 @@ def _soft_start_scale(self, now: float) -> float: return scale 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") + if any(len(values) != _NUM_MOTORS for values in (msg.q, msg.dq, msg.kp, msg.kd, msg.tau)): + logger.warning( + "Expected %d complete motor commands; ignoring inconsistent frame", _NUM_MOTORS + ) return with self._lock: