diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 8762ebd95b..5c33928e79 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -30,10 +30,15 @@ from __future__ import annotations -from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.components import ( + HardwareComponent, + HardwareType, + make_joints, + make_twist_base_joints, +) from dimos.control.coordinator import TaskConfig, control_coordinator from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.geometry_msgs import PoseStamped, Twist from dimos.msgs.sensor_msgs import JointState from dimos.teleop.quest.quest_types import Buttons from dimos.utils.data import LfsPath @@ -594,6 +599,80 @@ ) +# ============================================================================= +# Twist Base Blueprints (velocity-commanded platforms) +# ============================================================================= + +# Mock holonomic twist base (3-DOF: vx, vy, wz) +_base_joints = make_twist_base_joints("base") +coordinator_mock_twist_base = control_coordinator( + hardware=[ + HardwareComponent( + hardware_id="base", + hardware_type=HardwareType.BASE, + joints=_base_joints, + adapter_type="mock_twist_base", + ), + ], + tasks=[ + TaskConfig( + name="vel_base", + type="velocity", + joint_names=_base_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + + +# ============================================================================= +# Mobile Manipulation Blueprints (arm + twist base) +# ============================================================================= + +# Mock arm (7-DOF) + mock holonomic base (3-DOF) +_mm_base_joints = make_twist_base_joints("base") +coordinator_mobile_manip_mock = control_coordinator( + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 7), + adapter_type="mock", + ), + HardwareComponent( + hardware_id="base", + hardware_type=HardwareType.BASE, + joints=_mm_base_joints, + adapter_type="mock_twist_base", + ), + ], + tasks=[ + TaskConfig( + name="traj_arm", + type="trajectory", + joint_names=[f"arm_joint{i + 1}" for i in range(7)], + priority=10, + ), + TaskConfig( + name="vel_base", + type="velocity", + joint_names=_mm_base_joints, + priority=10, + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("twist_command", Twist): LCMTransport("/cmd_vel", Twist), + } +) + + # ============================================================================= # Raw Blueprints (for programmatic setup) # ============================================================================= @@ -624,8 +703,12 @@ # Dual arm "coordinator_dual_mock", "coordinator_dual_xarm", + # Mobile manipulation + "coordinator_mobile_manip_mock", # Single arm "coordinator_mock", + # Twist base + "coordinator_mock_twist_base", "coordinator_piper", "coordinator_piper_xarm", # Teleop IK diff --git a/dimos/control/components.py b/dimos/control/components.py index e3022468ed..8157a288d2 100644 --- a/dimos/control/components.py +++ b/dimos/control/components.py @@ -71,7 +71,41 @@ def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]: return [f"{hardware_id}_joint{i + 1}" for i in range(dof)] +# Maps virtual joint suffix → (Twist group, Twist field) +TWIST_SUFFIX_MAP: dict[str, tuple[str, str]] = { + "vx": ("linear", "x"), + "vy": ("linear", "y"), + "vz": ("linear", "z"), + "wx": ("angular", "x"), + "wy": ("angular", "y"), + "wz": ("angular", "z"), +} + +_DEFAULT_TWIST_SUFFIXES = ["vx", "vy", "wz"] + + +def make_twist_base_joints( + hardware_id: HardwareId, + suffixes: list[str] | None = None, +) -> list[JointName]: + """Create virtual joint names for a twist base. + + Args: + hardware_id: The hardware identifier (e.g., "base") + suffixes: Velocity DOF suffixes. Defaults to ["vx", "vy", "wz"] (holonomic). + + Returns: + List of joint names like ["base_vx", "base_vy", "base_wz"] + """ + suffixes = suffixes or _DEFAULT_TWIST_SUFFIXES + for s in suffixes: + if s not in TWIST_SUFFIX_MAP: + raise ValueError(f"Unknown twist suffix '{s}'. Valid: {list(TWIST_SUFFIX_MAP)}") + return [f"{hardware_id}_{s}" for s in suffixes] + + __all__ = [ + "TWIST_SUFFIX_MAP", "HardwareComponent", "HardwareId", "HardwareType", @@ -79,4 +113,5 @@ def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]: "JointState", "TaskName", "make_joints", + "make_twist_base_joints", ] diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 5685a9f9c7..c9182e6aa8 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -32,19 +32,32 @@ import time from typing import TYPE_CHECKING, Any -from dimos.control.components import HardwareComponent, HardwareId, JointName, TaskName -from dimos.control.hardware_interface import ConnectedHardware +from dimos.control.components import ( + TWIST_SUFFIX_MAP, + HardwareComponent, + HardwareId, + HardwareType, + JointName, + TaskName, +) +from dimos.control.hardware_interface import ConnectedHardware, ConnectedTwistBase from dimos.control.task import ControlTask from dimos.control.tick_loop import TickLoop from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig +from dimos.hardware.drive_trains.spec import ( + TwistBaseAdapter, +) from dimos.msgs.geometry_msgs import ( PoseStamped, # noqa: TC001 - needed at runtime for In[PoseStamped] + Twist, # noqa: TC001 - needed at runtime for In[Twist] ) from dimos.msgs.sensor_msgs import ( - JointState, # noqa: TC001 - needed at runtime for Out[JointState] + JointState, +) +from dimos.teleop.quest.quest_types import ( + Buttons, # noqa: TC001 - needed at runtime for In[Buttons] ) -from dimos.teleop.quest.quest_types import Buttons # noqa: TC001 - needed for teleop buttons from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -148,6 +161,9 @@ class ControlCoordinator(Module[ControlCoordinatorConfig]): # Uses frame_id as task name for routing cartesian_command: In[PoseStamped] + # Input: Streaming twist commands for velocity-commanded platforms + twist_command: In[Twist] + # Input: Teleop buttons for engage/disengage signaling buttons: In[Buttons] @@ -174,6 +190,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Subscription handles for streaming commands self._joint_command_unsub: Callable[[], None] | None = None self._cartesian_command_unsub: Callable[[], None] | None = None + self._twist_command_unsub: Callable[[], None] | None = None self._buttons_unsub: Callable[[], None] | None = None logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz") @@ -206,7 +223,11 @@ def _setup_from_config(self) -> None: def _setup_hardware(self, component: HardwareComponent) -> None: """Connect and add a single hardware adapter.""" - adapter = self._create_adapter(component) + adapter: ManipulatorAdapter | TwistBaseAdapter + if component.hardware_type == HardwareType.BASE: + adapter = self._create_twist_base_adapter(component) + else: + adapter = self._create_adapter(component) if not adapter.connect(): raise RuntimeError(f"Failed to connect to {component.adapter_type} adapter") @@ -230,6 +251,16 @@ def _create_adapter(self, component: HardwareComponent) -> ManipulatorAdapter: address=component.address, ) + def _create_twist_base_adapter(self, component: HardwareComponent) -> TwistBaseAdapter: + """Create a twist base adapter from component config.""" + from dimos.hardware.drive_trains.registry import twist_base_adapter_registry + + return twist_base_adapter_registry.create( + component.adapter_type, + dof=len(component.joints), + address=component.address, + ) + def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: """Create a control task from config.""" task_type = cfg.type.lower() @@ -310,19 +341,34 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: @rpc def add_hardware( self, - adapter: ManipulatorAdapter, + adapter: ManipulatorAdapter | TwistBaseAdapter, component: HardwareComponent, ) -> bool: """Register a hardware adapter with the coordinator.""" + is_base = component.hardware_type == HardwareType.BASE + if is_base != 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__}" + ) + with self._hardware_lock: if component.hardware_id in self._hardware: logger.warning(f"Hardware {component.hardware_id} already registered") return False - connected = ConnectedHardware( - adapter=adapter, - component=component, - ) + if isinstance(adapter, TwistBaseAdapter): + connected: ConnectedHardware = ConnectedTwistBase( + adapter=adapter, + component=component, + ) + else: + connected = ConnectedHardware( + adapter=adapter, + component=component, + ) + self._hardware[component.hardware_id] = connected for joint_name in connected.joint_names: @@ -490,6 +536,34 @@ def _on_cartesian_command(self, msg: PoseStamped) -> None: task.on_cartesian_command(msg, t_now) + def _on_twist_command(self, msg: Twist) -> None: + """Convert Twist → virtual joint velocities and route via _on_joint_command. + + Maps Twist fields to virtual joints using suffix convention: + base_vx ← linear.x, base_vy ← linear.y, base_wz ← angular.z, etc. + """ + names: list[str] = [] + velocities: list[float] = [] + + with self._hardware_lock: + for hw in self._hardware.values(): + if hw.component.hardware_type != HardwareType.BASE: + continue + for joint_name in hw.joint_names: + # Extract suffix (e.g., "base_vx" → "vx") + suffix = joint_name.rsplit("_", 1)[-1] + mapping = TWIST_SUFFIX_MAP.get(suffix) + if mapping is None: + continue + group, axis = mapping + value = getattr(getattr(msg, group), axis) + names.append(joint_name) + velocities.append(value) + + if names: + joint_state = JointState(name=names, velocity=velocities) + self._on_joint_command(joint_state) + def _on_buttons(self, msg: Buttons) -> None: """Forward button state to all tasks.""" with self._task_lock: @@ -536,6 +610,9 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool: if hw is None: logger.warning(f"Hardware '{hardware_id}' not found for gripper command") return False + if isinstance(hw, ConnectedTwistBase): + logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support") + return False return hw.adapter.write_gripper_position(position) @rpc @@ -549,6 +626,8 @@ def get_gripper_position(self, hardware_id: str) -> float | None: hw = self._hardware.get(hardware_id) if hw is None: return None + if isinstance(hw, ConnectedTwistBase): + return None return hw.adapter.read_gripper_position() # ========================================================================= @@ -610,6 +689,18 @@ def start(self) -> None: "Use task_invoke RPC or set transport via blueprint." ) + # Subscribe to twist commands if any twist base hardware configured + has_twist_base = any(c.hardware_type == HardwareType.BASE for c in self.config.hardware) + if has_twist_base: + try: + self._twist_command_unsub = self.twist_command.subscribe(self._on_twist_command) + logger.info("Subscribed to twist_command for twist base control") + except Exception: + logger.warning( + "Twist base configured but could not subscribe to twist_command. " + "Use task_invoke RPC or set transport via blueprint." + ) + # Subscribe to buttons if any teleop_ik tasks configured (engage/disengage) has_teleop_ik = any(t.type == "teleop_ik" for t in self.config.tasks) if has_teleop_ik: @@ -630,6 +721,9 @@ def stop(self) -> None: if self._cartesian_command_unsub: self._cartesian_command_unsub() self._cartesian_command_unsub = None + if self._twist_command_unsub: + self._twist_command_unsub() + self._twist_command_unsub = None if self._buttons_unsub: self._buttons_unsub() self._buttons_unsub = None diff --git a/dimos/control/examples/twist_base_keyboard_teleop.py b/dimos/control/examples/twist_base_keyboard_teleop.py new file mode 100644 index 0000000000..2d7651145a --- /dev/null +++ b/dimos/control/examples/twist_base_keyboard_teleop.py @@ -0,0 +1,59 @@ +# 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. + +"""Keyboard teleop for twist base via ControlCoordinator. + +Runs a mock holonomic twist base with pygame keyboard control. +WASD keys publish Twist → coordinator's twist_command port → virtual joints +→ tick loop → MockTwistBaseAdapter. + +Controls: + W/S: Forward/backward (linear.x) + Q/E: Strafe left/right (linear.y) + A/D: Turn left/right (angular.z) + Shift: 2x boost + Ctrl: 0.5x slow + Space: Emergency stop + ESC: Quit + +Usage: + python -m dimos.control.examples.twist_base_keyboard_teleop +""" + +from __future__ import annotations + +from dimos.control.blueprints import coordinator_mock_twist_base +from dimos.robot.unitree.keyboard_teleop import keyboard_teleop + + +def main() -> None: + """Run mock twist base + keyboard teleop.""" + coord = coordinator_mock_twist_base.build() + teleop = keyboard_teleop().build() + + print("Starting mock twist base coordinator + keyboard teleop...") + print("Coordinator tick loop: 100Hz") + print("Keyboard teleop: 50Hz on /cmd_vel") + print() + + coord.start() + teleop.start() + + # Block until Ctrl+C — loop() handles KeyboardInterrupt and calls stop() + coord.loop() + teleop.stop() + + +if __name__ == "__main__": + main() diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 9f6eb99851..4e5d1d634c 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -14,10 +14,12 @@ """Connected hardware for the ControlCoordinator. -Wraps ManipulatorAdapter with coordinator-specific features: -- Namespaced joint names (e.g., "left_joint1") -- Unified read/write interface -- Hold-last-value for partial commands +Provides two wrapper types: +- ConnectedHardware: Wraps ManipulatorAdapter for joint-controlled arms +- ConnectedTwistBase: Wraps TwistBaseAdapter for velocity-commanded platforms + +Both share the same duck-type interface (read_state, write_command, etc.) +so the tick loop treats them uniformly. """ from __future__ import annotations @@ -30,6 +32,7 @@ if TYPE_CHECKING: from dimos.control.components import HardwareComponent, HardwareId, JointName, JointState + from dimos.hardware.drive_trains.spec import TwistBaseAdapter logger = logging.getLogger(__name__) @@ -193,6 +196,98 @@ def _build_ordered_command(self) -> list[float]: return [self._last_commanded[name] for name in self._joint_names] +class ConnectedTwistBase(ConnectedHardware): + """Runtime wrapper for a twist base connected to the coordinator. + + Inherits from ConnectedHardware and overrides behavior for + velocity-commanded platforms (holonomic bases, drones, quadrupeds, etc.). + + Key differences from ConnectedHardware: + - Positions come from odometry (or zeros if unavailable) + - Efforts are always zero + - write_command always sends velocities regardless of mode + - No retry loop for initialization (twist bases start at zero velocity) + """ + + _twist_adapter: TwistBaseAdapter + + def __init__( + self, + 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 + + # Twist bases start at zero velocity — no need to read from hardware + self._last_commanded: dict[str, float] = {name: 0.0 for name in self._joint_names} + self._initialized = True + self._warned_unknown_joints: set[str] = set() + self._current_mode: ControlMode | None = None + + @property + def adapter(self) -> TwistBaseAdapter: # type: ignore[override] + """The underlying twist base adapter.""" + return self._twist_adapter + + def disconnect(self) -> None: + """Disconnect the underlying adapter.""" + self._twist_adapter.disconnect() + + def read_state(self) -> dict[JointName, JointState]: + """Read state as {joint_name: JointState}. + + Positions come from odometry (zeros if unavailable). + Velocities from adapter. Efforts are always zero. + """ + from dimos.control.components import JointState + + velocities = self._twist_adapter.read_velocities() + odometry = self._twist_adapter.read_odometry() + positions = odometry if odometry is not None else [0.0] * self.dof + + return { + name: JointState( + position=positions[i], + velocity=velocities[i], + effort=0.0, + ) + for i, name in enumerate(self._joint_names) + } + + def write_command(self, commands: dict[str, float], _mode: ControlMode) -> bool: + """Write velocity commands — always sends velocities regardless of mode. + + Args: + commands: {joint_name: velocity} - can be partial + _mode: Control mode (ignored — twist bases always use velocity) + + Returns: + True if command was sent successfully + """ + # Update last commanded for joints we received + for joint_name, value in commands.items(): + if joint_name in self._last_commanded: + self._last_commanded[joint_name] = value + elif joint_name not in self._warned_unknown_joints: + logger.warning( + f"TwistBase {self.hardware_id} received command for unknown joint " + f"{joint_name}. Valid joints: {self._joint_names}" + ) + self._warned_unknown_joints.add(joint_name) + + # Build ordered velocity list and send + ordered = self._build_ordered_command() + return self._twist_adapter.write_velocities(ordered) + + __all__ = [ "ConnectedHardware", + "ConnectedTwistBase", ] diff --git a/dimos/hardware/drive_trains/__init__.py b/dimos/hardware/drive_trains/__init__.py new file mode 100644 index 0000000000..c6e843feea --- /dev/null +++ b/dimos/hardware/drive_trains/__init__.py @@ -0,0 +1,15 @@ +# 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. + +"""Drive train hardware adapters for velocity-commanded platforms.""" diff --git a/dimos/hardware/drive_trains/flowbase/__init__.py b/dimos/hardware/drive_trains/flowbase/__init__.py new file mode 100644 index 0000000000..25f95e399c --- /dev/null +++ b/dimos/hardware/drive_trains/flowbase/__init__.py @@ -0,0 +1,15 @@ +# 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. + +"""FlowBase twist base adapter for holonomic base control via Portal RPC.""" diff --git a/dimos/hardware/drive_trains/flowbase/adapter.py b/dimos/hardware/drive_trains/flowbase/adapter.py new file mode 100644 index 0000000000..5b5563792d --- /dev/null +++ b/dimos/hardware/drive_trains/flowbase/adapter.py @@ -0,0 +1,206 @@ +# 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. + +"""FlowBase adapter — wraps Portal RPC client for holonomic base control. + +Frame convention: FlowBase uses inverted Y-axis compared to standard convention. +We negate vy and wz when sending to the hardware. + + Standard (ROS): FlowBase: + +Y -Y + ↑ ↑ + ───┼──→ +X ───┼──→ +X + | | +""" + +from __future__ import annotations + +import logging +import threading +from typing import TYPE_CHECKING + +import numpy as np + +if TYPE_CHECKING: + from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry + +logger = logging.getLogger(__name__) + + +class FlowBaseAdapter: + """TwistBaseAdapter implementation for FlowBase holonomic platform. + + Communicates with FlowBase controller via Portal RPC over TCP. + Expects 3 DOF: [vx, vy, wz] (holonomic base). + + Args: + dof: Number of velocity DOFs (must be 3 for FlowBase) + address: Portal RPC address as "host:port" (default: "172.6.2.20:11323") + """ + + def __init__(self, dof: int = 3, address: str | None = None, **_: object) -> None: + if dof != 3: + raise ValueError(f"FlowBase only supports 3 DOF (holonomic), got {dof}") + + self._address = address or "172.6.2.20:11323" + self._client = None + self._connected = False + self._enabled = False + self._lock = threading.Lock() + + # Last commanded velocities (in standard frame, before negation) + self._last_velocities = [0.0, 0.0, 0.0] + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + """Connect to FlowBase controller via Portal RPC.""" + try: + import portal # type: ignore[import-untyped] + + self._client = portal.Client(self._address) + self._connected = True + logger.info(f"Connected to FlowBase at {self._address}") + return True + except Exception as e: + logger.error(f"Failed to connect to FlowBase at {self._address}: {e}") + self._connected = False + return False + + def disconnect(self) -> None: + """Disconnect and send zero velocity.""" + if self._connected and self._client: + try: + self._send_velocity(0.0, 0.0, 0.0) + except Exception: + pass + try: + self._client.close() + except Exception: + pass + self._connected = False + self._client = None + + def is_connected(self) -> bool: + """Check if connected to FlowBase.""" + return self._connected + + # ========================================================================= + # Info + # ========================================================================= + + def get_dof(self) -> int: + """FlowBase is always 3 DOF (vx, vy, wz).""" + return 3 + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_velocities(self) -> list[float]: + """Return last commanded velocities (FlowBase doesn't report actual).""" + with self._lock: + return self._last_velocities.copy() + + def read_odometry(self) -> list[float] | None: + """Read odometry from FlowBase as [x, y, theta].""" + if not self._connected or not self._client: + return None + + try: + with self._lock: + odom = self._client.get_odometry({}).result() + + if odom is None: + return None + + translation = odom["translation"] # [x, y] + rotation = odom["rotation"] # theta in radians + return [float(translation[0]), float(translation[1]), float(rotation)] + except Exception as e: + logger.error(f"Error reading FlowBase odometry: {e}") + return None + + # ========================================================================= + # Control + # ========================================================================= + + def write_velocities(self, velocities: list[float]) -> bool: + """Send velocity command to FlowBase. + + Args: + velocities: [vx, vy, wz] in standard frame (m/s, rad/s) + """ + if len(velocities) != 3: + return False + + if not self._connected or not self._client: + return False + + vx, vy, wz = velocities + with self._lock: + self._last_velocities = list(velocities) + + # Negate vy and wz for FlowBase's inverted Y-axis frame + return self._send_velocity(vx, -vy, -wz) + + def write_stop(self) -> bool: + """Stop all motion.""" + with self._lock: + self._last_velocities = [0.0, 0.0, 0.0] + if not self._connected or not self._client: + return False + return self._send_velocity(0.0, 0.0, 0.0) + + # ========================================================================= + # Enable/Disable + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable/disable the platform (FlowBase is always enabled when connected).""" + self._enabled = enable + return True + + def read_enabled(self) -> bool: + """Check if platform is enabled.""" + return self._enabled + + # ========================================================================= + # Internal + # ========================================================================= + + def _send_velocity(self, vx: float, vy: float, wz: float) -> bool: + """Send raw velocity to FlowBase via Portal RPC.""" + try: + command = { + "target_velocity": np.array([vx, vy, wz]), + "frame": "local", + } + with self._lock: + assert self._client is not None + self._client.set_target_velocity(command).result() + return True + except Exception as e: + logger.error(f"Error sending FlowBase velocity: {e}") + return False + + +def register(registry: TwistBaseAdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("flowbase", FlowBaseAdapter) + + +__all__ = ["FlowBaseAdapter"] diff --git a/dimos/hardware/drive_trains/mock/__init__.py b/dimos/hardware/drive_trains/mock/__init__.py new file mode 100644 index 0000000000..9b6f630040 --- /dev/null +++ b/dimos/hardware/drive_trains/mock/__init__.py @@ -0,0 +1,30 @@ +# 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 twist base adapter for testing without hardware. + +Usage: + >>> from dimos.hardware.drive_trains.mock import MockTwistBaseAdapter + >>> adapter = MockTwistBaseAdapter(dof=3) + >>> adapter.connect() + True + >>> adapter.write_velocities([0.5, 0.0, 0.1]) + True + >>> adapter.read_velocities() + [0.5, 0.0, 0.1] +""" + +from dimos.hardware.drive_trains.mock.adapter import MockTwistBaseAdapter + +__all__ = ["MockTwistBaseAdapter"] diff --git a/dimos/hardware/drive_trains/mock/adapter.py b/dimos/hardware/drive_trains/mock/adapter.py new file mode 100644 index 0000000000..2091ec59d0 --- /dev/null +++ b/dimos/hardware/drive_trains/mock/adapter.py @@ -0,0 +1,137 @@ +# 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 twist base adapter for testing - no hardware required. + +Usage: + >>> from dimos.hardware.drive_trains.mock import MockTwistBaseAdapter + >>> adapter = MockTwistBaseAdapter(dof=3) + >>> adapter.connect() + True + >>> adapter.write_velocities([0.5, 0.0, 0.1]) + True +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.hardware.drive_trains.registry import TwistBaseAdapterRegistry + + +class MockTwistBaseAdapter: + """Fake twist base adapter for unit tests. + + Implements TwistBaseAdapter protocol with in-memory state. + Useful for: + - Unit testing coordinator logic without hardware + - Integration testing with predictable behavior + - Development without a physical base + """ + + def __init__(self, dof: int = 3, **_: object) -> None: + self._dof = dof + self._velocities = [0.0] * dof + self._odometry: list[float] | None = [0.0] * dof + self._enabled = False + self._connected = False + + # ========================================================================= + # Connection + # ========================================================================= + + def connect(self) -> bool: + """Simulate connection.""" + self._connected = True + return True + + def disconnect(self) -> None: + """Simulate disconnection.""" + self._connected = False + + def is_connected(self) -> bool: + """Check mock connection status.""" + return self._connected + + # ========================================================================= + # Info + # ========================================================================= + + def get_dof(self) -> int: + """Return DOF.""" + return self._dof + + # ========================================================================= + # State Reading + # ========================================================================= + + def read_velocities(self) -> list[float]: + """Return mock velocities.""" + return self._velocities.copy() + + def read_odometry(self) -> list[float] | None: + """Return mock odometry.""" + if self._odometry is None: + return None + return self._odometry.copy() + + # ========================================================================= + # Control + # ========================================================================= + + def write_velocities(self, velocities: list[float]) -> bool: + """Set mock velocities.""" + if len(velocities) != self._dof: + return False + self._velocities = list(velocities) + return True + + def write_stop(self) -> bool: + """Stop mock motion.""" + self._velocities = [0.0] * self._dof + return True + + # ========================================================================= + # Enable/Disable + # ========================================================================= + + def write_enable(self, enable: bool) -> bool: + """Enable/disable mock platform.""" + self._enabled = enable + return True + + def read_enabled(self) -> bool: + """Check mock enable state.""" + return self._enabled + + # ========================================================================= + # Test Helpers (not part of Protocol) + # ========================================================================= + + def set_odometry(self, odometry: list[float] | None) -> None: + """Set odometry directly for testing.""" + self._odometry = list(odometry) if odometry is not None else None + + def set_velocities_directly(self, velocities: list[float]) -> None: + """Set velocities directly for testing (bypasses DOF check).""" + self._velocities = list(velocities) + + +def register(registry: TwistBaseAdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("mock_twist_base", MockTwistBaseAdapter) + + +__all__ = ["MockTwistBaseAdapter"] diff --git a/dimos/hardware/drive_trains/registry.py b/dimos/hardware/drive_trains/registry.py new file mode 100644 index 0000000000..0a513d2bd4 --- /dev/null +++ b/dimos/hardware/drive_trains/registry.py @@ -0,0 +1,98 @@ +# 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. + +"""TwistBase adapter registry with auto-discovery. + +Automatically discovers and registers twist base adapters from subpackages. +Each adapter provides a `register()` function in its adapter.py module. + +Usage: + from dimos.hardware.drive_trains.registry import twist_base_adapter_registry + + # Create an adapter by name + adapter = twist_base_adapter_registry.create("mock_twist_base", dof=3) + adapter = twist_base_adapter_registry.create("flowbase", dof=3, address="172.6.2.20:11323") + + # List available adapters + print(twist_base_adapter_registry.available()) # ["flowbase", "mock_twist_base"] +""" + +from __future__ import annotations + +import importlib +import logging +import pkgutil +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from dimos.hardware.drive_trains.spec import TwistBaseAdapter + +logger = logging.getLogger(__name__) + + +class TwistBaseAdapterRegistry: + """Registry for twist base adapters with auto-discovery.""" + + def __init__(self) -> None: + self._adapters: dict[str, type[TwistBaseAdapter]] = {} + + def register(self, name: str, cls: type[TwistBaseAdapter]) -> None: + """Register an adapter class.""" + self._adapters[name.lower()] = cls + + def create(self, name: str, **kwargs: Any) -> TwistBaseAdapter: + """Create an adapter instance by name. + + Args: + name: Adapter name (e.g., "mock_twist_base", "flowbase") + **kwargs: Arguments passed to adapter constructor + + Returns: + Configured adapter instance + + Raises: + KeyError: If adapter name is not found + """ + key = name.lower() + if key not in self._adapters: + raise KeyError(f"Unknown twist base 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. + + Can be called multiple times to pick up newly added adapters. + """ + import dimos.hardware.drive_trains as pkg + + for _, name, ispkg in pkgutil.iter_modules(pkg.__path__): + if not ispkg: + continue + try: + module = importlib.import_module(f"dimos.hardware.drive_trains.{name}.adapter") + if hasattr(module, "register"): + module.register(self) + except ImportError as e: + logger.warning(f"Skipping twist base adapter {name}: {e}") + + +twist_base_adapter_registry = TwistBaseAdapterRegistry() +twist_base_adapter_registry.discover() + +__all__ = ["TwistBaseAdapterRegistry", "twist_base_adapter_registry"] diff --git a/dimos/hardware/drive_trains/spec.py b/dimos/hardware/drive_trains/spec.py new file mode 100644 index 0000000000..0b288edfd4 --- /dev/null +++ b/dimos/hardware/drive_trains/spec.py @@ -0,0 +1,95 @@ +# 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. + +"""TwistBase adapter protocol for velocity-commanded platforms. + +Lightweight protocol for mobile bases, quadrupeds, drones, RC cars, +and any other platform that accepts Twist (velocity) commands. + +Virtual joint ordering is defined by the HardwareComponent.joints list. +For a holonomic base: [vx, vy, wz] maps to joints ["base_vx", "base_vy", "base_wz"]. +""" + +from typing import Protocol, runtime_checkable + + +@runtime_checkable +class TwistBaseAdapter(Protocol): + """Protocol for velocity-commanded platform IO. + + Implement this per vendor SDK. All methods use SI units: + - Linear velocity: m/s + - Angular velocity: rad/s + - Position: meters + - Angle: radians + """ + + # --- 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.""" + ... + + # --- Info --- + + def get_dof(self) -> int: + """Get number of velocity DOFs (e.g., 3 for holonomic, 2 for differential).""" + ... + + # --- State Reading --- + + def read_velocities(self) -> list[float]: + """Read current velocities in virtual joint order (m/s or rad/s).""" + ... + + def read_odometry(self) -> list[float] | None: + """Read position estimate in virtual joint order. + + For a holonomic base this would be [x, y, theta]. + Returns None if the platform doesn't provide odometry. + """ + ... + + # --- Control --- + + def write_velocities(self, velocities: list[float]) -> bool: + """Command velocities in virtual joint order. Returns success.""" + ... + + def write_stop(self) -> bool: + """Stop all motion immediately (zero velocities).""" + ... + + # --- Enable/Disable --- + + def write_enable(self, enable: bool) -> bool: + """Enable or disable the platform. Returns success.""" + ... + + def read_enabled(self) -> bool: + """Check if platform is enabled.""" + ... + + +__all__ = [ + "TwistBaseAdapter", +] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index bdfd98cd17..0e23c82065 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -27,7 +27,9 @@ "coordinator-combined-xarm6": "dimos.control.blueprints:coordinator_combined_xarm6", "coordinator-dual-mock": "dimos.control.blueprints:coordinator_dual_mock", "coordinator-dual-xarm": "dimos.control.blueprints:coordinator_dual_xarm", + "coordinator-mobile-manip-mock": "dimos.control.blueprints:coordinator_mobile_manip_mock", "coordinator-mock": "dimos.control.blueprints:coordinator_mock", + "coordinator-mock-twist-base": "dimos.control.blueprints:coordinator_mock_twist_base", "coordinator-piper": "dimos.control.blueprints:coordinator_piper", "coordinator-piper-xarm": "dimos.control.blueprints:coordinator_piper_xarm", "coordinator-teleop-dual": "dimos.control.blueprints:coordinator_teleop_dual", diff --git a/pyproject.toml b/pyproject.toml index 9dea7e1921..ee7c4778b2 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -119,6 +119,7 @@ misc = [ # Hardware SDKs "xarm-python-sdk>=1.17.0", + "portal", ] visualization = [ diff --git a/uv.lock b/uv.lock index d971dcfeaa..53b2454b40 100644 --- a/uv.lock +++ b/uv.lock @@ -1964,6 +1964,7 @@ misc = [ { name = "onnx" }, { name = "open-clip-torch" }, { name = "opencv-contrib-python" }, + { name = "portal" }, { name = "python-multipart" }, { name = "scikit-learn", version = "1.7.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, { name = "scikit-learn", version = "1.8.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, @@ -2121,6 +2122,7 @@ requires-dist = [ { name = "plotly", marker = "extra == 'manipulation'", specifier = ">=5.9.0" }, { name = "plum-dispatch", specifier = "==2.5.7" }, { name = "plum-dispatch", marker = "extra == 'docker'", specifier = "==2.5.7" }, + { name = "portal", marker = "extra == 'misc'" }, { name = "pre-commit", marker = "extra == 'dev'", specifier = "==4.2.0" }, { name = "psycopg2-binary", marker = "extra == 'psql'", specifier = ">=2.9.11" }, { name = "py-spy", marker = "extra == 'dev'" }, @@ -6944,6 +6946,22 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/bf/18/72c216f4ab0c82b907009668f79183ae029116ff0dd245d56ef58aac48e7/polars_runtime_32-1.38.1-cp310-abi3-win_arm64.whl", hash = "sha256:6d07d0cc832bfe4fb54b6e04218c2c27afcfa6b9498f9f6bbf262a00d58cc7c4", size = 41639413, upload-time = "2026-02-06T18:12:22.044Z" }, ] +[[package]] +name = "portal" +version = "3.7.4" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cloudpickle" }, + { name = "msgpack" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "psutil" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/57/11/c67a1b771901e4c941fe3dcda763b78a29b6c45308e3ebaf99bac96820d8/portal-3.7.4.tar.gz", hash = "sha256:67234267d1eb319fe790653822d4a8d0e0e5312fb29fd8f440d8287066f478b9", size = 17380, upload-time = "2026-01-12T18:17:45.727Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c3/14/0f7d227894831d2d7eb7f2c6946e8cad8e86da6135b6f902bb961d948f04/portal-3.7.4-py3-none-any.whl", hash = "sha256:3801a489766d3ec2eb73ca8cefd29c54e166d4cf5cfdf1a079ac93fe1130bedb", size = 23486, upload-time = "2026-01-12T18:17:44.326Z" }, +] + [[package]] name = "portalocker" version = "3.2.0"