From 3a8f67c72f03c479d81930626c2a57ba4a255706 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Mon, 3 Aug 2026 21:46:15 -0700 Subject: [PATCH 01/11] feat(manipulation): drive bimanual OpenArm through the Damiao whole-body adapter Add OpenArmDamiaoAdapter: both v10 arms (2x DM8006, 2x DM4340, 3x DM4310, send ids 0x01..0x07) and both grippers (DM4310 at 0x08) as one whole-body device over two CAN buses (left=can1, right=can0), with gravity compensation from the bimanual URDF (14 joints, validated order left1..7 then right1..7). Rewrite the OpenArm hardware config on the OpenYAM pattern: one 16-joint WHOLE_BODY component, mock/real selection via global_config.simulation, hardware-measured MIT gains carried over from the legacy adapter, and per-side planning models gaining an explicit coordinator->URDF joint name mapping. Blueprints: coordinator-openarm, openarm-planner-coordinator, keyboard-teleop-openarm and keyboard-teleop-openarm-planner. The keyboard jogs the left arm while the right arm's twist task holds pose; a single servo task drives both grippers, enabled by a new KeyboardTeleopConfig.gripper_joint_names field. The e2e planning-groups test moves to openarm-planner-coordinator since the harness's --simulation flag now selects the in-memory adapter. --- .../test_manipulation_planning_groups.py | 4 +- dimos/hardware/test_adapter_registries.py | 1 + .../whole_body/openarm_damiao/_registry.py | 17 + .../whole_body/openarm_damiao/adapter.py | 111 +++++ .../whole_body/openarm_damiao/test_adapter.py | 67 +++ dimos/robot/all_blueprints.py | 10 +- .../manipulators/openarm/blueprints/basic.py | 72 ++-- .../openarm/blueprints/planner.py | 53 --- .../manipulators/openarm/blueprints/teleop.py | 102 +++-- dimos/robot/manipulators/openarm/config.py | 116 ++--- .../teleop/keyboard/keyboard_teleop_module.py | 7 +- docs/capabilities/manipulation/index.md | 7 + .../manipulation/openarm_integration.md | 400 ++---------------- 13 files changed, 414 insertions(+), 553 deletions(-) create mode 100644 dimos/hardware/whole_body/openarm_damiao/_registry.py create mode 100644 dimos/hardware/whole_body/openarm_damiao/adapter.py create mode 100644 dimos/hardware/whole_body/openarm_damiao/test_adapter.py delete mode 100644 dimos/robot/manipulators/openarm/blueprints/planner.py diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 390db63fde..1d8bc367a8 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -39,7 +39,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_robot_info( diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 9b00544624..25b8a6feb3 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -67,6 +67,7 @@ }, "whole_body": { "mock_whole_body", + "openarm_damiao", "openyam_damiao", "sim_mujoco_g1", "transport_lcm", diff --git a/dimos/hardware/whole_body/openarm_damiao/_registry.py b/dimos/hardware/whole_body/openarm_damiao/_registry.py new file mode 100644 index 0000000000..0c16f69170 --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/_registry.py @@ -0,0 +1,17 @@ +# 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. + +ADAPTER_FACTORIES = { + "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..d7b195e6ab --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -0,0 +1,111 @@ +# 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 v10 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 + +# Per-arm motor models, shoulder to wrist, from +# openarm_description/config/arm/v10/joint_limits.yaml. Both arms use the same +# CAN send ids 0x01..0x07 because each arm owns a dedicated bus. +_ARM_MOTOR_TYPES = ( + damiao.MotorType.DM8006, + damiao.MotorType.DM8006, + damiao.MotorType.DM4340, + damiao.MotorType.DM4340, + damiao.MotorType.DM4310, + damiao.MotorType.DM4310, + damiao.MotorType.DM4310, +) + + +def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: + return [ + can_motor_control.MotorSpec(f"openarm_{side}_joint{index}", motor_type, index, index | 0x10) + for index, motor_type in enumerate(_ARM_MOTOR_TYPES, start=1) + ] + + +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 v10 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", + } + # Linux assigns can0/can1 in USB enumeration order; remap a swapped rig + # through DamiaoRuntimeConfig.bus_addresses instead of editing topology. + 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_v10_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/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ae938521ef..7e583cb719 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", @@ -70,7 +67,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", @@ -87,8 +84,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", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index 012d3ceb97..bf09190813 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -16,53 +16,45 @@ 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_hardware, + openarm_model_config, ) -def openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: - return trajectory_task(hw, name=name) - - -mock_left = openarm_hardware(side="left") -mock_right = openarm_hardware(side="right") - -coordinator_openarm_mock = ControlCoordinator.blueprint( - hardware=[mock_left, mock_right], - tasks=[trajectory_task(mock_left, mock_right)], +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}, + ) + + +_openarm_planner_hw = openarm_hardware() + +openarm_planner_coordinator = autoconnect( + planner( + robots=[ + openarm_model_config("left"), + openarm_model_config("right"), + ], + ), + 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 33e8b27f98..299ef9d85c 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -16,48 +16,96 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator +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.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_V10_FK_MODEL, - openarm_single_hardware, - openarm_single_model_config, + OPENARM_DOF, + OPENARM_GRIPPER_JOINTS, + OPENARM_LEFT_MODEL, + OPENARM_RIGHT_MODEL, + openarm_arm_joints, + openarm_hardware, + openarm_model_config, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_teleop_hw = openarm_single_hardware() +# The keyboard publishes twists to one task by name; the other arm's task +# keeps holding its anchor pose. +KEYBOARD_EEF_TASK_NAME = "eef_twist_left_arm" -keyboard_teleop_openarm_mock = autoconnect( - KeyboardTeleopModule.blueprint(), +_openarm_keyboard_hw = openarm_hardware() + + +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={ + "model_path": OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, + "ee_joint_id": OPENARM_DOF, + }, + ) + + +def _trajectory_task(*, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=[*openarm_arm_joints("left"), *openarm_arm_joints("right")], + priority=priority, + params={"start_position_tolerance": 0.05}, + ) + + +def _gripper_task() -> TaskConfig: + return TaskConfig( + name="servo_grippers", + type="servo", + joint_names=list(OPENARM_GRIPPER_JOINTS), + priority=20, + params={"timeout": 0.0}, + ) + + +keyboard_teleop_openarm = autoconnect( + KeyboardTeleopModule.blueprint( + task_name=KEYBOARD_EEF_TASK_NAME, + gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), + ), ControlCoordinator.blueprint( - hardware=[_teleop_hw], - tasks=[eef_twist_task(_teleop_hw, model_path=OPENARM_V10_FK_MODEL, ee_joint_id=7)], + hardware=[_openarm_keyboard_hw], + tasks=[ + _eef_twist_task("left"), + _eef_twist_task("right"), + _gripper_task(), + ], ), ManipulationModule.blueprint( - robots=[openarm_single_model_config()], - visualization={"backend": "meshcat"}, + robots=[openarm_model_config("left"), openarm_model_config("right")], + 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(), - ControlCoordinator.blueprint( - hardware=[_teleop_real_hw], +keyboard_teleop_openarm_planner = autoconnect( + KeyboardTeleopModule.blueprint( + task_name=KEYBOARD_EEF_TASK_NAME, + gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), + ), + planner(robots=[openarm_model_config("left"), openarm_model_config("right")]), + coordinator( + hardware=[_openarm_keyboard_planner_hw], tasks=[ - eef_twist_task( - _teleop_real_hw, - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, - ) + _eef_twist_task("left", priority=10), + _eef_twist_task("right", priority=10), + _gripper_task(), + _trajectory_task(priority=20), ], ), - ManipulationModule.blueprint( - robots=[openarm_single_model_config()], - visualization={"backend": "meshcat"}, - ), ) diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 307450d054..651c2e1f83 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -12,19 +12,32 @@ # 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_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_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ ("openarm_left_link5", "openarm_left_link7"), ("openarm_right_link5", "openarm_right_link7"), @@ -34,55 +47,51 @@ 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_GRAVITY_MODEL_PATH = OPENARM_PKG / "urdf/robot/openarm_v10_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 v10 hardware (legacy adapter): 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: - validate_side(side) - kwargs = {"side": side} - if adapter_kwargs: - kwargs.update(adapter_kwargs) +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: + """Build one side's seven-joint planning model.""" validate_side(side) resolved_name = name or f"{side}_arm" - local_joint_names = openarm_joints(side) + local_joint_names = [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] return RobotModelConfig( name=resolved_name, model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, @@ -102,42 +111,11 @@ def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, - home_joints=[0.0] * 7, - ) - - -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_single_model_config() -> RobotModelConfig: - local_joint_names = openarm_joints("left") - return RobotModelConfig( - name="arm", - model_path=OPENARM_V10_FK_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), - base_link="openarm_body_link0", - tip_link="openarm_left_link7", + joint_name_mapping={ + coordinator_name: urdf_name + for coordinator_name, urdf_name in zip( + openarm_arm_joints(side), local_joint_names, strict=True ) - ], - package_paths=OPENARM_PACKAGE_PATHS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - home_joints=[0.0] * 7, + }, + home_joints=[0.0] * OPENARM_DOF, ) diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index 80ad6e1471..a08ae1bdd6 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -63,7 +63,6 @@ # Normalized gripper command values. GRIPPER_OPEN_POSITION = 1.0 GRIPPER_CLOSED_POSITION = 0.0 -# TODO: Improve gripper handling. GRIPPER_JOINT_NAME = "arm/gripper" TwistVector = tuple[float, float, float] @@ -74,6 +73,9 @@ class KeyboardTeleopConfig(ModuleConfig): linear_speed: float = DEFAULT_LINEAR_SPEED angular_speed: float = DEFAULT_ANGULAR_SPEED gripper_open_position: float = GRIPPER_OPEN_POSITION + # All named joints receive the same opening; multi-gripper robots list + # every gripper joint here. + gripper_joint_names: list[str] = [GRIPPER_JOINT_NAME] def _motion_key_codes() -> frozenset[int]: @@ -260,7 +262,8 @@ def _set_gripper_position(self, position: float) -> None: if self._gripper_position == position: return self._gripper_position = position - self.joint_command.publish(JointState(name=[GRIPPER_JOINT_NAME], position=[position])) + names = list(self.config.gripper_joint_names) + self.joint_command.publish(JointState(name=names, position=[position] * len(names))) def _twist_from_keys( diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index bdf48b09f6..9a51f6fd2c 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-piper # Piper 6-DOF dimos run keyboard-teleop-openyam # OpenYAM 6-DOF + gripper dimos run keyboard-teleop-xarm6 # XArm6 6-DOF @@ -32,6 +33,12 @@ 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; the right arm holds its +pose, and `[` / `]` drive both grippers together. + 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..099f83f2b3 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -1,387 +1,79 @@ --- 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: - -``` -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) +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/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) +Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): +2x DM8006, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. -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) -``` +Gravity compensation uses the bimanual URDF +(`openarm_description/urdf/robot/openarm_v10_bimanual.urdf`, resolved lazily +from LFS at connect time) and is preflighted against the declared joint order +before the motors enable. -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 can setup can0 +dimos 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`. +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. -**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). +## Blueprints -### 2. Verify all 16 motors are alive - -```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 -``` - -Meshcat will appear at http://localhost:7000. - -### 5. Drive the arms from the manipulation client +| `coordinator-openarm` | coordinator + trajectory task over both arms | +| `openarm-planner-coordinator` | planner (per-side models) + coordinator | +| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + gripper servo + viser | +| `keyboard-teleop-openarm-planner` | teleop + planner + preempting trajectory task | -With `openarm-planner-coordinator` running in one terminal, open a second terminal and start the REPL client: +All blueprints run against the in-memory whole-body adapter under +`--simulation`; the physical adapter is selected automatically otherwise. -```bash -python -i -m dimos.manipulation.planning.examples.manipulation_client -``` +The keyboard jogs the left arm (`eef_twist_left_arm`); the right arm's twist +task holds its anchor pose. `[` opens and `]` closes both grippers together +via a single servo task over both gripper joints. -This gives you an interactive Python prompt with these functions: +## Files -| 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" -``` +| `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 | -No other code changes are needed. - -### 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. From 1358bb7d09372308fa87167d4f518e3a82beb705 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Mon, 3 Aug 2026 21:46:30 -0700 Subject: [PATCH 02/11] refactor(manipulation): remove the superseded OpenArm manipulator driver The hand-rolled Damiao CAN driver and manipulator-protocol adapter are fully replaced by OpenArmDamiaoAdapter on the whole-body path, which also wires the previously unimplemented grippers. Drop the legacy CAN bring-up scripts (superseded by 'dimos can setup') and the openarm manipulator registry entry. --- .../manipulators/openarm/_registry.py | 17 - .../hardware/manipulators/openarm/adapter.py | 430 ------------------ dimos/hardware/manipulators/openarm/driver.py | 329 -------------- .../manipulators/openarm/test_driver.py | 270 ----------- .../manipulators/test_adapter_lifecycle.py | 50 -- dimos/hardware/test_adapter_registries.py | 1 - .../openarm/scripts/openarm_can_probe.py | 206 --------- .../openarm/scripts/openarm_can_up.sh | 36 -- .../openarm/scripts/openarm_set_mit_mode.py | 140 ------ 9 files changed, 1479 deletions(-) delete mode 100644 dimos/hardware/manipulators/openarm/_registry.py delete mode 100644 dimos/hardware/manipulators/openarm/adapter.py delete mode 100644 dimos/hardware/manipulators/openarm/driver.py delete mode 100644 dimos/hardware/manipulators/openarm/test_driver.py delete mode 100755 dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py delete mode 100755 dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh delete mode 100755 dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py diff --git a/dimos/hardware/manipulators/openarm/_registry.py b/dimos/hardware/manipulators/openarm/_registry.py deleted file mode 100644 index eed680a4be..0000000000 --- a/dimos/hardware/manipulators/openarm/_registry.py +++ /dev/null @@ -1,17 +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. - -ADAPTER_FACTORIES = { - "openarm": "dimos.hardware.manipulators.openarm.adapter:OpenArmAdapter", -} 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 25b8a6feb3..04dfb36aaa 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -53,7 +53,6 @@ "manipulators": { "a750", "mock", - "openarm", "piper", "sim_mujoco", "xarm", 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()) From 7be88dfa56d7d949d3def7e263f5ed8424b38e6e Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 12:31:54 -0700 Subject: [PATCH 03/11] fix(manipulation): address OpenArm review round 1 DM8009 shoulders (DM8006 was a legacy typo), MotorSpecs as plain lists, keyboard teleop module restored to upstream with gripper bindings deferred to a follow-up PR, and dual-arm planning switched to a single bimanual robot model with left_manipulator and right_manipulator groups fed by a hand-written SRDF, since generated SRDFs cannot express cross-robot collision exclusions. Planner blueprint verified against the in-memory adapter in simulation. --- .../test_manipulation_planning_groups.py | 50 ++++++++-------- .../whole_body/openarm_damiao/adapter.py | 26 +++------ .../manipulators/openarm/blueprints/basic.py | 9 +-- .../manipulators/openarm/blueprints/teleop.py | 34 +++-------- dimos/robot/manipulators/openarm/config.py | 58 +++++++++++-------- .../openarm/openarm_v10_bimanual.srdf | 5 ++ .../teleop/keyboard/keyboard_teleop_module.py | 7 +-- docs/capabilities/manipulation/index.md | 4 +- .../manipulation/openarm_integration.md | 21 ++++--- 9 files changed, 103 insertions(+), 111 deletions(-) create mode 100644 dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 1d8bc367a8..7cbd5b68e0 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -42,6 +42,10 @@ # 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" +# Both arms plan as one robot; joint order is left 1..7 then right 1..7. +ROBOT_NAME = "openarm" +LEFT_SLICE = slice(0, 7) +RIGHT_SLICE = slice(7, 14) def _wait_for_robot_info( @@ -130,21 +134,23 @@ def _prepare_for_planning(client: RPCClient, robot_names: tuple[str, ...]) -> No _wait_for_manipulation_state(client, "IDLE") -def _planning_group_id(info: dict[str, Any]) -> str: - groups = info["planning_groups"] - assert len(groups) == 1 - group = groups[0] - if isinstance(group, PlanningGroup): - return group.id - group_id = group["id"] - assert isinstance(group_id, str) - return group_id +def _planning_group_ids(info: dict[str, Any]) -> dict[str, str]: + ids: dict[str, str] = {} + for group in info["planning_groups"]: + if isinstance(group, PlanningGroup): + ids[group.group_name] = group.id + else: + group_id = group["id"] + assert isinstance(group_id, str) + ids[group["group_name"]] = group_id + assert set(ids) == {"left_manipulator", "right_manipulator"} + return ids -def _offset_target(client: RPCClient, robot_name: str, delta: float) -> JointState: - current = client.get_current_joints(robot_name) +def _offset_target(client: RPCClient, group_slice: slice, delta: float) -> JointState: + current = client.get_current_joints(ROBOT_NAME) assert current is not None - return JointState(position=[position + delta for position in current]) + return JointState(position=[position + delta for position in current[group_slice]]) def _start_openarm_mock_planner( @@ -165,15 +171,15 @@ def test_single_arm_plans_and_executes_through_control_coordinator( client = RPCClient(None, ManipulationModule) coordinator_client = RPCClient(None, ControlCoordinator) try: - left_info = _wait_for_robot_info(client, "left_arm") - left_id = _planning_group_id(left_info) + info = _wait_for_robot_info(client, ROBOT_NAME) + left_id = _planning_group_ids(info)["left_manipulator"] tasks = coordinator_client.list_tasks() assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME] - _prepare_for_planning(client, ("left_arm",)) + _prepare_for_planning(client, (ROBOT_NAME,)) - planned = client.plan_to_joint_targets({left_id: _offset_target(client, "left_arm", 0.02)}) + planned = client.plan_to_joint_targets({left_id: _offset_target(client, LEFT_SLICE, 0.02)}) assert planned, client.get_error() assert client.has_planned_path() assert client.execute_plan() @@ -194,20 +200,18 @@ def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( client = RPCClient(None, ManipulationModule) coordinator_client = RPCClient(None, ControlCoordinator) try: - left_info = _wait_for_robot_info(client, "left_arm") - right_info = _wait_for_robot_info(client, "right_arm") - left_id = _planning_group_id(left_info) - right_id = _planning_group_id(right_info) + info = _wait_for_robot_info(client, ROBOT_NAME) + group_ids = _planning_group_ids(info) tasks = coordinator_client.list_tasks() assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME] - _prepare_for_planning(client, ("left_arm", "right_arm")) + _prepare_for_planning(client, (ROBOT_NAME,)) planned = client.plan_to_joint_targets( { - left_id: _offset_target(client, "left_arm", 0.02), - right_id: _offset_target(client, "right_arm", -0.02), + group_ids["left_manipulator"]: _offset_target(client, LEFT_SLICE, 0.02), + group_ids["right_manipulator"]: _offset_target(client, RIGHT_SLICE, -0.02), } ) assert planned, client.get_error() diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py index d7b195e6ab..42b2fa748b 100644 --- a/dimos/hardware/whole_body/openarm_damiao/adapter.py +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -24,24 +24,16 @@ from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter from dimos.utils.data import LfsPath -# Per-arm motor models, shoulder to wrist, from -# openarm_description/config/arm/v10/joint_limits.yaml. Both arms use the same -# CAN send ids 0x01..0x07 because each arm owns a dedicated bus. -_ARM_MOTOR_TYPES = ( - damiao.MotorType.DM8006, - damiao.MotorType.DM8006, - damiao.MotorType.DM4340, - damiao.MotorType.DM4340, - damiao.MotorType.DM4310, - damiao.MotorType.DM4310, - damiao.MotorType.DM4310, -) - def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: return [ - can_motor_control.MotorSpec(f"openarm_{side}_joint{index}", motor_type, index, index | 0x10) - for index, motor_type in enumerate(_ARM_MOTOR_TYPES, start=1) + 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), ] @@ -65,8 +57,8 @@ class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): "left_gripper": "left_arm/gripper", "right_gripper": "right_arm/gripper", } - # Linux assigns can0/can1 in USB enumeration order; remap a swapped rig - # through DamiaoRuntimeConfig.bus_addresses instead of editing topology. + # 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)), diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index bf09190813..36afa57c52 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -22,8 +22,8 @@ from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( OPENARM_ARM_JOINTS, + openarm_bimanual_model_config, openarm_hardware, - openarm_model_config, ) @@ -40,12 +40,7 @@ def _trajectory_task() -> TaskConfig: _openarm_planner_hw = openarm_hardware() openarm_planner_coordinator = autoconnect( - planner( - robots=[ - openarm_model_config("left"), - openarm_model_config("right"), - ], - ), + planner(robots=[openarm_bimanual_model_config()]), coordinator( hardware=[_openarm_planner_hw], tasks=[_trajectory_task()], diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index 299ef9d85c..435f61a457 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -22,17 +22,17 @@ 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 ( + OPENARM_ARM_JOINTS, OPENARM_DOF, - OPENARM_GRIPPER_JOINTS, OPENARM_LEFT_MODEL, OPENARM_RIGHT_MODEL, openarm_arm_joints, + openarm_bimanual_model_config, openarm_hardware, - openarm_model_config, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -# The keyboard publishes twists to one task by name; the other arm's task +# 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" @@ -56,37 +56,23 @@ def _trajectory_task(*, priority: int = 10) -> TaskConfig: return TaskConfig( name=DEFAULT_TRAJECTORY_TASK_NAME, type="trajectory", - joint_names=[*openarm_arm_joints("left"), *openarm_arm_joints("right")], + joint_names=list(OPENARM_ARM_JOINTS), priority=priority, params={"start_position_tolerance": 0.05}, ) -def _gripper_task() -> TaskConfig: - return TaskConfig( - name="servo_grippers", - type="servo", - joint_names=list(OPENARM_GRIPPER_JOINTS), - priority=20, - params={"timeout": 0.0}, - ) - - keyboard_teleop_openarm = autoconnect( - KeyboardTeleopModule.blueprint( - task_name=KEYBOARD_EEF_TASK_NAME, - gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), - ), + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), ControlCoordinator.blueprint( hardware=[_openarm_keyboard_hw], tasks=[ _eef_twist_task("left"), _eef_twist_task("right"), - _gripper_task(), ], ), ManipulationModule.blueprint( - robots=[openarm_model_config("left"), openarm_model_config("right")], + robots=[openarm_bimanual_model_config()], visualization={"backend": "viser"}, ), ) @@ -94,17 +80,13 @@ def _gripper_task() -> TaskConfig: _openarm_keyboard_planner_hw = openarm_hardware() keyboard_teleop_openarm_planner = autoconnect( - KeyboardTeleopModule.blueprint( - task_name=KEYBOARD_EEF_TASK_NAME, - gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), - ), - planner(robots=[openarm_model_config("left"), openarm_model_config("right")]), + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), + planner(robots=[openarm_bimanual_model_config()]), coordinator( hardware=[_openarm_keyboard_planner_hw], tasks=[ _eef_twist_task("left", priority=10), _eef_twist_task("right", priority=10), - _gripper_task(), _trajectory_task(priority=20), ], ), diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 651c2e1f83..ebde7dd663 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -38,21 +38,16 @@ OPENARM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] -OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("openarm_left_link5", "openarm_left_link7"), - ("openarm_right_link5", "openarm_right_link7"), -] - 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_GRAVITY_MODEL_PATH = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" +OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" +OPENARM_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v10_bimanual.srdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# MIT gains measured on v10 hardware (legacy adapter): 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. +# MIT gains measured on v10 hardware: 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) @@ -67,6 +62,11 @@ def openarm_arm_joints(side: str) -> list[str]: return [f"{side}_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +def openarm_urdf_joints(side: str) -> list[str]: + validate_side(side) + 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" @@ -87,35 +87,45 @@ def openarm_hardware() -> HardwareComponent: ) -def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: - """Build one side's seven-joint planning model.""" - validate_side(side) - resolved_name = name or f"{side}_arm" - local_joint_names = [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] +def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModelConfig: + """Build the single fourteen-joint planning model with one group per arm. + + SRDF generation does not compose collision exclusions across robots, so + both arms plan as one robot and the exclusions come from a hand-written + SRDF. + """ + local_joint_names = [*openarm_urdf_joints("left"), *openarm_urdf_joints("right")] return RobotModelConfig( - name=resolved_name, - model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_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=f"openarm_{side}_link7", - ) + tip_link="openarm_left_link7", + ), + PlanningGroupDefinition( + name="right_manipulator", + joint_names=tuple(openarm_urdf_joints("right")), + base_link="openarm_body_link0", + tip_link="openarm_right_link7", + ), ], package_paths=OPENARM_PACKAGE_PATHS, - collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, + srdf_path=OPENARM_BIMANUAL_SRDF, auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, joint_name_mapping={ coordinator_name: urdf_name + for side in OPENARM_SIDES for coordinator_name, urdf_name in zip( - openarm_arm_joints(side), local_joint_names, strict=True + openarm_arm_joints(side), openarm_urdf_joints(side), strict=True ) }, - home_joints=[0.0] * OPENARM_DOF, + home_joints=[0.0] * (2 * OPENARM_DOF), ) diff --git a/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf new file mode 100644 index 0000000000..68fd5991f8 --- /dev/null +++ b/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf @@ -0,0 +1,5 @@ + + + + + diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index a08ae1bdd6..80ad6e1471 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -63,6 +63,7 @@ # Normalized gripper command values. GRIPPER_OPEN_POSITION = 1.0 GRIPPER_CLOSED_POSITION = 0.0 +# TODO: Improve gripper handling. GRIPPER_JOINT_NAME = "arm/gripper" TwistVector = tuple[float, float, float] @@ -73,9 +74,6 @@ class KeyboardTeleopConfig(ModuleConfig): linear_speed: float = DEFAULT_LINEAR_SPEED angular_speed: float = DEFAULT_ANGULAR_SPEED gripper_open_position: float = GRIPPER_OPEN_POSITION - # All named joints receive the same opening; multi-gripper robots list - # every gripper joint here. - gripper_joint_names: list[str] = [GRIPPER_JOINT_NAME] def _motion_key_codes() -> frozenset[int]: @@ -262,8 +260,7 @@ def _set_gripper_position(self, position: float) -> None: if self._gripper_position == position: return self._gripper_position = position - names = list(self.config.gripper_joint_names) - self.joint_command.publish(JointState(name=names, position=[position] * len(names))) + self.joint_command.publish(JointState(name=[GRIPPER_JOINT_NAME], position=[position])) def _twist_from_keys( diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 9a51f6fd2c..7aaadd3c24 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -36,8 +36,8 @@ 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; the right arm holds its -pose, and `[` / `]` drive both grippers together. +`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. diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 099f83f2b3..517c81a5fb 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -27,18 +27,24 @@ cycle. The command vector order is `left_arm/joint1..7`, `right_arm/joint1..7`, (`0.0` closed, `1.0` open). Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): -2x DM8006, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. +2x DM8009, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. Gravity compensation uses the bimanual URDF (`openarm_description/urdf/robot/openarm_v10_bimanual.urdf`, resolved lazily from LFS at connect time) and is preflighted against the declared joint order before the motors enable. +Planning also uses the bimanual URDF: one robot model with a +`left_manipulator` and a `right_manipulator` planning group. Automatic SRDF +generation does not compose collision exclusions across robots, so the +exclusions come from a hand-written SRDF +(`dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf`). + ## Bring-up ```bash -dimos can setup can0 -dimos can setup can1 +dimos hardware can setup can0 +dimos hardware can setup can1 dimos run keyboard-teleop-openarm ``` @@ -52,16 +58,17 @@ editing the adapter topology. | Blueprint | Contents | |---|---| | `coordinator-openarm` | coordinator + trajectory task over both arms | -| `openarm-planner-coordinator` | planner (per-side models) + coordinator | -| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + gripper servo + viser | +| `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 | All blueprints run against the in-memory whole-body adapter under `--simulation`; the physical adapter is selected automatically otherwise. The keyboard jogs the left arm (`eef_twist_left_arm`); the right arm's twist -task holds its anchor pose. `[` opens and `]` closes both grippers together -via a single servo task over both gripper joints. +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. ## Files From 678cc0514ac654f5e8b1d58636816d63d8343da1 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 13:33:13 -0700 Subject: [PATCH 04/11] feat(manipulation): use official OpenArm 2.0 description Replace the stale v1.0 data package with URDFs generated from the official OpenArm v2.0 preset pipeline (enactic/openarm_description @ 6c7b720f1ba, default_bimanual plus per-side pinch gripper presets). Pinch gripper finger joints are fixed in the generated models so each arm exposes exactly its seven driven joints while keeping gripper geometry and mass; mesh URIs stay package relative and ros2_control blocks are dropped. The package ships v2.0 meshes, the three URDFs, and a PROVENANCE file, and shrinks from 70 MB to 8 MB. The v2.0 generator collapses link7, so planning tips move to openarm_{side}_ee_base_link and the SRDF now disables the sibling finger pair per hand. Joint naming is unchanged from v1.0, so the adapter topology and gravity joint order carry over. Verified with pinocchio (14 and 7 DOF models, finite gravity) and a planner blueprint run in simulation. --- data/.lfs/openarm_description.tar.gz | 4 ++-- .../whole_body/openarm_damiao/adapter.py | 6 +++--- dimos/robot/manipulators/openarm/config.py | 19 ++++++++++--------- .../openarm/openarm_v10_bimanual.srdf | 5 ----- .../openarm/openarm_v20_bimanual.srdf | 5 +++++ .../manipulation/openarm_integration.md | 4 ++-- 6 files changed, 22 insertions(+), 21 deletions(-) delete mode 100644 dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf create mode 100644 dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf diff --git a/data/.lfs/openarm_description.tar.gz b/data/.lfs/openarm_description.tar.gz index 54aa76da41..74fe2a9ee4 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:b064cb32f95abb8b0d75c803d06246bbbf4c07a5226905f0e78297896a82fb45 +size 8095150 diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py index 42b2fa748b..4b64ae287a 100644 --- a/dimos/hardware/whole_body/openarm_damiao/adapter.py +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""OpenArm v10 bimanual physical topology for the generic Damiao whole-body adapter.""" +"""OpenArm v2.0 bimanual physical topology for the generic Damiao whole-body adapter.""" from __future__ import annotations @@ -47,7 +47,7 @@ def _gripper_motor(side: str) -> can_motor_control.MotorSpec: class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): - """Two OpenArm v10 arms with grippers, one CAN bus per arm.""" + """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)), @@ -68,7 +68,7 @@ class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): @property def gravity_model_path(self) -> Path: """Return the lazy bimanual gravity-compensation URDF path.""" - return LfsPath("openarm_description") / "urdf/robot/openarm_v10_bimanual.urdf" + return LfsPath("openarm_description") / "urdf/robot/openarm_v20_bimanual.urdf" def _build_robot(self) -> can_motor_control.Robot: return ( diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index ebde7dd663..820c66a5e2 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -39,15 +39,16 @@ 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_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" -OPENARM_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v10_bimanual.srdf" +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_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v20_bimanual.srdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# MIT gains measured on v10 hardware: 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. +# 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) @@ -106,13 +107,13 @@ def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModel name="left_manipulator", joint_names=tuple(openarm_urdf_joints("left")), base_link="openarm_body_link0", - tip_link="openarm_left_link7", + tip_link="openarm_left_ee_base_link", ), PlanningGroupDefinition( name="right_manipulator", joint_names=tuple(openarm_urdf_joints("right")), base_link="openarm_body_link0", - tip_link="openarm_right_link7", + tip_link="openarm_right_ee_base_link", ), ], package_paths=OPENARM_PACKAGE_PATHS, diff --git a/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf deleted file mode 100644 index 68fd5991f8..0000000000 --- a/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf new file mode 100644 index 0000000000..ddd46939c7 --- /dev/null +++ b/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf @@ -0,0 +1,5 @@ + + + + + diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 517c81a5fb..38f1e4161b 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -30,7 +30,7 @@ Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): 2x DM8009, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. Gravity compensation uses the bimanual URDF -(`openarm_description/urdf/robot/openarm_v10_bimanual.urdf`, resolved lazily +(`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. @@ -38,7 +38,7 @@ Planning also uses the bimanual URDF: one robot model with a `left_manipulator` and a `right_manipulator` planning group. Automatic SRDF generation does not compose collision exclusions across robots, so the exclusions come from a hand-written SRDF -(`dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf`). +(`dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf`). ## Bring-up From 782e4b2d3ed3c23bd60f4a9d2b46924b0f4fdeb6 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 14:53:11 -0700 Subject: [PATCH 05/11] feat(manipulation): compose planning groups within one robot RoboPlan generated composite planning groups only for selections spanning two or more robots, so a bimanual robot modeled as one URDF with two planning groups could not plan both arms in a single request. Drop the robot-count restriction; the joint-disjointness requirement and the composite group cap still apply, and overlapping selections are already rejected at the selection layer. Verified in process on the OpenArm 2.0 bimanual model: left, right, and combined fourteen-joint plans all succeed. --- .../planning/world/roboplan_model.py | 2 -- dimos/manipulation/test_roboplan_world.py | 23 ++++++++++++++++--- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index 75f5b1d492..a6208abcf7 100644 --- a/dimos/manipulation/planning/world/roboplan_model.py +++ b/dimos/manipulation/planning/world/roboplan_model.py @@ -323,8 +323,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_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..9583a39f31 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -1394,7 +1394,7 @@ def test_native_selected_planner_accepts_local_joint_names( assert result.path[-1].position == [0.2, 0.4] -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( @@ -1415,8 +1415,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( From 414620241873bce7a57e20c0f01ee166e79df540 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 18:40:29 -0700 Subject: [PATCH 06/11] fix(manipulation): keep converted meshes with equal stems distinct Converted OBJ files were named by stem only, so a robot whose visual and collision meshes share a stem (OpenArm v2.0 uses visual/link3.dae and collision/link3.stl) had the collision conversion overwrite the visual one, and viewers rendered collision geometry in visual mode. Suffix the converted name with a hash of the source path and pin the behavior with tests. --- .../manipulation/planning/utils/mesh_utils.py | 7 ++- .../planning/utils/test_mesh_utils.py | 59 +++++++++++++++++++ 2 files changed, 64 insertions(+), 2 deletions(-) create mode 100644 dimos/manipulation/planning/utils/test_mesh_utils.py 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 From caf14861366b78055afd00c5707a86224aa3bdc4 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 19:01:52 -0700 Subject: [PATCH 07/11] feat(manipulation): put OpenArm pose targets at the grasp frame Regenerate the v2.0 URDFs with emit_grasp_frame enabled and move the planning group tip links from the ee flange to openarm_{side}_grasp_frame, matching the OpenYAM gripper_tip convention. Model DOF, joint order, and gravity behavior are unchanged; planning verified for left, right, and combined requests. --- data/.lfs/openarm_description.tar.gz | 4 ++-- dimos/robot/manipulators/openarm/config.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/data/.lfs/openarm_description.tar.gz b/data/.lfs/openarm_description.tar.gz index 74fe2a9ee4..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:b064cb32f95abb8b0d75c803d06246bbbf4c07a5226905f0e78297896a82fb45 -size 8095150 +oid sha256:3e9a568ec8bded5ca32b2e3de92d27ab78732bb6f2bf4d3d6d16e5093ca30997 +size 8095302 diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 820c66a5e2..e90a498840 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -107,13 +107,13 @@ def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModel name="left_manipulator", joint_names=tuple(openarm_urdf_joints("left")), base_link="openarm_body_link0", - tip_link="openarm_left_ee_base_link", + 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_ee_base_link", + tip_link="openarm_right_grasp_frame", ), ], package_paths=OPENARM_PACKAGE_PATHS, From 64efdee7ada08a4323c6ff2f2d4d0ada2558c9c3 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 20:21:53 -0700 Subject: [PATCH 08/11] fix(manipulation): merge target ghost state across groups on one robot The optimistic target ghost rebuilt its per-robot joint values from the current state for every selected group, so with two planning groups on one robot the group processed last discarded the other group's target and the ghost only ever showed one arm's goal. Seed the merge once per robot and overlay each group's target into the same values. --- dimos/manipulation/visualization/viser/gui.py | 4 ++- .../visualization/viser/test_gui.py | 32 +++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 02145d9fc5..1f90d4585c 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -969,7 +969,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] From 3565452cfd0d8e57dc37cd893805c291fb081da8 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Wed, 5 Aug 2026 10:01:55 -0700 Subject: [PATCH 09/11] refactor(manipulation): drop the unneeded OpenArm SRDF The pinch gripper finger joints are fixed in the generated models and their collision meshes do not intersect at the fixed pose, so the manual exclusions were dead weight. Verified left, right, and combined plans still succeed without the file. --- dimos/robot/manipulators/openarm/config.py | 6 +----- dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf | 5 ----- docs/capabilities/manipulation/openarm_integration.md | 6 ++---- 3 files changed, 3 insertions(+), 14 deletions(-) delete mode 100644 dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index e90a498840..cdfbef7a35 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -42,7 +42,6 @@ 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_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v20_bimanual.srdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} # MIT gains measured on v1.0 hardware, carried over as the v2.0 starting @@ -91,9 +90,7 @@ def openarm_hardware() -> HardwareComponent: def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModelConfig: """Build the single fourteen-joint planning model with one group per arm. - SRDF generation does not compose collision exclusions across robots, so - both arms plan as one robot and the exclusions come from a hand-written - SRDF. + 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( @@ -117,7 +114,6 @@ def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModel ), ], package_paths=OPENARM_PACKAGE_PATHS, - srdf_path=OPENARM_BIMANUAL_SRDF, auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, diff --git a/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf deleted file mode 100644 index ddd46939c7..0000000000 --- a/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 38f1e4161b..ed8b1d07f1 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -35,10 +35,8 @@ from LFS at connect time) and is preflighted against the declared joint order before the motors enable. Planning also uses the bimanual URDF: one robot model with a -`left_manipulator` and a `right_manipulator` planning group. Automatic SRDF -generation does not compose collision exclusions across robots, so the -exclusions come from a hand-written SRDF -(`dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf`). +`left_manipulator` and a `right_manipulator` planning group, since collision +exclusions cannot span robots. ## Bring-up From 05ccdbd12d24bb93082d59221d356f7bc30078c6 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Wed, 5 Aug 2026 15:31:10 -0700 Subject: [PATCH 10/11] fix(manipulation): stream arm state before gripper calibration Gripper opening calibrates during activation, and reading it earlier raises, so a connected but not activated whole-body adapter failed the entire state read and read-only bring-up sessions (connect without enable) streamed nothing. Report placeholder gripper states until the adapter is active; arms stream immediately and real openings appear after activation. Found on OpenArm hardware during the read-only phase. --- dimos/hardware/whole_body/damiao/adapter.py | 5 +++++ dimos/hardware/whole_body/damiao/test_adapter.py | 14 ++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index 6cbe6d4eb2..87ecd1f47f 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 cf3373edcd..2f88af2766 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -723,3 +723,17 @@ 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)] From 352e8bbcc7ec73364708d652ae12e82b4c2261aa Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Wed, 5 Aug 2026 15:35:46 -0700 Subject: [PATCH 11/11] fix(manipulation): pump feedback from the read path while inactive Damiao feedback only updates when the bus is ticked, which the write path does once per control cycle while the adapter is active. A connected but not activated adapter never ticked, so read-only sessions streamed the connect-time snapshot forever. Refresh from the read path whenever the adapter is inactive; the active path is unchanged and still ticks exactly once per cycle. --- dimos/hardware/whole_body/damiao/adapter.py | 5 +++++ dimos/hardware/whole_body/damiao/test_adapter.py | 16 ++++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index 87ecd1f47f..102c3d277d 100644 --- a/dimos/hardware/whole_body/damiao/adapter.py +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -237,6 +237,11 @@ def has_motor_states(self) -> bool: def read_motor_states(self) -> list[MotorState]: if not self._connected: raise RuntimeError("Damiao whole-body adapter is not connected") + if not self._active: + # The write path pumps the bus once per control cycle while + # active; without it feedback would stay frozen at the connect + # snapshot, so keep it flowing for read-only sessions. + self._refresh() states: list[MotorState] = [] for name, expected_joints in self.arm_joints.items(): arm = self._arms[name] diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index 2f88af2766..951f604034 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -737,3 +737,19 @@ def test_read_motor_states_inactive_gripper_reports_placeholder( 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