diff --git a/dimos/hardware/manipulators/damiao/base_adapter.py b/dimos/hardware/manipulators/damiao/base_adapter.py index 9528f05362..ecd3ad823c 100644 --- a/dimos/hardware/manipulators/damiao/base_adapter.py +++ b/dimos/hardware/manipulators/damiao/base_adapter.py @@ -20,7 +20,6 @@ from typing import Any, cast import numpy as np -from numpy.typing import NDArray from dimos.hardware.manipulators.damiao.specs import DamiaoArmSpec from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo @@ -28,7 +27,14 @@ logger = setup_logger() -FloatArray = NDArray[np.float64] +# Shared adapter defaults. Subclasses reference these so the base and its +# subclasses can't drift apart. +_DEFAULT_TICK_DEADLINE_US = 1_000 +_DEFAULT_STATE_CACHE_TTL_S = 0.002 +# Conventional first SocketCAN interface; used when no address is configured. +_DEFAULT_ADDRESS = "can0" +# Position of each control mode in declaration order, reported by read_state(). +_CONTROL_MODE_INDEX = {mode: index for index, mode in enumerate(ControlMode)} _can_motor_control: Any | None _damiao: Any | None @@ -110,11 +116,11 @@ def __init__( gravity_model_path: str | Path | None = None, gravity_torque_limits: list[float] | tuple[float, ...] | None = None, supported_control_modes: tuple[ControlMode, ...] | None = None, - address: str | Path | None = "can0", + address: str | Path | None = _DEFAULT_ADDRESS, config_path: str | Path | None = None, use_mock_bus: bool = False, - tick_deadline_us: int = 1_000, - state_cache_ttl_s: float = 0.002, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, ) -> None: arm_spec.validate() if dof is not None and dof != arm_spec.dof: @@ -157,7 +163,7 @@ def __init__( self._last_positions: list[float] | None = None self._pin_model = None self._pin_data: object | None = None - self._address = str(address) if address is not None else "can0" + self._address = str(address) if address is not None else _DEFAULT_ADDRESS self._config_path = str(config_path) if config_path is not None else None self._arm_name = arm_spec.arm_name self._bus_name = arm_spec.bus_name @@ -182,18 +188,6 @@ def _validate_command_lengths(self, **commands: list[float]) -> None: def _zero_vector(self) -> list[float]: return [0.0] * self._dof - def _mit_command_rows( - self, - *, - q: list[float], - dq: list[float], - kp: list[float], - kd: list[float], - tau: list[float], - ) -> list[tuple[float, float, float, float, float]]: - self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - return list(zip(q, dq, kp, kd, tau, strict=True)) - def get_info(self) -> ManipulatorInfo: return ManipulatorInfo( vendor=self._arm_spec.vendor, @@ -261,9 +255,12 @@ def connect(self) -> bool: self.refresh_state(force=True) except self._binding_error_type: raise - except Exception as exc: - logger.error( - f"{type(self).__name__} {self._hardware_id}@{self._address} connect failed: {exc}" + except Exception: + logger.exception( + "damiao adapter connect failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + address=self._address, ) self._robot = None self._arm = None @@ -304,9 +301,12 @@ def disconnect(self) -> None: if self._robot is not None: try: self._robot.disable() - except Exception as exc: + except Exception: logger.warning( - f"{type(self).__name__} {self._hardware_id} disable on disconnect failed: {exc}" + "damiao adapter disable on disconnect failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, ) self._enabled = False self._connected = False @@ -330,9 +330,9 @@ def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float self._arm.refresh() self._robot.tick(self._tick_deadline_us) state = ( - [float(value) for value in self._arm.positions().astype(np.float64)], - [float(value) for value in self._arm.velocities().astype(np.float64)], - [float(value) for value in self._arm.torques().astype(np.float64)], + self._arm.positions().astype(np.float64).tolist(), + self._arm.velocities().astype(np.float64).tolist(), + self._arm.torques().astype(np.float64).tolist(), ) if any(len(values) != self._dof for values in state): raise RuntimeError("can_motor_control state length does not match configured DOF") @@ -353,7 +353,7 @@ def read_joint_efforts(self) -> list[float]: def read_state(self) -> dict[str, int]: return { "state": 1 if self._enabled else 0, - "mode": list(ControlMode).index(self._control_mode), + "mode": _CONTROL_MODE_INDEX[self._control_mode], } def read_error(self) -> tuple[int, str]: @@ -379,6 +379,14 @@ def write_joint_positions(self, positions: list[float], velocity: float = 1.0) - try: tau = self.compute_gravity_torques(self.read_joint_positions()) except RuntimeError: + # State read failed: fall back to zero feed-forward torque, but + # surface it so the dropped gravity term isn't silent. + logger.warning( + "damiao 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() @@ -413,12 +421,15 @@ def write_joint_torques(self, efforts: list[float]) -> bool: def write_gravity_compensation(self, damping: float | list[float] = 0.0) -> bool: try: q, dq, _ = self.refresh_state(force=True) - tau = self.compute_gravity_torques(q) - except Exception as exc: + except RuntimeError: logger.warning( - f"Skipping {type(self).__name__} gravity compensation due to invalid state: {exc}" + "skipping damiao gravity compensation; state read failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, ) return False + tau = self.compute_gravity_torques(q) kd = [float(damping)] * self._dof if isinstance(damping, int | float) else list(damping) return self.write_mit_commands(q=q, dq=dq, kp=self._zero_vector(), kd=kd, tau=tau) @@ -427,10 +438,9 @@ def write_mit_commands( ) -> bool: if self._arm is None or self._robot is None or not self._enabled: return False - rows = self._mit_command_rows(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - self._arm.mit_control( - np.array([(row[2], row[3], row[0], row[1], row[4]) for row in rows], dtype=np.float64) - ) + self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) + # MIT command columns are (kp, kd, q, dq, tau) per joint. + self._arm.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) self._robot.tick(self._tick_deadline_us) self._state_cache = None self._last_positions = list(q) @@ -456,8 +466,13 @@ def write_stop(self) -> bool: ) try: self._robot.disable() - except Exception as exc: - logger.warning(f"{type(self).__name__} {self._hardware_id} stop disable failed: {exc}") + except Exception: + logger.warning( + "damiao adapter stop disable failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, + ) return False self._enabled = False return True @@ -467,14 +482,23 @@ def write_enable(self, enable: bool) -> bool: return False try: self._robot.enable() if enable else self._robot.disable() - except Exception as exc: - logger.error(f"{type(self).__name__} {self._hardware_id} enable={enable} failed: {exc}") + except Exception: + logger.exception( + "damiao adapter enable failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + enable=enable, + ) return False self._enabled = enable if enable: positions = self.read_joint_positions() if not self.write_joint_positions(positions): - logger.error(f"{type(self).__name__} {self._hardware_id} startup hold failed") + logger.error( + "damiao adapter startup hold failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) return False return True @@ -484,19 +508,29 @@ def write_clear_errors(self) -> bool: try: self._robot.disable() self._robot.enable() - except Exception as exc: - logger.error(f"{type(self).__name__} {self._hardware_id} clear errors failed: {exc}") + except Exception: + logger.exception( + "damiao adapter clear errors failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) return False self._enabled = True positions = self.read_joint_positions() if not self.write_joint_positions(positions): - logger.error(f"{type(self).__name__} {self._hardware_id} clear-error hold failed") + logger.error( + "damiao adapter clear-error hold failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) return False return True def _load_gravity_model(self) -> None: if not self._gravity_comp or self._gravity_model_path is None: return + # Lazy import: pinocchio is a heavy optional dep only needed when + # gravity compensation is enabled, so keep it out of module import. import pinocchio build_model_from_urdf = _dynamic_attr(pinocchio, "buildModelFromUrdf") @@ -507,6 +541,8 @@ 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 [0.0] * self._dof + # Lazy import: pinocchio is a heavy optional dep only needed when + # gravity compensation is enabled, so keep it out of module import. import pinocchio compute_generalized_gravity = _dynamic_attr(pinocchio, "computeGeneralizedGravity") diff --git a/dimos/hardware/manipulators/damiao/test_base_adapter.py b/dimos/hardware/manipulators/damiao/test_base_adapter.py index 82357f94c4..400da6f7c5 100644 --- a/dimos/hardware/manipulators/damiao/test_base_adapter.py +++ b/dimos/hardware/manipulators/damiao/test_base_adapter.py @@ -101,7 +101,7 @@ def test_base_adapter_info_limits_and_modes() -> None: def test_base_adapter_validates_command_lengths() -> None: adapter = DamiaoArmAdapterBase(arm_spec=_arm_spec()) with pytest.raises(ValueError, match="q length 1 does not match dof 2"): - adapter._mit_command_rows( + adapter._validate_command_lengths( q=[0.0], dq=[0.0, 0.0], kp=[0.0, 0.0], diff --git a/dimos/hardware/manipulators/openarm/test_adapter.py b/dimos/hardware/manipulators/openarm/test_adapter.py index 2f15ef1ee1..66eb87cb69 100644 --- a/dimos/hardware/manipulators/openarm/test_adapter.py +++ b/dimos/hardware/manipulators/openarm/test_adapter.py @@ -14,6 +14,7 @@ from __future__ import annotations +import time from unittest.mock import MagicMock import pytest @@ -24,8 +25,6 @@ class FakeState: def __init__(self, index: int) -> None: - import time - self.q = 0.1 * index self.dq = 0.2 * index self.tau = 0.3 * index diff --git a/dimos/hardware/manipulators/openarm_rs/adapter.py b/dimos/hardware/manipulators/openarm_rs/adapter.py index 99d6f4337e..9a70c9c7dc 100644 --- a/dimos/hardware/manipulators/openarm_rs/adapter.py +++ b/dimos/hardware/manipulators/openarm_rs/adapter.py @@ -18,6 +18,9 @@ from typing import TYPE_CHECKING from dimos.hardware.manipulators.damiao.base_adapter import ( + _DEFAULT_ADDRESS, + _DEFAULT_STATE_CACHE_TTL_S, + _DEFAULT_TICK_DEADLINE_US, DamiaoArmAdapterBase, DamiaoBindingUnavailableError, ) @@ -26,8 +29,6 @@ if TYPE_CHECKING: from dimos.hardware.manipulators.registry import AdapterRegistry -OpenArmRSMotorSpecConfig = DamiaoMotorSpec - class OpenArmRSBindingUnavailableError(DamiaoBindingUnavailableError): pass @@ -55,7 +56,7 @@ class OpenArmRSAdapter(DamiaoArmAdapterBase): def __init__( self, - address: str | Path | None = "can0", + address: str | Path | None = _DEFAULT_ADDRESS, dof: int = 7, *, hardware_id: str = "arm", @@ -73,11 +74,10 @@ def __init__( kp: list[float] | None = None, kd: list[float] | None = None, gravity_comp: bool = True, - tick_deadline_us: int = 1_000, - state_cache_ttl_s: float = 0.002, + 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, - **_: object, ) -> None: if dof != len(self._DEFAULT_OPENARM_MOTORS): raise ValueError(f"OpenArmRSAdapter only supports 7 DOF (got {dof})") @@ -128,6 +128,5 @@ def register(registry: AdapterRegistry) -> None: __all__ = [ "OpenArmRSAdapter", "OpenArmRSBindingUnavailableError", - "OpenArmRSMotorSpecConfig", "register", ] diff --git a/dimos/robot/manipulators/openarm/blueprints.py b/dimos/robot/manipulators/openarm/blueprints.py index 90f20041c6..ecd60dd39d 100644 --- a/dimos/robot/manipulators/openarm/blueprints.py +++ b/dimos/robot/manipulators/openarm/blueprints.py @@ -59,12 +59,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, - "gravity_comp": True, - "canfd": True, -} +_OPENARM_RS_ADAPTER_KWARGS = {"gravity_model_path": OPENARM_V10_RIGHT_MODEL} _left_hw = _openarm( side="left", address=LEFT_CAN, diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index d75f7ce0a0..8d1f676538 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -258,12 +258,7 @@ No other code changes are needed. The `coordinator-openarm-rs` and `openarm-rs-p ### Gain tuning (MIT kp/kd) -Defaults live in the adapter implementations. The binding-backed `openarm_rs` path uses the upstream OpenArm ROS2 hardware presets from `openarm_hardware/openarm_simple_hardware.hpp`; the in-tree `openarm` adapter keeps its existing gains. OpenArm RS/OpenArm ROS2 presets are: - -```python -_DEFAULT_KP = [70.0, 70.0, 70.0, 60.0, 10.0, 10.0, 10.0] -_DEFAULT_KD = [2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5] -``` +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.