diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..80f0d72a46 --- /dev/null +++ b/CONTEXT.md @@ -0,0 +1,21 @@ +# DimOS Robotics Context + +DimOS describes robots, actuators, and control surfaces using precise robotics terminology. This glossary records domain language only, not implementation details. + +## Language + +**Damiao-based Robot**: +A robot whose joints are actuated by one or more Damiao motors, possibly spread across multiple CAN buses and physical limbs. +_Avoid_: Damiao arm when the robot may contain multiple motor groups + +**Damiao Joint Group**: +An ordered set of Damiao-driven joints that forms a meaningful physical group such as an arm, torso, or other controllable body section. +_Avoid_: Arm when the group is not necessarily an arm + +**Damiao Bus**: +A named communication channel used by a Damiao-based Robot to reach one or more Damiao motors. +_Avoid_: Treating a bus as owned by a single joint group when multiple groups may share a channel + +**OpenArm**: +An OpenArm robot configuration built from Damiao motors, with OpenArm-specific joints, side naming, limits, and robot description. +_Avoid_: Damiao robot when referring to OpenArm-specific geometry or naming diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 5e5bb2a3da..05cecda3c7 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -281,16 +281,17 @@ 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 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) + mode: Control mode (ignored — twist bases always use velocity) Returns: True if command was sent successfully """ + del mode # Update last commanded for joints we received for joint_name, value in commands.items(): if joint_name in self._last_commanded: @@ -390,15 +391,7 @@ 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 per-joint PD gains. - - Only POSITION / SERVO_POSITION are supported; other modes are warned - and dropped (matches ConnectedHardware's warn-and-skip pattern). - Per-joint kp/kd come from ``component.wb_config`` (resolved in - ``__init__``); fall back to ``_DEFAULT_KP``/``_DEFAULT_KD`` when - the blueprint didn't supply gains. - """ - from dimos.hardware.whole_body.spec import MotorCommand + """Dispatch coordinator joint-position commands by control mode.""" if mode not in (ControlMode.POSITION, ControlMode.SERVO_POSITION): logger.warning( @@ -406,13 +399,23 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: f"got {mode.name} — skipping" ) return False + return self.write_position(commands) + + def write_position(self, commands: dict[str, float]) -> bool: + """Write named position commands using native adapter position IO when available. + + Unknown joints are warned once and ignored. Partial commands hold the + previous commanded value for omitted joints. The hold-last cache is + committed only after the underlying adapter accepts the full frame. + """ if not self._initialized and not self._try_initialize_last_commanded(): return False + candidate = dict(self._last_commanded) for joint_name, value in commands.items(): if joint_name in self._joint_names: - self._last_commanded[joint_name] = value + candidate[joint_name] = value elif joint_name not in self._warned_unknown_joints: logger.warning( f"WholeBody {self.hardware_id} received command for unknown joint " @@ -420,9 +423,19 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: ) self._warned_unknown_joints.add(joint_name) + positions = [candidate[name] for name in self._joint_names] + write_joint_positions = getattr(self._wb_adapter, "write_joint_positions", None) + if callable(write_joint_positions): + ok = bool(write_joint_positions(positions)) + if ok: + self._last_commanded = candidate + return ok + + from dimos.hardware.whole_body.spec import MotorCommand + motor_cmds = [ MotorCommand( - q=self._last_commanded[name], + q=candidate[name], dq=0.0, kp=self._kp_by_name[name], kd=self._kd_by_name[name], @@ -430,7 +443,10 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: ) for name in self._joint_names ] - return self._wb_adapter.write_motor_commands(motor_cmds) + ok = self._wb_adapter.write_motor_commands(motor_cmds) + if ok: + self._last_commanded = candidate + return ok def write_motor_commands(self, commands: list[MotorCommand]) -> bool: """Direct pass-through to adapter for full MotorCommand control.""" @@ -441,6 +457,12 @@ def _try_initialize_last_commanded(self) -> bool: if not self._wb_adapter.has_motor_states(): return False states = self._wb_adapter.read_motor_states() + if len(states) != len(self._joint_names): + logger.warning( + f"WholeBody {self.hardware_id} read {len(states)} motor states for " + f"{len(self._joint_names)} joints; skipping command initialization" + ) + return False for i, name in enumerate(self._joint_names): self._last_commanded[name] = states[i].q self._initialized = True diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index ae6bc1e9de..f9b7a30f9f 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -18,15 +18,17 @@ import threading import time +from typing import cast from unittest.mock import MagicMock import pytest -from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.components import HardwareComponent, HardwareType, TaskName, make_joints from dimos.control.coordinator import ControlCoordinator from dimos.control.hardware_interface import ConnectedHardware from dimos.control.task import ( ControlMode, + ControlTask, CoordinatorState, JointCommandOutput, JointStateSnapshot, @@ -469,7 +471,7 @@ def test_tick_loop_starts_and_stops(self, mock_adapter): ) hw = ConnectedHardware(mock_adapter, component) hardware = {"arm": hw} - tasks: dict = {} + tasks: dict[TaskName, ControlTask] = {} joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} tick_loop = TickLoop( @@ -512,7 +514,7 @@ def test_tick_loop_calls_compute(self, mock_adapter): mode=ControlMode.POSITION, ) - tasks = {"test_task": mock_task} + tasks: dict[TaskName, ControlTask] = {"test_task": cast("ControlTask", mock_task)} joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} tick_loop = TickLoop( @@ -546,7 +548,7 @@ def test_full_trajectory_execution(self, mock_adapter): priority=10, ) traj_task = JointTrajectoryTask(name="traj_arm", config=config) - tasks = {"traj_arm": traj_task} + tasks: dict[TaskName, ControlTask] = {"traj_arm": traj_task} joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} diff --git a/dimos/control/test_hardware_interface.py b/dimos/control/test_hardware_interface.py new file mode 100644 index 0000000000..13767b17ad --- /dev/null +++ b/dimos/control/test_hardware_interface.py @@ -0,0 +1,107 @@ +# 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. + +from __future__ import annotations + +from dimos.control.components import HardwareComponent, HardwareType +from dimos.control.hardware_interface import ConnectedWholeBody +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState + + +class _NativePositionWholeBodyAdapter: + def __init__(self) -> None: + self.accept = False + self.position_writes: list[list[float]] = [] + + def connect(self) -> bool: + return True + + def disconnect(self) -> None: + return None + + def is_connected(self) -> bool: + return True + + def read_motor_states(self) -> list[MotorState]: + return [MotorState(q=0.0), MotorState(q=0.0)] + + def has_motor_states(self) -> bool: + return True + + def read_imu(self) -> IMUState: + return IMUState() + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + raise AssertionError("native position path should not call write_motor_commands") + + def write_joint_positions(self, positions: list[float]) -> bool: + self.position_writes.append(list(positions)) + return self.accept + + +class _MotorCommandWholeBodyAdapter: + def __init__(self) -> None: + self.motor_writes: list[list[MotorCommand]] = [] + + def connect(self) -> bool: + return True + + def disconnect(self) -> None: + return None + + def is_connected(self) -> bool: + return True + + def read_motor_states(self) -> list[MotorState]: + return [MotorState(q=0.0), MotorState(q=0.0)] + + def has_motor_states(self) -> bool: + return True + + def read_imu(self) -> IMUState: + return IMUState() + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + self.motor_writes.append(commands) + return True + + +def _component() -> HardwareComponent: + return HardwareComponent( + hardware_id="body", + hardware_type=HardwareType.WHOLE_BODY, + joints=["body/j1", "body/j2"], + ) + + +def test_whole_body_write_position_uses_native_adapter_and_commits_only_on_success() -> None: + adapter = _NativePositionWholeBodyAdapter() + connected = ConnectedWholeBody(adapter=adapter, component=_component()) + + assert connected.write_position({"body/j1": 2.0}) is False + adapter.accept = True + assert connected.write_position({"body/j2": 3.0}) is True + + assert adapter.position_writes == [[2.0, 0.0], [0.0, 3.0]] + + +def test_whole_body_write_position_falls_back_to_motor_commands() -> None: + adapter = _MotorCommandWholeBodyAdapter() + connected = ConnectedWholeBody(adapter=adapter, component=_component()) + + assert connected.write_position({"body/j1": 1.0}) is True + + commands = adapter.motor_writes[-1] + assert [command.q for command in commands] == [1.0, 0.0] + assert [command.dq for command in commands] == [0.0, 0.0] diff --git a/dimos/hardware/damiao/__init__.py b/dimos/hardware/damiao/__init__.py new file mode 100644 index 0000000000..dc923c1c3e --- /dev/null +++ b/dimos/hardware/damiao/__init__.py @@ -0,0 +1,38 @@ +# 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. + +"""Shared Damiao actuator/runtime adapters.""" + +from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter +from dimos.hardware.damiao.runtime import DamiaoBindingUnavailableError, DamiaoRobotRuntime +from dimos.hardware.damiao.specs import ( + DamiaoArmSpec, + DamiaoBusSpec, + DamiaoJointGroupSpec, + DamiaoMotorSpec, + DamiaoRobotSpec, +) +from dimos.hardware.damiao.whole_body_adapter import DamiaoWholeBodyAdapter + +__all__ = [ + "DamiaoArmAdapter", + "DamiaoArmSpec", + "DamiaoBindingUnavailableError", + "DamiaoBusSpec", + "DamiaoJointGroupSpec", + "DamiaoMotorSpec", + "DamiaoRobotRuntime", + "DamiaoRobotSpec", + "DamiaoWholeBodyAdapter", +] diff --git a/dimos/hardware/damiao/arm_adapter.py b/dimos/hardware/damiao/arm_adapter.py new file mode 100644 index 0000000000..23de06bc71 --- /dev/null +++ b/dimos/hardware/damiao/arm_adapter.py @@ -0,0 +1,408 @@ +# 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. + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np + +from dimos.hardware.damiao.runtime import ( + _DEFAULT_ADDRESS, + _DEFAULT_STATE_CACHE_TTL_S, + _DEFAULT_TICK_DEADLINE_US, + DamiaoBindingUnavailableError, + DamiaoRobotRuntime, +) +from dimos.hardware.damiao.specs import DamiaoArmSpec, DamiaoRobotSpec +from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +_CONTROL_MODE_INDEX = {mode: index for index, mode in enumerate(ControlMode)} + + +def _dynamic_attr(value: object, name: str) -> Any: + return getattr(value, name) + + +class DamiaoArmAdapter: + """ManipulatorAdapter facade over one Damiao joint group.""" + + _adapter_type: str = "damiao" + _binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError + _supported_control_modes: tuple[ControlMode, ...] = ( + ControlMode.POSITION, + ControlMode.SERVO_POSITION, + ControlMode.TORQUE, + ) + + def __init__( + self, + *, + robot_spec: DamiaoRobotSpec, + group_name: str, + dof: int | None = None, + hardware_id: str = "arm", + kp: list[float] | None = None, + kd: list[float] | None = None, + gravity_comp: bool = True, + gravity_model_path: str | Path | None = None, + gravity_torque_limits: list[float] | tuple[float, ...] | None = None, + supported_control_modes: tuple[ControlMode, ...] | None = None, + use_mock_bus: bool = False, + config_path: str | Path | None = None, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + ) -> None: + robot_spec.validate() + if group_name not in robot_spec.groups: + raise ValueError(f"unknown Damiao group {group_name!r}") + group_spec = robot_spec.groups[group_name] + if dof is not None and dof != group_spec.dof: + raise ValueError( + f"{type(self).__name__} only supports {group_spec.dof} DOF (got {dof})" + ) + self._robot_spec = robot_spec + self._group_name = group_name + self._group_spec = group_spec + self._hardware_id = hardware_id + self._dof = group_spec.dof + self._position_lower = list(group_spec.position_lower) + self._position_upper = list(group_spec.position_upper) + self._velocity_max = list(group_spec.velocity_max) + self._kp = list(kp) if kp is not None else list(group_spec.kp) + self._kd = list(kd) if kd is not None else list(group_spec.kd) + self._validate_length("kp", self._kp) + self._validate_length("kd", self._kd) + self._gravity_comp = gravity_comp + resolved_gravity_model = ( + gravity_model_path if gravity_model_path is not None else group_spec.gravity_model_path + ) + self._gravity_model_path = str(resolved_gravity_model) if resolved_gravity_model else None + resolved_torque_limits = ( + gravity_torque_limits + if gravity_torque_limits is not None + else group_spec.gravity_torque_limits + ) + self._gravity_torque_limits = ( + list(resolved_torque_limits) if resolved_torque_limits else None + ) + if self._gravity_torque_limits is not None: + self._validate_length("gravity_torque_limits", self._gravity_torque_limits) + self._supported_control_modes = ( + supported_control_modes or type(self)._supported_control_modes + ) + self._control_mode = ControlMode.POSITION + self._last_positions: list[float] | None = None + self._pin_model: object | None = None + self._pin_data: object | None = None + self._use_mock_bus = use_mock_bus + self._config_path = config_path + self._tick_deadline_us = tick_deadline_us + self._state_cache_ttl_s = state_cache_ttl_s + self._runtime: DamiaoRobotRuntime | None = None + self._connected = False + self._enabled = False + + @classmethod + def from_arm_spec( + cls, + *, + arm_spec: DamiaoArmSpec, + address: str | Path | None = _DEFAULT_ADDRESS, + **kwargs: Any, + ) -> DamiaoArmAdapter: + """Build a one-group adapter from a compatibility arm spec.""" + + robot_spec = DamiaoRobotSpec.from_arm_spec( + arm_spec, + address=str(address) if address is not None else _DEFAULT_ADDRESS, + ) + return cls(robot_spec=robot_spec, group_name=arm_spec.arm_name, **kwargs) + + def _create_runtime(self) -> DamiaoRobotRuntime: + return DamiaoRobotRuntime( + robot_spec=self._robot_spec, + adapter_type=self._adapter_type, + binding_error_type=self._binding_error_type, + use_mock_bus=self._use_mock_bus, + config_path=self._config_path, + tick_deadline_us=self._tick_deadline_us, + state_cache_ttl_s=self._state_cache_ttl_s, + ) + + def _validate_length(self, name: str, values: list[float]) -> None: + if len(values) != self._dof: + raise ValueError(f"{name} length {len(values)} does not match dof {self._dof}") + + def _validate_command_lengths(self, **commands: list[float]) -> None: + for name, values in commands.items(): + self._validate_length(name, values) + + def _zero_vector(self) -> list[float]: + return [0.0] * self._dof + + def connect(self) -> bool: + try: + runtime = self._create_runtime() + if not runtime.connect(): + return False + self._runtime = runtime + self._load_gravity_model() + self._connected = True + self.refresh_state(force=True) + except self._binding_error_type: + raise + except Exception: + logger.exception( + "damiao arm adapter connect failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) + self.disconnect() + return False + return True + + def disconnect(self) -> None: + if self._runtime is not None: + self._runtime.disconnect() + self._runtime = None + self._connected = False + self._enabled = False + + def is_connected(self) -> bool: + return self._connected + + def activate(self) -> bool: + return self.write_enable(True) + + def deactivate(self) -> bool: + stopped = self.write_stop() + disabled = self.write_enable(False) + return stopped and disabled + + def get_info(self) -> ManipulatorInfo: + return ManipulatorInfo( + vendor=self._robot_spec.vendor, + model=self._robot_spec.model, + dof=self._dof, + firmware_version=None, + serial_number=None, + ) + + def get_dof(self) -> int: + return self._dof + + def get_limits(self) -> JointLimits: + return JointLimits( + position_lower=list(self._position_lower), + position_upper=list(self._position_upper), + velocity_max=list(self._velocity_max), + ) + + def set_control_mode(self, mode: ControlMode) -> bool: + if mode not in self._supported_control_modes: + return False + self._control_mode = mode + return True + + def get_control_mode(self) -> ControlMode: + return self._control_mode + + def read_enabled(self) -> bool: + return self._enabled + + def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float], list[float]]: + if self._runtime is None: + raise RuntimeError(f"{type(self).__name__} is not connected") + state = self._runtime.refresh_group_state(self._group_name, force=force) + self._last_positions = list(state.q) + return list(state.q), list(state.dq), list(state.tau) + + def read_joint_positions(self) -> list[float]: + return list(self.refresh_state()[0]) + + def read_joint_velocities(self) -> list[float]: + return list(self.refresh_state()[1]) + + def read_joint_efforts(self) -> list[float]: + return list(self.refresh_state()[2]) + + def read_state(self) -> dict[str, int]: + return {"state": 1 if self._enabled else 0, "mode": _CONTROL_MODE_INDEX[self._control_mode]} + + def read_error(self) -> tuple[int, str]: + return 0, "" + + def read_cartesian_position(self) -> dict[str, float] | None: + return None + + def write_cartesian_position(self, pose: dict[str, float], velocity: float = 1.0) -> bool: + return False + + def read_gripper_position(self) -> float | None: + return None + + def write_gripper_position(self, position: float) -> bool: + return False + + def read_force_torque(self) -> list[float] | None: + return None + + def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: + if self._runtime is None or not self._enabled or len(positions) != self._dof: + return False + velocity = max(0.0, min(1.0, velocity)) + if self._gravity_comp: + try: + tau = self.compute_gravity_torques(self.read_joint_positions()) + except RuntimeError: + logger.warning( + "damiao arm adapter dropping gravity feed-forward; state read failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, + ) + tau = self._zero_vector() + else: + tau = self._zero_vector() + return self.write_mit_commands( + q=list(positions), + dq=self._zero_vector(), + kp=[kp * velocity for kp in self._kp], + kd=list(self._kd), + tau=tau, + ) + + def write_joint_velocities(self, velocities: list[float]) -> bool: + return False + + def write_joint_torques(self, efforts: list[float]) -> bool: + if self._runtime is None or not self._enabled or len(efforts) != self._dof: + return False + q = ( + self._last_positions + if self._last_positions is not None + else self.read_joint_positions() + ) + return self.write_mit_commands( + q=q, + dq=self._zero_vector(), + kp=self._zero_vector(), + kd=self._zero_vector(), + tau=efforts, + ) + + def write_mit_commands( + self, + *, + q: list[float], + dq: list[float], + kp: list[float], + kd: list[float], + tau: list[float], + ) -> bool: + if self._runtime is None or not self._enabled: + return False + self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) + ok = self._runtime.write_group_mit_commands( + group_name=self._group_name, + q=q, + dq=dq, + kp=kp, + kd=kd, + tau=tau, + ) + if ok: + self._last_positions = list(q) + self._control_mode = ( + ControlMode.TORQUE if all(k == 0.0 for k in kp) else ControlMode.POSITION + ) + return ok + + def write_stop(self) -> bool: + if self._runtime is None: + return False + if self._gravity_comp and self._enabled: + try: + q_now = self.read_joint_positions() + except RuntimeError: + self._runtime.disable() + self._enabled = False + return False + return self.write_mit_commands( + q=q_now, + dq=self._zero_vector(), + kp=list(self._kp), + kd=list(self._kd), + tau=self.compute_gravity_torques(q_now), + ) + disabled = self._runtime.disable() + if disabled: + self._enabled = False + return disabled + + def write_enable(self, enable: bool) -> bool: + if self._runtime is None: + return False + ok = self._runtime.enable() if enable else self._runtime.disable() + if not ok: + return False + self._enabled = enable + if enable: + positions = self.read_joint_positions() + if not self.write_joint_positions(positions): + self._runtime.disable() + self._enabled = False + return False + return True + + def write_clear_errors(self) -> bool: + if self._runtime is None: + return False + if not self._runtime.disable() or not self._runtime.enable(): + return False + self._enabled = True + return self.write_joint_positions(self.read_joint_positions()) + + def _load_gravity_model(self) -> None: + if not self._gravity_comp or self._gravity_model_path is None or self._runtime is None: + return + loaded = self._runtime.load_gravity_model(self._group_name, self._gravity_model_path) + if loaded is not None: + self._pin_model, self._pin_data = loaded + + def compute_gravity_torques(self, q: list[float]) -> list[float]: + self._validate_length("q", q) + if self._pin_model is None or self._pin_data is None: + return self._zero_vector() + import pinocchio # type: ignore[import-not-found] + + compute_generalized_gravity = _dynamic_attr(pinocchio, "computeGeneralizedGravity") + tau = compute_generalized_gravity( + self._pin_model, self._pin_data, np.array(q, dtype=np.float64) + ) + values = [float(tau[i]) for i in range(self._dof)] + if self._gravity_torque_limits is None: + return values + return [ + float(np.clip(value, -limit, limit)) + for value, limit in zip(values, self._gravity_torque_limits, strict=False) + ] + + +__all__ = ["DamiaoArmAdapter"] diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py new file mode 100644 index 0000000000..cf431b005d --- /dev/null +++ b/dimos/hardware/damiao/runtime.py @@ -0,0 +1,379 @@ +# 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. + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +import importlib +from pathlib import Path +import time +from typing import Any, cast + +import numpy as np + +from dimos.hardware.damiao.specs import DamiaoJointGroupSpec, DamiaoRobotSpec +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +_DEFAULT_TICK_DEADLINE_US = 1_000 +_DEFAULT_STATE_CACHE_TTL_S = 0.002 +_DEFAULT_ADDRESS = "can0" + + +class DamiaoBindingUnavailableError(RuntimeError): + """Raised when the optional can_motor_control binding is unavailable.""" + + +@dataclass(frozen=True) +class DamiaoGroupState: + """State vectors for one Damiao joint group.""" + + q: list[float] + dq: list[float] + tau: list[float] + + +def _load_can_motor_control( + *, + adapter_type: str, + error_type: type[RuntimeError] = DamiaoBindingUnavailableError, +) -> tuple[Any, Any]: + """Lazily load the optional Rust-backed binding and Damiao codec module.""" + + try: + can_motor_control = importlib.import_module("can_motor_control") + damiao = importlib.import_module("can_motor_control.damiao") + except ImportError as exc: + raise error_type( + f"The selected '{adapter_type}' adapter requires the Rust-backed " + "can-motor-control Python binding in the active environment. Install " + f"dimos[manipulation] before selecting adapter_type='{adapter_type}'." + ) from exc + return can_motor_control, damiao + + +def _dynamic_attr(value: object, name: str) -> Any: + return getattr(value, name) + + +class DamiaoRobotRuntime: + """Binding-backed runtime for one Damiao-based robot spec.""" + + def __init__( + self, + *, + robot_spec: DamiaoRobotSpec, + adapter_type: str = "damiao", + binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError, + use_mock_bus: bool = False, + config_path: str | Path | None = None, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + ) -> None: + robot_spec.validate() + self._robot_spec = robot_spec + self._adapter_type = adapter_type + self._binding_error_type = binding_error_type + self._use_mock_bus = use_mock_bus + self._config_path = str(config_path) if config_path is not None else None + self._tick_deadline_us = tick_deadline_us + self._state_cache_ttl_s = state_cache_ttl_s + self._robot: Any | None = None + self._groups: dict[str, Any] = {} + self._state_cache: dict[str, DamiaoGroupState] = {} + self._state_cache_time: dict[str, float] = {} + self._can_motor_control: Any | None = None + self._damiao: Any | None = None + self._connected = False + self._enabled = False + + @property + def robot_spec(self) -> DamiaoRobotSpec: + return self._robot_spec + + def connect(self) -> bool: + """Connect the binding robot and cache group handles.""" + + try: + self._can_motor_control, self._damiao = _load_can_motor_control( + adapter_type=self._adapter_type, + error_type=self._binding_error_type, + ) + robot = self._build_robot() + robot.connect() + groups: dict[str, Any] = {} + for group_name, group_spec in self._robot_spec.groups.items(): + group = robot[group_name] + if len(group) != group_spec.dof: + raise RuntimeError( + f"can_motor_control group {group_name!r} has {len(group)} joints, " + f"expected {group_spec.dof}" + ) + groups[group_name] = group + self._robot = robot + self._groups = groups + self._connected = True + for group_name in self._robot_spec.groups: + self.refresh_group_state(group_name, force=True) + except self._binding_error_type: + raise + except Exception: + logger.exception("damiao runtime connect failed", adapter=self._adapter_type) + self.disconnect() + return False + return True + + def _build_robot(self) -> Any: + if self._can_motor_control is None or self._damiao is None: + raise RuntimeError("can_motor_control binding is not loaded") + if self._config_path is not None: + return self._can_motor_control.Robot.from_config(self._config_path) + builder = self._can_motor_control.Robot.builder() + codec = self._damiao.DamiaoCodec() + for bus_name, bus_spec in self._robot_spec.buses.items(): + address = str(bus_spec.address or _DEFAULT_ADDRESS) + transport = ( + self._can_motor_control.MockCanBus.new_fd(address) + if self._use_mock_bus and bus_spec.fd + else self._can_motor_control.MockCanBus(address) + if self._use_mock_bus + else self._can_motor_control.SocketCanBus(address, fd=bus_spec.fd) + ) + builder = builder.add_bus(bus_name, transport, codec) + for group_name, group_spec in self._robot_spec.groups.items(): + binding_specs = [ + self._can_motor_control.MotorSpec( + motor.name, + cast("int", self._resolve_motor_type(motor.type)), + motor.send_id, + motor.effective_recv_id, + ) + for motor in group_spec.motors + ] + builder = builder.add_arm(group_name, bus=group_spec.bus_name, motors=binding_specs) + return builder.build() + + def _resolve_motor_type(self, motor_type: object) -> object: + if self._damiao is None: + raise RuntimeError("Damiao binding module is not loaded") + if isinstance(motor_type, str): + try: + return getattr(self._damiao.MotorType, motor_type) + except AttributeError as exc: + raise ValueError(f"Unknown Damiao motor type {motor_type!r}") from exc + if not isinstance(motor_type, int): + return motor_type + for name in dir(self._damiao.MotorType): + if name.startswith("_"): + continue + candidate = getattr(self._damiao.MotorType, name) + try: + candidate_value = int(candidate) + except (TypeError, ValueError): + continue + if candidate_value == motor_type: + return candidate + raise ValueError(f"Unknown Damiao motor type value {motor_type!r}") + + def disconnect(self) -> None: + """Disable and drop the underlying binding robot.""" + + if self._robot is not None: + try: + self._robot.disable() + except Exception: + logger.warning("damiao runtime disable on disconnect failed", exc_info=True) + self._enabled = False + self._connected = False + self._robot = None + self._groups = {} + self._state_cache = {} + self._state_cache_time = {} + + def is_connected(self) -> bool: + return self._connected + + def enable(self) -> bool: + if self._robot is None: + return False + try: + self._robot.enable() + except Exception: + logger.exception("damiao runtime enable failed", adapter=self._adapter_type) + return False + self._enabled = True + return True + + def disable(self) -> bool: + if self._robot is None: + return False + try: + self._robot.disable() + except Exception: + logger.exception("damiao runtime disable failed", adapter=self._adapter_type) + return False + self._enabled = False + return True + + def is_enabled(self) -> bool: + return self._enabled + + def group_spec(self, group_name: str) -> DamiaoJointGroupSpec: + try: + return self._robot_spec.groups[group_name] + except KeyError as exc: + raise ValueError(f"unknown Damiao group {group_name!r}") from exc + + def refresh_group_state(self, group_name: str, *, force: bool = False) -> DamiaoGroupState: + group_spec = self.group_spec(group_name) + group = self._groups.get(group_name) + if self._robot is None or group is None: + raise RuntimeError("DamiaoRobotRuntime is not connected") + now = time.monotonic() + cached = self._state_cache.get(group_name) + cached_at = self._state_cache_time.get(group_name, 0.0) + if not force and cached is not None and now - cached_at <= self._state_cache_ttl_s: + return cached + group.refresh() + self._robot.tick(self._tick_deadline_us) + state = DamiaoGroupState( + q=group.positions().astype(np.float64).tolist(), + dq=group.velocities().astype(np.float64).tolist(), + tau=group.torques().astype(np.float64).tolist(), + ) + if any(len(values) != group_spec.dof for values in (state.q, state.dq, state.tau)): + raise RuntimeError( + f"state length does not match configured DOF for group {group_name!r}" + ) + self._state_cache[group_name] = state + self._state_cache_time[group_name] = time.monotonic() + return state + + def has_group_states(self, group_names: Sequence[str]) -> bool: + """Return true only when every requested group has a fresh complete state.""" + + try: + for group_name in group_names: + self.refresh_group_state(group_name, force=False) + except Exception: + return False + return True + + def read_group_states(self, group_names: Sequence[str]) -> list[DamiaoGroupState]: + """Read state for groups in the requested order.""" + + return [self.refresh_group_state(group_name, force=False) for group_name in group_names] + + def write_group_mit_commands( + self, + *, + group_name: str, + q: Sequence[float], + dq: Sequence[float], + kp: Sequence[float], + kd: Sequence[float], + tau: Sequence[float], + ) -> bool: + """Write one MIT command frame to a group.""" + + group_spec = self.group_spec(group_name) + group = self._groups.get(group_name) + if self._robot is None or group is None or not self._enabled: + return False + if any(len(values) != group_spec.dof for values in (q, dq, kp, kd, tau)): + raise ValueError( + f"command length does not match configured DOF for group {group_name!r}" + ) + try: + group.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) + self._robot.tick(self._tick_deadline_us) + except Exception: + logger.exception("damiao runtime MIT command failed", group_name=group_name) + return False + self._state_cache.pop(group_name, None) + self._state_cache_time.pop(group_name, None) + return True + + def write_groups_mit_commands( + self, + commands: Mapping[ + str, + tuple[ + Sequence[float], Sequence[float], Sequence[float], Sequence[float], Sequence[float] + ], + ], + ) -> bool: + """Stage MIT commands for multiple groups and tick once. + + The binding's group ``mit_control`` call stages commands; ``robot.tick`` + sends them. Validate all groups and command lengths before staging so a + bad frame is rejected without sending a partial whole-body command. + """ + + if self._robot is None or not self._enabled: + return False + for group_name, values in commands.items(): + group_spec = self.group_spec(group_name) + group = self._groups.get(group_name) + if group is None: + return False + q, dq, kp, kd, tau = values + if any(len(vector) != group_spec.dof for vector in (q, dq, kp, kd, tau)): + raise ValueError( + f"command length does not match configured DOF for group {group_name!r}" + ) + try: + for group_name, values in commands.items(): + q, dq, kp, kd, tau = values + self._groups[group_name].mit_control( + np.column_stack([kp, kd, q, dq, tau]).astype(np.float64) + ) + self._robot.tick(self._tick_deadline_us) + except Exception: + logger.exception("damiao runtime batched MIT command failed") + return False + for group_name in commands: + self._state_cache.pop(group_name, None) + self._state_cache_time.pop(group_name, None) + return True + + def load_gravity_model( + self, + group_name: str, + model_path: str | Path | None = None, + ) -> tuple[object, object] | None: + """Load a Pinocchio gravity model for a configured group, if present.""" + + resolved_model_path = ( + model_path if model_path is not None else self.group_spec(group_name).gravity_model_path + ) + if resolved_model_path is None: + return None + import pinocchio # type: ignore[import-not-found] + + build_model_from_urdf = _dynamic_attr(pinocchio, "buildModelFromUrdf") + model = build_model_from_urdf(str(resolved_model_path)) + return model, _dynamic_attr(model, "createData")() + + +__all__ = [ + "_DEFAULT_ADDRESS", + "_DEFAULT_STATE_CACHE_TTL_S", + "_DEFAULT_TICK_DEADLINE_US", + "DamiaoBindingUnavailableError", + "DamiaoGroupState", + "DamiaoRobotRuntime", +] diff --git a/dimos/hardware/damiao/specs.py b/dimos/hardware/damiao/specs.py new file mode 100644 index 0000000000..f67d6a0ecf --- /dev/null +++ b/dimos/hardware/damiao/specs.py @@ -0,0 +1,316 @@ +# 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. + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from pathlib import Path + + +@dataclass(frozen=True) +class DamiaoMotorSpec: + """Typed metadata for one Damiao motor in adapter joint order.""" + + name: str + type: object + send_id: int + recv_id: int | None = None + + @property + def effective_recv_id(self) -> int: + """Return the explicit receive CAN ID, or Damiao's default response ID.""" + + return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) + + +@dataclass(frozen=True) +class DamiaoBusSpec: + """Named communication channel for Damiao motors.""" + + address: str | Path = "can0" + fd: bool = False + + +@dataclass(frozen=True) +class DamiaoJointGroupSpec: + """Ordered Damiao joints forming a controllable physical group.""" + + bus_name: str + motors: tuple[DamiaoMotorSpec, ...] + position_lower: tuple[float, ...] + position_upper: tuple[float, ...] + velocity_max: tuple[float, ...] + kp: tuple[float, ...] + kd: tuple[float, ...] + gravity_model_path: str | Path | None = None + gravity_torque_limits: tuple[float, ...] | None = None + supports_velocity: bool = False + + @property + def dof(self) -> int: + """Return the number of joints described by this group spec.""" + + return len(self.motors) + + @property + def joint_names(self) -> tuple[str, ...]: + """Return joint names in command-vector order.""" + + return tuple(motor.name for motor in self.motors) + + def validate(self, *, group_name: str, bus_names: set[str] | None = None) -> None: + """Validate per-group metadata and optional bus reference.""" + + if not self.motors: + raise ValueError(f"DamiaoJointGroupSpec {group_name!r} requires at least one motor") + if bus_names is not None and self.bus_name not in bus_names: + raise ValueError(f"group {group_name!r} references unknown bus {self.bus_name!r}") + send_ids = [motor.send_id for motor in self.motors] + if len(set(send_ids)) != len(send_ids): + raise ValueError(f"duplicate send_id in group {group_name!r}: {send_ids}") + recv_ids = [motor.effective_recv_id for motor in self.motors] + if len(set(recv_ids)) != len(recv_ids): + raise ValueError(f"duplicate recv_id in group {group_name!r}: {recv_ids}") + joint_names = [motor.name for motor in self.motors] + if len(set(joint_names)) != len(joint_names): + raise ValueError(f"duplicate joint name in group {group_name!r}: {joint_names}") + for name, values in { + "position_lower": self.position_lower, + "position_upper": self.position_upper, + "velocity_max": self.velocity_max, + "kp": self.kp, + "kd": self.kd, + }.items(): + if len(values) != self.dof: + raise ValueError( + f"{name} length {len(values)} does not match dof {self.dof} " + f"for group {group_name!r}" + ) + for index, (lower, upper) in enumerate( + zip(self.position_lower, self.position_upper, strict=True), + ): + if lower > upper: + raise ValueError( + f"position_lower[{index}] > position_upper[{index}] for group {group_name!r}" + ) + if self.gravity_torque_limits is not None and len(self.gravity_torque_limits) != self.dof: + raise ValueError( + f"gravity_torque_limits length does not match dof for group {group_name!r}" + ) + + +@dataclass(frozen=True) +class DamiaoRobotSpec: + """Python-native Damiao robot config with named buses and joint groups.""" + + name: str + vendor: str + model: str + buses: Mapping[str, DamiaoBusSpec] + groups: Mapping[str, DamiaoJointGroupSpec] + requires_binding: bool = False + + @property + def joint_names(self) -> tuple[str, ...]: + """Return all group joint names in mapping iteration order.""" + + return tuple(joint for group in self.groups.values() for joint in group.joint_names) + + def group_joint_names(self, group_names: Sequence[str]) -> tuple[str, ...]: + """Return concatenated joint names for the requested groups.""" + + return tuple( + joint for group_name in group_names for joint in self.groups[group_name].joint_names + ) + + def validate(self) -> None: + """Validate bus/group references and global joint-name uniqueness.""" + + if not self.buses: + raise ValueError("DamiaoRobotSpec requires at least one bus") + if not self.groups: + raise ValueError("DamiaoRobotSpec requires at least one joint group") + bus_names = set(self.buses) + all_joint_names: list[str] = [] + ids_by_bus: dict[str, set[int]] = {bus_name: set() for bus_name in bus_names} + for group_name, group in self.groups.items(): + group.validate(group_name=group_name, bus_names=bus_names) + all_joint_names.extend(group.joint_names) + bus_ids = ids_by_bus[group.bus_name] + for motor in group.motors: + if motor.send_id in bus_ids: + raise ValueError(f"duplicate send_id {motor.send_id} on bus {group.bus_name!r}") + bus_ids.add(motor.send_id) + if len(set(all_joint_names)) != len(all_joint_names): + raise ValueError(f"duplicate joint names across DamiaoRobotSpec: {all_joint_names}") + + @classmethod + def from_arm_spec( + cls, + arm_spec: DamiaoArmSpec, + *, + address: str | Path = "can0", + ) -> DamiaoRobotSpec: + """Build a one-group robot spec from a compatibility arm spec.""" + + return cls( + name=arm_spec.name, + vendor=arm_spec.vendor, + model=arm_spec.model, + buses={arm_spec.bus_name: DamiaoBusSpec(address=address, fd=arm_spec.fd)}, + groups={ + arm_spec.arm_name: DamiaoJointGroupSpec( + bus_name=arm_spec.bus_name, + motors=arm_spec.motors, + position_lower=arm_spec.position_lower, + position_upper=arm_spec.position_upper, + velocity_max=arm_spec.velocity_max, + kp=arm_spec.kp, + kd=arm_spec.kd, + gravity_model_path=arm_spec.gravity_model_path, + gravity_torque_limits=arm_spec.gravity_torque_limits, + supports_velocity=arm_spec.supports_velocity, + ) + }, + requires_binding=arm_spec.requires_binding, + ) + + +@dataclass(frozen=True) +class DamiaoArmSpec: + """Compatibility metadata for a single Damiao arm/group adapter.""" + + name: str + vendor: str + model: str + motors: tuple[DamiaoMotorSpec, ...] + position_lower: tuple[float, ...] + position_upper: tuple[float, ...] + velocity_max: tuple[float, ...] + kp: tuple[float, ...] + kd: tuple[float, ...] + gravity_model_path: str | Path | None = None + gravity_torque_limits: tuple[float, ...] | None = None + requires_binding: bool = False + bus_name: str = "can" + arm_name: str = "arm" + fd: bool = False + supports_velocity: bool = False + + @property + def dof(self) -> int: + """Return the number of joints described by this arm spec.""" + + return len(self.motors) + + @property + def joint_names(self) -> tuple[str, ...]: + """Return joint names in adapter and command-vector order.""" + + return tuple(motor.name for motor in self.motors) + + @classmethod + def from_values( + cls, + *, + name: str, + vendor: str, + model: str, + motors: Sequence[Mapping[str, object] | DamiaoMotorSpec], + position_lower: list[float] | tuple[float, ...], + position_upper: list[float] | tuple[float, ...], + velocity_max: list[float] | tuple[float, ...], + kp: list[float] | tuple[float, ...], + kd: list[float] | tuple[float, ...], + gravity_model_path: str | Path | None = None, + gravity_torque_limits: list[float] | tuple[float, ...] | None = None, + requires_binding: bool = False, + bus_name: str = "can", + arm_name: str = "arm", + fd: bool = False, + supports_velocity: bool = False, + ) -> DamiaoArmSpec: + """Build a typed arm spec from list/tuple metadata values.""" + + return cls( + name=name, + vendor=vendor, + model=model, + motors=coerce_motor_specs(motors, len(motors)), + position_lower=tuple(float(value) for value in position_lower), + position_upper=tuple(float(value) for value in position_upper), + velocity_max=tuple(float(value) for value in velocity_max), + kp=tuple(float(value) for value in kp), + kd=tuple(float(value) for value in kd), + gravity_model_path=gravity_model_path, + gravity_torque_limits=( + tuple(float(value) for value in gravity_torque_limits) + if gravity_torque_limits is not None + else None + ), + requires_binding=requires_binding, + bus_name=bus_name, + arm_name=arm_name, + fd=fd, + supports_velocity=supports_velocity, + ) + + def validate(self) -> None: + """Validate CAN ID uniqueness and per-joint metadata lengths.""" + + DamiaoRobotSpec.from_arm_spec(self).validate() + + +def coerce_motor_specs( + motor_specs: Sequence[Mapping[str, object] | DamiaoMotorSpec], + dof: int, +) -> tuple[DamiaoMotorSpec, ...]: + """Normalize mapping or dataclass motor metadata into typed motor specs.""" + + specs: list[DamiaoMotorSpec] = [] + for spec in motor_specs: + if isinstance(spec, DamiaoMotorSpec): + specs.append(spec) + else: + name = spec.get("name") + send_id = spec.get("send_id") + recv_id = spec.get("recv_id") + if not isinstance(name, str): + raise TypeError("motor spec name must be a string") + if not isinstance(send_id, int): + raise TypeError("motor spec send_id must be an integer") + if recv_id is not None and not isinstance(recv_id, int): + raise TypeError("motor spec recv_id must be an integer") + specs.append( + DamiaoMotorSpec( + name=name, + type=spec.get("type"), + send_id=send_id, + recv_id=recv_id, + ) + ) + if len(specs) != dof: + raise ValueError(f"motor_specs length {len(specs)} does not match dof {dof}") + return tuple(specs) + + +__all__ = [ + "DamiaoArmSpec", + "DamiaoBusSpec", + "DamiaoJointGroupSpec", + "DamiaoMotorSpec", + "DamiaoRobotSpec", + "coerce_motor_specs", +] diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py new file mode 100644 index 0000000000..243c82ce44 --- /dev/null +++ b/dimos/hardware/damiao/test_adapters.py @@ -0,0 +1,309 @@ +# 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. + +from __future__ import annotations + +import pytest + +from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter +from dimos.hardware.damiao.runtime import DamiaoGroupState +from dimos.hardware.damiao.specs import ( + DamiaoArmSpec, + DamiaoBusSpec, + DamiaoJointGroupSpec, + DamiaoMotorSpec, + DamiaoRobotSpec, +) +from dimos.hardware.damiao.whole_body_adapter import DamiaoWholeBodyAdapter +from dimos.hardware.manipulators.spec import ControlMode + + +class _FakeRuntime: + def __init__(self, *, fresh: bool = True, write_ok: bool = True) -> None: + self.fresh = fresh + self.write_ok = write_ok + self.connected = False + self.enabled = False + self.disconnect_calls = 0 + self.batched_calls = 0 + self.writes: list[ + tuple[str, list[float], list[float], list[float], list[float], list[float]] + ] = [] + self.loaded_gravity_models: list[tuple[str, str | None]] = [] + self.states = { + "left": DamiaoGroupState(q=[0.1], dq=[0.2], tau=[0.3]), + "right": DamiaoGroupState(q=[-0.1], dq=[-0.2], tau=[-0.3]), + "arm": DamiaoGroupState(q=[0.4, -0.4], dq=[0.5, -0.5], tau=[0.6, -0.6]), + } + + def connect(self) -> bool: + self.connected = True + return True + + def disconnect(self) -> None: + self.disconnect_calls += 1 + self.connected = False + self.enabled = False + + def enable(self) -> bool: + self.enabled = True + return True + + def disable(self) -> bool: + self.enabled = False + return True + + def is_enabled(self) -> bool: + return self.enabled + + def refresh_group_state(self, group_name: str, *, force: bool = False) -> DamiaoGroupState: + del force + return self.states[group_name] + + def has_group_states(self, group_names: tuple[str, ...]) -> bool: + return self.fresh and all(group_name in self.states for group_name in group_names) + + def read_group_states(self, group_names: tuple[str, ...]) -> list[DamiaoGroupState]: + if not self.has_group_states(group_names): + raise RuntimeError("stale state") + return [self.states[group_name] for group_name in group_names] + + def write_group_mit_commands( + self, + *, + group_name: str, + q: list[float], + dq: list[float], + kp: list[float], + kd: list[float], + tau: list[float], + ) -> bool: + if not self.write_ok: + return False + self.writes.append((group_name, list(q), list(dq), list(kp), list(kd), list(tau))) + return True + + def write_groups_mit_commands( + self, + commands: dict[str, tuple[list[float], list[float], list[float], list[float], list[float]]], + ) -> bool: + self.batched_calls += 1 + if not self.write_ok: + return False + for group_name, values in commands.items(): + q, dq, kp, kd, tau = values + self.writes.append((group_name, list(q), list(dq), list(kp), list(kd), list(tau))) + return True + + def load_gravity_model(self, group_name: str, model_path: str | None = None) -> None: + self.loaded_gravity_models.append((group_name, model_path)) + return None + + +def _arm_spec() -> DamiaoArmSpec: + return DamiaoArmSpec( + name="test_damiao", + vendor="Damiao", + model="TestArm", + motors=( + DamiaoMotorSpec("j1", "DM4310", 0x01, 0x11), + DamiaoMotorSpec("j2", "DM4310", 0x02, 0x12), + ), + position_lower=(-1.0, -2.0), + position_upper=(1.0, 2.0), + velocity_max=(3.0, 4.0), + kp=(5.0, 6.0), + kd=(0.1, 0.2), + gravity_torque_limits=(7.0, 8.0), + ) + + +def _whole_body_spec() -> DamiaoRobotSpec: + return DamiaoRobotSpec( + name="test_body", + vendor="Damiao", + model="TestBody", + buses={ + "left_can": DamiaoBusSpec(address="can1", fd=True), + "right_can": DamiaoBusSpec(address="can0", fd=True), + }, + groups={ + "left": DamiaoJointGroupSpec( + bus_name="left_can", + motors=(DamiaoMotorSpec("left_joint", "DM4310", 0x01, 0x11),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(3.0,), + kp=(5.0,), + kd=(0.1,), + ), + "right": DamiaoJointGroupSpec( + bus_name="right_can", + motors=(DamiaoMotorSpec("right_joint", "DM4310", 0x01, 0x11),), + position_lower=(-2.0,), + position_upper=(2.0,), + velocity_max=(4.0,), + kp=(6.0,), + kd=(0.2,), + ), + }, + ) + + +def test_robot_spec_rejects_unknown_group_bus() -> None: + spec = DamiaoRobotSpec( + name="bad", + vendor="Damiao", + model="Bad", + buses={"can": DamiaoBusSpec()}, + groups={ + "arm": DamiaoJointGroupSpec( + bus_name="missing", + motors=(DamiaoMotorSpec("j1", "DM4310", 0x01, 0x11),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(1.0,), + kp=(1.0,), + kd=(0.1,), + ) + }, + ) + + with pytest.raises(ValueError, match="unknown bus"): + spec.validate() + + +def test_robot_spec_rejects_duplicate_send_ids_on_shared_bus() -> None: + spec = DamiaoRobotSpec( + name="bad_ids", + vendor="Damiao", + model="BadIds", + buses={"can": DamiaoBusSpec()}, + groups={ + "left": DamiaoJointGroupSpec( + bus_name="can", + motors=(DamiaoMotorSpec("left_joint", "DM4310", 0x01, 0x11),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(1.0,), + kp=(1.0,), + kd=(0.1,), + ), + "right": DamiaoJointGroupSpec( + bus_name="can", + motors=(DamiaoMotorSpec("right_joint", "DM4310", 0x01, 0x12),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(1.0,), + kp=(1.0,), + kd=(0.1,), + ), + }, + ) + + with pytest.raises(ValueError, match="duplicate send_id 1 on bus 'can'"): + spec.validate() + + +def test_arm_adapter_reports_limits_and_modes() -> None: + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + + assert adapter.get_dof() == 2 + assert adapter.get_limits().position_lower == [-1.0, -2.0] + assert adapter.set_control_mode(ControlMode.TORQUE) is True + assert adapter.set_control_mode(ControlMode.VELOCITY) is False + + +def test_arm_adapter_uses_fake_runtime_for_startup_hold(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + assert adapter.write_enable(True) is True + + assert runtime.writes[-1] == ( + "arm", + [0.4, -0.4], + [0.0, 0.0], + [5.0, 6.0], + [0.1, 0.2], + [0.0, 0.0], + ) + + +def test_arm_adapter_passes_gravity_model_override_to_runtime(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec( + arm_spec=_arm_spec(), + gravity_model_path="override.urdf", + ) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + + assert runtime.loaded_gravity_models == [("arm", "override.urdf")] + + +def test_whole_body_requires_all_group_states(mocker) -> None: + runtime = _FakeRuntime(fresh=False) + adapter = DamiaoWholeBodyAdapter(robot_spec=_whole_body_spec(), group_names=("left", "right")) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is False + assert adapter.has_motor_states() is False + assert adapter.write_joint_positions([0.0, 0.0]) is False + assert runtime.writes == [] + + +def test_whole_body_rejects_out_of_limit_frame_without_partial_send( + mocker, +) -> None: + runtime = _FakeRuntime() + adapter = DamiaoWholeBodyAdapter(robot_spec=_whole_body_spec(), group_names=("left", "right")) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + runtime.writes.clear() + assert adapter.write_joint_positions([0.0, 3.0]) is False + assert runtime.writes == [] + + +def test_whole_body_position_write_splits_groups(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoWholeBodyAdapter(robot_spec=_whole_body_spec(), group_names=("left", "right")) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + runtime.writes.clear() + assert adapter.write_joint_positions([0.5, -0.5]) is True + + assert runtime.batched_calls == 2 + assert runtime.writes == [ + ("left", [0.5], [0.0], [5.0], [0.1], [0.0]), + ("right", [-0.5], [0.0], [6.0], [0.2], [0.0]), + ] + + +def test_whole_body_connect_sends_current_position_hold(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoWholeBodyAdapter(robot_spec=_whole_body_spec(), group_names=("left", "right")) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + + assert runtime.writes == [ + ("left", [0.1], [0.0], [5.0], [0.1], [0.0]), + ("right", [-0.1], [0.0], [6.0], [0.2], [0.0]), + ] diff --git a/dimos/hardware/damiao/whole_body_adapter.py b/dimos/hardware/damiao/whole_body_adapter.py new file mode 100644 index 0000000000..397df28580 --- /dev/null +++ b/dimos/hardware/damiao/whole_body_adapter.py @@ -0,0 +1,251 @@ +# 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. + +from __future__ import annotations + +from collections.abc import Sequence +from typing import Any + +import numpy as np + +from dimos.hardware.damiao.runtime import ( + _DEFAULT_STATE_CACHE_TTL_S, + _DEFAULT_TICK_DEADLINE_US, + DamiaoBindingUnavailableError, + DamiaoRobotRuntime, +) +from dimos.hardware.damiao.specs import DamiaoRobotSpec +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _dynamic_attr(value: object, name: str) -> Any: + return getattr(value, name) + + +class DamiaoWholeBodyAdapter: + """Position-level WholeBodyAdapter facade over ordered Damiao groups.""" + + _adapter_type: str = "damiao_whole_body" + _binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError + + def __init__( + self, + *, + robot_spec: DamiaoRobotSpec, + group_names: Sequence[str], + dof: int | None = None, + hardware_id: str = "body", + gravity_comp: bool = True, + use_mock_bus: bool = False, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + ) -> None: + robot_spec.validate() + if not group_names: + raise ValueError("DamiaoWholeBodyAdapter requires at least one group") + unknown_groups = [name for name in group_names if name not in robot_spec.groups] + if unknown_groups: + raise ValueError(f"unknown Damiao groups: {unknown_groups}") + self._robot_spec = robot_spec + self._group_names = tuple(group_names) + self._hardware_id = hardware_id + self._gravity_comp = gravity_comp + self._use_mock_bus = use_mock_bus + self._tick_deadline_us = tick_deadline_us + self._state_cache_ttl_s = state_cache_ttl_s + self._dof = sum(robot_spec.groups[name].dof for name in self._group_names) + if dof is not None and dof != self._dof: + raise ValueError(f"{type(self).__name__} supports {self._dof} DOF (got {dof})") + self._position_lower = [ + value + for group_name in self._group_names + for value in robot_spec.groups[group_name].position_lower + ] + self._position_upper = [ + value + for group_name in self._group_names + for value in robot_spec.groups[group_name].position_upper + ] + self._kp = [ + value for group_name in self._group_names for value in robot_spec.groups[group_name].kp + ] + self._kd = [ + value for group_name in self._group_names for value in robot_spec.groups[group_name].kd + ] + self._runtime: DamiaoRobotRuntime | None = None + self._connected = False + self._enabled = False + self._pin_models: dict[str, object] = {} + self._pin_data: dict[str, object] = {} + + @property + def joint_names(self) -> tuple[str, ...]: + return self._robot_spec.group_joint_names(self._group_names) + + def _create_runtime(self) -> DamiaoRobotRuntime: + return DamiaoRobotRuntime( + robot_spec=self._robot_spec, + adapter_type=self._adapter_type, + binding_error_type=self._binding_error_type, + use_mock_bus=self._use_mock_bus, + tick_deadline_us=self._tick_deadline_us, + state_cache_ttl_s=self._state_cache_ttl_s, + ) + + def connect(self) -> bool: + try: + runtime = self._create_runtime() + if not runtime.connect(): + return False + self._runtime = runtime + self._load_gravity_models() + self._connected = True + # Whole-body v1 intentionally exposes no optional lifecycle hooks, + # so make the position surface usable after coordinator connect(). + self._enabled = runtime.enable() + if not self._enabled: + self.disconnect() + return False + current_positions = [state.q for state in self.read_motor_states()] + if not self.write_joint_positions(current_positions): + logger.error("damiao whole-body startup hold failed", hardware_id=self._hardware_id) + self.disconnect() + return False + except self._binding_error_type: + raise + except Exception: + logger.exception( + "damiao whole-body adapter connect failed", hardware_id=self._hardware_id + ) + self.disconnect() + return False + return True + + def disconnect(self) -> None: + if self._runtime is not None: + self._runtime.disconnect() + self._runtime = None + self._connected = False + self._enabled = False + self._pin_models = {} + self._pin_data = {} + + def is_connected(self) -> bool: + return self._connected + + def has_motor_states(self) -> bool: + if self._runtime is None: + return False + return self._runtime.has_group_states(self._group_names) + + def read_motor_states(self) -> list[MotorState]: + if self._runtime is None: + raise RuntimeError(f"{type(self).__name__} is not connected") + group_states = self._runtime.read_group_states(self._group_names) + states: list[MotorState] = [] + for group_state in group_states: + states.extend( + MotorState(q=q, dq=dq, tau=tau) + for q, dq, tau in zip(group_state.q, group_state.dq, group_state.tau, strict=True) + ) + if len(states) != self._dof: + raise RuntimeError(f"expected {self._dof} motor states, got {len(states)}") + return states + + def read_imu(self) -> IMUState: + return IMUState() + + def write_joint_positions(self, positions: Sequence[float]) -> bool: + """Write a full ordered position frame using configured PD gains.""" + + if self._runtime is None or not self._enabled or len(positions) != self._dof: + return False + q_all = [float(value) for value in positions] + if not self._within_limits(q_all): + return False + if not self.has_motor_states(): + return False + + offset = 0 + frames: dict[ + str, tuple[list[float], list[float], list[float], list[float], list[float]] + ] = {} + for group_name in self._group_names: + group_spec = self._robot_spec.groups[group_name] + width = group_spec.dof + q = q_all[offset : offset + width] + tau = ( + self.compute_gravity_torques(group_name, q) if self._gravity_comp else [0.0] * width + ) + frames[group_name] = (q, [0.0] * width, list(group_spec.kp), list(group_spec.kd), tau) + offset += width + return self._runtime.write_groups_mit_commands(frames) + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + raise NotImplementedError( + "TODO: Implement Damiao raw MotorCommand support after the Damiao whole-body " + "command interface is refactored." + ) + + def _within_limits(self, positions: Sequence[float]) -> bool: + for value, lower, upper in zip( + positions, self._position_lower, self._position_upper, strict=True + ): + if value < lower or value > upper: + logger.warning( + "damiao whole-body position outside limits", + hardware_id=self._hardware_id, + value=value, + lower=lower, + upper=upper, + ) + return False + return True + + def _load_gravity_models(self) -> None: + if not self._gravity_comp or self._runtime is None: + return + for group_name in self._group_names: + loaded = self._runtime.load_gravity_model(group_name) + if loaded is None: + continue + self._pin_models[group_name], self._pin_data[group_name] = loaded + + def compute_gravity_torques(self, group_name: str, q: list[float]) -> list[float]: + group_spec = self._robot_spec.groups[group_name] + if group_name not in self._pin_models or group_name not in self._pin_data: + return [0.0] * group_spec.dof + if len(q) != group_spec.dof: + raise ValueError(f"q length does not match dof for group {group_name!r}") + import pinocchio # type: ignore[import-not-found] + + compute_generalized_gravity = _dynamic_attr(pinocchio, "computeGeneralizedGravity") + tau = compute_generalized_gravity( + self._pin_models[group_name], + self._pin_data[group_name], + np.array(q, dtype=np.float64), + ) + values = [float(tau[i]) for i in range(group_spec.dof)] + if group_spec.gravity_torque_limits is None: + return values + return [ + float(np.clip(value, -limit, limit)) + for value, limit in zip(values, group_spec.gravity_torque_limits, strict=False) + ] + + +__all__ = ["DamiaoWholeBodyAdapter"] diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators/README.md index 35e6b710f0..4fdd37c1e3 100644 --- a/dimos/hardware/manipulators/README.md +++ b/dimos/hardware/manipulators/README.md @@ -33,12 +33,15 @@ This module provides manipulator arm drivers: Protocol-only with injectable adap ``` manipulators/ ├── spec.py # ManipulatorAdapter Protocol + shared types -├── registry.py # Adapter registry with auto-discovery +├── registry.py # Adapter registry with manifest-based discovery ├── mock/ +│ ├── __registry__.py # Adapter key → implementation path │ └── adapter.py # MockAdapter for testing ├── xarm/ +│ ├── __registry__.py │ ├── adapter.py # XArmAdapter (SDK wrapper) └── piper/ + ├── __registry__.py ├── adapter.py # PiperAdapter (SDK wrapper) ``` @@ -93,7 +96,17 @@ class MyArmAdapter: # No inheritance needed - just match the Protocol # ... implement other Protocol methods ``` -2. **Create the driver** (`arm.py`): +2. **Create the registry manifest** (`__registry__.py`): + +```python +ADAPTER_FACTORIES = { + "myarm": "dimos.hardware.manipulators.myarm.adapter:MyArmAdapter", +} +``` + +The registry imports this lightweight manifest during discovery. The adapter implementation is imported only when selected with `adapter_registry.create("myarm", ...)`. + +3. **Create the driver** (`arm.py`): ```python from dimos.core.core import rpc @@ -115,7 +128,7 @@ class MyArm(Module[MyArmConfig]): # ... setup control loops ``` -3. **Create blueprints** (`blueprints.py`) for common configurations. +4. **Create blueprints** (`blueprints.py`) for common configurations. ## ManipulatorAdapter Protocol diff --git a/dimos/hardware/manipulators/a750/__registry__.py b/dimos/hardware/manipulators/a750/__registry__.py new file mode 100644 index 0000000000..0a7ae4cab2 --- /dev/null +++ b/dimos/hardware/manipulators/a750/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "a750": "dimos.hardware.manipulators.a750.adapter:A750Adapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/manipulators/mock/__registry__.py b/dimos/hardware/manipulators/mock/__registry__.py new file mode 100644 index 0000000000..eff7d0e1b5 --- /dev/null +++ b/dimos/hardware/manipulators/mock/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "mock": "dimos.hardware.manipulators.mock.adapter:MockAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/manipulators/openarm/__registry__.py b/dimos/hardware/manipulators/openarm/__registry__.py new file mode 100644 index 0000000000..d235423637 --- /dev/null +++ b/dimos/hardware/manipulators/openarm/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "openarm": "dimos.hardware.manipulators.openarm.adapter:OpenArmAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/manipulators/openarm/test_adapter.py b/dimos/hardware/manipulators/openarm/test_adapter.py new file mode 100644 index 0000000000..fd0ed75065 --- /dev/null +++ b/dimos/hardware/manipulators/openarm/test_adapter.py @@ -0,0 +1,150 @@ +# 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. + +from __future__ import annotations + +import time + +import pytest + +from dimos.hardware.manipulators.openarm.adapter import OpenArmAdapter, register +from dimos.hardware.manipulators.registry import AdapterRegistry +from dimos.hardware.manipulators.spec import ControlMode, ManipulatorAdapter + + +class FakeState: + def __init__(self, index: int) -> None: + self.q: float = 0.1 * index + self.dq: float = 0.2 * index + self.tau: float = 0.3 * index + self.t_rotor: float = 30 + index + self.timestamp: float = time.monotonic() + + +class FakeOpenArmBus: + last: FakeOpenArmBus | None = None + + def __init__(self, channel: str, motors: list[object], *, fd: bool, interface: str) -> None: + FakeOpenArmBus.last = self + self.channel: str = channel + self.motors: list[object] = motors + self.fd: bool = fd + self.interface: str = interface + self.opened: bool = False + self.closed: bool = False + self.disabled_count: int = 0 + self.ctrl_mode_ids: list[int] = [] + self.mit_commands: list[list[tuple[float, float, float, float, float]]] = [] + self.states: list[FakeState] = [FakeState(index) for index in range(len(motors))] + + def open(self) -> None: + self.opened = True + + def close(self) -> None: + self.closed = True + + def write_ctrl_mode(self, send_id: int, _mode: int) -> None: + self.ctrl_mode_ids.append(send_id) + + def get_states(self) -> list[FakeState]: + return self.states + + def send_mit_many(self, commands: list[tuple[float, float, float, float, float]]) -> None: + self.mit_commands.append(commands) + + def enable_all(self) -> None: + return None + + def disable_all(self) -> None: + self.disabled_count += 1 + + +def test_implements_manipulator_adapter() -> None: + assert isinstance(OpenArmAdapter(gravity_comp=False), ManipulatorAdapter) + + +def test_register_preserves_openarm_key() -> None: + registry = AdapterRegistry() + register(registry) + adapter = registry.create("openarm", gravity_comp=False) + assert isinstance(adapter, OpenArmAdapter) + + +def test_constructor_validates_dof_side_and_gain_lengths() -> None: + with pytest.raises(ValueError, match="only supports 7 DOF"): + _ = OpenArmAdapter(dof=6, gravity_comp=False) + with pytest.raises(ValueError, match="side must be 'left' or 'right'"): + _ = OpenArmAdapter(side="middle", gravity_comp=False) + with pytest.raises(ValueError, match="kp/kd must be length 7"): + _ = OpenArmAdapter(kp=[1.0], gravity_comp=False) + with pytest.raises(ValueError, match="kp/kd must be length 7"): + _ = OpenArmAdapter(kd=[1.0], gravity_comp=False) + + +def test_side_selects_openarm_identity_limits_and_modes() -> None: + left = OpenArmAdapter(side="left", gravity_comp=False) + right = OpenArmAdapter(side="right", gravity_comp=False) + + assert left.get_info().vendor == "Enactic" + assert left.get_info().model == "OpenArm v10 (left)" + assert right.get_info().model == "OpenArm v10 (right)" + assert left.get_limits().position_lower[:2] == [-3.45, -3.30] + assert right.get_limits().position_lower[:2] == [-1.35, -0.15] + assert left.set_control_mode(ControlMode.VELOCITY) is True + assert left.get_control_mode() == ControlMode.VELOCITY + assert left.set_control_mode(ControlMode.CARTESIAN) is False + + +def test_disconnected_surface_returns_safe_defaults() -> None: + adapter = OpenArmAdapter(gravity_comp=False) + + assert adapter.is_connected() is False + assert adapter.write_joint_positions([0.0] * 7) is False + assert adapter.write_stop() is False + assert adapter.write_enable(True) is False + + +def test_lifecycle_state_commands_and_disconnect(monkeypatch: pytest.MonkeyPatch) -> None: + monkeypatch.setattr( + "dimos.hardware.manipulators.openarm.adapter.OpenArmBus", + FakeOpenArmBus, + ) + adapter = OpenArmAdapter(interface="virtual", gravity_comp=False) + + assert adapter.connect() is True + bus = FakeOpenArmBus.last + assert bus is not None + assert bus.opened is True + + assert adapter.write_enable(True) is True + assert adapter.read_enabled() is True + assert [round(position, 1) for position in adapter.read_joint_positions()] == [ + 0.0, + 0.1, + 0.2, + 0.3, + 0.4, + 0.5, + 0.6, + ] + + assert adapter.write_joint_positions([0.1] * 7, velocity=0.5) is True + assert bus.mit_commands[-1][0] == (0.1, 0.0, 50.0, 1.5, 0.0) + assert adapter.write_stop() is True + assert bus.mit_commands[-1][0] == (0.0, 0.0, 100.0, 1.5, 0.0) + + adapter.disconnect() + assert adapter.read_enabled() is False + assert bus.closed is True + assert bus.disabled_count == 1 diff --git a/dimos/hardware/manipulators/openarm_rs/__registry__.py b/dimos/hardware/manipulators/openarm_rs/__registry__.py new file mode 100644 index 0000000000..dbc9e04b19 --- /dev/null +++ b/dimos/hardware/manipulators/openarm_rs/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "openarm_rs": "dimos.hardware.manipulators.openarm_rs.adapter:OpenArmRSAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/manipulators/openarm_rs/adapter.py b/dimos/hardware/manipulators/openarm_rs/adapter.py new file mode 100644 index 0000000000..107cdd9fe7 --- /dev/null +++ b/dimos/hardware/manipulators/openarm_rs/adapter.py @@ -0,0 +1,136 @@ +# 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. + +from __future__ import annotations + +from pathlib import Path +from typing import TYPE_CHECKING + +from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter +from dimos.hardware.damiao.runtime import ( + _DEFAULT_ADDRESS, + _DEFAULT_STATE_CACHE_TTL_S, + _DEFAULT_TICK_DEADLINE_US, + DamiaoBindingUnavailableError, +) +from dimos.hardware.damiao.specs import DamiaoArmSpec, DamiaoMotorSpec, DamiaoRobotSpec + +if TYPE_CHECKING: + from dimos.hardware.manipulators.registry import AdapterRegistry + + +class OpenArmRSBindingUnavailableError(DamiaoBindingUnavailableError): + pass + + +class OpenArmRSAdapter(DamiaoArmAdapter): + _adapter_type: str = "openarm_rs" + _binding_error_type: type[RuntimeError] = OpenArmRSBindingUnavailableError + _DEFAULT_OPENARM_MOTORS: tuple[DamiaoMotorSpec, ...] = ( + DamiaoMotorSpec("joint1", "DM8006", 0x01, 0x11), + DamiaoMotorSpec("joint2", "DM8006", 0x02, 0x12), + DamiaoMotorSpec("joint3", "DM4340", 0x03, 0x13), + DamiaoMotorSpec("joint4", "DM4340", 0x04, 0x14), + DamiaoMotorSpec("joint5", "DM4310", 0x05, 0x15), + DamiaoMotorSpec("joint6", "DM4310", 0x06, 0x16), + DamiaoMotorSpec("joint7", "DM4310", 0x07, 0x17), + ) + _POSITION_LOWER_LEFT: tuple[float, ...] = (-3.45, -3.30, -1.50, -0.01, -1.50, -0.75, -1.50) + _POSITION_UPPER_LEFT: tuple[float, ...] = (1.35, 0.15, 1.50, 2.40, 1.50, 0.75, 1.50) + _POSITION_LOWER_RIGHT: tuple[float, ...] = (-1.35, -0.15, -1.50, -0.01, -1.50, -0.75, -1.50) + _POSITION_UPPER_RIGHT: tuple[float, ...] = (3.45, 3.30, 1.50, 2.40, 1.50, 0.75, 1.50) + _DEFAULT_VELOCITY_MAX: tuple[float, ...] = (45.0, 45.0, 8.0, 8.0, 30.0, 30.0, 30.0) + _DEFAULT_KP: tuple[float, ...] = (70.0, 70.0, 70.0, 60.0, 10.0, 10.0, 10.0) + _DEFAULT_KD: tuple[float, ...] = (2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5) + + def __init__( + self, + address: str | Path | None = _DEFAULT_ADDRESS, + dof: int = 7, + *, + hardware_id: str = "arm", + config_path: str | Path | None = None, + arm_name: str = "arm", + bus_name: str = "can", + fd: bool | None = None, + canfd: bool = True, + side: str = "left", + use_mock_bus: bool = False, + motor_specs: list[dict[str, object] | DamiaoMotorSpec] | None = None, + position_lower: list[float] | None = None, + position_upper: list[float] | None = None, + velocity_max: list[float] | None = None, + kp: list[float] | None = None, + kd: list[float] | None = None, + gravity_comp: bool = True, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + gravity_model_path: str | Path | None = None, + gravity_torque_limits: list[float] | None = None, + ) -> None: + if dof != len(self._DEFAULT_OPENARM_MOTORS): + raise ValueError(f"OpenArmRSAdapter only supports 7 DOF (got {dof})") + if side not in ("left", "right"): + raise ValueError(f"side must be 'left' or 'right', got {side!r}") + if motor_specs is not None: + raise ValueError("openarm_rs is OpenArm-only and does not accept custom motor_specs") + if position_lower is not None or position_upper is not None or velocity_max is not None: + raise ValueError( + "openarm_rs uses fixed OpenArm limits; custom limits require a separate adapter" + ) + arm_spec = DamiaoArmSpec.from_values( + name="openarm_rs", + vendor="Enactic", + model="OpenArm RS v10", + motors=self._DEFAULT_OPENARM_MOTORS, + position_lower=self._POSITION_LOWER_LEFT + if side == "left" + else self._POSITION_LOWER_RIGHT, + position_upper=self._POSITION_UPPER_LEFT + if side == "left" + else self._POSITION_UPPER_RIGHT, + velocity_max=self._DEFAULT_VELOCITY_MAX, + kp=kp if kp is not None else self._DEFAULT_KP, + kd=kd if kd is not None else self._DEFAULT_KD, + gravity_model_path=gravity_model_path, + gravity_torque_limits=gravity_torque_limits, + bus_name=bus_name, + arm_name=arm_name, + fd=canfd if fd is None else fd, + ) + robot_spec = DamiaoRobotSpec.from_arm_spec( + arm_spec, + address=str(address) if address is not None else _DEFAULT_ADDRESS, + ) + super().__init__( + robot_spec=robot_spec, + group_name=arm_name, + hardware_id=hardware_id, + config_path=config_path, + use_mock_bus=use_mock_bus, + gravity_comp=gravity_comp, + tick_deadline_us=tick_deadline_us, + state_cache_ttl_s=state_cache_ttl_s, + ) + + +def register(registry: AdapterRegistry) -> None: + registry.register("openarm_rs", OpenArmRSAdapter) + + +__all__ = [ + "OpenArmRSAdapter", + "OpenArmRSBindingUnavailableError", + "register", +] diff --git a/dimos/hardware/manipulators/openarm_rs/test_adapter.py b/dimos/hardware/manipulators/openarm_rs/test_adapter.py new file mode 100644 index 0000000000..16738779b6 --- /dev/null +++ b/dimos/hardware/manipulators/openarm_rs/test_adapter.py @@ -0,0 +1,80 @@ +# 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. + +from __future__ import annotations + +import pytest + +from dimos.hardware.manipulators.openarm_rs.adapter import OpenArmRSAdapter, register +from dimos.hardware.manipulators.registry import AdapterRegistry +from dimos.hardware.manipulators.spec import ManipulatorAdapter + + +def test_implements_manipulator_adapter() -> None: + assert isinstance(OpenArmRSAdapter(use_mock_bus=True), ManipulatorAdapter) + + +def test_register_preserves_openarm_rs_key() -> None: + registry = AdapterRegistry() + + register(registry) + + adapter = registry.create("openarm_rs", use_mock_bus=True) + assert isinstance(adapter, OpenArmRSAdapter) + + +def test_side_selects_openarm_joint_limits() -> None: + left = OpenArmRSAdapter(side="left", use_mock_bus=True) + right = OpenArmRSAdapter(side="right", use_mock_bus=True) + + assert left.get_limits().position_lower[:2] == [-3.45, -3.30] + assert right.get_limits().position_lower[:2] == [-1.35, -0.15] + + +def test_invalid_side_is_rejected() -> None: + with pytest.raises(ValueError, match="side must be 'left' or 'right'"): + _ = OpenArmRSAdapter(side="middle", use_mock_bus=True) + + +def test_non_openarm_dof_is_rejected() -> None: + with pytest.raises(ValueError, match="only supports 7 DOF"): + _ = OpenArmRSAdapter(dof=2, use_mock_bus=True) + + +def test_custom_non_openarm_metadata_is_rejected() -> None: + with pytest.raises(ValueError, match="does not accept custom motor_specs"): + _ = OpenArmRSAdapter( + use_mock_bus=True, + motor_specs=[ + {"name": "shoulder", "type": "DM4310", "send_id": 1, "recv_id": 17}, + ], + ) + + with pytest.raises(ValueError, match="fixed OpenArm limits"): + _ = OpenArmRSAdapter( + use_mock_bus=True, + position_lower=[-0.5] * 7, + ) + + +def test_openarm_rs_reports_openarm_specific_identity() -> None: + adapter = OpenArmRSAdapter( + use_mock_bus=True, + kp=[3.0] * 7, + kd=[0.3] * 7, + ) + + assert adapter.get_info().vendor == "Enactic" + assert adapter.get_info().model == "OpenArm RS v10" + assert adapter.get_dof() == 7 diff --git a/dimos/hardware/manipulators/piper/__registry__.py b/dimos/hardware/manipulators/piper/__registry__.py new file mode 100644 index 0000000000..17842d38a6 --- /dev/null +++ b/dimos/hardware/manipulators/piper/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "piper": "dimos.hardware.manipulators.piper.adapter:PiperAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/manipulators/registry.py b/dimos/hardware/manipulators/registry.py index a8c87c149c..84c76fa59f 100644 --- a/dimos/hardware/manipulators/registry.py +++ b/dimos/hardware/manipulators/registry.py @@ -12,10 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Adapter registry with auto-discovery. +"""Adapter registry with lazy auto-discovery. -Automatically discovers and registers manipulator adapters from subpackages. -Each adapter provides a `register()` function in its adapter.py module. +Automatically discovers manipulator adapters from lightweight subpackage +``__registry__.py`` manifests. Adapter implementation modules are imported +only when their adapter key is selected via :meth:`AdapterRegistry.create`. Usage: from dimos.hardware.manipulators.registry import adapter_registry @@ -31,28 +32,43 @@ from __future__ import annotations +from collections.abc import Callable, Mapping import importlib -from typing import TYPE_CHECKING, Any - -from dimos.utils.logging_config import setup_logger +from pathlib import Path +from typing import TYPE_CHECKING, cast if TYPE_CHECKING: from dimos.hardware.manipulators.spec import ManipulatorAdapter -logger = setup_logger() +AdapterFactory = Callable[..., "ManipulatorAdapter"] class AdapterRegistry: - """Registry for manipulator adapters with auto-discovery.""" + """Registry for manipulator adapters with lazy auto-discovery.""" def __init__(self) -> None: - self._adapters: dict[str, type[ManipulatorAdapter]] = {} + self._adapter_paths: dict[str, str] = {} + self._adapters: dict[str, AdapterFactory] = {} - def register(self, name: str, cls: type[ManipulatorAdapter]) -> None: - """Register an adapter class.""" + def register(self, name: str, cls: AdapterFactory) -> None: + """Register an already-imported adapter factory.""" self._adapters[name.lower()] = cls - def create(self, name: str, **kwargs: Any) -> ManipulatorAdapter: + def register_path(self, name: str, factory_path: str) -> None: + """Register a lazy adapter factory import path.""" + if ":" not in factory_path: + raise ValueError(f"Invalid adapter factory path: {factory_path!r}") + module_name, attr = factory_path.split(":", maxsplit=1) + if not module_name or not attr: + raise ValueError(f"Invalid adapter factory path: {factory_path!r}") + + key = name.lower() + existing = self._adapter_paths.get(key) + if existing is not None and existing != factory_path: + raise ValueError(f"Duplicate adapter {key!r}: {existing!r} vs {factory_path!r}") + self._adapter_paths[key] = factory_path + + def create(self, name: str, **kwargs: object) -> ManipulatorAdapter: """Create an adapter instance by name. Args: @@ -66,40 +82,69 @@ def create(self, name: str, **kwargs: Any) -> ManipulatorAdapter: KeyError: If adapter name is not found """ key = name.lower() - if key not in self._adapters: + if key not in self._adapters and key not in self._adapter_paths: raise KeyError(f"Unknown adapter: {name}. Available: {self.available()}") - return self._adapters[key](**kwargs) + return self._resolve_adapter(key)(**kwargs) def available(self) -> list[str]: """List available adapter names.""" - return sorted(self._adapters.keys()) + return sorted(set(self._adapter_paths) | set(self._adapters)) def discover(self) -> None: - """Discover and register adapters from subpackages. + """Discover and register adapter manifests from subpackages. - Scans for subdirectories containing an adapter.py module. + Scans for subdirectories containing a ``__registry__.py`` manifest. Can be called multiple times to pick up newly added adapters. """ - from pathlib import Path - - pkg_dir = Path(__file__).parent - for child in sorted(pkg_dir.iterdir()): - if not child.is_dir() or child.name.startswith(("_", ".")): - continue - if not (child / "adapter.py").exists(): - continue - try: - module = importlib.import_module( - f"dimos.hardware.manipulators.{child.name}.adapter" - ) - if hasattr(module, "register"): - module.register(self) - except ImportError as e: - logger.debug(f"Skipping adapter {child.name}: {e}") + import dimos.hardware.manipulators as pkg + + for root in pkg.__path__: + for child in sorted(Path(root).iterdir()): + if not child.is_dir() or child.name.startswith(("_", ".")): + continue + if not (child / "__registry__.py").exists(): + continue + + module_name = f"dimos.hardware.manipulators.{child.name}.__registry__" + module = importlib.import_module(module_name) + adapter_factories_obj = getattr(module, "ADAPTER_FACTORIES", None) + if not isinstance(adapter_factories_obj, Mapping): + raise TypeError(f"{module_name} must define ADAPTER_FACTORIES") + adapter_factories = cast("Mapping[object, object]", adapter_factories_obj) + for name, factory_path in adapter_factories.items(): + if not isinstance(name, str) or not isinstance(factory_path, str): + raise TypeError( + f"{module_name}.ADAPTER_FACTORIES must map strings to strings" + ) + self.register_path(name, factory_path) + + def _resolve_adapter(self, key: str) -> AdapterFactory: + if key in self._adapters: + return self._adapters[key] + factory_path = self._adapter_paths[key] + module_name, attr = factory_path.split(":", maxsplit=1) + try: + module = importlib.import_module(module_name) + except ModuleNotFoundError as exc: + if exc.name is not None and module_name.startswith(exc.name): + raise ImportError( + f"Adapter {key!r} is registered to missing module {module_name!r}" + ) from exc + raise + try: + factory = cast("AdapterFactory", getattr(module, attr)) + except AttributeError as exc: + raise ImportError( + f"Adapter {key!r} is registered to missing factory {factory_path!r}" + ) from exc + if not callable(factory): + raise TypeError(f"Adapter factory {factory_path!r} is not callable") + self._adapters[key] = factory + return factory adapter_registry = AdapterRegistry() adapter_registry.discover() -__all__ = ["AdapterRegistry", "adapter_registry"] +__all__ = ["AdapterFactory", "AdapterRegistry", "adapter_registry"] diff --git a/dimos/hardware/manipulators/sim/__registry__.py b/dimos/hardware/manipulators/sim/__registry__.py new file mode 100644 index 0000000000..b0124244d6 --- /dev/null +++ b/dimos/hardware/manipulators/sim/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "sim_mujoco": "dimos.hardware.manipulators.sim.adapter:ShmMujocoAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/manipulators/test_registry.py b/dimos/hardware/manipulators/test_registry.py new file mode 100644 index 0000000000..35695c87d9 --- /dev/null +++ b/dimos/hardware/manipulators/test_registry.py @@ -0,0 +1,194 @@ +# 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. + +from __future__ import annotations + +import builtins +import importlib +from pathlib import Path +import sys +from types import ModuleType +from typing import Protocol, cast + +import pytest + +import dimos.hardware.manipulators as manipulators_pkg +from dimos.hardware.manipulators.mock.adapter import MockAdapter +from dimos.hardware.manipulators.registry import AdapterRegistry, adapter_registry + +EXPECTED_ADAPTER_KEYS = { + "a750", + "mock", + "openarm", + "openarm_rs", + "piper", + "sim_mujoco", + "xarm", +} + + +class _HasKwargs(Protocol): + kwargs: dict[str, object] + + +def _write_package( + root: Path, + name: str, + registry_source: str, + adapter_source: str = "", +) -> None: + package = root / name + package.mkdir() + _ = (package / "__init__.py").write_text("", encoding="utf-8") + _ = (package / "__registry__.py").write_text(registry_source, encoding="utf-8") + if adapter_source: + _ = (package / "adapter.py").write_text(adapter_source, encoding="utf-8") + + +def _clear_fake_modules() -> None: + for module_name in list(sys.modules): + if module_name.startswith("dimos.hardware.manipulators.fake"): + del sys.modules[module_name] + + +@pytest.fixture(autouse=True) +def clear_fake_modules() -> None: + _clear_fake_modules() + importlib.invalidate_caches() + + +def test_available_does_not_import_adapter_implementation( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + _write_package( + tmp_path, + "fake_lazy", + 'ADAPTER_FACTORIES = {"fake": "dimos.hardware.manipulators.fake_lazy.adapter:Fake"}\n', + 'raise AssertionError("adapter implementation imported during discovery")\n', + ) + monkeypatch.setattr(manipulators_pkg, "__path__", [str(tmp_path)]) + + registry = AdapterRegistry() + registry.discover() + + assert registry.available() == ["fake"] + assert "dimos.hardware.manipulators.fake_lazy.adapter" not in sys.modules + + +def test_create_imports_selected_adapter_and_passes_kwargs( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + _write_package( + tmp_path, + "fake_selected", + 'ADAPTER_FACTORIES = {"fake": "dimos.hardware.manipulators.fake_selected.adapter:Fake"}\n', + "\n".join( + [ + "class Fake:", + " def __init__(self, **kwargs: object) -> None:", + " self.kwargs = kwargs", + "", + ] + ), + ) + monkeypatch.setattr(manipulators_pkg, "__path__", [str(tmp_path)]) + + registry = AdapterRegistry() + registry.discover() + adapter = registry.create("fake", address="can0", dof=7) + + assert adapter.__class__.__name__ == "Fake" + assert cast("_HasKwargs", adapter).kwargs == {"address": "can0", "dof": 7} + assert "dimos.hardware.manipulators.fake_selected.adapter" in sys.modules + + +def test_direct_registration_still_creates_adapter() -> None: + registry = AdapterRegistry() + registry.register("mock", MockAdapter) + + adapter = registry.create("mock", dof=3) + + assert isinstance(adapter, MockAdapter) + assert adapter.get_dof() == 3 + + +def test_manifest_validation_rejects_bad_mapping( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + _write_package(tmp_path, "fake_bad", "ADAPTER_FACTORIES = {'fake': 1}\n") + monkeypatch.setattr(manipulators_pkg, "__path__", [str(tmp_path)]) + + registry = AdapterRegistry() + with pytest.raises(TypeError, match="must map strings to strings"): + registry.discover() + + +def test_register_path_rejects_duplicate_and_invalid_paths() -> None: + registry = AdapterRegistry() + registry.register_path("fake", "pkg.mod:Factory") + + with pytest.raises(ValueError, match="Duplicate adapter"): + registry.register_path("fake", "pkg.other:Factory") + with pytest.raises(ValueError, match="Invalid adapter factory path"): + registry.register_path("bad", "pkg.mod.Factory") + with pytest.raises(ValueError, match="Invalid adapter factory path"): + registry.register_path("bad", "pkg.mod:") + + +def test_create_reports_missing_selected_module_and_attribute() -> None: + registry = AdapterRegistry() + registry.register_path( + "missing_module", "dimos.hardware.manipulators.fake_missing.adapter:Fake" + ) + registry.register_path("missing_attr", "dimos.hardware.manipulators.registry:MissingFactory") + + with pytest.raises(ImportError, match="missing_module.*missing module"): + _ = registry.create("missing_module") + with pytest.raises(ImportError, match="missing_attr.*missing factory"): + _ = registry.create("missing_attr") + with pytest.raises(KeyError, match="Unknown adapter: unknown"): + _ = registry.create("unknown") + + +def test_builtin_registry_preserves_adapter_keys() -> None: + assert EXPECTED_ADAPTER_KEYS.issubset(set(adapter_registry.available())) + + +def test_discovery_does_not_import_can_motor_control( + monkeypatch: pytest.MonkeyPatch, +) -> None: + real_import = builtins.__import__ + + def fail_can_motor_control( + name: str, + globals: dict[str, object] | None = None, + locals: dict[str, object] | None = None, + fromlist: tuple[str, ...] = (), + level: int = 0, + ) -> ModuleType: + if name.startswith("can_motor_control"): + raise AssertionError("can_motor_control imported during discovery") + module_obj = cast("object", real_import(name, globals, locals, fromlist, level)) + if not isinstance(module_obj, ModuleType): + raise TypeError(f"expected module import for {name}") + return module_obj + + monkeypatch.delitem(sys.modules, "can_motor_control", raising=False) + monkeypatch.delitem(sys.modules, "can_motor_control.damiao", raising=False) + monkeypatch.setattr(builtins, "__import__", fail_can_motor_control) + + registry = AdapterRegistry() + registry.discover() + + assert "openarm_rs" in registry.available() diff --git a/dimos/hardware/manipulators/xarm/__registry__.py b/dimos/hardware/manipulators/xarm/__registry__.py new file mode 100644 index 0000000000..5545e1ae01 --- /dev/null +++ b/dimos/hardware/manipulators/xarm/__registry__.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "xarm": "dimos.hardware.manipulators.xarm.adapter:XArmAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] diff --git a/dimos/hardware/whole_body/openarm/__init__.py b/dimos/hardware/whole_body/openarm/__init__.py new file mode 100644 index 0000000000..8a519f884c --- /dev/null +++ b/dimos/hardware/whole_body/openarm/__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. + +"""OpenArm whole-body adapters.""" diff --git a/dimos/hardware/whole_body/openarm/adapter.py b/dimos/hardware/whole_body/openarm/adapter.py new file mode 100644 index 0000000000..811337733b --- /dev/null +++ b/dimos/hardware/whole_body/openarm/adapter.py @@ -0,0 +1,169 @@ +# 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. + +from __future__ import annotations + +from pathlib import Path +from typing import TYPE_CHECKING + +from dimos.hardware.damiao.runtime import ( + _DEFAULT_STATE_CACHE_TTL_S, + _DEFAULT_TICK_DEADLINE_US, + DamiaoBindingUnavailableError, +) +from dimos.hardware.damiao.specs import ( + DamiaoBusSpec, + DamiaoJointGroupSpec, + DamiaoMotorSpec, + DamiaoRobotSpec, +) +from dimos.hardware.damiao.whole_body_adapter import DamiaoWholeBodyAdapter +from dimos.utils.data import LfsPath + +if TYPE_CHECKING: + from dimos.hardware.whole_body.registry import WholeBodyAdapterRegistry + + +class OpenArmDualBindingUnavailableError(DamiaoBindingUnavailableError): + pass + + +class OpenArmDualWholeBodyAdapter(DamiaoWholeBodyAdapter): + """Dual-arm OpenArm whole-body adapter over the shared Damiao runtime.""" + + _adapter_type: str = "openarm_dual" + _binding_error_type: type[RuntimeError] = OpenArmDualBindingUnavailableError + + _LEFT_GROUP = "left_arm" + _RIGHT_GROUP = "right_arm" + _DEFAULT_MOTOR_TYPES: tuple[str, ...] = ( + "DM8006", + "DM8006", + "DM4340", + "DM4340", + "DM4310", + "DM4310", + "DM4310", + ) + _DEFAULT_SEND_IDS: tuple[int, ...] = (0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07) + _DEFAULT_RECV_IDS: tuple[int, ...] = (0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17) + _POSITION_LOWER_LEFT: tuple[float, ...] = (-3.45, -3.30, -1.50, -0.01, -1.50, -0.75, -1.50) + _POSITION_UPPER_LEFT: tuple[float, ...] = (1.35, 0.15, 1.50, 2.40, 1.50, 0.75, 1.50) + _POSITION_LOWER_RIGHT: tuple[float, ...] = (-1.35, -0.15, -1.50, -0.01, -1.50, -0.75, -1.50) + _POSITION_UPPER_RIGHT: tuple[float, ...] = (3.45, 3.30, 1.50, 2.40, 1.50, 0.75, 1.50) + _DEFAULT_VELOCITY_MAX: tuple[float, ...] = (45.0, 45.0, 8.0, 8.0, 30.0, 30.0, 30.0) + _DEFAULT_KP: tuple[float, ...] = (70.0, 70.0, 70.0, 60.0, 10.0, 10.0, 10.0) + _DEFAULT_KD: tuple[float, ...] = (2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5) + _OPENARM_PKG = LfsPath("openarm_description") + _LEFT_GRAVITY_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" + _RIGHT_GRAVITY_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" + + def __init__( + self, + address: str | Path | None = None, + dof: int = 14, + *, + hardware_id: str = "openarm", + domain_id: int = 0, + left_address: str | Path = "can1", + right_address: str | Path = "can0", + canfd: bool = True, + gravity_comp: bool = True, + use_mock_bus: bool = False, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + ) -> None: + del address, domain_id + robot_spec = self._make_robot_spec( + left_address=left_address, + right_address=right_address, + canfd=canfd, + ) + super().__init__( + robot_spec=robot_spec, + group_names=(self._LEFT_GROUP, self._RIGHT_GROUP), + dof=dof, + hardware_id=hardware_id, + gravity_comp=gravity_comp, + use_mock_bus=use_mock_bus, + tick_deadline_us=tick_deadline_us, + state_cache_ttl_s=state_cache_ttl_s, + ) + + @classmethod + def _make_robot_spec( + cls, + *, + left_address: str | Path, + right_address: str | Path, + canfd: bool, + ) -> DamiaoRobotSpec: + return DamiaoRobotSpec( + name="openarm_dual", + vendor="Enactic", + model="OpenArm RS v10 Dual", + buses={ + "left_can": DamiaoBusSpec(address=left_address, fd=canfd), + "right_can": DamiaoBusSpec(address=right_address, fd=canfd), + }, + groups={ + cls._LEFT_GROUP: DamiaoJointGroupSpec( + bus_name="left_can", + motors=cls._motors("openarm_left"), + position_lower=cls._POSITION_LOWER_LEFT, + position_upper=cls._POSITION_UPPER_LEFT, + velocity_max=cls._DEFAULT_VELOCITY_MAX, + kp=cls._DEFAULT_KP, + kd=cls._DEFAULT_KD, + gravity_model_path=cls._LEFT_GRAVITY_MODEL, + ), + cls._RIGHT_GROUP: DamiaoJointGroupSpec( + bus_name="right_can", + motors=cls._motors("openarm_right"), + position_lower=cls._POSITION_LOWER_RIGHT, + position_upper=cls._POSITION_UPPER_RIGHT, + velocity_max=cls._DEFAULT_VELOCITY_MAX, + kp=cls._DEFAULT_KP, + kd=cls._DEFAULT_KD, + gravity_model_path=cls._RIGHT_GRAVITY_MODEL, + ), + }, + requires_binding=True, + ) + + @classmethod + def _motors(cls, prefix: str) -> tuple[DamiaoMotorSpec, ...]: + return tuple( + DamiaoMotorSpec( + name=f"{prefix}_joint{index + 1}", + type=motor_type, + send_id=send_id, + recv_id=recv_id, + ) + for index, (motor_type, send_id, recv_id) in enumerate( + zip( + cls._DEFAULT_MOTOR_TYPES, + cls._DEFAULT_SEND_IDS, + cls._DEFAULT_RECV_IDS, + strict=True, + ) + ) + ) + + +def register(registry: WholeBodyAdapterRegistry) -> None: + registry.register("openarm_dual", OpenArmDualWholeBodyAdapter) + + +__all__ = ["OpenArmDualBindingUnavailableError", "OpenArmDualWholeBodyAdapter", "register"] diff --git a/dimos/hardware/whole_body/openarm/test_adapter.py b/dimos/hardware/whole_body/openarm/test_adapter.py new file mode 100644 index 0000000000..7d3c43a53e --- /dev/null +++ b/dimos/hardware/whole_body/openarm/test_adapter.py @@ -0,0 +1,135 @@ +# 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. + +from __future__ import annotations + +import importlib + +import pytest + +from dimos.hardware.manipulators.openarm_rs.adapter import ( + OpenArmRSAdapter, + OpenArmRSBindingUnavailableError, +) +import dimos.hardware.whole_body.openarm.adapter as openarm_dual_adapter +from dimos.hardware.whole_body.openarm.adapter import ( + OpenArmDualBindingUnavailableError, + OpenArmDualWholeBodyAdapter, + register, +) +from dimos.hardware.whole_body.registry import WholeBodyAdapterRegistry +from dimos.hardware.whole_body.spec import WholeBodyAdapter + + +def test_openarm_dual_constructs_without_binding() -> None: + adapter = OpenArmDualWholeBodyAdapter(use_mock_bus=True) + + assert isinstance(adapter, WholeBodyAdapter) + assert adapter.joint_names == ( + "openarm_left_joint1", + "openarm_left_joint2", + "openarm_left_joint3", + "openarm_left_joint4", + "openarm_left_joint5", + "openarm_left_joint6", + "openarm_left_joint7", + "openarm_right_joint1", + "openarm_right_joint2", + "openarm_right_joint3", + "openarm_right_joint4", + "openarm_right_joint5", + "openarm_right_joint6", + "openarm_right_joint7", + ) + + +def test_openarm_dual_configures_side_gravity_models() -> None: + adapter = OpenArmDualWholeBodyAdapter(use_mock_bus=True) + + left = adapter._robot_spec.groups["left_arm"] + right = adapter._robot_spec.groups["right_arm"] + assert str(left.gravity_model_path).endswith("openarm_v10_left.urdf") + assert str(right.gravity_model_path).endswith("openarm_v10_right.urdf") + + +def test_openarm_dual_rejects_non_14_dof() -> None: + with pytest.raises(ValueError, match="supports 14 DOF"): + _ = OpenArmDualWholeBodyAdapter(dof=7, use_mock_bus=True) + + +def test_register_preserves_openarm_dual_key() -> None: + registry = WholeBodyAdapterRegistry() + + register(registry) + + assert registry.available() == ["openarm_dual"] + assert isinstance( + registry.create("openarm_dual", use_mock_bus=True), OpenArmDualWholeBodyAdapter + ) + + +def test_openarm_dual_module_import_does_not_probe_binding(mocker) -> None: + mocker.patch( + "dimos.hardware.damiao.runtime.importlib.import_module", + side_effect=AssertionError("binding import must stay lazy"), + ) + + reloaded = importlib.reload(openarm_dual_adapter) + registry = WholeBodyAdapterRegistry() + reloaded.register(registry) + + assert registry.available() == ["openarm_dual"] + + +def test_openarm_rs_preserves_constructor_deployment_options() -> None: + adapter = OpenArmRSAdapter( + address="can9", + config_path="robot.toml", + arm_name="right_arm", + bus_name="right_can", + fd=False, + canfd=True, + side="right", + use_mock_bus=True, + gravity_model_path="override.urdf", + ) + + assert adapter._group_name == "right_arm" + assert adapter._config_path == "robot.toml" + assert adapter._gravity_model_path == "override.urdf" + assert adapter._robot_spec.buses["right_can"].address == "can9" + assert adapter._robot_spec.buses["right_can"].fd is False + assert adapter.get_limits().position_lower[0] == -1.35 + + +def test_openarm_dual_connect_raises_specific_binding_error(mocker) -> None: + adapter = OpenArmDualWholeBodyAdapter(use_mock_bus=True) + mocker.patch( + "dimos.hardware.damiao.runtime.importlib.import_module", + side_effect=ImportError("missing binding"), + ) + + with pytest.raises(OpenArmDualBindingUnavailableError): + adapter.connect() + + +def test_openarm_rs_connect_raises_specific_binding_error(mocker) -> None: + adapter = OpenArmRSAdapter(use_mock_bus=True) + mocker.patch( + "dimos.hardware.damiao.runtime.importlib.import_module", + side_effect=ImportError("missing binding"), + ) + + with pytest.raises(OpenArmRSBindingUnavailableError): + adapter.connect() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 291af8540e..8e9c0db290 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -33,6 +33,7 @@ "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_left", "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_mock", "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_right", + "coordinator-openarm-rs": "dimos.robot.manipulators.openarm.blueprints:coordinator_openarm_rs", "coordinator-piper": "dimos.control.blueprints.basic:coordinator_piper", "coordinator-piper-xarm": "dimos.control.blueprints.dual:coordinator_piper_xarm", "coordinator-servo-xarm6": "dimos.control.blueprints.teleop:coordinator_servo_xarm6", @@ -72,8 +73,10 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "mid360-pointlio": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio", "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", + "openarm-dual-whole-body": "dimos.robot.manipulators.openarm.blueprints:openarm_dual_whole_body", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", + "openarm-rs-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_rs_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", diff --git a/dimos/robot/catalog/openarm.py b/dimos/robot/catalog/openarm.py index b6e1238cf2..a02cc6f65e 100644 --- a/dimos/robot/catalog/openarm.py +++ b/dimos/robot/catalog/openarm.py @@ -37,6 +37,9 @@ _OPENARM_LEFT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" _OPENARM_RIGHT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" +# Public per-side and single-arm URDF paths used by blueprints/adapters. +OPENARM_V10_LEFT_MODEL = _OPENARM_LEFT_MODEL +OPENARM_V10_RIGHT_MODEL = _OPENARM_RIGHT_MODEL # Pre-expanded single-arm URDF for Pinocchio FK (keyboard teleop, IK, etc.) OPENARM_V10_FK_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" @@ -117,4 +120,10 @@ def openarm_single( return RobotConfig(**defaults) -__all__ = ["OPENARM_V10_FK_MODEL", "openarm_arm", "openarm_single"] +__all__ = [ + "OPENARM_V10_FK_MODEL", + "OPENARM_V10_LEFT_MODEL", + "OPENARM_V10_RIGHT_MODEL", + "openarm_arm", + "openarm_single", +] diff --git a/dimos/robot/manipulators/openarm/blueprints.py b/dimos/robot/manipulators/openarm/blueprints.py index 53388102fb..9e06db5bc4 100644 --- a/dimos/robot/manipulators/openarm/blueprints.py +++ b/dimos/robot/manipulators/openarm/blueprints.py @@ -16,11 +16,15 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator +from dimos.control.components import HardwareComponent, HardwareType +from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect +from dimos.core.transport import LCMTransport from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.catalog.openarm import ( OPENARM_V10_FK_MODEL, + OPENARM_V10_RIGHT_MODEL, openarm_arm as _openarm, openarm_single as _openarm_single, ) @@ -51,7 +55,10 @@ # replaced / factory-reset). AUTO_SET_MIT_MODE = True +# gravity_comp and canfd are already True by default on OpenArmRSAdapter, so +# the blueprint only overrides the gravity model path. _ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} +_OPENARM_RS_ADAPTER_KWARGS = {"gravity_model_path": OPENARM_V10_RIGHT_MODEL} _left_hw = _openarm( side="left", address=LEFT_CAN, @@ -64,6 +71,18 @@ adapter_type="openarm", adapter_kwargs=_ADAPTER_KWARGS, ) +_openarm_rs_hw = _openarm( + side="right", + name="arm", + adapter_type="openarm_rs", + address=RIGHT_CAN, + adapter_kwargs=_OPENARM_RS_ADAPTER_KWARGS, +) + +OPENARM_DUAL_WHOLE_BODY_JOINTS = [ + *[f"openarm/openarm_left_joint{i}" for i in range(1, 8)], + *[f"openarm/openarm_right_joint{i}" for i in range(1, 8)], +] coordinator_openarm_left = ControlCoordinator.blueprint( hardware=[_left_hw.to_hardware_component()], @@ -84,6 +103,54 @@ ], ) +coordinator_openarm_rs = ControlCoordinator.blueprint( + hardware=[_openarm_rs_hw.to_hardware_component()], + tasks=[_openarm_rs_hw.to_task_config()], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + +openarm_dual_whole_body = ControlCoordinator.blueprint( + hardware=[ + HardwareComponent( + hardware_id="openarm", + hardware_type=HardwareType.WHOLE_BODY, + joints=OPENARM_DUAL_WHOLE_BODY_JOINTS, + adapter_type="openarm_dual", + adapter_kwargs={ + "left_address": LEFT_CAN, + "right_address": RIGHT_CAN, + "gravity_comp": True, + }, + ) + ], + tasks=[ + TaskConfig( + name="traj_openarm", + type="trajectory", + joint_names=OPENARM_DUAL_WHOLE_BODY_JOINTS, + ) + ], +) + +openarm_rs_planner_coordinator = autoconnect( + ManipulationModule.blueprint( + robots=[_openarm_rs_hw.to_robot_model_config()], + planning_timeout=10.0, + enable_viz=True, + ), + ControlCoordinator.blueprint( + hardware=[_openarm_rs_hw.to_hardware_component()], + tasks=[_openarm_rs_hw.to_task_config()], + ), +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + # ── Planner + coordinator (mock): Drake plans, mock adapters execute ──── # Great for visualizing motions in Meshcat with no hardware. @@ -172,8 +239,11 @@ "coordinator_openarm_left", "coordinator_openarm_mock", "coordinator_openarm_right", + "coordinator_openarm_rs", "keyboard_teleop_openarm", "keyboard_teleop_openarm_mock", + "openarm_dual_whole_body", "openarm_mock_planner_coordinator", "openarm_planner_coordinator", + "openarm_rs_planner_coordinator", ] diff --git a/docs/capabilities/manipulation/a750.md b/docs/capabilities/manipulation/a750.md index 51b78f8205..a7039ebacf 100644 --- a/docs/capabilities/manipulation/a750.md +++ b/docs/capabilities/manipulation/a750.md @@ -79,7 +79,7 @@ The adapter reads and commands gripper position in meters using the `a750_contro ## Hardware Adapter -The adapter is registered as `a750` in [`dimos/hardware/manipulators/a750/adapter.py`](/dimos/hardware/manipulators/a750/adapter.py#L40). +The adapter is registered as `a750` through the manipulator adapter manifest in [`dimos/hardware/manipulators/a750/__registry__.py`](/dimos/hardware/manipulators/a750/__registry__.py#L16). It supports: diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 3d33f8877a..3f695a9720 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -45,12 +45,13 @@ Create a new directory for your arm under `dimos/hardware/manipulators/`: ``` dimos/hardware/manipulators/ ├── spec.py # ManipulatorAdapter Protocol (don't modify) -├── registry.py # Auto-discovery registry (don't modify) +├── registry.py # Manifest-based adapter registry (don't modify) ├── mock/ ├── xarm/ ├── piper/ └── yourarm/ # ← New directory ├── __init__.py + ├── __registry__.py └── adapter.py ``` @@ -76,14 +77,10 @@ DimOS Units: angles=radians, distance=meters, velocity=rad/s from __future__ import annotations import math -from typing import TYPE_CHECKING # Import your vendor SDK from yourarm_sdk import YourArmSDK -if TYPE_CHECKING: - from dimos.hardware.manipulators.registry import AdapterRegistry - from dimos.hardware.manipulators.spec import ( ControlMode, JointLimits, @@ -343,13 +340,6 @@ class YourArmAdapter: """Read F/T sensor data [fx, fy, fz, tx, ty, tz]. None if no sensor.""" return None - -# ── Registry hook (required for auto-discovery) ─────────────────── -def register(registry: AdapterRegistry) -> None: - """Register this adapter with the registry.""" - registry.register("yourarm", YourArmAdapter) - - __all__ = ["YourArmAdapter"] ``` @@ -389,15 +379,27 @@ from dimos.hardware.manipulators.yourarm.adapter import YourArmAdapter __all__ = ["YourArmAdapter"] ``` +### `__registry__.py` + +Create a lightweight manifest next to `adapter.py`: + +```python skip +ADAPTER_FACTORIES = { + "yourarm": "dimos.hardware.manipulators.yourarm.adapter:YourArmAdapter", +} + +__all__ = ["ADAPTER_FACTORIES"] +``` + ### How auto-discovery works -The `AdapterRegistry` in `dimos/hardware/manipulators/registry.py` automatically discovers your adapter at import time: +The `AdapterRegistry` in `dimos/hardware/manipulators/registry.py` automatically discovers your adapter metadata at import time: 1. It iterates over all subpackages under `dimos/hardware/manipulators/` -2. For each subpackage, it tries to import `.adapter` -3. If that module has a `register()` function, it calls it +2. For each subpackage, it imports `.__registry__` +3. It reads `ADAPTER_FACTORIES` entries such as `"yourarm": "dimos.hardware.manipulators.yourarm.adapter:YourArmAdapter"` -This means **no manual registration is needed** — just having the `register()` function in your `adapter.py` is sufficient. +This means **no central registry edit is needed**. The adapter implementation module is imported lazily only when the adapter is selected through `adapter_registry.create("yourarm", ...)`, so unrelated adapter listing does not import your vendor SDK. You can verify discovery works: @@ -691,7 +693,8 @@ adapter.disconnect() Files to create: - [ ] `dimos/hardware/manipulators/yourarm/__init__.py` -- [ ] `dimos/hardware/manipulators/yourarm/adapter.py` (implements Protocol + `register()`) +- [ ] `dimos/hardware/manipulators/yourarm/__registry__.py` (maps adapter key to implementation path) +- [ ] `dimos/hardware/manipulators/yourarm/adapter.py` (implements Protocol) - [ ] `dimos/robot/yourarm/__init__.py` - [ ] `dimos/robot/yourarm/blueprints.py` (coordinator + planning blueprints) @@ -702,5 +705,6 @@ Files to modify: Verification: - [ ] `adapter_registry.available()` includes `"yourarm"` +- [ ] `adapter_registry.create("yourarm", address="192.168.1.100", dof=6)` returns your adapter - [ ] `pytest dimos/robot/test_all_blueprints_generation.py` passes (regenerates `all_blueprints.py`) - [ ] `dimos run coordinator-yourarm` starts successfully diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 6869864f5a..8d1f676538 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -22,7 +22,16 @@ Every other arm in dimos wraps a vendor Python SDK: | Go2 / G1 | WebRTC | Unitree SDK | | Panda | FCI | `panda-py` | -**OpenArm ships no Python SDK.** The only interface is raw CAN frames on the wire, speaking the Damiao MIT-mode protocol. So dimos includes a from-scratch driver that encodes/decodes the protocol directly on a SocketCAN bus. The reference implementation is the Enactic C++ library at [enactic/openarm_can](https://github.com/enactic/openarm_can) — we port the frame layout from there. +**OpenArm historically shipped no stable Python SDK.** The default `openarm` adapter still uses dimos' in-tree raw-CAN driver and remains the existing production path, so users still select `adapter_type="openarm"` for OpenArm hardware. DimOS also provides an opt-in `openarm_rs` adapter for environments that install the Rust-backed `can-motor-control` Python binding through the manipulation extra. + +## Adapter paths + +| Adapter | Hardware API | Dependency expectation | Typical use | +|---|---|---|---| +| `openarm` | In-tree SocketCAN Damiao driver | `python-can` plus Pinocchio for gravity feed-forward | Existing OpenArm coordinator, planner, and teleop blueprints. | +| `openarm_rs` | Rust-backed `can-motor-control` Python binding | Install `dimos[manipulation]` so `can_motor_control` is importable | OpenArm RS bring-up, binding-backed coordinator operation, and gravity-compensation-only validation. | + +Selecting `openarm_rs` is explicit through blueprint or hardware config. Registry discovery remains available without `can_motor_control`; selecting the adapter fails with a clear missing-binding error if the package is absent. Future Damiao-based arms should subclass the shared Damiao adapter base with their own typed motor/gain/limit metadata instead of relying on OpenArm defaults. ## Architecture @@ -113,10 +122,12 @@ The register is persistent across power cycles, so you only need this once per m | `coordinator-openarm-bimanual` | Both arms, real hardware, no planner. | | `openarm-planner-coordinator` | **Main usable blueprint** — Drake planner + both arms on real hardware. | | `keyboard-teleop-openarm-mock` / `keyboard-teleop-openarm` | Single-arm Cartesian IK + pygame keyboard, mock / real. | +| `coordinator-openarm-rs` | Opt-in single-arm coordinator path using the new `adapter_type="openarm_rs"` driver, without the planner. | +| `openarm-rs-planner-coordinator` | **New driver usable blueprint** — `ManipulationModule` + `ControlCoordinator` + `openarm_rs` on one arm. | **Safety before hot-plugging hardware:** hold the arms before starting. On connect, the adapter enables all motors and sends gravity-comp holds — the arms go slightly stiff but don't leap. Ctrl-C to cleanly disable and exit. -First-time recommendation: mock planner to verify everything wires up, then real single-arm, then bimanual. +First-time recommendation for the existing `openarm` adapter: mock planner to verify everything wires up, then real single-arm, then bimanual. ```bash # smoke test (no hardware) @@ -129,11 +140,26 @@ dimos run coordinator-openarm-left dimos run openarm-planner-coordinator ``` +For the `openarm_rs` binding path, stage validation before trajectory control: binding mock or vcan, one motor enable/read, full-arm state monitor, adapter gravity compensation, then trajectory-control validation. + +```bash +# requires the can-motor-control binding from dimos[manipulation] +sudo MODE=fd ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 + +# low-level coordinator: enables the arm and sends one current-position hold frame +dimos run coordinator-openarm-rs + +# manipulation workflow: startup hold, then plan, preview, and execute through the new driver +dimos run openarm-rs-planner-coordinator +``` + +`OpenArmRSAdapter` opens CAN-FD by default (`canfd=True`) and computes model gravity feed-forward in-place when `gravity_comp=True` (the OpenArm blueprint default). It intentionally supports position and effort semantics only: position commands are sent as MIT commands with preset `kp/kd` gains and optional gravity feed-forward, while effort/gravity-only commands use `kp=0` so the arm does not hold a target pose. Velocity commands are rejected because nonzero gains make MIT commands maintain the supplied `q`. Set `gravity_comp=False` in adapter kwargs to keep position MIT commands but omit model feed-forward torque. + Meshcat will appear at http://localhost:7000. -### 5. Drive the arms from the manipulation client +### 5. Drive the new driver from the manipulation client -With `openarm-planner-coordinator` running in one terminal, open a second terminal and start the REPL client: +With `openarm-rs-planner-coordinator` running in one terminal, open a second terminal and start the REPL client: ```bash python -i -m dimos.manipulation.planning.examples.manipulation_client @@ -143,7 +169,7 @@ This gives you an interactive Python prompt with these functions: | Function | Purpose | |---|---| -| `robots()` | List configured robots (here: `["left_arm", "right_arm"]`) | +| `robots()` | List configured robots (for `openarm-rs-planner-coordinator`: `["arm"]`) | | `joints(robot_name)` | Read current joint positions (7 floats) | | `ee(robot_name)` | Read current end-effector pose | | `state()` | Module state: `IDLE`, `PLANNING`, `EXECUTING`, `FAULT`, etc. | @@ -158,16 +184,16 @@ This gives you an interactive Python prompt with these functions: ```python skip >>> robots() -['left_arm', 'right_arm'] +['arm'] ->>> joints(robot_name="left_arm") +>>> joints() [0.02, -0.01, -0.13, 0.15, 0.17, -0.07, 0.10] >>> # One-liner: plan → preview in Meshcat → execute on hardware ->>> plan([0.3, 0, 0, 0, 0, 0, 0], robot_name="left_arm") and preview(robot_name="left_arm") and execute(robot_name="left_arm") +>>> plan([0.3, 0, 0, 0, 0, 0, 0]) and preview() and execute() True ->>> joints(robot_name="left_arm") +>>> joints() [0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00] # arm is now at the commanded pose ``` @@ -180,7 +206,9 @@ If you ever get stuck in a `FAULT` state (e.g. an invalid plan was sent), reset 'Reset to IDLE — ready for new commands' ``` -#### Example session — bimanual +#### Example session — bimanual legacy `openarm` blueprint + +Use this pattern with `openarm-planner-coordinator`, which configures `left_arm` and `right_arm`. The new `openarm-rs-planner-coordinator` blueprint is intentionally single-arm while the binding-backed driver path is being validated. ```python skip >>> # Move both arms to mirrored poses @@ -195,10 +223,10 @@ Each arm plans and executes independently — the coordinator runs both trajecto #### Example session — Cartesian target ```python skip ->>> ee(robot_name="left_arm") # see where the EE currently is ->>> plan_pose(0.1, 0.3, 0.5, robot_name="left_arm") and preview(robot_name="left_arm") +>>> ee() # see where the EE currently is +>>> plan_pose(0.1, 0.3, 0.5) and preview() True ->>> execute(robot_name="left_arm") +>>> execute() True ``` @@ -222,25 +250,21 @@ If you don't know which Cartesian targets are reachable, check first with the wo Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants at the top of [blueprints.py](/dimos/robot/manipulators/openarm/blueprints.py): ```python -LEFT_CAN = "can0" -RIGHT_CAN = "can1" +LEFT_CAN = "can1" +RIGHT_CAN = "can0" ``` -No other code changes are needed. +No other code changes are needed. The `coordinator-openarm-rs` and `openarm-rs-planner-coordinator` blueprints currently target `RIGHT_CAN` (`can0`) and pass `canfd=True`; use `sudo MODE=fd ... openarm_can_up.sh can0` before running either one. ### Gain tuning (MIT kp/kd) -Defaults live in [adapter.py](/dimos/hardware/manipulators/openarm/adapter.py). Gains are per-joint because the shoulder motors (DM8006, 40 Nm) tolerate higher kp than the wrist motors (DM4310, 10 Nm): - -```python -_DEFAULT_KP = [100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0] -_DEFAULT_KD = [1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8] -``` +Defaults live in the adapter implementations — pointing here instead of restating the numbers so docs can't drift from code. The binding-backed `openarm_rs` path uses the upstream OpenArm ROS2 hardware presets from `openarm_hardware/openarm_simple_hardware.hpp`; see `OpenArmRSAdapter._DEFAULT_KP` / `_DEFAULT_KD` in [openarm_rs/adapter.py](/dimos/hardware/manipulators/openarm_rs/adapter.py). The in-tree `openarm` adapter keeps its own `_DEFAULT_KP` / `_DEFAULT_KD` in [openarm/adapter.py](/dimos/hardware/manipulators/openarm/adapter.py). Guidelines: - `kp ∈ [0, 500]` in MIT mode. Higher kp = stiffer position tracking; too high → oscillation. - `kd ∈ [0, 5]`. Higher kd = more damping, but values above ~2 on these gearboxes cause high-frequency buzz/grinding. -- Gravity compensation is on by default (`gravity_comp=True`) — the adapter uses Pinocchio to compute `G(q)` and adds it as feedforward torque. This removes the need for very high kp to fight gravity, so prefer low kp + gravity comp over high kp. +- Gravity compensation is on by default (`gravity_comp=True`) for OpenArm-style adapters. The adapter uses Pinocchio to compute `G(q)` and adds it as feedforward torque in the same command path. This removes the need for very high kp to fight gravity, so prefer upstream-tuned kp/kd + gravity comp over increasing kp. +- For `openarm_rs`, use position commands for trajectory/coordinator control and direct effort/gravity-only commands for torque bring-up. Velocity commands are intentionally unsupported. ### Physical joint limits @@ -346,7 +370,7 @@ Persistent across power cycles. ## Design decisions - **Driver separate from adapter.** `driver.py` has zero dimos deps → unit-testable with a virtual CAN bus, reusable outside dimos. -- **MIT mode for everything.** MIT can emulate position (high kp), velocity (kp=0, nonzero kd+dq), and torque (kp=kd=0, nonzero tau). One code path. +- **MIT mode for OpenArm RS position/effort.** The binding-backed `openarm_rs` path intentionally rejects velocity commands; nonzero MIT gains hold `q`, so velocity semantics are not exposed. Position uses preset `kp/kd`; effort/gravity-only commands use `kp=0`. - **Gravity compensation on by default.** Eliminates steady-state position error without needing high kp. Needs Pinocchio + the per-side URDFs. - **One adapter per CAN bus, keyed by `address`.** Matches the Piper adapter pattern. Bimanual = two adapters with different `address` values. - **Per-side URDFs for Drake planning.** Loading the full 14-DOF bimanual URDF twice (once per robot instance) creates phantom-arm collisions with the "other" arm frozen at zero. The per-side URDFs keep only one arm's links + the torso, avoiding the phantom collisions while matching the bimanual kinematics exactly. diff --git a/pyproject.toml b/pyproject.toml index a5415b272f..7abfac8cf1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -257,6 +257,7 @@ manipulation = [ "qpsolvers[proxqp]>=4.12.0", # Hardware SDKs + "can-motor-control>=0.0.2; sys_platform == 'linux' and platform_machine == 'x86_64'", "piper-sdk", "pyrealsense2-extended; sys_platform != 'darwin'", "xarm-python-sdk>=1.17.0", @@ -449,7 +450,7 @@ tests-self-hosted = [ required-version = ">=0.9.17" default-groups = ["tests"] exclude-newer = "7 days" -exclude-newer-package = { dimos-viewer = false, pyrealsense2-extended = false, dimos-lcm = false, lcm-dimos-fork = false } +exclude-newer-package = { dimos-viewer = false, pyrealsense2-extended = false, dimos-lcm = false, lcm-dimos-fork = false, can-motor-control = false } override-dependencies = [ # moondream pins pillow<11 but we need >=12.2.0 for security fixes # (CVE-2026-25990, CVE-2026-40192, CVE-2026-42311). diff --git a/uv.lock b/uv.lock index 7eee927cc4..b42da9ddab 100644 --- a/uv.lock +++ b/uv.lock @@ -44,6 +44,7 @@ exclude-newer = "0001-01-01T00:00:00Z" # This has no effect and is included for exclude-newer-span = "P7D" [options.exclude-newer-package] +can-motor-control = false dimos-viewer = false dimos-lcm = false lcm-dimos-fork = false @@ -567,6 +568,19 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/c5/0d/84a4380f930db0010168e0aa7b7a8fed9ba1835a8fbb1472bc6d0201d529/build-1.4.0-py3-none-any.whl", hash = "sha256:6a07c1b8eb6f2b311b96fcbdbce5dab5fe637ffda0fd83c9cac622e927501596", size = 24141, upload-time = "2026-01-08T16:41:46.453Z" }, ] +[[package]] +name = "can-motor-control" +version = "0.0.2" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32'" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/0b/c1/d4a796df3a503c5b15d188fe65793631038db2f59f51c7863f8d776e7f3c/can_motor_control-0.0.2.tar.gz", hash = "sha256:7ca4942c680244ca8fe86f5dbb0cb870a4c36d786827ece1b7f2e56dfe186ee7", size = 114974, upload-time = "2026-06-07T01:33:14.484Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c7/06/76d8389a658795e1fe7e9f771e1179f25a8fb75f20c763d90719d9e0b603/can_motor_control-0.0.2-cp310-abi3-manylinux_2_34_x86_64.whl", hash = "sha256:5f1a993dba3ee2efc6e92f9e7a2ca5503d9fb2c2c436c7377e9e5a46b31aa94e", size = 510110, upload-time = "2026-06-07T01:33:12.825Z" }, +] + [[package]] name = "catkin-pkg" version = "1.1.0" @@ -1941,6 +1955,7 @@ agents = [ all = [ { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "anthropic" }, + { name = "can-motor-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "catkin-pkg" }, { name = "cerebras-cloud-sdk" }, { name = "ctransformers" }, @@ -2076,6 +2091,7 @@ drone = [ ] manipulation = [ { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "can-motor-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "drake", version = "1.45.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, { name = "drake", version = "1.49.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, { name = "kaleido" }, @@ -2377,6 +2393,7 @@ requires-dist = [ { name = "annotation-protocol", specifier = ">=1.4.0" }, { name = "anthropic", marker = "extra == 'agents'", specifier = ">=0.19.0" }, { name = "bleak", specifier = ">=3.0.2" }, + { name = "can-motor-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux' and extra == 'manipulation'", specifier = ">=0.0.2" }, { name = "catkin-pkg", marker = "extra == 'misc'" }, { name = "cerebras-cloud-sdk", marker = "extra == 'misc'" }, { name = "colorlog", specifier = "==6.9.0" },