diff --git a/data/.lfs/openarm_description.tar.gz b/data/.lfs/openarm_description.tar.gz index 54aa76da41..4a46e74a88 100644 --- a/data/.lfs/openarm_description.tar.gz +++ b/data/.lfs/openarm_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4da176b6c210b9796bb2ee1a29c15ee9a67578b9ae906eb89a6ec8a44b7f303a -size 70064687 +oid sha256:3e9a568ec8bded5ca32b2e3de92d27ab78732bb6f2bf4d3d6d16e5093ca30997 +size 8095302 diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 3f93d00265..27c0a56a2c 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -42,7 +42,9 @@ pytestmark = [pytest.mark.self_hosted_large] JOINT_STATE_TOPIC = "/coordinator_joint_state#sensor_msgs.JointState" -BLUEPRINT = "openarm-mock-planner-coordinator" +# The e2e harness always passes --simulation (DimosCliCall.simulator), so the +# blueprint's hardware selection resolves to the in-memory whole-body adapter. +BLUEPRINT = "openarm-planner-coordinator" def _wait_for_groups( @@ -155,7 +157,8 @@ def test_single_arm_plans_and_executes_through_control_coordinator( client = RPCClient(None, ManipulationModule) coordinator_client = RPCClient(None, ControlCoordinator) try: - [left] = _wait_for_groups(client, 1) + groups = _wait_for_groups(client, 2) + [left] = [group for group in groups if group.id.endswith("/left_manipulator")] left_id = left.id tasks = coordinator_client.list_tasks() @@ -187,7 +190,9 @@ def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( coordinator_client = RPCClient(None, ControlCoordinator) try: groups = _wait_for_groups(client, 2) - left_id, right_id = (group.id for group in groups) + group_ids = {group.id.rsplit("/", 1)[-1]: group.id for group in groups} + left_id = group_ids["left_manipulator"] + right_id = group_ids["right_manipulator"] tasks = coordinator_client.list_tasks() assert tasks == [JOINT_TRAJECTORY_TASK_NAME] diff --git a/dimos/hardware/manipulators/openarm/adapter.py b/dimos/hardware/manipulators/openarm/adapter.py deleted file mode 100644 index 4881e03b50..0000000000 --- a/dimos/hardware/manipulators/openarm/adapter.py +++ /dev/null @@ -1,430 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""OpenArm ManipulatorAdapter — wraps the Damiao MIT-mode driver. SI units.""" - -from __future__ import annotations - -from pathlib import Path -import time -from typing import Any - -import numpy as np - -from dimos.hardware.manipulators.openarm.driver import ( - CTRL_MODE_MIT, - DamiaoMotor, - MotorType, - OpenArmBus, -) -from dimos.hardware.manipulators.spec import ( - ControlMode, - JointLimits, - ManipulatorInfo, -) -from dimos.utils.data import LfsPath - - -def _socketcan_iface_up(name: str) -> bool: - try: - flags_path = Path("/sys/class/net") / name / "flags" - if not flags_path.exists(): - return False - return (int(flags_path.read_text().strip(), 16) & 0x1) == 0x1 - except OSError: - return False - - -# OpenArm v10 BOM — (send_id, MotorType) per joint, derived from the torque -# column of data/openarm_description/config/arm/v10/joint_limits.yaml. -_OPENARM_V10_ARM_MOTORS: list[tuple[int, MotorType]] = [ - (0x01, MotorType.DM8006), # joint1 - (0x02, MotorType.DM8006), # joint2 - (0x03, MotorType.DM4340), # joint3 - (0x04, MotorType.DM4340), # joint4 - (0x05, MotorType.DM4310), # joint5 - (0x06, MotorType.DM4310), # joint6 - (0x07, MotorType.DM4310), # joint7 -] -# Gripper (motor id 0x08, DM4310) is on the bus but not currently wired up -# through the adapter — see the gripper-write methods which return None/False. - -# Physical joint limits (measured). Joints 1 & 2 are mirrored between sides. -_V10_POS_LOWER_LEFT = [-3.45, -3.30, -1.50, -0.01, -1.50, -0.75, -1.50] -_V10_POS_UPPER_LEFT = [1.35, 0.15, 1.50, 2.40, 1.50, 0.75, 1.50] -_V10_POS_LOWER_RIGHT = [-1.35, -0.15, -1.50, -0.01, -1.50, -0.75, -1.50] -_V10_POS_UPPER_RIGHT = [3.45, 3.30, 1.50, 2.40, 1.50, 0.75, 1.50] -_V10_VEL_MAX = [16.754666, 16.754666, 5.445426, 5.445426, 20.943946, 20.943946, 20.943946] - -# Default MIT gains per joint for POSITION mode. -# kp range is [0, 500], kd range is [0, 5]. -# With gravity compensation enabled, the PD gains only handle transient -# tracking — they don't fight gravity. Lower kp = smoother, less buzz. -# High kd causes high-frequency buzz/grinding from the gearbox. -_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] -_STATE_MAX_AGE_S = 0.1 - - -class OpenArmAdapter: - """7-DOF OpenArm on one SocketCAN bus. side=left|right picks URDF + limits.""" - - # Per-side URDFs for Pinocchio gravity model (LFS-backed) - _URDF_LEFT = LfsPath("openarm_description/urdf/robot/openarm_v10_left.urdf") - _URDF_RIGHT = LfsPath("openarm_description/urdf/robot/openarm_v10_right.urdf") - - def __init__( - self, - address: str = "can0", - dof: int = 7, - *, - side: str = "left", - fd: bool = False, - interface: str = "socketcan", - kp: list[float] | None = None, - kd: list[float] | None = None, - gravity_comp: bool = True, - auto_set_mit_mode: bool = True, - **_: Any, - ) -> None: - if dof != 7: - raise ValueError(f"OpenArmAdapter only supports 7 DOF (got {dof})") - if side not in ("left", "right"): - raise ValueError(f"side must be 'left' or 'right', got {side!r}") - self._address = address - self._dof = dof - self._side = side - self._fd = fd - self._interface = interface - self._kp = list(kp) if kp is not None else list(_DEFAULT_KP) - self._kd = list(kd) if kd is not None else list(_DEFAULT_KD) - if len(self._kp) != dof or len(self._kd) != dof: - raise ValueError("kp/kd must be length 7") - self._gravity_comp = gravity_comp - self._auto_set_mit_mode = auto_set_mit_mode - - self._motors = [DamiaoMotor(sid, mt) for sid, mt in _OPENARM_V10_ARM_MOTORS] - self._bus: OpenArmBus | None = None - self._control_mode: ControlMode = ControlMode.POSITION - self._enabled: bool = False - # Last successful position command — used as q_target for VELOCITY mode - self._last_cmd_q: list[float] | None = None - - # Pinocchio model for gravity compensation (loaded lazily in connect()) - self._pin_model: Any = None - self._pin_data: Any = None - - def connect(self) -> bool: - # Preflight: verify the SocketCAN interface is up before opening the bus. - # Bringing the interface up requires root privileges, so we don't do it - # here — just fail early with a helpful message. - if self._interface == "socketcan" and not _socketcan_iface_up(self._address): - print( - f"ERROR: SocketCAN interface '{self._address}' is not UP.\n" - f" Run: sudo ip link set {self._address} up type can bitrate 1000000\n" - f" (or: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {self._address})" - ) - return False - - try: - self._bus = OpenArmBus( - channel=self._address, - motors=self._motors, - fd=self._fd, - interface=self._interface, - ) - self._bus.open() - except Exception as e: - print(f"ERROR: OpenArm {self._side}@{self._address} connect failed: {e}") - self._bus = None - return False - - # Ensure every motor is in MIT control mode. The write is idempotent - # (setting CTRL_MODE=MIT when it's already MIT is a no-op), so we - # write unconditionally rather than query-then-write. - if self._auto_set_mit_mode: - try: - for m in self._motors: - self._bus.write_ctrl_mode(m.send_id, CTRL_MODE_MIT) - except Exception as e: - print(f"ERROR: failed to set MIT mode on {self._address}: {e}") - self._bus.close() - self._bus = None - return False - else: - print( - f"OpenArm {self._side}@{self._address}: " - "auto_set_mit_mode disabled — relying on persisted register" - ) - - # Load Pinocchio model for gravity compensation - if self._gravity_comp: - try: - import pinocchio - - urdf = str(self._URDF_LEFT if self._side == "left" else self._URDF_RIGHT) - self._pin_model = pinocchio.buildModelFromUrdf(urdf) - self._pin_data = self._pin_model.createData() - print( - f"OpenArm {self._side}: gravity compensation enabled (nq={self._pin_model.nq})" - ) - except Exception as e: - print(f"WARNING: gravity comp disabled — {e}") - self._pin_model = None - self._pin_data = None - - return True - - def disconnect(self) -> None: - if self._bus is None: - return - try: - self._bus.disable_all() - except Exception: - pass - self._enabled = False - self._bus.close() - self._bus = None - - def is_connected(self) -> bool: - return self._bus is not None - - 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="Enactic", - model=f"OpenArm v10 ({self._side})", - dof=self._dof, - firmware_version=None, - serial_number=None, - ) - - def get_dof(self) -> int: - return self._dof - - def get_limits(self) -> JointLimits: - if self._side == "left": - lower, upper = _V10_POS_LOWER_LEFT, _V10_POS_UPPER_LEFT - else: - lower, upper = _V10_POS_LOWER_RIGHT, _V10_POS_UPPER_RIGHT - return JointLimits( - position_lower=list(lower), - position_upper=list(upper), - velocity_max=list(_V10_VEL_MAX), - ) - - def set_control_mode(self, mode: ControlMode) -> bool: - # OpenArm runs exclusively in Damiao MIT register mode; we emulate - # dimos ControlModes by tuning kp/kd/q/dq/tau on each MIT frame. - # Cartesian/impedance control are outside this adapter's scope. - if mode in ( - ControlMode.POSITION, - ControlMode.SERVO_POSITION, - ControlMode.VELOCITY, - ControlMode.TORQUE, - ): - self._control_mode = mode - return True - return False - - def get_control_mode(self) -> ControlMode: - return self._control_mode - - def _states_or_raise(self) -> list[Any]: - # Raises on missing or stale data so hardware_interface.py can retry - # (init) or skip the tick (steady-state). - if self._bus is None: - raise RuntimeError("OpenArmAdapter not connected") - now = time.monotonic() - states = self._bus.get_states() - for i, s in enumerate(states): - if s is None: - raise RuntimeError(f"motor {i + 1} has no state yet") - if now - s.timestamp > _STATE_MAX_AGE_S: - age_ms = (now - s.timestamp) * 1000 - raise RuntimeError(f"motor {i + 1} state stale ({age_ms:.0f} ms)") - return states - - def read_joint_positions(self) -> list[float]: - return [s.q for s in self._states_or_raise()] - - def read_joint_velocities(self) -> list[float]: - return [s.dq for s in self._states_or_raise()] - - def read_joint_efforts(self) -> list[float]: - return [s.tau for s in self._states_or_raise()] - - def read_state(self) -> dict[str, int]: - if self._bus is None: - return {"state": 0, "mode": 0} - states = self._bus.get_states() - # report the hottest rotor temperature so callers can monitor thermal - # stress with a single scalar - t_rotor = max((s.t_rotor for s in states if s is not None), default=0) - return { - "state": 1 if self._enabled else 0, - "mode": 1, # MIT - "t_rotor_max": int(t_rotor), - } - - def read_error(self) -> tuple[int, str]: - # The Damiao motors don't report a structured error code in the state - # frame; over-temperature / over-torque are detected by the host from - # the normal state fields. Surface a soft thermal warning here. - if self._bus is None: - return 0, "" - states = self._bus.get_states() - t_rotor = max((s.t_rotor for s in states if s is not None), default=0) - if t_rotor >= 85: - return 1, f"rotor over-temperature ({t_rotor}°C)" - return 0, "" - - def _compute_gravity_torques(self, q: list[float]) -> list[float]: - # Pinocchio G(q), clamped to motor torque limits. - if self._pin_model is None or self._pin_data is None: - return [0.0] * self._dof - import pinocchio - - q_arr = np.array(q, dtype=np.float64) - tau_g = pinocchio.computeGeneralizedGravity(self._pin_model, self._pin_data, q_arr) - # Clamp to motor torque limits for safety - limits = [m.limits for m in self._motors] # (p_max, v_max, t_max) - return [float(np.clip(tau_g[i], -lim[2], lim[2])) for i, lim in enumerate(limits)] - - def write_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - ) -> bool: - if self._bus is None or not self._enabled: - return False - if len(positions) != self._dof: - return False - velocity = max(0.0, min(1.0, velocity)) - # Gravity feedforward: compute tau needed to hold the arm at the - # current configuration. The PD gains handle the rest. Tolerate - # transient state-cache misses (e.g. startup, brief CAN gap) — fall - # back to commanded q with no feedforward instead of crashing. - try: - q_current = self.read_joint_positions() - tau_ff = self._compute_gravity_torques(q_current) - except RuntimeError: - tau_ff = [0.0] * self._dof - commands = [ - (q, 0.0, kp * velocity, kd, tau) - for q, kp, kd, tau in zip(positions, self._kp, self._kd, tau_ff, strict=False) - ] - self._bus.send_mit_many(commands) - self._last_cmd_q = list(positions) - return True - - def write_joint_velocities(self, velocities: list[float]) -> bool: - # MIT velocity tracking: kp=0, send dq directly, anchor q at the - # last-commanded position so the motor doesn't drift. Gravity - # feedforward is still needed — with kp=0 the only restoring force - # is damping, so without tau_ff the arm droops under its own weight. - if self._bus is None or not self._enabled: - return False - if len(velocities) != self._dof: - return False - # Seed anchor from current pose if we don't have a last-commanded one. - # If state isn't ready yet, can't safely anchor velocity tracking → bail. - if self._last_cmd_q is None: - try: - self._last_cmd_q = self.read_joint_positions() - except RuntimeError: - return False - anchor = self._last_cmd_q - try: - q_current = self.read_joint_positions() - tau_ff = self._compute_gravity_torques(q_current) - except RuntimeError: - tau_ff = [0.0] * self._dof - commands = [ - (q_anchor, dq, 0.0, kd, tau) - for q_anchor, dq, kd, tau in zip(anchor, velocities, self._kd, tau_ff, strict=False) - ] - self._bus.send_mit_many(commands) - return True - - def write_stop(self) -> bool: - if self._bus is None: - return False - # Without current positions we can't safely command "hold here" — sending - # any guessed q would torque the arm toward that pose. Bail out instead. - try: - q_now = self.read_joint_positions() - except RuntimeError: - return False - tau_ff = self._compute_gravity_torques(q_now) - commands = [ - (q, 0.0, kp, kd, tau) - for q, kp, kd, tau in zip(q_now, self._kp, self._kd, tau_ff, strict=False) - ] - self._bus.send_mit_many(commands) - self._last_cmd_q = q_now - return True - - def write_enable(self, enable: bool) -> bool: - if self._bus is None: - return False - self._enabled = False - try: - if enable: - self._bus.enable_all() - else: - self._bus.disable_all() - except Exception: - return False - self._enabled = enable - return True - - def read_enabled(self) -> bool: - return self._enabled - - def write_clear_errors(self) -> bool: - # Damiao motors have no separate clear-error command; re-enabling - # after a fault is the recovery path. - if self._bus is None: - return False - self._enabled = False - try: - self._bus.disable_all() - self._bus.enable_all() - except Exception: - return False - self._enabled = True - return True - - 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 diff --git a/dimos/hardware/manipulators/openarm/driver.py b/dimos/hardware/manipulators/openarm/driver.py deleted file mode 100644 index f7c9243cfa..0000000000 --- a/dimos/hardware/manipulators/openarm/driver.py +++ /dev/null @@ -1,329 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Damiao MIT-mode CAN driver for OpenArm. SI units throughout. - -Ported from ``enactic/openarm_can`` (C++). No dimos deps — testable with -``can.Bus(interface="virtual")``. -""" - -from __future__ import annotations - -from dataclasses import dataclass -import enum -import errno -import struct -import threading -import time -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - import can - - -class MotorType(str, enum.Enum): - """Damiao motor types used on OpenArm. Values match the reference library.""" - - DM3507 = "DM3507" - DM4310 = "DM4310" - DM4310_48V = "DM4310_48V" - DM4340 = "DM4340" - DM4340_48V = "DM4340_48V" - DM6006 = "DM6006" - DM8006 = "DM8006" - DM8009 = "DM8009" - DM10010L = "DM10010L" - DM10010 = "DM10010" - DMH3510 = "DMH3510" - DMH6215 = "DMH6215" - DMG6220 = "DMG6220" - - -# (p_max [rad], v_max [rad/s], t_max [Nm]) -_MOTOR_LIMITS: dict[MotorType, tuple[float, float, float]] = { - MotorType.DM3507: (12.5, 50.0, 5.0), - MotorType.DM4310: (12.5, 30.0, 10.0), - MotorType.DM4310_48V: (12.5, 50.0, 10.0), - MotorType.DM4340: (12.5, 8.0, 28.0), - MotorType.DM4340_48V: (12.5, 10.0, 28.0), - MotorType.DM6006: (12.5, 45.0, 20.0), - MotorType.DM8006: (12.5, 45.0, 40.0), - MotorType.DM8009: (12.5, 45.0, 54.0), - MotorType.DM10010L: (12.5, 25.0, 200.0), - MotorType.DM10010: (12.5, 20.0, 200.0), - MotorType.DMH3510: (12.5, 280.0, 1.0), - MotorType.DMH6215: (12.5, 45.0, 10.0), - MotorType.DMG6220: (12.5, 45.0, 10.0), -} - -# MIT gain ranges (protocol-fixed, same for every motor type) -KP_MIN, KP_MAX = 0.0, 500.0 -KD_MIN, KD_MAX = 0.0, 5.0 - -# Broadcast/control CAN IDs -_BROADCAST_ID = 0x7FF -_CMD_ENABLE = 0xFC -_CMD_DISABLE = 0xFD -_RID_CTRL_MODE = 10 -CTRL_MODE_MIT = 1 - - -def _clamp(x: float, lo: float, hi: float) -> float: - if x < lo: - return lo - if x > hi: - return hi - return x - - -def float_to_uint(x: float, lo: float, hi: float, bits: int) -> int: - x = _clamp(x, lo, hi) - return int((x - lo) / (hi - lo) * ((1 << bits) - 1)) - - -def uint_to_float(u: int, lo: float, hi: float, bits: int) -> float: - return u / ((1 << bits) - 1) * (hi - lo) + lo - - -def pack_mit_frame( - motor_type: MotorType, - q: float, - dq: float, - kp: float, - kd: float, - tau: float, -) -> bytes: - p_max, v_max, t_max = _MOTOR_LIMITS[motor_type] - q_u = float_to_uint(q, -p_max, p_max, 16) - dq_u = float_to_uint(dq, -v_max, v_max, 12) - kp_u = float_to_uint(kp, KP_MIN, KP_MAX, 12) - kd_u = float_to_uint(kd, KD_MIN, KD_MAX, 12) - tau_u = float_to_uint(tau, -t_max, t_max, 12) - return bytes( - [ - (q_u >> 8) & 0xFF, - q_u & 0xFF, - (dq_u >> 4) & 0xFF, - ((dq_u & 0xF) << 4) | ((kp_u >> 8) & 0xF), - kp_u & 0xFF, - (kd_u >> 4) & 0xFF, - ((kd_u & 0xF) << 4) | ((tau_u >> 8) & 0xF), - tau_u & 0xFF, - ] - ) - - -@dataclass(frozen=True) -class MotorState: - """Decoded state from a Damiao reply frame.""" - - q: float # rad - dq: float # rad/s - tau: float # Nm - t_mos: int # °C - t_rotor: int # °C - timestamp: float # monotonic seconds when received - - -def parse_state_frame(motor_type: MotorType, data: bytes) -> MotorState | None: - """Decode an 8-byte Damiao state reply. Returns None if too short.""" - if len(data) < 8: - return None - p_max, v_max, t_max = _MOTOR_LIMITS[motor_type] - q_u = (data[1] << 8) | data[2] - dq_u = (data[3] << 4) | (data[4] >> 4) - tau_u = ((data[4] & 0x0F) << 8) | data[5] - return MotorState( - q=uint_to_float(q_u, -p_max, p_max, 16), - dq=uint_to_float(dq_u, -v_max, v_max, 12), - tau=uint_to_float(tau_u, -t_max, t_max, 12), - t_mos=int(data[6]), - t_rotor=int(data[7]), - timestamp=time.monotonic(), - ) - - -def _pack_control_command(cmd: int) -> bytes: - return bytes([0xFF] * 7 + [cmd & 0xFF]) - - -def pack_write_param_frame(send_id: int, rid: int, value_u32: int) -> bytes: - """Broadcast parameter-write frame sent to CAN id 0x7FF.""" - val = struct.pack("> 8) & 0xFF, - 0x55, - rid & 0xFF, - val[0], - val[1], - val[2], - val[3], - ] - ) - - -@dataclass(frozen=True) -class DamiaoMotor: - """One Damiao motor on a CAN bus. recv_id defaults to send_id | 0x10.""" - - send_id: int - motor_type: MotorType - recv_id: int | None = None - - @property - def effective_recv_id(self) -> int: - return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) - - @property - def limits(self) -> tuple[float, float, float]: - return _MOTOR_LIMITS[self.motor_type] - - -class OpenArmBus: - """One SocketCAN bus with a background RX thread caching latest state.""" - - def __init__( - self, - channel: str, - motors: list[DamiaoMotor], - *, - fd: bool = False, - interface: str = "socketcan", - ) -> None: - if not motors: - raise ValueError("OpenArmBus needs at least one motor") - # Enforce unique IDs — silent overlap would make state routing ambiguous. - send_ids = [m.send_id for m in motors] - if len(set(send_ids)) != len(send_ids): - raise ValueError(f"duplicate send_id in {send_ids}") - recv_ids = [m.effective_recv_id for m in motors] - if len(set(recv_ids)) != len(recv_ids): - raise ValueError(f"duplicate recv_id in {recv_ids}") - - self._channel = channel - self._motors = list(motors) - self._fd = fd - self._interface = interface - self._by_recv: dict[int, DamiaoMotor] = {m.effective_recv_id: m for m in motors} - - self._bus: can.BusABC | None = None - self._rx_thread: threading.Thread | None = None - self._rx_stop = threading.Event() - self._state_lock = threading.Lock() - self._states: dict[int, MotorState] = {} - - def open(self) -> None: - """Open the CAN bus and start the background RX thread.""" - if self._bus is not None: - return - import can # local import — python-can is optional - - self._bus = can.Bus(interface=self._interface, channel=self._channel, fd=self._fd) - self._rx_stop.clear() - self._rx_thread = threading.Thread( - target=self._rx_loop, name=f"openarm-rx-{self._channel}", daemon=True - ) - self._rx_thread.start() - - def close(self) -> None: - """Stop the RX thread and close the CAN bus.""" - self._rx_stop.set() - if self._rx_thread is not None: - self._rx_thread.join(timeout=1.0) - self._rx_thread = None - if self._bus is not None: - try: - self._bus.shutdown() - finally: - self._bus = None - - def enable_all(self) -> None: - for m in self._motors: - self._send_raw(m.send_id, _pack_control_command(_CMD_ENABLE)) - - def disable_all(self) -> None: - for m in self._motors: - self._send_raw(m.send_id, _pack_control_command(_CMD_DISABLE)) - - def write_ctrl_mode(self, send_id: int, mode: int = CTRL_MODE_MIT) -> None: - self._send_raw( - _BROADCAST_ID, - pack_write_param_frame(send_id, _RID_CTRL_MODE, mode), - ) - - def send_mit_many( - self, - commands: list[tuple[float, float, float, float, float]], - ) -> None: - """One MIT frame per motor; commands[i] → self.motors[i] = (q, dq, kp, kd, tau).""" - if len(commands) != len(self._motors): - raise ValueError(f"expected {len(self._motors)} commands, got {len(commands)}") - for motor, cmd in zip(self._motors, commands, strict=False): - q, dq, kp, kd, tau = cmd - data = pack_mit_frame(motor.motor_type, q, dq, kp, kd, tau) - self._send_raw(motor.send_id, data) - - def get_state(self, send_id: int) -> MotorState | None: - motor = next((m for m in self._motors if m.send_id == send_id), None) - if motor is None: - return None - with self._state_lock: - return self._states.get(motor.effective_recv_id) - - def get_states(self) -> list[MotorState | None]: - with self._state_lock: - return [self._states.get(m.effective_recv_id) for m in self._motors] - - def _send_raw(self, arbitration_id: int, data: bytes) -> None: - if self._bus is None: - raise RuntimeError("bus not open — call .open() first") - import can - - msg = can.Message( - arbitration_id=arbitration_id, - data=data, - is_extended_id=False, - is_fd=self._fd, - bitrate_switch=self._fd, - ) - # Retry on TX buffer full (ENOBUFS) — gs_usb's kernel-side TX queue - # is small. python-can chains the OSError via `raise ... from`, - # so the original errno is on __cause__. - for attempt in range(4): - try: - self._bus.send(msg) - return - except can.CanOperationError as e: - cause = e.__cause__ or e - if getattr(cause, "errno", None) == errno.ENOBUFS and attempt < 3: - time.sleep(0.001 * (attempt + 1)) - else: - raise - - def _rx_loop(self) -> None: - assert self._bus is not None - while not self._rx_stop.is_set(): - msg = self._bus.recv(timeout=0.05) - if msg is None: - continue - motor = self._by_recv.get(int(msg.arbitration_id)) - if motor is None: - continue - state = parse_state_frame(motor.motor_type, bytes(msg.data)) - if state is None: - continue - with self._state_lock: - self._states[motor.effective_recv_id] = state diff --git a/dimos/hardware/manipulators/openarm/test_driver.py b/dimos/hardware/manipulators/openarm/test_driver.py deleted file mode 100644 index c65a972bd6..0000000000 --- a/dimos/hardware/manipulators/openarm/test_driver.py +++ /dev/null @@ -1,270 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Unit tests for the Damiao MIT-mode driver — no hardware required. - -Uses ``can.Bus(interface="virtual")`` for loopback. -""" - -from __future__ import annotations - -import struct -import time - -import pytest - -can = pytest.importorskip("can") - -from dimos.hardware.manipulators.openarm.driver import ( - CTRL_MODE_MIT, - KD_MAX, - KP_MAX, - DamiaoMotor, - MotorType, - OpenArmBus, - float_to_uint, - pack_mit_frame, - pack_write_param_frame, - parse_state_frame, - uint_to_float, -) - - -def test_float_to_uint_endpoints_and_roundtrip() -> None: - # Endpoints - assert float_to_uint(-12.5, -12.5, 12.5, 16) == 0 - assert float_to_uint(12.5, -12.5, 12.5, 16) == (1 << 16) - 1 - # Midpoint is half the full range (rounded down) - mid = float_to_uint(0.0, -12.5, 12.5, 16) - assert mid in ((1 << 16) // 2 - 1, (1 << 16) // 2) - # Out-of-range clamps - assert float_to_uint(-100.0, -12.5, 12.5, 16) == 0 - assert float_to_uint(100.0, -12.5, 12.5, 16) == (1 << 16) - 1 - - -def test_roundtrip_all_gain_ranges() -> None: - # Quantization error should be tiny - for bits, lo, hi in [(16, -12.5, 12.5), (12, 0.0, KP_MAX), (12, 0.0, KD_MAX)]: - step = (hi - lo) / ((1 << bits) - 1) - for k in range(0, 1 << bits, max(1, (1 << bits) // 50)): - x = lo + k * step - u = float_to_uint(x, lo, hi, bits) - x2 = uint_to_float(u, lo, hi, bits) - assert abs(x - x2) <= step - - -def test_mit_frame_kp_kd_zero_and_pos_zero() -> None: - # q=dq=kp=kd=tau=0 → q_u = 32767 (16-bit midpoint), dq_u = 2047 (12-bit), - # tau_u = 2047. kp_u = kd_u = 0 (min of their 0-positive range). - data = pack_mit_frame(MotorType.DM4310, 0.0, 0.0, 0.0, 0.0, 0.0) - assert len(data) == 8 - # Reconstruct fields from bytes - q_u = (data[0] << 8) | data[1] - dq_u = (data[2] << 4) | (data[3] >> 4) - kp_u = ((data[3] & 0xF) << 8) | data[4] - kd_u = (data[5] << 4) | (data[6] >> 4) - tau_u = ((data[6] & 0xF) << 8) | data[7] - assert kp_u == 0 - assert kd_u == 0 - # 16-bit midpoint of symmetric range - assert q_u in (32767, 32768) - assert dq_u in (2047, 2048) - assert tau_u in (2047, 2048) - - -def test_mit_frame_full_positive() -> None: - # Command at every max → every _u field saturates. - data = pack_mit_frame(MotorType.DM4310, 12.5, 30.0, 500.0, 5.0, 10.0) - q_u = (data[0] << 8) | data[1] - dq_u = (data[2] << 4) | (data[3] >> 4) - kp_u = ((data[3] & 0xF) << 8) | data[4] - kd_u = (data[5] << 4) | (data[6] >> 4) - tau_u = ((data[6] & 0xF) << 8) | data[7] - assert q_u == 0xFFFF - assert dq_u == 0xFFF - assert kp_u == 0xFFF - assert kd_u == 0xFFF - assert tau_u == 0xFFF - - -def test_parse_state_roundtrip() -> None: - # Build a synthetic reply frame with known values and verify decode. - # Byte layout for state: [echo, q_hi, q_lo, dq_hi, dq_lo|tau_hi, tau_lo, t_mos, t_rotor] - motor = MotorType.DM4340 - p_max, v_max, t_max = 12.5, 8.0, 28.0 - q_u = float_to_uint(0.3, -p_max, p_max, 16) - dq_u = float_to_uint(-1.0, -v_max, v_max, 12) - tau_u = float_to_uint(2.0, -t_max, t_max, 12) - data = bytes( - [ - 0x03, - (q_u >> 8) & 0xFF, - q_u & 0xFF, - (dq_u >> 4) & 0xFF, - ((dq_u & 0xF) << 4) | ((tau_u >> 8) & 0xF), - tau_u & 0xFF, - 33, - 28, - ] - ) - state = parse_state_frame(motor, data) - assert state is not None - assert abs(state.q - 0.3) < 0.001 - assert abs(state.dq - (-1.0)) < 0.01 - assert abs(state.tau - 2.0) < 0.02 - assert state.t_mos == 33 - assert state.t_rotor == 28 - - -def test_parse_state_rejects_short_frames() -> None: - assert parse_state_frame(MotorType.DM4310, b"\x00" * 4) is None - - -def test_pack_write_param_ctrl_mode_mit() -> None: - data = pack_write_param_frame(0x05, 10, CTRL_MODE_MIT) - assert data[0] == 0x05 - assert data[1] == 0x00 - assert data[2] == 0x55 - assert data[3] == 10 - assert struct.unpack(" OpenArmBus: - return OpenArmBus(channel=channel, motors=motors, fd=False, interface="virtual") - - -def test_bus_validates_unique_ids() -> None: - with pytest.raises(ValueError, match="duplicate send_id"): - OpenArmBus( - channel="v0", - motors=[ - DamiaoMotor(0x01, MotorType.DM4310), - DamiaoMotor(0x01, MotorType.DM4310), - ], - fd=False, - interface="virtual", - ) - - -def test_bus_empty_motor_list_rejected() -> None: - with pytest.raises(ValueError): - OpenArmBus(channel="v0", motors=[], fd=False, interface="virtual") - - -def test_rx_thread_populates_state_cache() -> None: - # Two peers on the same virtual channel loop back to each other. - motors = [ - DamiaoMotor(0x01, MotorType.DM8006), - DamiaoMotor(0x05, MotorType.DM4310), - ] - bus = _make_bus("openarm-test-rx", motors) - # A raw sender on the same virtual channel injects state replies. - sender = can.Bus(interface="virtual", channel="openarm-test-rx") - try: - bus.open() - # Forge a reply for motor 0x01 (recv 0x11) at q = 0.25 rad - q_u = float_to_uint(0.25, -12.5, 12.5, 16) - dq_u = float_to_uint(0.0, -45.0, 45.0, 12) - tau_u = float_to_uint(0.0, -40.0, 40.0, 12) - payload = bytes( - [ - 0x01, - (q_u >> 8) & 0xFF, - q_u & 0xFF, - (dq_u >> 4) & 0xFF, - ((dq_u & 0xF) << 4) | ((tau_u >> 8) & 0xF), - tau_u & 0xFF, - 30, - 28, - ] - ) - sender.send(can.Message(arbitration_id=0x11, data=payload, is_extended_id=False)) - # Poll briefly for the RX thread to consume it - deadline = time.monotonic() + 0.5 - s = None - while s is None and time.monotonic() < deadline: - s = bus.get_state(0x01) - time.sleep(0.01) - assert s is not None, "RX thread did not pick up synthetic state reply" - assert abs(s.q - 0.25) < 0.001 - # Motor 0x05 never got a reply → state should still be None - assert bus.get_state(0x05) is None - finally: - bus.close() - sender.shutdown() - - -def test_send_mit_many_fans_out_one_per_motor() -> None: - motors = [ - DamiaoMotor(0x01, MotorType.DM8006), - DamiaoMotor(0x02, MotorType.DM8006), - DamiaoMotor(0x05, MotorType.DM4310), - ] - bus = _make_bus("openarm-test-send", motors) - listener = can.Bus(interface="virtual", channel="openarm-test-send") - try: - bus.open() - bus.send_mit_many( - [ - (0.1, 0.0, 10.0, 0.5, 0.0), - (0.2, 0.0, 10.0, 0.5, 0.0), - (0.3, 0.0, 10.0, 0.5, 0.0), - ] - ) - seen_ids: set[int] = set() - deadline = time.monotonic() + 0.5 - while len(seen_ids) < 3 and time.monotonic() < deadline: - msg = listener.recv(timeout=0.1) - if msg is not None: - seen_ids.add(int(msg.arbitration_id)) - assert seen_ids == {0x01, 0x02, 0x05} - finally: - bus.close() - listener.shutdown() - - -def test_send_mit_many_size_mismatch() -> None: - bus = _make_bus( - "openarm-test-mismatch", - [DamiaoMotor(0x01, MotorType.DM4310), DamiaoMotor(0x02, MotorType.DM4310)], - ) - try: - bus.open() - with pytest.raises(ValueError): - bus.send_mit_many([(0.0, 0.0, 0.0, 0.0, 0.0)]) - finally: - bus.close() - - -def test_enable_disable_frames_sent() -> None: - bus = _make_bus( - "openarm-test-enable", - [DamiaoMotor(0x01, MotorType.DM4310), DamiaoMotor(0x05, MotorType.DM4310)], - ) - listener = can.Bus(interface="virtual", channel="openarm-test-enable") - try: - bus.open() - bus.enable_all() - seen = {} - deadline = time.monotonic() + 0.3 - while len(seen) < 2 and time.monotonic() < deadline: - msg = listener.recv(timeout=0.1) - if msg is not None: - seen[int(msg.arbitration_id)] = bytes(msg.data) - assert set(seen) == {0x01, 0x05} - for data in seen.values(): - assert data == bytes([0xFF] * 7 + [0xFC]) - finally: - bus.close() - listener.shutdown() diff --git a/dimos/hardware/manipulators/test_adapter_lifecycle.py b/dimos/hardware/manipulators/test_adapter_lifecycle.py index d56f25c48a..c0ac135c1c 100644 --- a/dimos/hardware/manipulators/test_adapter_lifecycle.py +++ b/dimos/hardware/manipulators/test_adapter_lifecycle.py @@ -19,14 +19,12 @@ from typing import Any import pytest -from typing_extensions import override piper_sdk_module = ModuleType("piper_sdk") piper_sdk_module.__dict__["C_PiperInterface_V2"] = lambda **_: None sys.modules.setdefault("piper_sdk", piper_sdk_module) from dimos.hardware.manipulators.a750.adapter import A750Adapter -from dimos.hardware.manipulators.openarm.adapter import OpenArmAdapter from dimos.hardware.manipulators.piper import adapter as piper_adapter from dimos.hardware.manipulators.piper.adapter import PiperAdapter @@ -149,54 +147,6 @@ def test_piper_gripper_uses_sdk_units_and_clamps(piper_sdk: Any) -> None: assert piper_sdk.GripperCtrl.call_args.args[0] == 80_000 -class _OpenArmLifecycle: - def __init__(self) -> None: - self.actions: list[str] = [] - - def enable_all(self) -> None: - self.actions.append("enable") - - def disable_all(self) -> None: - self.actions.append("disable") - - -class _LifecycleOpenArmAdapter(OpenArmAdapter): - def __init__(self, lifecycle: _OpenArmLifecycle) -> None: - super().__init__() - self._lifecycle: _OpenArmLifecycle - self._lifecycle = lifecycle - - @override - def read_joint_positions(self) -> list[float]: - return [0.0] * 7 - - @override - def _compute_gravity_torques(self, q: list[float]) -> list[float]: - return [0.0] * len(q) - - @override - def write_enable(self, enable: bool) -> bool: - if enable: - self._lifecycle.enable_all() - else: - self._lifecycle.disable_all() - return True - - @override - def write_stop(self) -> bool: - self._lifecycle.actions.append("hold") - return True - - -def test_openarm_lifecycle_enables_then_holds_and_disables() -> None: - lifecycle = _OpenArmLifecycle() - adapter = _LifecycleOpenArmAdapter(lifecycle) - - assert adapter.activate() - assert adapter.deactivate() - assert lifecycle.actions == ["enable", "hold", "disable"] - - class _A750Robot: def __init__(self) -> None: self.actions: list[str] = [] diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 497a688e23..7061cee54c 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -55,7 +55,6 @@ "a750", "galaxea_a1z", "mock", - "openarm", "piper", "sim_mujoco", "xarm", @@ -69,6 +68,7 @@ }, "whole_body": { "mock_whole_body", + "openarm_damiao", "openyam_damiao", "sim_mujoco_g1", "transport_lcm", diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index d952d2cd99..21f4514233 100644 --- a/dimos/hardware/whole_body/damiao/adapter.py +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -250,6 +250,11 @@ def read_motor_states(self) -> list[MotorState]: for position, velocity, effort in zip(q, dq, tau, strict=True) ) for name in self.gripper_joints: + if not self._active: + # Gripper opening calibrates during activation; report a + # placeholder so read-only sessions still stream arm state. + states.append(MotorState(q=0.0, dq=0.0, tau=0.0)) + continue opening = float(self._grippers[name].opening) if not np.isfinite(opening) or not 0.0 <= opening <= 1.0: raise RuntimeError(f"gripper {name!r} returned invalid opening {opening}") diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index 3d2c7dfa1f..e13494f639 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -725,3 +725,33 @@ def test_write_motor_commands_gravity_enabled_adds_computed_torque( assert compute_gravity.call_count == 1 assert cast("FakeArm", dual_robot["left_arm"]).commands[-1][:, 4].tolist() == [1.5, 2.5] assert cast("FakeArm", dual_robot["right_arm"]).commands[-1][:, 4].tolist() == [3.5, 4.5] + + +def test_read_motor_states_inactive_gripper_reports_placeholder( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + """Gripper opening calibrates at activation; before that the read path + must not touch it so read-only bring-up sessions still stream arm state.""" + cast("FakeGripper", dual_robot["left_gripper"]).opening = None + cast("FakeGripper", dual_robot["right_gripper"]).opening = None + + states = connected_dual_adapter.read_motor_states() + + assert states[4:] == [MotorState(q=0.0), MotorState(q=0.0)] + + +def test_read_motor_states_inactive_adapter_pumps_feedback( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + """Without the active write path ticking the bus, the read path must + refresh feedback itself or read-only sessions stream a frozen snapshot.""" + refreshes_before = dual_robot.refresh_count + ticks_before = dual_robot.tick_count + + connected_dual_adapter.read_motor_states() + connected_dual_adapter.read_motor_states() + + assert dual_robot.refresh_count == refreshes_before + 2 + assert dual_robot.tick_count == ticks_before + 2 diff --git a/dimos/hardware/manipulators/openarm/_registry.py b/dimos/hardware/whole_body/openarm_damiao/_registry.py similarity index 86% rename from dimos/hardware/manipulators/openarm/_registry.py rename to dimos/hardware/whole_body/openarm_damiao/_registry.py index eed680a4be..0c16f69170 100644 --- a/dimos/hardware/manipulators/openarm/_registry.py +++ b/dimos/hardware/whole_body/openarm_damiao/_registry.py @@ -13,5 +13,5 @@ # limitations under the License. ADAPTER_FACTORIES = { - "openarm": "dimos.hardware.manipulators.openarm.adapter:OpenArmAdapter", + "openarm_damiao": ("dimos.hardware.whole_body.openarm_damiao.adapter:OpenArmDamiaoAdapter"), } diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py new file mode 100644 index 0000000000..4b64ae287a --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -0,0 +1,103 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm v2.0 bimanual physical topology for the generic Damiao whole-body adapter.""" + +from __future__ import annotations + +from pathlib import Path + +import can_motor_control +from can_motor_control import damiao + +from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter +from dimos.utils.data import LfsPath + + +def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: + return [ + can_motor_control.MotorSpec(f"openarm_{side}_joint1", damiao.MotorType.DM8009, 0x01, 0x11), + can_motor_control.MotorSpec(f"openarm_{side}_joint2", damiao.MotorType.DM8009, 0x02, 0x12), + can_motor_control.MotorSpec(f"openarm_{side}_joint3", damiao.MotorType.DM4340, 0x03, 0x13), + can_motor_control.MotorSpec(f"openarm_{side}_joint4", damiao.MotorType.DM4340, 0x04, 0x14), + can_motor_control.MotorSpec(f"openarm_{side}_joint5", damiao.MotorType.DM4310, 0x05, 0x15), + can_motor_control.MotorSpec(f"openarm_{side}_joint6", damiao.MotorType.DM4310, 0x06, 0x16), + can_motor_control.MotorSpec(f"openarm_{side}_joint7", damiao.MotorType.DM4310, 0x07, 0x17), + ] + + +def _gripper_motor(side: str) -> can_motor_control.MotorSpec: + return can_motor_control.MotorSpec( + f"openarm_{side}_gripper", + damiao.MotorType.DM4310, + 0x08, + 0x18, + ) + + +class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): + """Two OpenArm v2.0 arms with grippers, one CAN bus per arm.""" + + arm_joints = { + "left_arm": tuple(f"left_arm/joint{index}" for index in range(1, 8)), + "right_arm": tuple(f"right_arm/joint{index}" for index in range(1, 8)), + } + gripper_joints = { + "left_gripper": "left_arm/gripper", + "right_gripper": "right_arm/gripper", + } + # can0/can1 follow USB enumeration order; remap through + # DamiaoRuntimeConfig.bus_addresses if the rig comes up swapped. + bus_defaults = {"left": "can1", "right": "can0"} + gravity_joint_names = ( + *(f"openarm_left_joint{index}" for index in range(1, 8)), + *(f"openarm_right_joint{index}" for index in range(1, 8)), + ) + + @property + def gravity_model_path(self) -> Path: + """Return the lazy bimanual gravity-compensation URDF path.""" + return LfsPath("openarm_description") / "urdf/robot/openarm_v20_bimanual.urdf" + + def _build_robot(self) -> can_motor_control.Robot: + return ( + can_motor_control.Robot.builder() + .add_bus( + "left", + can_motor_control.SocketCanBus(self.bus_address("left")), + damiao.DamiaoCodec(), + ) + .add_bus( + "right", + can_motor_control.SocketCanBus(self.bus_address("right")), + damiao.DamiaoCodec(), + ) + .add_arm("left_arm", bus="left", motors=_arm_motors("left")) + .add_arm("right_arm", bus="right", motors=_arm_motors("right")) + .add_gripper( + "left_gripper", + bus="left", + motor=_gripper_motor("left"), + opening_direction="decreasing_position", + default_current=0.15, + ) + .add_gripper( + "right_gripper", + bus="right", + motor=_gripper_motor("right"), + opening_direction="decreasing_position", + default_current=0.15, + ) + .build() + ) diff --git a/dimos/hardware/whole_body/openarm_damiao/test_adapter.py b/dimos/hardware/whole_body/openarm_damiao/test_adapter.py new file mode 100644 index 0000000000..bddfa5cf96 --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/test_adapter.py @@ -0,0 +1,67 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from collections.abc import Iterator +import runpy + +import can_motor_control +import pytest +from pytest_mock import MockerFixture + +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.openarm_damiao import adapter as adapter_module +from dimos.hardware.whole_body.openarm_damiao.adapter import OpenArmDamiaoAdapter +from dimos.robot.manipulators.openarm.config import OPENARM_DOF, OPENARM_JOINTS + + +@pytest.fixture +def openarm_adapter(mocker: MockerFixture) -> Iterator[OpenArmDamiaoAdapter]: + mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) + adapter = OpenArmDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) + yield adapter + adapter.disconnect() + + +def test_import_lazy_gravity_model_does_not_resolve_lfs(mocker: MockerFixture) -> None: + get_data = mocker.patch("dimos.utils.data.get_data") + + runpy.run_path(adapter_module.__file__) + + get_data.assert_not_called() + + +def test_openarm_topology_connects_arms_and_grippers( + openarm_adapter: OpenArmDamiaoAdapter, +) -> None: + robot = openarm_adapter._build_robot() + + assert robot.group_names() == ["left_arm", "right_arm", "left_gripper", "right_gripper"] + assert robot.bus_names() == ["left", "right"] + assert isinstance(robot["left_arm"], can_motor_control.Arm) + assert isinstance(robot["right_arm"], can_motor_control.Arm) + assert len(robot["left_arm"]) == OPENARM_DOF + assert len(robot["right_arm"]) == OPENARM_DOF + assert isinstance(robot["left_gripper"], can_motor_control.Gripper) + assert isinstance(robot["right_gripper"], can_motor_control.Gripper) + assert openarm_adapter.connect() + + +def test_openarm_joint_order_matches_hardware_component( + openarm_adapter: OpenArmDamiaoAdapter, +) -> None: + """Commands are routed positionally: the config joint list must equal the + adapter's declared order or motors silently receive each other's targets.""" + assert list(openarm_adapter.joint_names) == OPENARM_JOINTS diff --git a/dimos/manipulation/planning/utils/mesh_utils.py b/dimos/manipulation/planning/utils/mesh_utils.py index 33fce6d6c8..14c430780c 100644 --- a/dimos/manipulation/planning/utils/mesh_utils.py +++ b/dimos/manipulation/planning/utils/mesh_utils.py @@ -231,9 +231,12 @@ def convert_mesh(match: re.Match[str]) -> str: # Load mesh mesh = trimesh.load(original_path, force="mesh") - # Generate output path + # Generate output path. Include a source-path hash: different + # meshes may share a stem (visual/link3.dae vs collision/link3.stl) + # and stem-only names would overwrite each other. mesh_name = Path(original_path).stem - obj_path = mesh_dir / f"{mesh_name}.obj" + path_tag = hashlib.md5(original_path.encode()).hexdigest()[:8] + obj_path = mesh_dir / f"{mesh_name}_{path_tag}.obj" # Export as OBJ (trimesh.export returns None, ignore) mesh.export(str(obj_path), file_type="obj") # type: ignore[no-untyped-call] diff --git a/dimos/manipulation/planning/utils/test_mesh_utils.py b/dimos/manipulation/planning/utils/test_mesh_utils.py new file mode 100644 index 0000000000..f376f854d2 --- /dev/null +++ b/dimos/manipulation/planning/utils/test_mesh_utils.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import re + +import trimesh + +from dimos.manipulation.planning.utils.mesh_utils import _convert_meshes + + +def _write_box_mesh(path: Path, extents: tuple[float, float, float]) -> None: + trimesh.creation.box(extents=extents).export(str(path)) + + +def test_convert_meshes_same_stem_different_dirs_stay_distinct(tmp_path: Path) -> None: + """Visual and collision meshes often share a file stem; converted OBJs + must not overwrite each other.""" + visual_dir = tmp_path / "visual" + collision_dir = tmp_path / "collision" + visual_dir.mkdir() + collision_dir.mkdir() + _write_box_mesh(visual_dir / "link3.stl", (1.0, 1.0, 1.0)) + _write_box_mesh(collision_dir / "link3.stl", (2.0, 2.0, 2.0)) + + urdf = ( + f'' + f'' + ) + converted = _convert_meshes(urdf, tmp_path) + + obj_paths = [Path(p) for p in re.findall(r'filename="([^"]+\.obj)"', converted)] + assert len(obj_paths) == 2 + assert obj_paths[0] != obj_paths[1] + sizes = sorted(trimesh.load(str(p), force="mesh").extents[0] for p in obj_paths) + assert sizes[0] == 1.0 + assert sizes[1] == 2.0 + + +def test_convert_meshes_same_file_referenced_twice_converts_once(tmp_path: Path) -> None: + mesh = tmp_path / "part.stl" + _write_box_mesh(mesh, (1.0, 1.0, 1.0)) + + urdf = f'' + converted = _convert_meshes(urdf, tmp_path) + + obj_paths = set(re.findall(r'filename="([^"]+\.obj)"', converted)) + assert len(obj_paths) == 1 diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index c7ea33ce84..ba4046eba2 100644 --- a/dimos/manipulation/planning/world/roboplan_model.py +++ b/dimos/manipulation/planning/world/roboplan_model.py @@ -337,8 +337,6 @@ def _groups( generated = 0 for size in range(2, len(configured) + 1): for selected in combinations(configured, size): - if len({group.robot_name for group in selected}) < 2: - continue if len({name for group in selected for name in group.joint_names}) != sum( len(group.joint_names) for group in selected ): diff --git a/dimos/manipulation/test_roboplan.py b/dimos/manipulation/test_roboplan.py index 710efb2470..5a14f0d3a9 100644 --- a/dimos/manipulation/test_roboplan.py +++ b/dimos/manipulation/test_roboplan.py @@ -1682,7 +1682,7 @@ def capture_scene_start( assert observed_scene_start[:2] == pytest.approx(start.position) -def test_native_selected_planner_rejects_multi_group_selection( +def test_native_selected_planner_composes_disjoint_groups_within_one_robot( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: config = robot_config.model_copy( @@ -1703,8 +1703,25 @@ def test_native_selected_planner_rejects_multi_group_selection( JointState(name=list(selection.joint_names), position=[0.1, 0.1]), ) - assert result.status == PlanningStatus.UNSUPPORTED - assert "no generated group" in result.message + assert result.status == PlanningStatus.SUCCESS + assert result.path + + +def test_overlapping_group_selection_rejected_before_planning( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + config = robot_config.model_copy( + update={ + "planning_groups": [ + PlanningGroupDefinition("left", ("joint1", "joint2"), "base", "left_tip"), + PlanningGroupDefinition("right", ("joint2",), "base", "right_tip"), + ] + } + ) + _make_world(fake_roboplan, config) + + with pytest.raises(ValueError, match="overlap"): + _selection((config,), "arm/left", "arm/right") def test_native_planner_coordinates_groups_across_two_robots( diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index c99032b9e2..cf100b42b7 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -1010,7 +1010,9 @@ def _target_ghost_states( current = self.get_current_joint_state(robot_name) if config is None or current is None: continue - values = self._local_values_for_robot(robot_name, current) + values = merged.get(robot_name) + if values is None: + values = self._local_values_for_robot(robot_name, current) target_raw = self._state_values_by_local_name(target) for local_name, global_name in zip( group.local_joint_names, group.joint_names, strict=True diff --git a/dimos/manipulation/visualization/viser/test_gui.py b/dimos/manipulation/visualization/viser/test_gui.py index 9d8be1e168..7e5d7b96c4 100644 --- a/dimos/manipulation/visualization/viser/test_gui.py +++ b/dimos/manipulation/visualization/viser/test_gui.py @@ -16,6 +16,7 @@ from collections.abc import Callable from dataclasses import dataclass +from types import SimpleNamespace import pytest @@ -470,3 +471,34 @@ def test_gui_ignores_stale_timed_out_operation_finish() -> None: assert gui.state.action_status == ActionStatus.FAILED assert gui.state.error == "Operation timed out after 5.0s" + + +def test_target_ghost_states_merge_groups_sharing_one_robot( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Two planning groups on one robot must both contribute to the ghost state.""" + gui = make_gui() + left = planning_group("bot", "left_manipulator", ("j1",)) + right = planning_group("bot", "right_manipulator", ("j2",)) + gui.state.selected_group_ids = (str(left.id), str(right.id)) + + monkeypatch.setattr(gui, "_groups_by_id", lambda: {str(left.id): left, str(right.id): right}) + monkeypatch.setattr( + gui, + "get_robot_config", + lambda _name: SimpleNamespace(joint_names=("j1", "j2")), + ) + monkeypatch.setattr( + gui, + "get_current_joint_state", + lambda _name: JointState({"name": ["bot/j1", "bot/j2"], "position": [0.0, 0.0]}), + ) + + targets = { + str(left.id): JointState({"name": ["bot/j1"], "position": [0.5]}), + str(right.id): JointState({"name": ["bot/j2"], "position": [-0.5]}), + } + ghost_states = gui._target_ghost_states(targets) + + assert list(ghost_states) == ["bot"] + assert list(ghost_states["bot"].position) == [0.5, -0.5] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 44c35d644d..c3e59815e8 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -31,10 +31,7 @@ "coordinator-mobile-manip-mock": "dimos.control.blueprints.mobile:coordinator_mobile_manip_mock", "coordinator-mock": "dimos.robot.manipulators.common.mock:coordinator_mock", "coordinator-mock-twist-base": "dimos.control.blueprints.mobile:coordinator_mock_twist_base", - "coordinator-openarm-bimanual": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_bimanual", - "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_left", - "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_mock", - "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_right", + "coordinator-openarm": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm", "coordinator-openyam": "dimos.robot.manipulators.openyam.blueprints.basic:coordinator_openyam", "coordinator-piper": "dimos.robot.manipulators.piper.blueprints.basic:coordinator_piper", "coordinator-piper-xarm": "dimos.robot.manipulators.common.mixed:coordinator_piper_xarm", @@ -71,7 +68,7 @@ "keyboard-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:keyboard_teleop_a1z", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints.teleop:keyboard_teleop_a750", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm", - "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm_mock", + "keyboard-teleop-openarm-planner": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm_planner", "keyboard-teleop-openyam": "dimos.robot.manipulators.openyam.blueprints.teleop:keyboard_teleop_openyam", "keyboard-teleop-openyam-planner": "dimos.robot.manipulators.openyam.blueprints.teleop:keyboard_teleop_openyam_planner", "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:keyboard_teleop_piper", @@ -88,8 +85,7 @@ "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", "mid360-realsense-record": "dimos.robot.assembly.mid360_realsense_30:mid360_realsense_record", "mid360-realsense-record-with-pcap": "dimos.robot.assembly.mid360_realsense_30:mid360_realsense_record_with_pcap", - "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", - "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", + "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.basic:openarm_planner_coordinator", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", "spot": "dimos.experimental.robot.bosdyn.spot.blueprints.spot:spot", "spot-record": "dimos.experimental.robot.bosdyn.spot.blueprints.spot_record:spot_record", diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index 863c187314..36afa57c52 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -16,53 +16,40 @@ from __future__ import annotations -from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.robot.manipulators.common.blueprints import trajectory_task +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( - LEFT_CAN, - OPENARM_ADAPTER_KWARGS, - RIGHT_CAN, + OPENARM_ARM_JOINTS, + openarm_bimanual_model_config, openarm_hardware, ) -def openarm_task(hw: HardwareComponent) -> TaskConfig: - return trajectory_task(hw) +def _trajectory_task() -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(OPENARM_ARM_JOINTS), + priority=10, + params={"start_position_tolerance": 0.05}, + ) -mock_left = openarm_hardware(side="left") -mock_right = openarm_hardware(side="right") +_openarm_planner_hw = openarm_hardware() -coordinator_openarm_mock = ControlCoordinator.blueprint( - hardware=[mock_left, mock_right], - tasks=[trajectory_task(mock_left, mock_right)], +openarm_planner_coordinator = autoconnect( + planner(robots=[openarm_bimanual_model_config()]), + coordinator( + hardware=[_openarm_planner_hw], + tasks=[_trajectory_task()], + ), ) -left_hw = openarm_hardware( - side="left", - address=LEFT_CAN, - adapter_type="openarm", - adapter_kwargs=OPENARM_ADAPTER_KWARGS, -) -right_hw = openarm_hardware( - side="right", - address=RIGHT_CAN, - adapter_type="openarm", - adapter_kwargs=OPENARM_ADAPTER_KWARGS, -) - -coordinator_openarm_left = ControlCoordinator.blueprint( - hardware=[left_hw], - tasks=[openarm_task(left_hw)], -) - -coordinator_openarm_right = ControlCoordinator.blueprint( - hardware=[right_hw], - tasks=[openarm_task(right_hw)], -) +_openarm_hw = openarm_hardware() -coordinator_openarm_bimanual = ControlCoordinator.blueprint( - hardware=[left_hw, right_hw], - tasks=[trajectory_task(left_hw, right_hw)], +coordinator_openarm = ControlCoordinator.blueprint( + hardware=[_openarm_hw], + tasks=[_trajectory_task()], ) diff --git a/dimos/robot/manipulators/openarm/blueprints/planner.py b/dimos/robot/manipulators/openarm/blueprints/planner.py deleted file mode 100644 index 6872b15157..0000000000 --- a/dimos/robot/manipulators/openarm/blueprints/planner.py +++ /dev/null @@ -1,53 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""OpenArm planner + coordinator blueprints.""" - -from __future__ import annotations - -from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.manipulators.common.blueprints import coordinator, planner, trajectory_task -from dimos.robot.manipulators.openarm.blueprints.basic import ( - left_hw, - mock_left, - mock_right, - right_hw, -) -from dimos.robot.manipulators.openarm.config import openarm_model_config - -openarm_mock_planner_coordinator = autoconnect( - planner( - robots=[ - openarm_model_config("left"), - openarm_model_config("right"), - ], - ), - coordinator( - hardware=[mock_left, mock_right], - tasks=[trajectory_task(mock_left, mock_right)], - ), -) - -openarm_planner_coordinator = autoconnect( - planner( - robots=[ - openarm_model_config("left"), - openarm_model_config("right"), - ], - ), - coordinator( - hardware=[left_hw, right_hw], - tasks=[trajectory_task(left_hw, right_hw)], - ), -) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index 17000394bd..c6f298a3ae 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -16,58 +16,75 @@ from __future__ import annotations +from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.robot.manipulators.common.blueprints import ( - eef_twist_task, -) -from dimos.robot.manipulators.common.coordinators import ( - ArmTwistCoordinator, -) +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( - LEFT_CAN, - openarm_single_hardware, - openarm_single_model_config, + OPENARM_ARM_JOINTS, + openarm_arm_joints, + openarm_bimanual_model_config, + openarm_control_model_config, + openarm_hardware, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_teleop_hw = openarm_single_hardware() -_openarm_model = openarm_single_model_config() +# The keyboard publishes twists to one task by name; the right arm's task +# keeps holding its anchor pose. +KEYBOARD_EEF_TASK_NAME = "eef_twist_left_arm" + +_openarm_keyboard_hw = openarm_hardware() +_openarm_control_models = {side: openarm_control_model_config(side) for side in ("left", "right")} +_openarm_planning_model = openarm_bimanual_model_config() + + +def _eef_twist_task(side: str, *, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=f"eef_twist_{side}_arm", + type="eef_twist", + joint_names=openarm_arm_joints(side), + priority=priority, + params={"control_ik": {"robot_model": _openarm_control_models[side]}}, + ) + -keyboard_teleop_openarm_mock = autoconnect( - KeyboardTeleopModule.blueprint(), - ArmTwistCoordinator.blueprint( - instance_name="ControlCoordinator", - hardware=[_teleop_hw], +def _trajectory_task(*, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(OPENARM_ARM_JOINTS), + priority=priority, + params={"start_position_tolerance": 0.05}, + ) + + +keyboard_teleop_openarm = autoconnect( + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), + ControlCoordinator.blueprint( + hardware=[_openarm_keyboard_hw], tasks=[ - eef_twist_task( - _teleop_hw, - robot_model=_openarm_model, - ) + _eef_twist_task("left"), + _eef_twist_task("right"), ], ), ManipulationModule.blueprint( - robots=[_openarm_model], - visualization={"backend": "meshcat"}, + robots=[_openarm_planning_model], + visualization={"backend": "viser"}, ), ) -_teleop_real_hw = openarm_single_hardware(adapter_type="openarm", address=LEFT_CAN) +_openarm_keyboard_planner_hw = openarm_hardware() -keyboard_teleop_openarm = autoconnect( - KeyboardTeleopModule.blueprint(), - ArmTwistCoordinator.blueprint( - instance_name="ControlCoordinator", - hardware=[_teleop_real_hw], +keyboard_teleop_openarm_planner = autoconnect( + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), + planner(robots=[_openarm_planning_model]), + coordinator( + hardware=[_openarm_keyboard_planner_hw], tasks=[ - eef_twist_task( - _teleop_real_hw, - robot_model=_openarm_model, - ) + _eef_twist_task("left", priority=10), + _eef_twist_task("right", priority=10), + _trajectory_task(priority=20), ], ), - ManipulationModule.blueprint( - robots=[_openarm_model], - visualization={"backend": "meshcat"}, - ), ) diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 307450d054..e1d9538ed6 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -12,79 +12,87 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""OpenArm hardware and planning model configuration helpers.""" +"""OpenArm hardware and planning model configuration.""" from __future__ import annotations from pathlib import Path -from typing import Any from dimos.control.components import HardwareComponent, HardwareType +from dimos.core.global_config import global_config +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import base_pose from dimos.utils.data import LfsPath -OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("openarm_left_link5", "openarm_left_link7"), - ("openarm_right_link5", "openarm_right_link7"), -] +OPENARM_DOF = 7 +OPENARM_HARDWARE_ID = "openarm" +OPENARM_SIDES = ("left", "right") +# Order must match OpenArmDamiaoAdapter.joint_names: all arm groups in +# declaration order (left then right), then all grippers. +OPENARM_LEFT_ARM_JOINTS = [f"left_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +OPENARM_RIGHT_ARM_JOINTS = [f"right_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +OPENARM_ARM_JOINTS = [*OPENARM_LEFT_ARM_JOINTS, *OPENARM_RIGHT_ARM_JOINTS] +OPENARM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] +OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] OPENARM_PKG = LfsPath("openarm_description") -OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" -OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" -OPENARM_V10_FK_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" +OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_left.urdf" +OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_right.urdf" +OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_bimanual.urdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# Linux assigns can0/can1 in USB enumeration order, which is not guaranteed stable. -# Flip these if physical arms come up swapped. -LEFT_CAN = "can1" -RIGHT_CAN = "can0" - -# Leave true for normal operation; it is idempotent and ensures motors are in -# the expected CTRL_MODE=MIT mode at connect time. -AUTO_SET_MIT_MODE = True -OPENARM_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} +# MIT gains measured on v1.0 hardware, carried over as the v2.0 starting +# point: with gravity compensation active the PD terms only handle transient +# tracking, and high kd excites gearbox buzz. Gripper slots bypass MIT +# control, so their gains are 0. +_ARM_KP = (100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0) +_ARM_KD = (1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8) def validate_side(side: str) -> None: - if side not in ("left", "right"): + if side not in OPENARM_SIDES: raise ValueError(f"side must be 'left' or 'right', got {side!r}") -def openarm_joints(side: str) -> list[str]: +def openarm_arm_joints(side: str) -> list[str]: validate_side(side) - return [f"openarm_{side}_joint{i}" for i in range(1, 8)] + return [f"{side}_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] -def openarm_hardware( - side: str, - name: str | None = None, - *, - adapter_type: str = "mock", - address: str | None = None, - adapter_kwargs: dict[str, Any] | None = None, -) -> HardwareComponent: +def openarm_urdf_joints(side: str) -> list[str]: validate_side(side) - kwargs = {"side": side} - if adapter_kwargs: - kwargs.update(adapter_kwargs) + return [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] + + +def openarm_hardware() -> HardwareComponent: + """Select the physical or in-memory whole-body adapter for OpenArm.""" + adapter_type = "mock_whole_body" if global_config.simulation else "openarm_damiao" + adapter_kwargs: dict[str, object] = {} + if not global_config.simulation: + adapter_kwargs["runtime_config"] = DamiaoRuntimeConfig(gravity_comp=True) return HardwareComponent( - hardware_id=name or f"{side}_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=openarm_joints(side), + hardware_id=OPENARM_HARDWARE_ID, + hardware_type=HardwareType.WHOLE_BODY, + joints=list(OPENARM_JOINTS), adapter_type=adapter_type, - address=address, - adapter_kwargs=kwargs, + auto_enable=True, + adapter_kwargs=adapter_kwargs, + wb_config=WholeBodyConfig( + kp=(*_ARM_KP, *_ARM_KP, 0.0, 0.0), + kd=(*_ARM_KD, *_ARM_KD, 0.0, 0.0), + ), ) -def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: +def openarm_control_model_config(side: str) -> RobotModelConfig: + """Build one arm's seven-joint model for its independent control-IK task.""" validate_side(side) - resolved_name = name or f"{side}_arm" - local_joint_names = openarm_joints(side) + local_joint_names = openarm_urdf_joints(side) return RobotModelConfig( - name=resolved_name, + name=f"{side}_arm", model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, base_pose=base_pose(), joint_names=local_joint_names, @@ -98,46 +106,48 @@ def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig ) ], package_paths=OPENARM_PACKAGE_PATHS, - collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - home_joints=[0.0] * 7, + joint_name_mapping=dict(zip(openarm_arm_joints(side), local_joint_names, strict=True)), + home_joints=[0.0] * OPENARM_DOF, ) -def openarm_single_hardware( - *, - adapter_type: str = "mock", - address: str | None = None, -) -> HardwareComponent: - return openarm_hardware( - "left", - name="arm", - adapter_type=adapter_type, - address=address, - ) - +def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModelConfig: + """Build the single fourteen-joint planning model with one group per arm. -def openarm_single_model_config() -> RobotModelConfig: - local_joint_names = openarm_joints("left") + Collision exclusions cannot span robots, so both arms plan as one robot. + """ + local_joint_names = [*openarm_urdf_joints("left"), *openarm_urdf_joints("right")] return RobotModelConfig( - name="arm", - model_path=OPENARM_V10_FK_MODEL, + name=name, + model_path=OPENARM_BIMANUAL_MODEL, base_pose=base_pose(), joint_names=local_joint_names, base_link="openarm_body_link0", planning_groups=[ PlanningGroupDefinition( - name="manipulator", - joint_names=tuple(local_joint_names), + name="left_manipulator", + joint_names=tuple(openarm_urdf_joints("left")), base_link="openarm_body_link0", - tip_link="openarm_left_link7", - ) + tip_link="openarm_left_grasp_frame", + ), + PlanningGroupDefinition( + name="right_manipulator", + joint_names=tuple(openarm_urdf_joints("right")), + base_link="openarm_body_link0", + tip_link="openarm_right_grasp_frame", + ), ], package_paths=OPENARM_PACKAGE_PATHS, auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, - home_joints=[0.0] * 7, + joint_name_mapping={ + coordinator_name: urdf_name + for side in OPENARM_SIDES + for coordinator_name, urdf_name in zip( + openarm_arm_joints(side), openarm_urdf_joints(side), strict=True + ) + }, + home_joints=[0.0] * (2 * OPENARM_DOF), ) diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py b/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py deleted file mode 100755 index 9c740ef485..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Probe an OpenArm on a SocketCAN interface. - -Enumerates all 8 expected Damiao motors (7 arm joints + gripper) on one CAN bus -(classical by default, use --fd for CAN-FD), enables each, reads back one state -frame, then disables. Phase-0 hardware-verification script. - -Run AFTER bringing the bus up with dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh. - -Usage: - python dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can0 - python dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can1 --ids 1,2,3,4,5,6,7 -""" - -from __future__ import annotations - -import argparse -import sys -import time - -try: - import can -except ImportError: - sys.exit("python-can not installed. Run: pip install 'python-can>=4.3'") - -# ---- Damiao motor limit tables (from enactic/openarm_can dm_motor_constants.hpp) -# [p_max rad, v_max rad/s, t_max Nm] -LIMITS: dict[str, tuple[float, float, float]] = { - "DM4310": (12.5, 30.0, 10.0), - "DM4340": (12.5, 8.0, 28.0), - "DM8006": (12.5, 45.0, 40.0), -} - -# OpenArm v10 per-joint motor assignment (derived from joint_limits.yaml effort column) -DEFAULT_MOTORS: list[tuple[int, str]] = [ - (0x01, "DM8006"), # joint1 - (0x02, "DM8006"), # joint2 - (0x03, "DM4340"), # joint3 - (0x04, "DM4340"), # joint4 - (0x05, "DM4310"), # joint5 - (0x06, "DM4310"), # joint6 - (0x07, "DM4310"), # joint7 - (0x08, "DM4310"), # gripper -] - -ENABLE = bytes([0xFF] * 7 + [0xFC]) -DISABLE = bytes([0xFF] * 7 + [0xFD]) - -FD = False # set by --fd at runtime; defaults to classical CAN @ 1 Mbit - - -def uint_to_float(x: int, lo: float, hi: float, bits: int) -> float: - return x / ((1 << bits) - 1) * (hi - lo) + lo - - -def parse_state(motor_type: str, data: bytes) -> tuple[float, float, float, int, int] | None: - """Decode an 8-byte DM motor state reply. Returns (q, dq, tau, t_mos, t_rotor).""" - if len(data) < 8: - return None - p_max, v_max, t_max = LIMITS[motor_type] - q_u = (data[1] << 8) | data[2] - dq_u = (data[3] << 4) | (data[4] >> 4) - tau_u = ((data[4] & 0x0F) << 8) | data[5] - q = uint_to_float(q_u, -p_max, p_max, 16) - dq = uint_to_float(dq_u, -v_max, v_max, 12) - tau = uint_to_float(tau_u, -t_max, t_max, 12) - return q, dq, tau, data[6], data[7] - - -def probe_motor( - bus: can.BusABC, send_id: int, recv_id: int, motor_type: str, timeout: float = 0.2 -) -> bool: - """Enable motor, wait for state reply on recv_id, print result, disable.""" - # Flush any stale frames - while bus.recv(0.0) is not None: - pass - - bus.send( - can.Message( - arbitration_id=send_id, data=ENABLE, is_extended_id=False, is_fd=FD, bitrate_switch=FD - ) - ) - t0 = time.monotonic() - while time.monotonic() - t0 < timeout: - msg = bus.recv(timeout - (time.monotonic() - t0)) - if msg is None: - break - if msg.arbitration_id != recv_id: - continue - parsed = parse_state(motor_type, bytes(msg.data)) - if parsed is None: - print(f" 0x{send_id:02X} ({motor_type}): short reply {list(msg.data)}") - bus.send( - can.Message( - arbitration_id=send_id, - data=DISABLE, - is_extended_id=False, - is_fd=FD, - bitrate_switch=FD, - ) - ) - return False - q, dq, tau, t_mos, t_rot = parsed - print( - f" 0x{send_id:02X} ({motor_type:>6}): " - f"q={q:+.3f} rad dq={dq:+.3f} rad/s tau={tau:+.3f} Nm " - f"T_mos={t_mos}C T_rotor={t_rot}C" - ) - bus.send( - can.Message( - arbitration_id=send_id, - data=DISABLE, - is_extended_id=False, - is_fd=FD, - bitrate_switch=FD, - ) - ) - return True - - print( - f" 0x{send_id:02X} ({motor_type:>6}): NO REPLY on 0x{recv_id:02X} within {timeout * 1e3:.0f}ms" - ) - bus.send( - can.Message( - arbitration_id=send_id, data=DISABLE, is_extended_id=False, is_fd=FD, bitrate_switch=FD - ) - ) - return False - - -def main() -> int: - ap = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - ap.add_argument("--channel", default="can0", help="SocketCAN interface (default: can0)") - ap.add_argument( - "--fd", - action="store_true", - help="Use CAN-FD (requires FD-capable adapter). Default is classical CAN @ 1 Mbit, which is what most gs_usb adapters support.", - ) - ap.add_argument("--ids", default=None, help="Comma-separated send IDs to probe (default: 1..8)") - ap.add_argument("--timeout", type=float, default=0.2, help="Reply timeout per motor (s)") - args = ap.parse_args() - - global FD - FD = args.fd - motors = DEFAULT_MOTORS - if args.ids: - wanted = {int(x, 0) for x in args.ids.split(",")} - motors = [m for m in DEFAULT_MOTORS if m[0] in wanted] - - # Preflight: is the interface up? - try: - flags = int(open(f"/sys/class/net/{args.channel}/flags").read().strip(), 16) - iface_up = bool(flags & 0x1) - except OSError: - print(f"ERROR: interface '{args.channel}' not found", file=sys.stderr) - return 1 - if not iface_up: - print(f"ERROR: SocketCAN interface '{args.channel}' is DOWN.", file=sys.stderr) - print( - f" Run: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {args.channel}", - file=sys.stderr, - ) - return 1 - - print(f"Opening {args.channel} ({'CAN-FD' if FD else 'classical CAN'})...") - try: - bus = can.Bus(interface="socketcan", channel=args.channel, fd=FD) - except Exception as e: - print(f"ERROR opening {args.channel}: {e}", file=sys.stderr) - print( - " Did you run 'sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh' first?", - file=sys.stderr, - ) - return 1 - - try: - print(f"Probing {len(motors)} motor(s) on {args.channel}:") - ok = 0 - for send_id, motor_type in motors: - recv_id = send_id | 0x10 - if probe_motor(bus, send_id, recv_id, motor_type, args.timeout): - ok += 1 - print(f"\n{ok}/{len(motors)} motors replied.") - return 0 if ok == len(motors) else 2 - finally: - bus.shutdown() - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh b/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh deleted file mode 100755 index d25fc41e43..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env bash -# Bring up CAN interfaces for OpenArm. Default is classical CAN @ 1 Mbit, -# which is what most gs_usb (OpenMoko / Geschwister Schneider) USB-CAN -# adapters support. Use MODE=fd if you have a CAN-FD-capable adapter. -# Run with sudo or as root. -# -# Usage: -# sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh # classical 1M, can0 and can1 -# sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 # single interface -# sudo MODE=fd ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 # CAN-FD 1M/5M -set -euo pipefail - -BITRATE=1000000 -DBITRATE=5000000 -MODE="${MODE:-classical}" # classical | fd -IFACES_ARG="${*:-can0 can1}" -# shellcheck disable=SC2206 -IFACES=(${IFACES_ARG[@]}) - -for IF in "${IFACES[@]}"; do - if ! ip link show "$IF" >/dev/null 2>&1; then - echo "[skip] $IF not present" - continue - fi - ip link set "$IF" down || true - if [ "$MODE" = "classical" ]; then - echo "[up ] $IF ${BITRATE} (classical CAN)" - ip link set "$IF" type can bitrate "$BITRATE" - else - echo "[up ] $IF ${BITRATE}/${DBITRATE} fd on" - ip link set "$IF" type can bitrate "$BITRATE" dbitrate "$DBITRATE" fd on - fi - ip link set "$IF" up - ip link set "$IF" txqueuelen 1000 - ip -details link show "$IF" | grep -E "can |bitrate" || true -done diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py b/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py deleted file mode 100755 index 04bf3912a3..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Write CTRL_MODE = MIT (1) to one or all OpenArm motors. - -Damiao motors have a persistent CTRL_MODE register (RID=10). If a motor was -previously configured in POS_VEL (2) / VEL (3) / POS_FORCE (4) mode, it will -respond to enable/disable but IGNORE MIT control frames — exactly the -"motor doesn't move, error grows" symptom. - -This script writes CTRL_MODE=1 (MIT) via the 0x7FF broadcast-write frame -format used by enactic/openarm_can: - - ID=0x7FF data = [id_lo, id_hi, 0x55, RID=10, val[0], val[1], val[2], val[3]] - -Run once per motor after CAN bring-up. The value is persistent across power -cycles. - -Usage: - # All 8 motors on can0 (classical CAN @ 1 Mbit, default) - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 - - # Single motor - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 --id 0x05 - - # CAN-FD (only if your adapter supports it) - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 --fd -""" - -from __future__ import annotations - -import argparse -import struct -import sys -import time - -try: - import can -except ImportError: - sys.exit("python-can not installed") - -RID_CTRL_MODE = 10 -MIT_MODE = 1 -DEFAULT_IDS = [0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08] - - -def write_ctrl_mode(bus: can.BusABC, send_id: int, fd: bool) -> bool: - val = struct.pack("> 8) & 0xFF, 0x55, RID_CTRL_MODE, val[0], val[1], val[2], val[3]] - ) - # Flush - while bus.recv(0.0) is not None: - pass - bus.send( - can.Message( - arbitration_id=0x7FF, data=data, is_extended_id=False, is_fd=fd, bitrate_switch=fd - ) - ) - # Wait for ack on 0x7FF (per openarm_can param response) - t0 = time.monotonic() - while time.monotonic() - t0 < 0.2: - msg = bus.recv(0.2 - (time.monotonic() - t0)) - if msg is None: - break - # Reply on 0x7FF: [id_lo, id_hi, 0x33|0x55, rid, value[0..3]] - if msg.arbitration_id != 0x7FF or len(msg.data) < 8: - continue - if msg.data[2] not in (0x33, 0x55): - continue - if msg.data[0] != (send_id & 0xFF) or msg.data[1] != ((send_id >> 8) & 0xFF): - continue # ack from a different motor - rid = msg.data[3] - if rid == RID_CTRL_MODE: - echoed = int(struct.unpack(" int: - ap = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - ap.add_argument("--channel", default="can0") - ap.add_argument("--fd", action="store_true", help="Use CAN-FD (default: classical CAN)") - ap.add_argument( - "--id", type=lambda s: int(s, 0), default=None, help="Single send ID (default: all 8)" - ) - args = ap.parse_args() - - fd = args.fd - ids = [args.id] if args.id is not None else DEFAULT_IDS - - # Preflight: is the interface up? - try: - flags = int(open(f"/sys/class/net/{args.channel}/flags").read().strip(), 16) - except OSError: - print(f"ERROR: interface '{args.channel}' not found", file=sys.stderr) - return 1 - if not (flags & 0x1): - print(f"ERROR: SocketCAN interface '{args.channel}' is DOWN.", file=sys.stderr) - print( - f" Run: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {args.channel}", - file=sys.stderr, - ) - return 1 - - print(f"Opening {args.channel} ({'CAN-FD' if fd else 'classical'})") - bus = can.Bus(interface="socketcan", channel=args.channel, fd=fd) - try: - ok = 0 - for i in ids: - if write_ctrl_mode(bus, i, fd): - ok += 1 - time.sleep(0.05) - print(f"\n{ok}/{len(ids)} motors set to MIT mode.") - return 0 if ok == len(ids) else 2 - finally: - bus.shutdown() - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index f1efc325f0..811359dd1a 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -19,6 +19,7 @@ Each blueprint launches the full stack — keyboard UI, mock controller, IK solv ```bash dimos run keyboard-teleop-a750 # A-750 6-DOF +dimos run keyboard-teleop-openarm # OpenArm bimanual 2x(7-DOF + gripper) dimos run keyboard-teleop-a1z # Galaxea A1Z 6-DOF dimos run keyboard-teleop-piper # Piper 6-DOF dimos run keyboard-teleop-openyam # OpenYAM 6-DOF + gripper @@ -26,6 +27,19 @@ dimos run keyboard-teleop-xarm6 # XArm6 6-DOF dimos run keyboard-teleop-xarm7 # XArm7 7-DOF ``` +OpenYAM is exposed as one whole-body device with six angular arm joints and a +normalized gripper joint. `arm/gripper` uses `0.0` for fully closed and `1.0` +for fully open; it does not use meters. Hardware activation calibrates both +mechanical endpoints, so clear the gripper jaws and workspace before startup. +The gripper has no default startup target and moves only after joint control has +an explicit target. + +OpenArm follows the same whole-body model with both arms and both grippers in +one device: fourteen angular joints (`left_arm/joint1..7`, +`right_arm/joint1..7`) plus two normalized gripper joints (`left_arm/gripper`, +`right_arm/gripper`). The keyboard jogs the left arm while the right arm holds +its pose; keyboard gripper bindings are a follow-up. + Open the Meshcat URL printed in the terminal (default `http://localhost:7000`) to see the robot. Keyboard controls: diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 4dd5b587dd..ed8b1d07f1 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -1,387 +1,84 @@ --- title: "OpenArm Integration" --- -Guide for running the **OpenArm** — an open-source bimanual 7-DOF research arm built from Damiao DM-J quasi-direct-drive motors — under the dimos manipulation + control stack. -**If you're standing in front of the hardware and just want to run it, skip to [Quick start](#quick-start).** +DimOS drives the [OpenArm](https://openarm.dev) bimanual platform (two 7-DOF +arms + grippers, Damiao motors, one CAN bus per arm) as a single whole-body +device through the generic Damiao adapter stack introduced for OpenYAM. Related: - Upstream hardware + C++ reference: [enactic/openarm_can](https://github.com/enactic/openarm_can) - How to integrate any new arm: [adding_a_custom_arm.md](/docs/capabilities/manipulation/adding_a_custom_arm.md) ---- - -## Why this integration is different - -Every other arm in dimos wraps a vendor Python SDK: - -| Arm | Transport | Python SDK | -|---|---|---| -| xArm | TCP/IP | `xarm-python-sdk` | -| Piper | CAN (via SDK) | `piper_sdk` | -| R1 Pro | Galaxea | Galaxea 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. - ## Architecture ``` -ManipulationModule → ControlCoordinator → OpenArmAdapter → OpenArmBus → SocketCAN → arm - (Drake plan) (100Hz tick loop) (dimos protocol) (CAN driver) +ControlCoordinator (100 Hz) + └── HardwareComponent "openarm" (WHOLE_BODY, 16 joints) + └── OpenArmDamiaoAdapter # dimos/hardware/whole_body/openarm_damiao/ + └── DamiaoWholeBodyAdapter # generic Damiao lifecycle + gravity comp + └── can-motor-control # Rust CAN transport + Damiao codec (PyPI) ``` -Code layout: +One adapter owns both arms: bus `left` (default `can1`) and bus `right` +(default `can0`) are commanded together in one synchronized tick per control +cycle. The command vector order is `left_arm/joint1..7`, `right_arm/joint1..7`, +`left_arm/gripper`, `right_arm/gripper`; gripper joints are normalized +(`0.0` closed, `1.0` open). -``` -dimos/hardware/manipulators/openarm/ -├── driver.py # OpenArmBus, DamiaoMotor — pure CAN driver, no dimos deps -├── adapter.py # OpenArmAdapter — implements dimos ManipulatorAdapter protocol -├── test_driver.py # 13 unit tests (virtual CAN loopback, no hardware) -└── test_adapter.py # 11 unit tests (virtual CAN + mock state frames) +Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): +2x DM8009, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. -dimos/robot/manipulators/openarm/ -├── blueprints.py # coordinator-*, planner-*, keyboard-teleop-* blueprints and model config -└── scripts/ # bring-up + diagnostic scripts (run manually by humans) - ├── openarm_can_up.sh # bring SocketCAN interfaces up (needs sudo) - ├── openarm_can_probe.py # enumerate & read state from all 8 motors - ├── openarm_set_mit_mode.py # one-time CTRL_MODE=MIT write per motor - └── ... (diagnostics) +Gravity compensation uses the bimanual URDF +(`openarm_description/urdf/robot/openarm_v20_bimanual.urdf`, resolved lazily +from LFS at connect time) and is preflighted against the declared joint order +before the motors enable. -data/openarm_description/ # URDF + meshes (in-tree; may migrate to LFS) -└── urdf/robot/ - ├── openarm_v10_bimanual.urdf # both arms (14 DOF, used by coordinator) - ├── openarm_v10_left.urdf # left arm + torso (7 DOF, per-side planning) - ├── openarm_v10_right.urdf # right arm + torso (7 DOF) - └── openarm_v10_single.urdf # standalone arm (Pinocchio FK for teleop) -``` +Planning also uses the bimanual URDF: one robot model with a +`left_manipulator` and a `right_manipulator` planning group, since collision +exclusions cannot span robots. -Workspace analysis is generic and lives in [dimos/utils/workspace.py](/dimos/utils/workspace.py) — works for any URDF, not just OpenArm. - ---- - -## Quick start - -You need: - -- 2× **OpenArm v10** arms, wired to USB-CAN adapters -- 2× **USB-CAN adapters** (we used gs_usb family, VID:PID `1d50:606f`, e.g. CANable 2.0). Classical CAN @ 1 Mbit is enough; CAN-FD not required -- **Python 3.12 venv with dimos installed** plus `python-can >= 4.3` and `pinocchio` -- **sudo** on first run (to bring up the CAN interfaces) - -### 1. Bring up the CAN buses +## Bring-up ```bash -sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 can1 +dimos hardware can setup can0 +dimos hardware can setup can1 +dimos run keyboard-teleop-openarm ``` -This sets both interfaces to classical CAN @ 1 Mbit with a 1000-frame TX queue (enough headroom for the 100 Hz tick loop). If only one bus is present, pass just that one: `sudo ... openarm_can_up.sh can0`. - -**Troubleshooting:** -- `Operation not permitted` → you forgot `sudo`. -- `Operation not supported` on `fd on` → your adapter doesn't support CAN-FD. The script defaults to classical, so this shouldn't happen unless you set `MODE=fd`. -- Only one `can*` interface appears → the other adapter isn't enumerating. On gs_usb boards, the **blue LED** indicates USB enumeration. If one adapter only shows red/green, swap the USB cable (many USB-C cables are charge-only). +Linux assigns `can0`/`can1` in USB enumeration order. If the arms come up +swapped, override the mapping through +`DamiaoRuntimeConfig(bus_addresses={"left": ..., "right": ...})` rather than +editing the adapter topology. -### 2. Verify all 16 motors are alive +## Blueprints -```bash -python ./dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can0 -python ./dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can1 -``` - -Expected: `8/8 motors replied` on each bus, with plausible joint positions and rotor temps around 25–30 °C. - -### 3. (First time only) Put motors in MIT mode - -Damiao motors have a persistent `CTRL_MODE` register. They ship in POS_VEL mode by default, which means they will reply to enable/state queries but **silently ignore** any MIT control frames — the "motor doesn't move, error grows" failure. The adapter writes MIT on every `connect()` by default, so this step is usually automatic. If you want to set it explicitly once: - -```bash -python ./dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 -python ./dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can1 -``` - -The register is persistent across power cycles, so you only need this once per motor (or after a firmware reset). - -### 4. Run a blueprint - -| Blueprint | What it does | +| Blueprint | Contents | |---|---| -| `coordinator-openarm-mock` | Bimanual, mock adapters. No hardware. | -| `openarm-mock-planner-coordinator` | Drake planner + bimanual mock, Meshcat viz. Great smoke test. | -| `coordinator-openarm-left` / `coordinator-openarm-right` | Single arm, real hardware on can0 / can1. | -| `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. | - -**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. - -```bash -# smoke test (no hardware) -dimos run openarm-mock-planner-coordinator - -# single-arm bring-up (hold the arm physically first) -dimos run coordinator-openarm-left - -# full bimanual with planner -dimos run openarm-planner-coordinator -``` +| `coordinator-openarm` | coordinator + trajectory task over both arms | +| `openarm-planner-coordinator` | planner (bimanual model) + coordinator | +| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + viser | +| `keyboard-teleop-openarm-planner` | teleop + planner + preempting trajectory task | -Meshcat will appear at http://localhost:7000. +All blueprints run against the in-memory whole-body adapter under +`--simulation`; the physical adapter is selected automatically otherwise. -### 5. Drive the arms from the manipulation client +The keyboard jogs the left arm (`eef_twist_left_arm`); the right arm's twist +task holds its anchor pose. Keyboard gripper bindings for the two grippers are +a follow-up; the gripper joints accept normalized `/joint_command` targets in +the meantime. -With `openarm-planner-coordinator` running in one terminal, open a second terminal and start the REPL client: +## Files -```bash -python -i -m dimos.manipulation.planning.examples.manipulation_client -``` - -This gives you an interactive Python prompt with these functions: - -| Function | Purpose | +| Path | Role | |---|---| -| `robots()` | List configured robots (here: `["left_arm", "right_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. | -| `plan([q1..q7], robot_name)` | Plan a collision-free trajectory to a joint configuration | -| `plan_pose(x, y, z, robot_name=...)` | Plan to a Cartesian EE pose (preserves current orientation) | -| `preview(robot_name)` | Animate the planned path in Meshcat without executing | -| `execute()` | Send the complete planned trajectory to the coordinator | -| `home(robot_name)` | Plan + execute to home joints | -| `commands()` | Print all available functions | - -#### Example session — simple joint moves - -```python skip ->>> robots() -['left_arm', 'right_arm'] - ->>> joints(robot_name="left_arm") -[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() -True - ->>> joints(robot_name="left_arm") -[0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00] # arm is now at the commanded pose -``` - -`plan()` returns `True` on success, `False` if planning failed (check the coordinator terminal for `COLLISION_AT_GOAL`, `INVALID_START`, `NO_SOLUTION`, etc). The `and` chaining is an idiom — if any step fails, the next one is short-circuited. - -If you ever get stuck in a `FAULT` state (e.g. an invalid plan was sent), reset the state machine: - -```python skip ->>> _client.reset() -'Reset to IDLE — ready for new commands' -``` - -#### Example session — bimanual - -```python skip ->>> # Move both arms to mirrored poses ->>> plan([0.5, 0, 0, 0, 0, 0, 0], robot_name="left_arm") and execute() -True ->>> plan([-0.5, 0, 0, 0, 0, 0, 0], robot_name="right_arm") and execute() -True -``` - -Each arm plans and executes independently — the coordinator runs both trajectories simultaneously on separate tick-loop tasks. - -#### 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") -True ->>> execute() -True -``` - -If you don't know which Cartesian targets are reachable, check first with the workspace tool — see [Workspace analysis](#workspace-analysis) below. `plan_pose` will fail with `NO_SOLUTION` if the IK can't find a configuration reaching the target. - -#### Adding obstacles - -```python skip ->>> add_box("table", 0.4, 0.0, 0.1, w=0.6, h=0.4, d=0.05) # rectangular obstacle ->>> add_sphere("ball", 0.3, 0.2, 0.4, radius=0.05) ->>> plan_pose(0.4, 0.0, 0.3, robot_name="left_arm") # now plans around it ->>> remove("table") # id returned by add_* -``` - ---- - -## Configuration - -### Which CAN bus is which arm - -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 in [config.py](/dimos/robot/manipulators/openarm/config.py): - -```python -LEFT_CAN = "can0" -RIGHT_CAN = "can1" -``` - -No other code changes are needed. +| `dimos/hardware/whole_body/openarm_damiao/adapter.py` | physical topology (motors, buses, gravity URDF) | +| `dimos/robot/manipulators/openarm/config.py` | joints, gains, hardware + planning model configs | +| `dimos/robot/manipulators/openarm/blueprints/` | coordinator/planner/teleop blueprints | -### 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] -``` - -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. - -### Physical joint limits - -The URDFs use the xacro-generated limits (which include per-side offsets for mirroring). The adapter's `get_limits()` reports the same per-side limits. If you measure tighter physical limits and want to enforce them, edit the URDFs directly — the planner will respect them. - -### Disabling auto MIT-mode write - -The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [config.py](/dimos/robot/manipulators/openarm/config.py) and restart — the arms should still respond. - ---- - -## Motor mapping (OpenArm v10) - -Derived from the URDF's `joint_limits.yaml` (effort column) cross-checked against the Damiao torque tables. Both arms are identical. - -| Send ID | Recv ID | Joint | Motor | vMax [rad/s] | tMax [Nm] | -|---|---|---|---|---|---| -| 0x01 | 0x11 | joint1 | DM8006 | 45 | 40 | -| 0x02 | 0x12 | joint2 | DM8006 | 45 | 40 | -| 0x03 | 0x13 | joint3 | DM4340 | 8 | 28 | -| 0x04 | 0x14 | joint4 | DM4340 | 8 | 28 | -| 0x05 | 0x15 | joint5 | DM4310 | 30 | 10 | -| 0x06 | 0x16 | joint6 | DM4310 | 30 | 10 | -| 0x07 | 0x17 | joint7 | DM4310 | 30 | 10 | -| 0x08 | 0x18 | gripper | DM4310 | 30 | 10 | - -Convention: `recv_id = send_id | 0x10`. - ---- - -## Damiao protocol essentials - -Ported from `enactic/openarm_can/src/openarm/damiao_motor/dm_motor_control.cpp`. You shouldn't need these unless you're modifying the driver. - -### Enable / disable / zero-position - -Send to the motor's send_id. 8-byte payload: - -``` -[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, CMD] - where CMD = 0xFC (enable) | 0xFD (disable) | 0xFE (zero current pose) -``` - -### MIT control frame (8 bytes) - -Bit layout: `q[16] | dq[12] | kp[12] | kd[12] | tau[12]`. Each float quantized via: - -```python -def float_to_uint(x, lo, hi, bits): - x = clamp(x, lo, hi) - return round((x - lo) / (hi - lo) * ((1 << bits) - 1)) -``` - -Gain ranges: `kp ∈ [0, 500]`, `kd ∈ [0, 5]`. Position/velocity/torque ranges come from the motor-type table above. - -Byte layout: -``` -byte0 = q_u >> 8 -byte1 = q_u & 0xFF -byte2 = dq_u >> 4 -byte3 = ((dq_u & 0xF) << 4) | ((kp_u >> 8) & 0xF) -byte4 = kp_u & 0xFF -byte5 = kd_u >> 4 -byte6 = ((kd_u & 0xF) << 4) | ((tau_u >> 8) & 0xF) -byte7 = tau_u & 0xFF -``` - -### State reply (8 bytes, on recv_id) - -Same `q | dq | tau` layout + 2 temperature bytes: - -``` -byte0 = motor_id_echo -byte1..5 = q | dq | tau (same packing as above) -byte6 = t_mos (°C) -byte7 = t_rotor (°C) -``` - -### CTRL_MODE register write - -Broadcast frame on CAN ID `0x7FF`: - -``` -data = [send_id_lo, send_id_hi, 0x55, RID=10, val[0..3]] - where val = 1 (MIT) | 2 (POS_VEL) | 3 (VEL) | 4 (POS_FORCE), little-endian uint32 -``` - -Persistent across power cycles. - ---- - -## Known gotchas - -- **`ip link ... fd on` → `Operation not supported`.** gs_usb firmware doesn't support CAN-FD. Use classical CAN @ 1 Mbit (our bringup script's default). -- **Motors reply to probes but commands do nothing.** CTRL_MODE is not MIT. The adapter now writes MIT on connect, but if you disabled that and motors got reset, run `openarm_set_mit_mode.py`. -- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm config module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. -- **`INVALID_START` during planning.** Hardware encoder noise pushed a joint 1 mrad past a URDF limit. Joint4 used to be exactly `lower=0.0` which tripped this — it's now `-0.01` to give breathing room. If you see it on a different joint, widen that limit by ~10 mrad. -- **"Transmit buffer full" (ENOBUFS) at 100 Hz.** Kernel TX queue too small. The bringup script sets `txqueuelen 1000`; the driver also retries on ENOBUFS. If you still see the error, check `ip -details link show canX | grep qlen`. -- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [config.py](/dimos/robot/manipulators/openarm/config.py). - ---- - -## 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. -- **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. -- **URDF stays in-tree (`data/openarm_description/`) for now.** Can migrate to LFS later — only the path constants in the OpenArm blueprint module change. -- **CAN bringup stays manual (`sudo`).** Auto-bringup from `connect()` would need sudo-in-a-library or a systemd unit; the explicit script is clearer and testable. For production, add a oneshot systemd unit that runs the script at boot. - ---- - -## Workspace analysis - -For figuring out which targets are reachable before planning, use the generic workspace tool: - -```bash -# Visualize the left arm's reachable workspace as a point cloud -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf - -# Check if a specific target is reachable -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf query 0.1 0.3 0.5 - -# Get a list of reachable poses near a target, ranked by manipulability -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf suggest 0.1 0.3 0.5 - -# Interactive: visualize + type targets to query -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf interactive -``` - -Points are colored by Yoshikawa manipulability index: green = dexterous, red = near singularity. Avoid planning targets in the red regions. - ---- - -## Testing +## Validation ```bash -# Unit tests (no hardware, use virtual CAN) -.venv/bin/python -m pytest dimos/hardware/manipulators/openarm/ -v +uv run pytest dimos/hardware/whole_body/openarm_damiao \ + dimos/hardware/test_adapter_registries.py ``` - -Expected: 24 passed (13 driver + 11 adapter). All tests use `can.Bus(interface="virtual")` loopback — no real hardware needed.