diff --git a/CONTEXT.md b/CONTEXT.md deleted file mode 100644 index d97571b990..0000000000 --- a/CONTEXT.md +++ /dev/null @@ -1,27 +0,0 @@ -# Manipulation Planning - -This context describes requests for planning robot motion through joint and Cartesian spaces. - -## Language - -**Cartesian Waypoint**: -One absolute TCP pose or relative rigid displacement within a Cartesian target. - -**Cartesian Target**: -An ordered, homogeneous sequence of Cartesian waypoints for one planning group, including its starting waypoint. An absolute target contains only `PoseStamped` waypoints and starts at the current TCP pose. A relative target contains only `Transform` waypoints, starts with the identity transform, and measures every waypoint from the planning-start TCP pose. -_Avoid_: Cartesian track - -**Cartesian Path Configuration**: -Per-planning-call policy that selects how Cartesian waypoints are connected and constrains that operation. It is independent of the startup configuration that selects and constructs a planner backend. - -**Standard Cartesian Planning**: -Cartesian waypoint planning through a backend's supported serializable options. For RoboPlan, this includes multi-waypoint and simultaneous multi-end-effector paths, bounded and time-optimal speed modes, tracking tolerances, and solver tuning. - -**Bounded Speed Mode**: -A Cartesian timing policy that treats configured tool speeds and accelerations as maxima and slows the motion further when required by tracking or joint limits. - -**Time-Optimal Speed Mode**: -A Cartesian timing policy that resolves the requested path into joint space and retimes it against joint limits, optionally blending intermediate corners. - -**Custom Planner Components**: -Backend-native solver tasks, constraints, and barriers injected as live objects. These are outside standard Cartesian planning and require a separate constrained-IK interface. diff --git a/data/.lfs/openarm_description.tar.gz b/data/.lfs/openarm_description.tar.gz index 54aa76da41..4a46e74a88 100644 --- a/data/.lfs/openarm_description.tar.gz +++ b/data/.lfs/openarm_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4da176b6c210b9796bb2ee1a29c15ee9a67578b9ae906eb89a6ec8a44b7f303a -size 70064687 +oid sha256:3e9a568ec8bded5ca32b2e3de92d27ab78732bb6f2bf4d3d6d16e5093ca30997 +size 8095302 diff --git a/data/.lfs/yam_description.tar.gz b/data/.lfs/yam_description.tar.gz index 7f7172ad6f..2ff8ba61d2 100644 --- a/data/.lfs/yam_description.tar.gz +++ b/data/.lfs/yam_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:24d843fdedc781d0623714b4cd777c7d8c4e4d8e9c256d162e14e4ad638b7dd0 -size 2182770 +oid sha256:eb8c04381e29ceb1340e818bc706ba866d706c37a254d5637babc7168a5de850 +size 5759450 diff --git a/dimos/cli/can.py b/dimos/cli/can.py new file mode 100644 index 0000000000..bf75ec354a --- /dev/null +++ b/dimos/cli/can.py @@ -0,0 +1,99 @@ +# 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. + +"""Linux CAN interface management commands.""" + +from __future__ import annotations + +import os +import shlex +import subprocess + +import typer + +app = typer.Typer(help="Configure and inspect Linux CAN interfaces", no_args_is_help=True) + + +def _run_ip(*args: str, privileged: bool = False) -> subprocess.CompletedProcess[str]: + command = ["ip", *args] + if privileged and os.geteuid() != 0: + command = ["sudo", "--", *command] + if privileged: + typer.echo(f"Running: {shlex.join(command)}") + try: + return subprocess.run( + command, + check=True, + capture_output=not privileged, + text=True, + ) + except FileNotFoundError as exc: + executable = command[0] + raise typer.BadParameter(f"the '{executable}' command is not installed") from exc + except subprocess.CalledProcessError as exc: + stderr = exc.stderr.strip() if exc.stderr else "" + stdout = exc.stdout.strip() if exc.stdout else "" + detail = stderr or stdout or f"exit code {exc.returncode}" + typer.echo(f"CAN interface command failed: {detail}", err=True) + raise typer.Exit(1) from exc + + +@app.command("status") +def status(interface: str = typer.Argument(..., help="Linux CAN interface name")) -> None: + """Show detailed CAN interface state and queue statistics.""" + result = _run_ip("-details", "-statistics", "link", "show", "dev", interface) + typer.echo(result.stdout.rstrip()) + + +@app.command("down") +def down(interface: str = typer.Argument(..., help="Linux CAN interface name")) -> None: + """Bring a CAN interface down.""" + _run_ip("link", "set", "dev", interface, "down", privileged=True) + typer.echo(f"CAN interface {interface} is down") + + +@app.command("up") +def up(interface: str = typer.Argument(..., help="Linux CAN interface name")) -> None: + """Bring an already configured CAN interface up.""" + _run_ip("link", "set", "dev", interface, "up", privileged=True) + typer.echo(f"CAN interface {interface} is up") + + +@app.command("setup") +def setup( + interface: str = typer.Argument(..., help="Linux CAN interface name"), + bitrate: int = typer.Option( + 1_000_000, + min=1, + help="Nominal CAN bitrate in bits per second", + ), + txqueuelen: int = typer.Option(1_000, min=1, help="Kernel transmit queue length"), +) -> None: + """Configure, bring up, and verify a classic CAN interface.""" + setup_interface(interface, bitrate=bitrate, txqueuelen=txqueuelen) + + +def setup_interface(interface: str, *, bitrate: int, txqueuelen: int = 1_000) -> None: + """Configure and verify one classic CAN interface.""" + + _run_ip("link", "show", "dev", interface) + _run_ip("link", "set", "dev", interface, "down", privileged=True) + _run_ip( + "link", "set", "dev", interface, "type", "can", "bitrate", str(bitrate), privileged=True + ) + _run_ip("link", "set", "dev", interface, "txqueuelen", str(txqueuelen), privileged=True) + _run_ip("link", "set", "dev", interface, "up", privileged=True) + result = _run_ip("-details", "-statistics", "link", "show", "dev", interface) + typer.echo(result.stdout.rstrip()) + typer.echo(f"Configured {interface}: bitrate={bitrate}, txqueuelen={txqueuelen}") diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index d1487798c3..dd30b475f0 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -50,6 +50,7 @@ from dimos.agents.mcp.mcp_adapter import McpAdapter, McpError from dimos.cli.cache import app as cache_app +from dimos.cli.can import app as can_app from dimos.cli.shell import shell from dimos.constants import CONFIG_DIR, LOG_DIR from dimos.core.daemon import daemonize, install_signal_handlers @@ -60,7 +61,6 @@ from dimos.mapping.cli.rename import main as _map_rename_main from dimos.mapping.cli.replay import main as _map_replay_main from dimos.mapping.cli.replay_marker import main as _map_replay_marker_main -from dimos.robot.manipulators.piper.cli import app as piper_app from dimos.robot.unitree.go2.cli.go2tool import app as go2tool_app from dimos.utils.cache import cache_usage_locked from dimos.utils.logging_config import setup_logger @@ -172,8 +172,10 @@ def callback(**kwargs) -> None: # type: ignore[no-untyped-def] main.callback()(create_dynamic_callback()) # type: ignore[no-untyped-call] +hardware_app = typer.Typer(help="Configure and inspect robot hardware", no_args_is_help=True) +hardware_app.add_typer(can_app, name="can") +main.add_typer(hardware_app, name="hardware") main.add_typer(go2tool_app, name="go2tool") -main.add_typer(piper_app, name="piper") main.command()(shell) main.add_typer(cache_app, name="cache") diff --git a/dimos/cli/test_can.py b/dimos/cli/test_can.py new file mode 100644 index 0000000000..509f58475e --- /dev/null +++ b/dimos/cli/test_can.py @@ -0,0 +1,202 @@ +# 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. + +import subprocess +from unittest.mock import Mock + +from click.testing import Result +import pytest +from pytest_mock import MockerFixture +from typer.testing import CliRunner + +from dimos.cli.dimos import main + + +def _subprocess_argv(run: Mock) -> list[list[str]]: + return [call.args[0] for call in run.call_args_list] + + +def _invoke_can(args: list[str]) -> Result: + return CliRunner().invoke(main, ["hardware", "can", *args]) + + +def test_can_commands_are_nested_under_hardware_scope() -> None: + result = CliRunner().invoke(main, ["hardware", "--help"]) + legacy = CliRunner().invoke(main, ["can", "--help"]) + + assert result.exit_code == 0, result.output + assert "can" in result.output + assert legacy.exit_code == 2 + + +def test_setup_valid_options_configures_and_verifies_can_interface( + mocker: MockerFixture, +) -> None: + mocker.patch("dimos.cli.can.os.geteuid", return_value=1000) + run = mocker.patch( + "dimos.cli.can.subprocess.run", + return_value=subprocess.CompletedProcess( + [], + 0, + stdout="4: follower_l: UP qlen 1000\n", + stderr="", + ), + ) + + result = _invoke_can(["setup", "follower_l"]) + + assert result.exit_code == 0, result.output + assert "Running: sudo -- ip link set dev follower_l down" in result.stdout + assert "bitrate=1000000, txqueuelen=1000" in result.stdout + assert _subprocess_argv(run) == [ + ["ip", "link", "show", "dev", "follower_l"], + ["sudo", "--", "ip", "link", "set", "dev", "follower_l", "down"], + [ + "sudo", + "--", + "ip", + "link", + "set", + "dev", + "follower_l", + "type", + "can", + "bitrate", + "1000000", + ], + [ + "sudo", + "--", + "ip", + "link", + "set", + "dev", + "follower_l", + "txqueuelen", + "1000", + ], + ["sudo", "--", "ip", "link", "set", "dev", "follower_l", "up"], + ["ip", "-details", "-statistics", "link", "show", "dev", "follower_l"], + ] + + +def test_setup_nonpositive_queue_length_returns_usage_error() -> None: + result = _invoke_can(["setup", "can0", "--txqueuelen", "0"]) + + assert result.exit_code == 2 + assert "x>=1" in result.output + + +def test_setup_nonpositive_bitrate_returns_usage_error() -> None: + result = _invoke_can(["setup", "can0", "--bitrate", "0"]) + + assert result.exit_code == 2 + assert "x>=1" in result.output + + +def test_status_existing_interface_prints_detailed_state(mocker: MockerFixture) -> None: + run = mocker.patch( + "dimos.cli.can.subprocess.run", + return_value=subprocess.CompletedProcess([], 0, stdout="can0: UP\n", stderr=""), + ) + + result = _invoke_can(["status", "can0"]) + + assert result.exit_code == 0, result.output + assert result.stdout == "can0: UP\n" + run.assert_called_once_with( + ["ip", "-details", "-statistics", "link", "show", "dev", "can0"], + check=True, + capture_output=True, + text=True, + ) + + +def test_down_nonroot_user_runs_privileged_command_with_sudo( + mocker: MockerFixture, +) -> None: + mocker.patch("dimos.cli.can.os.geteuid", return_value=1000) + run = mocker.patch( + "dimos.cli.can.subprocess.run", + return_value=subprocess.CompletedProcess([], 0), + ) + + result = _invoke_can(["down", "can1"]) + + assert result.exit_code == 0, result.output + assert "CAN interface can1 is down" in result.stdout + run.assert_called_once_with( + ["sudo", "--", "ip", "link", "set", "dev", "can1", "down"], + check=True, + capture_output=False, + text=True, + ) + + +def test_up_root_user_runs_ip_without_sudo(mocker: MockerFixture) -> None: + mocker.patch("dimos.cli.can.os.geteuid", return_value=0) + run = mocker.patch( + "dimos.cli.can.subprocess.run", + return_value=subprocess.CompletedProcess([], 0), + ) + + result = _invoke_can(["up", "can2"]) + + assert result.exit_code == 0, result.output + assert "CAN interface can2 is up" in result.stdout + run.assert_called_once_with( + ["ip", "link", "set", "dev", "can2", "up"], + check=True, + capture_output=False, + text=True, + ) + + +def test_status_missing_ip_command_returns_usage_error(mocker: MockerFixture) -> None: + mocker.patch("dimos.cli.can.subprocess.run", side_effect=FileNotFoundError) + + result = _invoke_can(["status", "can0"]) + + assert result.exit_code == 2 + assert "the 'ip' command is not installed" in result.output + + +@pytest.mark.parametrize( + ("stderr", "stdout", "expected_detail"), + [ + ("permission denied\n", "ignored\n", "permission denied"), + ("", "device not found\n", "device not found"), + ("", "", "exit code 7"), + ], +) +def test_status_failed_ip_command_reports_available_detail( + mocker: MockerFixture, + stderr: str, + stdout: str, + expected_detail: str, +) -> None: + mocker.patch( + "dimos.cli.can.subprocess.run", + side_effect=subprocess.CalledProcessError( + 7, + ["ip"], + output=stdout, + stderr=stderr, + ), + ) + + result = _invoke_can(["status", "can0"]) + + assert result.exit_code == 1 + assert f"CAN interface command failed: {expected_detail}" in result.output diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 99834b090f..e08e46f91c 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -970,6 +970,25 @@ def test_tick_loop_calls_compute(self, mock_adapter, wait_until): assert mock_task.compute.call_count > 0 + def test_write_all_hardware_rejected_command_logs_error(self, mocker): + hardware = {"arm": MagicMock()} + hardware["arm"].write_command.return_value = False + log_error = mocker.patch("dimos.control.tick_loop.logger.error") + tick_loop = TickLoop( + tick_rate=100.0, + hardware=hardware, + hardware_lock=threading.Lock(), + tasks={}, + task_lock=threading.Lock(), + joint_to_hardware={"arm/joint1": "arm"}, + ) + + tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.25}, ControlMode.SERVO_POSITION)}) + + log_error.assert_called_once_with( + "Hardware arm rejected SERVO_POSITION command from control task" + ) + class TestIntegration: def test_full_trajectory_execution(self, mock_adapter, wait_until): diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 3152bba393..7f640406ba 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -409,7 +409,11 @@ def _write_all_hardware( for hw_id, (positions, mode) in hw_commands.items(): if hw_id in self._hardware: try: - self._hardware[hw_id].write_command(positions, mode) + accepted = self._hardware[hw_id].write_command(positions, mode) + if not accepted: + logger.error( + f"Hardware {hw_id} rejected {mode.name} command from control task" + ) except Exception as e: logger.error(f"Failed to write to {hw_id}: {e}") diff --git a/dimos/core/coordination/blueprint_config/test_parser.py b/dimos/core/coordination/blueprint_config/test_parser.py index 6b24322489..040b8b5a24 100644 --- a/dimos/core/coordination/blueprint_config/test_parser.py +++ b/dimos/core/coordination/blueprint_config/test_parser.py @@ -27,6 +27,11 @@ from dimos.core.coordination.blueprints import TransportSpec, autoconnect from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Stream, Transport +from dimos.manipulation.manipulation_module import ( + ManipulationModule, + ManipulationModuleConfig, +) +from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig class NestedConfig(BaseModel): @@ -408,6 +413,22 @@ class UnionModule(Module): } +def test_parse_nested_viser_host_returns_overridden_config() -> None: + parsed = BlueprintConfigParser(ManipulationModule.blueprint()).parse( + [ + "--visualization.backend", + "viser", + "--visualization.host", + "0.0.0.0", + ], + environ={}, + ) + + config = ManipulationModuleConfig(**parsed.module_kwargs("manipulationmodule")) + assert isinstance(config.visualization, ViserVisualizationConfig) + assert config.visualization.host == "0.0.0.0" + + @pytest.mark.parametrize( "tokens", [ diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 390db63fde..7cbd5b68e0 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -39,7 +39,13 @@ 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" +# 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( @@ -128,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( @@ -163,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() @@ -192,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/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 13c38804b8..04dfb36aaa 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -42,12 +42,21 @@ } # Subpackages containing an adapter.py that intentionally register nothing. -UNREGISTERED_ADAPTER_DIRS: set[str] = set() +UNREGISTERED_ADAPTER_DIRS = { + # Abstract base used by concrete Damiao robot packages. + "dimos.hardware.whole_body.damiao", +} # Every name each registry must declare. Removing a name from a manifest is a # conscious change: update this set in the same PR. EXPECTED_NAMES = { - "manipulators": {"a750", "mock", "openarm", "piper", "sim_mujoco", "xarm"}, + "manipulators": { + "a750", + "mock", + "piper", + "sim_mujoco", + "xarm", + }, "drive_trains": { "flowbase", "mock_twist_base", @@ -55,7 +64,14 @@ "transport_ros", "unitree_go2", }, - "whole_body": {"sim_mujoco_g1", "transport_lcm", "transport_ros"}, + "whole_body": { + "mock_whole_body", + "openarm_damiao", + "openyam_damiao", + "sim_mujoco_g1", + "transport_lcm", + "transport_ros", + }, } FAMILIES = [ diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py new file mode 100644 index 0000000000..102c3d277d --- /dev/null +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -0,0 +1,393 @@ +# 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. + +"""Generic whole-body adapter for robots built with ``can-motor-control``.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from pathlib import Path + +import can_motor_control +import numpy as np +import pinocchio + +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class DamiaoWholeBodyAdapter(ABC): + """Map DimOS whole-body IO onto one upstream robot lifecycle owner. + + Subclasses own immutable physical topology by implementing ``_build_robot``. + The mappings declare how upstream arm and gripper groups appear in DimOS. + """ + + arm_joints: dict[str, tuple[str, ...]] = {} + gripper_joints: dict[str, str] = {} + bus_defaults: dict[str, str] = {} + gravity_joint_names: tuple[str, ...] = () + + def __init__( + self, + address: str | Path | None = None, + *, + runtime_config: DamiaoRuntimeConfig | None = None, + dof: int | None = None, + hardware_id: str = "whole_body", + domain_id: int = 0, + ) -> None: + """Initialize runtime settings for a subclass-declared Damiao topology. + + ``address`` is accepted for the coordinator's common adapter factory + convention, but one scalar cannot represent a multi-bus whole body. + Configure physical CAN interfaces by logical bus name through + ``runtime_config.bus_addresses`` instead. + """ + del domain_id + if address is not None: + raise ValueError("configure Damiao CAN buses through runtime_config.bus_addresses") + config = runtime_config or DamiaoRuntimeConfig() + unknown_buses = config.bus_addresses.keys() - self.bus_defaults.keys() + if unknown_buses: + raise ValueError(f"unknown CAN bus overrides: {sorted(unknown_buses)}") + + joint_names = self.joint_names + if len(joint_names) != len(set(joint_names)): + raise ValueError("whole-body joint mappings contain duplicate names") + if dof is not None and dof != len(joint_names): + raise ValueError(f"expected {len(joint_names)} joints, got {dof}") + + arm_joint_count = sum(len(names) for names in self.arm_joints.values()) + if self.gravity_joint_names and len(self.gravity_joint_names) != arm_joint_count: + raise ValueError("gravity joint mapping must contain every angular arm joint") + + self._runtime_config = config + self._hardware_id = hardware_id + self._connected = False + self._active = False + self._has_state = False + self._robot: can_motor_control.Robot + self._arms: dict[str, can_motor_control.Arm] + self._grippers: dict[str, can_motor_control.Gripper] + self._pin_model: pinocchio.Model + self._pin_data: pinocchio.Data + + @property + def joint_names(self) -> tuple[str, ...]: + return tuple( + joint for group_joints in self.arm_joints.values() for joint in group_joints + ) + tuple(self.gripper_joints.values()) + + def bus_address(self, name: str) -> str: + """Resolve a subclass-declared bus name through runtime overrides.""" + try: + return self._runtime_config.bus_addresses.get(name, self.bus_defaults[name]) + except KeyError as exc: + raise ValueError(f"subclass did not declare CAN bus {name!r}") from exc + + @property + def gravity_model_path(self) -> Path | None: + """Return the subclass's gravity URDF without resolving it at import time.""" + return None + + @abstractmethod + def _build_robot(self) -> can_motor_control.Robot: + """Construct the upstream robot from the subclass's physical topology.""" + + def connect(self) -> bool: + try: + robot = self._build_robot() + except Exception: + logger.exception( + "Damiao whole-body adapter failed to build", + hardware_id=self._hardware_id, + ) + return False + + try: + robot.connect() + arms = {name: self._require_arm(robot, name) for name in self.arm_joints} + grippers = {name: self._require_gripper(robot, name) for name in self.gripper_joints} + self._robot = robot + self._arms = arms + self._grippers = grippers + self._load_gravity_model() + self._connected = True + self._refresh() + return True + except Exception: + logger.exception( + "Damiao whole-body adapter failed to connect", + hardware_id=self._hardware_id, + ) + try: + if robot.is_connected(): + robot.disable() + except Exception: + logger.warning( + "Damiao whole-body connect rollback failed", + hardware_id=self._hardware_id, + exc_info=True, + ) + self._connected = False + self._active = False + self._has_state = False + return False + + @staticmethod + def _require_arm( + robot: can_motor_control.Robot, + name: str, + ) -> can_motor_control.Arm: + group = robot[name] + if not isinstance(group, can_motor_control.Arm): + raise TypeError(f"upstream group {name!r} is not an Arm") + return group + + @staticmethod + def _require_gripper( + robot: can_motor_control.Robot, + name: str, + ) -> can_motor_control.Gripper: + group = robot[name] + if not isinstance(group, can_motor_control.Gripper): + raise TypeError(f"upstream group {name!r} is not a Gripper") + return group + + def disconnect(self) -> None: + if not self._connected: + return + try: + self._robot.disable() + except Exception: + logger.warning( + "Damiao whole-body adapter failed to disable while disconnecting", + hardware_id=self._hardware_id, + exc_info=True, + ) + self._connected = False + self._active = False + self._has_state = False + + def is_connected(self) -> bool: + return self._connected and self._robot.is_connected() + + def activate(self) -> bool: + if not self._connected: + return False + try: + self._preflight_gravity() + for arm in self._arms.values(): + arm.set_mode("mit") + self._robot.enable() + self._active = True + self._refresh() + self.read_motor_states() + return True + except Exception: + logger.exception( + "Damiao whole-body adapter failed to activate", + hardware_id=self._hardware_id, + ) + try: + self._robot.disable() + except Exception: + logger.error( + "Damiao whole-body activation rollback failed", + hardware_id=self._hardware_id, + exc_info=True, + ) + self._active = False + return False + + def deactivate(self) -> bool: + if not self._connected: + return False + try: + self._robot.disable() + except Exception: + logger.exception( + "Damiao whole-body adapter failed to deactivate", + hardware_id=self._hardware_id, + ) + return False + self._active = False + return True + + def has_motor_states(self) -> bool: + if not self._connected or not self._has_state: + return False + return not self._grippers or self._active + + 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] + q = arm.positions().astype(np.float64).tolist() + dq = arm.velocities().astype(np.float64).tolist() + tau = arm.torques().astype(np.float64).tolist() + if any(len(values) != len(expected_joints) for values in (q, dq, tau)): + raise RuntimeError(f"upstream arm {name!r} returned the wrong state length") + states.extend( + MotorState(q=position, dq=velocity, tau=effort) + 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}") + states.append(MotorState(q=opening, dq=0.0, tau=0.0)) + self._validate_finite_states(states) + return states + + def read_imu(self) -> IMUState: + return IMUState() + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + if not self._connected or not self._active or len(commands) != len(self.joint_names): + return False + try: + arm_count = sum(len(joints) for joints in self.arm_joints.values()) + arm_values = np.asarray( + [ + (command.q, command.dq, command.kp, command.kd, command.tau) + for command in commands[:arm_count] + ], + dtype=np.float64, + ) + if not np.isfinite(arm_values).all(): + raise ValueError("arm command contains non-finite values") + for name, command in zip( + self.gripper_joints, + commands[arm_count:], + strict=True, + ): + if not np.isfinite(command.q) or not 0.0 <= command.q <= 1.0: + raise ValueError(f"gripper {name!r} opening must be in [0, 1]") + + gravity = self._gravity_torques() + offset = 0 + gravity_offset = 0 + for name, joints in self.arm_joints.items(): + count = len(joints) + group_commands = commands[offset : offset + count] + rows = np.asarray( + [ + [ + command.kp, + command.kd, + command.q, + command.dq, + command.tau + gravity[gravity_offset + index], + ] + for index, command in enumerate(group_commands) + ], + dtype=np.float64, + ) + self._arms[name].mit_control(rows) + offset += count + gravity_offset += count + + for name in self.gripper_joints: + opening = commands[offset].q + self._grippers[name].set_opening(opening) + offset += 1 + + self._robot.tick(self._runtime_config.tick_deadline_us) + return True + except Exception: + logger.exception( + "Damiao whole-body adapter rejected motor command", + hardware_id=self._hardware_id, + ) + return False + + def _refresh(self) -> None: + self._robot.refresh() + self._robot.tick(self._runtime_config.tick_deadline_us) + self._has_state = True + + def _load_gravity_model(self) -> None: + if not self._runtime_config.gravity_comp: + return + model_path = self.gravity_model_path + if model_path is None or not model_path.is_file(): + raise ValueError("gravity compensation requires an existing URDF") + self._pin_model = pinocchio.buildModelFromUrdf(str(model_path)) + self._pin_data = self._pin_model.createData() + + def _preflight_gravity(self) -> None: + if not self._runtime_config.gravity_comp: + return + q = self._arm_positions() + if self._pin_model.nq != len(q) or self._pin_model.nv != len(q): + raise ValueError( + f"gravity model dimensions ({self._pin_model.nq}, {self._pin_model.nv}) " + f"do not match {len(q)} angular joints" + ) + model_names = tuple(str(name) for name in self._pin_model.names[1:]) + if model_names != self.gravity_joint_names: + raise ValueError( + f"gravity model joint order {model_names!r} does not match " + f"{self.gravity_joint_names!r}" + ) + gravity = self._gravity_torques() + if len(gravity) != len(q) or not np.isfinite(gravity).all(): + raise ValueError("gravity compensation produced invalid torques") + + def _arm_positions(self) -> np.ndarray: + positions = np.concatenate( + [arm.positions().astype(np.float64) for arm in self._arms.values()] + ) + if not np.isfinite(positions).all(): + raise ValueError("arm feedback contains non-finite positions") + return positions + + def _gravity_torques(self) -> np.ndarray: + count = sum(len(joints) for joints in self.arm_joints.values()) + if not self._runtime_config.gravity_comp: + return np.zeros(count, dtype=np.float64) + return np.asarray( + pinocchio.computeGeneralizedGravity( + self._pin_model, + self._pin_data, + self._arm_positions(), + ), + dtype=np.float64, + ) + + @staticmethod + def _validate_finite_states(states: list[MotorState]) -> None: + values = np.asarray( + [(state.q, state.dq, state.tau) for state in states], + dtype=np.float64, + ) + if not np.isfinite(values).all(): + raise RuntimeError("whole-body feedback contains non-finite values") diff --git a/dimos/hardware/whole_body/damiao/config.py b/dimos/hardware/whole_body/damiao/config.py new file mode 100644 index 0000000000..278ce9e4ee --- /dev/null +++ b/dimos/hardware/whole_body/damiao/config.py @@ -0,0 +1,47 @@ +# 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 __future__ import annotations + +import attrs + + +@attrs.frozen +class DamiaoRuntimeConfig: + """Deployment values that may vary without changing robot topology.""" + + bus_addresses: dict[str, str] = attrs.field( + factory=dict, + validator=attrs.validators.deep_mapping( + key_validator=attrs.validators.and_( + attrs.validators.instance_of(str), + attrs.validators.min_len(1), + ), + value_validator=attrs.validators.and_( + attrs.validators.instance_of(str), + attrs.validators.min_len(1), + ), + ), + ) + gravity_comp: bool = attrs.field( + default=True, + validator=attrs.validators.instance_of(bool), + ) + tick_deadline_us: int = attrs.field( + default=1_000, + validator=attrs.validators.and_( + attrs.validators.instance_of(int), + attrs.validators.ge(1), + ), + ) diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py new file mode 100644 index 0000000000..951f604034 --- /dev/null +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -0,0 +1,755 @@ +# 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 __future__ import annotations + +from collections.abc import Callable, Iterator +from pathlib import Path +from typing import cast +from unittest.mock import Mock + +import can_motor_control +import numpy as np +import pytest +from pytest_mock import MockerFixture + +from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import MotorCommand, MotorState + + +class FakeArm: + def __init__(self, positions: list[float]) -> None: + self.position_values = np.asarray(positions, dtype=np.float64) + self.velocity_values = np.zeros_like(self.position_values) + self.torque_values = np.zeros_like(self.position_values) + self.mode_error: Exception | None = None + self.command_error: Exception | None = None + self.modes: list[str] = [] + self.commands: list[np.ndarray] = [] + + def positions(self) -> np.ndarray: + return self.position_values + + def velocities(self) -> np.ndarray: + return self.velocity_values + + def torques(self) -> np.ndarray: + return self.torque_values + + def set_mode(self, mode: str) -> None: + if self.mode_error is not None: + raise self.mode_error + self.modes.append(mode) + + def mit_control(self, commands: np.ndarray) -> None: + if self.command_error is not None: + raise self.command_error + self.commands.append(commands) + + +class FakeGripper: + def __init__(self, opening: float) -> None: + self.opening = opening + self.command_error: Exception | None = None + self.commands: list[float] = [] + + def set_opening(self, opening: float) -> None: + if self.command_error is not None: + raise self.command_error + self.commands.append(opening) + + +class FakeRobot: + def __init__(self, groups: dict[str, FakeArm | FakeGripper]) -> None: + self.groups = groups + self.connected = False + self.connect_error: Exception | None = None + self.enable_error: Exception | None = None + self.disable_error: Exception | None = None + self.refresh_error: Exception | None = None + self.tick_error: Exception | None = None + self.enable_count = 0 + self.disable_count = 0 + self.refresh_count = 0 + self.tick_count = 0 + + def __getitem__(self, name: str) -> FakeArm | FakeGripper: + return self.groups[name] + + def connect(self) -> None: + if self.connect_error is not None: + raise self.connect_error + self.connected = True + + def enable(self) -> None: + self.enable_count += 1 + if self.enable_error is not None: + raise self.enable_error + + def disable(self) -> None: + self.disable_count += 1 + if self.disable_error is not None: + raise self.disable_error + + def refresh(self) -> None: + self.refresh_count += 1 + if self.refresh_error is not None: + raise self.refresh_error + + def tick(self, _deadline: int) -> None: + self.tick_count += 1 + if self.tick_error is not None: + raise self.tick_error + + def is_connected(self) -> bool: + return self.connected + + def command_count(self) -> int: + arms = sum( + len(group.commands) for group in self.groups.values() if isinstance(group, FakeArm) + ) + grippers = sum( + len(group.commands) for group in self.groups.values() if isinstance(group, FakeGripper) + ) + return arms + grippers + + +class DualAdapter(DamiaoWholeBodyAdapter): + arm_joints = { + "left_arm": ("left_arm/joint1", "left_arm/joint2"), + "right_arm": ("right_arm/joint1", "right_arm/joint2"), + } + gripper_joints = { + "left_gripper": "left_arm/gripper", + "right_gripper": "right_arm/gripper", + } + bus_defaults = {"left": "can0", "right": "can1"} + + def __init__(self, robot: FakeRobot, **kwargs: object) -> None: + self.fake_robot = robot + super().__init__(**kwargs) + + def _build_robot(self) -> can_motor_control.Robot: + return cast("can_motor_control.Robot", self.fake_robot) + + +class GravityDualAdapter(DualAdapter): + gravity_joint_names = ("left1", "left2", "right1", "right2") + + def __init__(self, robot: FakeRobot, model_path: Path, **kwargs: object) -> None: + self.model_path = model_path + super().__init__(robot, **kwargs) + + @property + def gravity_model_path(self) -> Path: + return self.model_path + + +class FakePinModel: + def __init__( + self, + *, + nq: int = 4, + nv: int = 4, + names: tuple[str, ...] = ("universe", "left1", "left2", "right1", "right2"), + ) -> None: + self.nq = nq + self.nv = nv + self.names = names + self.data = object() + + def createData(self) -> object: + return self.data + + +@pytest.fixture +def dual_robot() -> FakeRobot: + return FakeRobot( + { + "left_arm": FakeArm([0.1, 0.2]), + "right_arm": FakeArm([0.3, 0.4]), + "left_gripper": FakeGripper(0.5), + "right_gripper": FakeGripper(0.6), + } + ) + + +@pytest.fixture +def adapter_factory(mocker: MockerFixture) -> Callable[..., DualAdapter]: + mocker.patch.object( + DualAdapter, + "_require_arm", + side_effect=lambda robot, name: robot[name], + ) + mocker.patch.object( + DualAdapter, + "_require_gripper", + side_effect=lambda robot, name: robot[name], + ) + + def create(robot: FakeRobot, **kwargs: object) -> DualAdapter: + kwargs.setdefault("runtime_config", DamiaoRuntimeConfig(gravity_comp=False)) + return DualAdapter(robot, **kwargs) + + return create + + +@pytest.fixture +def connected_dual_adapter( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> Iterator[DualAdapter]: + adapter = adapter_factory(dual_robot, dof=6) + assert adapter.connect() + yield adapter + adapter.disconnect() + + +@pytest.fixture +def active_dual_adapter(connected_dual_adapter: DualAdapter) -> DualAdapter: + assert connected_dual_adapter.activate() + return connected_dual_adapter + + +@pytest.fixture +def pin_model_builder(mocker: MockerFixture) -> Mock: + return mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + ) + + +@pytest.fixture +def gravity_adapter_factory( + adapter_factory: Callable[..., DualAdapter], + dual_robot: FakeRobot, + pin_model_builder: Mock, + tmp_path: Path, +) -> Iterator[Callable[..., GravityDualAdapter]]: + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + adapters: list[GravityDualAdapter] = [] + + def create(*, model: FakePinModel) -> GravityDualAdapter: + pin_model_builder.return_value = model + adapter = GravityDualAdapter( + dual_robot, + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + ) + adapters.append(adapter) + return adapter + + yield create + for adapter in adapters: + adapter.disconnect() + + +def test_init_scalar_address_raises_named_bus_configuration_error( + dual_robot: FakeRobot, +) -> None: + with pytest.raises(ValueError, match="runtime_config.bus_addresses"): + DualAdapter(dual_robot, address="can0") + + +def test_init_unknown_bus_override_raises_value_error(dual_robot: FakeRobot) -> None: + with pytest.raises(ValueError, match="unknown CAN bus"): + DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(bus_addresses={"missing": "can9"}), + ) + + +def test_init_mismatched_dof_raises_value_error(dual_robot: FakeRobot) -> None: + with pytest.raises(ValueError, match="expected 6 joints, got 5"): + DualAdapter(dual_robot, dof=5) + + +def test_init_duplicate_joint_mapping_raises_value_error(dual_robot: FakeRobot) -> None: + class DuplicateJointAdapter(DualAdapter): + arm_joints = {"left_arm": ("shared",), "right_arm": ("shared",)} + gripper_joints = {} + + with pytest.raises(ValueError, match="duplicate names"): + DuplicateJointAdapter(dual_robot) + + +def test_init_incomplete_gravity_mapping_raises_value_error(dual_robot: FakeRobot) -> None: + class IncompleteGravityAdapter(DualAdapter): + gravity_joint_names = ("left1",) + + with pytest.raises(ValueError, match="every angular arm joint"): + IncompleteGravityAdapter(dual_robot) + + +def test_bus_address_runtime_override_returns_configured_interface( + dual_robot: FakeRobot, +) -> None: + adapter = DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig( + bus_addresses={"left": "can8"}, + gravity_comp=False, + ), + ) + + assert adapter.bus_address("left") == "can8" + + +def test_bus_address_without_override_returns_declared_default( + dual_robot: FakeRobot, +) -> None: + adapter = DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) + + assert adapter.bus_address("right") == "can1" + + +def test_bus_address_undeclared_bus_raises_value_error(dual_robot: FakeRobot) -> None: + adapter = DualAdapter(dual_robot) + + with pytest.raises(ValueError, match="did not declare CAN bus 'missing'"): + adapter.bus_address("missing") + + +def test_connect_robot_build_failure_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + mocker: MockerFixture, +) -> None: + adapter = adapter_factory(dual_robot) + mocker.patch.object(adapter, "_build_robot", side_effect=RuntimeError("build failed")) + + assert not adapter.connect() + assert not adapter.is_connected() + + +def test_connect_invalid_upstream_group_rolls_back_robot( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + mocker: MockerFixture, +) -> None: + adapter = adapter_factory(dual_robot) + mocker.patch.object(adapter, "_require_arm", side_effect=TypeError("wrong group")) + + assert not adapter.connect() + assert dual_robot.disable_count == 1 + assert not adapter.is_connected() + + +def test_disconnect_connected_robot_disables_and_clears_state( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + assert not connected_dual_adapter.has_motor_states() + assert connected_dual_adapter.activate() + assert connected_dual_adapter.has_motor_states() + + connected_dual_adapter.disconnect() + + assert dual_robot.disable_count == 1 + assert not connected_dual_adapter.is_connected() + assert not connected_dual_adapter.has_motor_states() + + +def test_disconnect_disable_failure_still_clears_local_state( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + assert adapter.connect() + assert adapter.activate() + dual_robot.disable_error = RuntimeError("disable failed") + + adapter.disconnect() + + assert not adapter.is_connected() + assert not adapter.has_motor_states() + + +def test_activate_disconnected_adapter_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + + assert not adapter.activate() + assert dual_robot.enable_count == 0 + + +def test_activate_enable_failure_disables_robot( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + assert adapter.connect() + dual_robot.enable_error = RuntimeError("calibration failed") + + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_deactivate_connected_adapter_disables_robot( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + assert active_dual_adapter.has_motor_states() + assert active_dual_adapter.deactivate() + assert dual_robot.disable_count == 1 + assert not active_dual_adapter.has_motor_states() + + +def test_deactivate_disconnected_adapter_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + + assert not adapter.deactivate() + assert dual_robot.disable_count == 0 + + +def test_deactivate_disable_failure_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + assert adapter.connect() + assert adapter.activate() + dual_robot.disable_error = RuntimeError("disable failed") + + assert not adapter.deactivate() + assert adapter.has_motor_states() + + +def test_read_motor_states_disconnected_adapter_raises_runtime_error( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + + with pytest.raises(RuntimeError, match="not connected"): + adapter.read_motor_states() + + +def test_joint_names_multiple_groups_returns_declared_order( + active_dual_adapter: DualAdapter, +) -> None: + assert active_dual_adapter.joint_names == ( + "left_arm/joint1", + "left_arm/joint2", + "right_arm/joint1", + "right_arm/joint2", + "left_arm/gripper", + "right_arm/gripper", + ) + + +def test_read_motor_states_multiple_groups_returns_ordered_feedback( + active_dual_adapter: DualAdapter, +) -> None: + assert active_dual_adapter.read_motor_states() == [ + MotorState(q=0.1), + MotorState(q=0.2), + MotorState(q=0.3), + MotorState(q=0.4), + MotorState(q=0.5), + MotorState(q=0.6), + ] + + +def test_read_motor_states_valid_feedback_does_not_tick_bus( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + ticks_before = dual_robot.tick_count + + active_dual_adapter.read_motor_states() + + assert dual_robot.tick_count == ticks_before + + +def test_read_motor_states_wrong_arm_length_raises_runtime_error( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + cast("FakeArm", dual_robot["left_arm"]).velocity_values = np.asarray([0.0]) + + with pytest.raises(RuntimeError, match="wrong state length"): + active_dual_adapter.read_motor_states() + + +def test_read_motor_states_nonfinite_arm_feedback_raises_runtime_error( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + cast("FakeArm", dual_robot["left_arm"]).torque_values[0] = np.nan + + with pytest.raises(RuntimeError, match="non-finite values"): + active_dual_adapter.read_motor_states() + + +def test_read_motor_states_invalid_gripper_opening_raises_runtime_error( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + cast("FakeGripper", dual_robot["left_gripper"]).opening = 1.1 + + with pytest.raises(RuntimeError, match="invalid opening"): + active_dual_adapter.read_motor_states() + + +def test_write_motor_commands_disconnected_adapter_rejects_command( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + + assert not adapter.write_motor_commands([MotorCommand()] * 6) + + +def test_write_motor_commands_inactive_adapter_rejects_command( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + assert adapter.connect() + + assert not adapter.write_motor_commands([MotorCommand()] * 6) + + +def test_write_motor_commands_wrong_command_count_rejects_command( + active_dual_adapter: DualAdapter, +) -> None: + assert not active_dual_adapter.write_motor_commands([MotorCommand()] * 5) + + +def test_write_motor_commands_multiple_arms_routes_ordered_values( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + commands = [ + MotorCommand(q=1.0, kp=10.0), + MotorCommand(q=1.1, kp=11.0), + MotorCommand(q=2.0, kp=20.0), + MotorCommand(q=2.1, kp=21.0), + MotorCommand(q=0.25), + MotorCommand(q=0.75), + ] + + assert active_dual_adapter.write_motor_commands(commands) + + assert cast("FakeArm", dual_robot["left_arm"]).commands[-1][:, 2].tolist() == [1.0, 1.1] + assert cast("FakeArm", dual_robot["right_arm"]).commands[-1][:, 2].tolist() == [2.0, 2.1] + + +def test_write_motor_commands_encodes_complete_mit_command( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + command = MotorCommand(q=1.0, dq=2.0, kp=3.0, kd=4.0, tau=5.0) + commands = [command, *[MotorCommand(q=0.0)] * 3, *[MotorCommand(q=0.5)] * 2] + + assert active_dual_adapter.write_motor_commands(commands) + + assert cast("FakeArm", dual_robot["left_arm"]).commands[-1][0].tolist() == [ + 3.0, + 4.0, + 1.0, + 2.0, + 5.0, + ] + + +def test_write_motor_commands_grippers_routes_normalized_openings( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + commands = [MotorCommand(q=0.0)] * 4 + [MotorCommand(q=0.25), MotorCommand(q=0.75)] + + assert active_dual_adapter.write_motor_commands(commands) + + assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [0.25] + assert cast("FakeGripper", dual_robot["right_gripper"]).commands == [0.75] + + +def test_write_motor_commands_combined_command_ticks_once( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + ticks_before = dual_robot.tick_count + + assert active_dual_adapter.write_motor_commands([MotorCommand(q=0.5)] * 6) + + assert dual_robot.tick_count == ticks_before + 1 + + +def test_write_motor_commands_out_of_range_gripper_rejects_without_writes( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + commands = [MotorCommand(q=0.0)] * 4 + [MotorCommand(q=-0.1), MotorCommand(q=0.5)] + + assert not active_dual_adapter.write_motor_commands(commands) + assert dual_robot.command_count() == 0 + + +def test_write_motor_commands_nonfinite_arm_value_rejects_without_writes( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + commands = [MotorCommand(q=np.nan)] + [MotorCommand(q=0.0)] * 3 + [MotorCommand(q=0.5)] * 2 + + assert not active_dual_adapter.write_motor_commands(commands) + assert dual_robot.command_count() == 0 + + +def test_write_motor_commands_upstream_tick_failure_returns_false( + active_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + dual_robot.tick_error = RuntimeError("bus write failed") + + assert not active_dual_adapter.write_motor_commands([MotorCommand(q=0.5)] * 6) + + +def test_connect_missing_gravity_model_rolls_back_robot( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + tmp_path: Path, +) -> None: + adapter = GravityDualAdapter( + dual_robot, + tmp_path / "missing.urdf", + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + ) + + assert not adapter.connect() + assert dual_robot.disable_count == 1 + + +def test_connect_existing_gravity_model_loads_model( + gravity_adapter_factory: Callable[..., GravityDualAdapter], + pin_model_builder: Mock, +) -> None: + adapter = gravity_adapter_factory(model=FakePinModel()) + + assert adapter.connect() + pin_model_builder.assert_called_once() + + +def test_activate_gravity_model_dimension_mismatch_returns_false( + dual_robot: FakeRobot, + gravity_adapter_factory: Callable[..., GravityDualAdapter], +) -> None: + adapter = gravity_adapter_factory(model=FakePinModel(nq=3)) + assert adapter.connect() + + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_activate_gravity_joint_order_mismatch_returns_false( + dual_robot: FakeRobot, + gravity_adapter_factory: Callable[..., GravityDualAdapter], +) -> None: + adapter = gravity_adapter_factory( + model=FakePinModel(names=("universe", "right1", "left2", "left1", "right2")), + ) + assert adapter.connect() + + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_activate_nonfinite_arm_positions_returns_false( + dual_robot: FakeRobot, + gravity_adapter_factory: Callable[..., GravityDualAdapter], +) -> None: + adapter = gravity_adapter_factory(model=FakePinModel()) + assert adapter.connect() + cast("FakeArm", dual_robot["left_arm"]).position_values[0] = np.nan + + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_activate_nonfinite_gravity_output_returns_false( + dual_robot: FakeRobot, + gravity_adapter_factory: Callable[..., GravityDualAdapter], + mocker: MockerFixture, +) -> None: + adapter = gravity_adapter_factory(model=FakePinModel()) + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.computeGeneralizedGravity", + return_value=np.asarray([1.0, 2.0, np.nan, 4.0]), + ) + assert adapter.connect() + + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_write_motor_commands_gravity_enabled_adds_computed_torque( + dual_robot: FakeRobot, + gravity_adapter_factory: Callable[..., GravityDualAdapter], + mocker: MockerFixture, +) -> None: + adapter = gravity_adapter_factory(model=FakePinModel()) + compute_gravity = mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.computeGeneralizedGravity", + return_value=np.asarray([1.0, 2.0, 3.0, 4.0]), + ) + assert adapter.connect() + assert adapter.activate() + compute_gravity.reset_mock() + + commands = [MotorCommand(q=0.0, tau=0.5)] * 4 + [MotorCommand(q=0.5)] * 2 + assert adapter.write_motor_commands(commands) + + assert compute_gravity.call_count == 1 + assert cast("FakeArm", dual_robot["left_arm"]).commands[-1][:, 4].tolist() == [1.5, 2.5] + assert cast("FakeArm", dual_robot["right_arm"]).commands[-1][:, 4].tolist() == [3.5, 4.5] + + +def test_read_motor_states_inactive_gripper_reports_placeholder( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + """Gripper opening calibrates at activation; before that the read path + must not touch it so read-only bring-up sessions still stream arm state.""" + cast("FakeGripper", dual_robot["left_gripper"]).opening = None + cast("FakeGripper", dual_robot["right_gripper"]).opening = None + + states = connected_dual_adapter.read_motor_states() + + assert states[4:] == [MotorState(q=0.0), MotorState(q=0.0)] + + +def test_read_motor_states_inactive_adapter_pumps_feedback( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + """Without the active write path ticking the bus, the read path must + refresh feedback itself or read-only sessions stream a frozen snapshot.""" + refreshes_before = dual_robot.refresh_count + ticks_before = dual_robot.tick_count + + connected_dual_adapter.read_motor_states() + connected_dual_adapter.read_motor_states() + + assert dual_robot.refresh_count == refreshes_before + 2 + assert dual_robot.tick_count == ticks_before + 2 diff --git a/dimos/hardware/manipulators/openarm/_registry.py b/dimos/hardware/whole_body/mock/_registry.py similarity index 87% rename from dimos/hardware/manipulators/openarm/_registry.py rename to dimos/hardware/whole_body/mock/_registry.py index eed680a4be..fe6e27362b 100644 --- a/dimos/hardware/manipulators/openarm/_registry.py +++ b/dimos/hardware/whole_body/mock/_registry.py @@ -13,5 +13,5 @@ # limitations under the License. ADAPTER_FACTORIES = { - "openarm": "dimos.hardware.manipulators.openarm.adapter:OpenArmAdapter", + "mock_whole_body": "dimos.hardware.whole_body.mock.adapter:MockWholeBodyAdapter", } diff --git a/dimos/hardware/whole_body/mock/adapter.py b/dimos/hardware/whole_body/mock/adapter.py new file mode 100644 index 0000000000..0111a1f2d3 --- /dev/null +++ b/dimos/hardware/whole_body/mock/adapter.py @@ -0,0 +1,69 @@ +# 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. + +"""Generic in-memory whole-body adapter for blueprints and tests.""" + +from __future__ import annotations + +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState + + +class MockWholeBodyAdapter: + """Stateful ordered whole-body IO without robot-specific behavior.""" + + def __init__( + self, + *, + dof: int, + initial_positions: list[float] | None = None, + **_: object, + ) -> None: + positions = initial_positions or [0.0] * dof + if len(positions) != dof: + raise ValueError(f"expected {dof} initial positions, got {len(positions)}") + self._states = [MotorState(q=position) for position in positions] + self._connected = False + + def connect(self) -> bool: + self._connected = True + return True + + def disconnect(self) -> None: + self._connected = False + + def is_connected(self) -> bool: + return self._connected + + def activate(self) -> bool: + return self._connected + + def deactivate(self) -> bool: + return self._connected + + def read_motor_states(self) -> list[MotorState]: + return list(self._states) + + def has_motor_states(self) -> bool: + return self._connected + + def read_imu(self) -> IMUState: + return IMUState() + + def write_motor_commands(self, commands: list[MotorCommand]) -> bool: + if not self._connected or len(commands) != len(self._states): + return False + self._states = [ + MotorState(q=command.q, dq=command.dq, tau=command.tau) for command in commands + ] + return True diff --git a/dimos/hardware/whole_body/mock/test_adapter.py b/dimos/hardware/whole_body/mock/test_adapter.py new file mode 100644 index 0000000000..0e93d84e0e --- /dev/null +++ b/dimos/hardware/whole_body/mock/test_adapter.py @@ -0,0 +1,73 @@ +# 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. + +import pytest + +from dimos.hardware.whole_body.mock.adapter import MockWholeBodyAdapter +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState + + +def test_write_motor_commands_connected_adapter_applies_ordered_commands() -> None: + adapter = MockWholeBodyAdapter(dof=2, initial_positions=[0.1, 0.2]) + assert adapter.connect() + + assert adapter.write_motor_commands( + [ + MotorCommand(q=0.3, dq=0.4, tau=0.5), + MotorCommand(q=0.6, dq=0.7, tau=0.8), + ] + ) + + assert adapter.read_motor_states() == [ + MotorState(q=0.3, dq=0.4, tau=0.5), + MotorState(q=0.6, dq=0.7, tau=0.8), + ] + assert adapter.read_imu() == IMUState() + + +def test_write_motor_commands_wrong_command_count_rejects_without_state_change() -> None: + adapter = MockWholeBodyAdapter(dof=2) + assert adapter.connect() + + assert not adapter.write_motor_commands([MotorCommand(q=0.3)]) + assert adapter.read_motor_states() == [MotorState(), MotorState()] + + +def test_init_mismatched_initial_positions_raises_value_error() -> None: + with pytest.raises(ValueError, match="expected 2 initial positions, got 1"): + MockWholeBodyAdapter(dof=2, initial_positions=[0.1]) + + +def test_write_motor_commands_disconnected_adapter_rejects_command() -> None: + adapter = MockWholeBodyAdapter(dof=1) + + assert not adapter.write_motor_commands([MotorCommand(q=0.3)]) + assert adapter.read_motor_states() == [MotorState()] + + +def test_connection_lifecycle_controls_availability_and_activation() -> None: + adapter = MockWholeBodyAdapter(dof=1) + assert not adapter.activate() + assert not adapter.deactivate() + + assert adapter.connect() + assert adapter.is_connected() + assert adapter.has_motor_states() + assert adapter.activate() + assert adapter.deactivate() + + adapter.disconnect() + + assert not adapter.is_connected() + assert not adapter.has_motor_states() 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..d6029ec09b --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -0,0 +1,90 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm v2.0 bimanual physical topology for the generic Damiao whole-body adapter.""" + +from __future__ import annotations + +from pathlib import Path + +import can_motor_control +from can_motor_control import damiao + +from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter +from dimos.utils.data import LfsPath + + +def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: + return [ + can_motor_control.MotorSpec(f"openarm_{side}_joint1", damiao.MotorType.DM8009, 0x01, 0x11), + can_motor_control.MotorSpec(f"openarm_{side}_joint2", damiao.MotorType.DM8009, 0x02, 0x12), + can_motor_control.MotorSpec(f"openarm_{side}_joint3", damiao.MotorType.DM4340, 0x03, 0x13), + can_motor_control.MotorSpec(f"openarm_{side}_joint4", damiao.MotorType.DM4340, 0x04, 0x14), + can_motor_control.MotorSpec(f"openarm_{side}_joint5", damiao.MotorType.DM4310, 0x05, 0x15), + can_motor_control.MotorSpec(f"openarm_{side}_joint6", damiao.MotorType.DM4310, 0x06, 0x16), + can_motor_control.MotorSpec(f"openarm_{side}_joint7", damiao.MotorType.DM4310, 0x07, 0x17), + ] + + +def _gripper_motor(side: str) -> can_motor_control.MotorSpec: + return can_motor_control.MotorSpec( + f"openarm_{side}_gripper", + damiao.MotorType.DM4310, + 0x08, + 0x18, + ) + + +class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): + """Two OpenArm v2.0 arms with grippers, one CAN bus per arm.""" + + arm_joints = { + "left_arm": tuple(f"left_arm/joint{index}" for index in range(1, 8)), + "right_arm": tuple(f"right_arm/joint{index}" for index in range(1, 8)), + } + # LOCAL EDIT for first-power bring-up: grippers removed from the + # topology so enable never calibrates them. Restore before commit: + # gripper_joints = {"left_gripper": "left_arm/gripper", + # "right_gripper": "right_arm/gripper"} + gripper_joints = {} + # can0/can1 follow USB enumeration order; remap through + # DamiaoRuntimeConfig.bus_addresses if the rig comes up swapped. + bus_defaults = {"left": "can1", "right": "can0"} + gravity_joint_names = ( + *(f"openarm_left_joint{index}" for index in range(1, 8)), + *(f"openarm_right_joint{index}" for index in range(1, 8)), + ) + + @property + def gravity_model_path(self) -> Path: + """Return the lazy bimanual gravity-compensation URDF path.""" + return LfsPath("openarm_description") / "urdf/robot/openarm_v20_bimanual.urdf" + + def _build_robot(self) -> can_motor_control.Robot: + return ( + can_motor_control.Robot.builder() + .add_bus( + "left", + can_motor_control.SocketCanBus(self.bus_address("left")), + damiao.DamiaoCodec(), + ) + .add_bus( + "right", + can_motor_control.SocketCanBus(self.bus_address("right")), + damiao.DamiaoCodec(), + ) + .add_arm("left_arm", bus="left", motors=_arm_motors("left")) + .add_arm("right_arm", bus="right", motors=_arm_motors("right")) + .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/hardware/whole_body/openyam_damiao/_registry.py b/dimos/hardware/whole_body/openyam_damiao/_registry.py new file mode 100644 index 0000000000..f94cf9d533 --- /dev/null +++ b/dimos/hardware/whole_body/openyam_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 = { + "openyam_damiao": ("dimos.hardware.whole_body.openyam_damiao.adapter:OpenYamDamiaoAdapter"), +} diff --git a/dimos/hardware/whole_body/openyam_damiao/adapter.py b/dimos/hardware/whole_body/openyam_damiao/adapter.py new file mode 100644 index 0000000000..9493f506d1 --- /dev/null +++ b/dimos/hardware/whole_body/openyam_damiao/adapter.py @@ -0,0 +1,75 @@ +# 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. + +"""OpenYAM 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 + + +class OpenYamDamiaoAdapter(DamiaoWholeBodyAdapter): + """One OpenYAM arm and calibrated gripper on a shared CAN bus.""" + + bus_name = "openyam" + arm_joints = { + "arm": tuple(f"arm/joint{index}" for index in range(1, 7)), + } + gripper_joints = {"gripper": "arm/gripper"} + bus_defaults = {bus_name: "can0"} + gravity_joint_names = tuple(f"yam_joint{index}" for index in range(1, 7)) + + @property + def gravity_model_path(self) -> Path: + """Return the lazy gravity-compensation URDF path.""" + return LfsPath("yam_description") / "urdf/yam_gripper_gravity.urdf" + + def _build_robot(self) -> can_motor_control.Robot: + arm_motors = [ + can_motor_control.MotorSpec("yam_joint1", damiao.MotorType.DM4340, 0x01, 0x11), + can_motor_control.MotorSpec("yam_joint2", damiao.MotorType.DM4340, 0x02, 0x12), + can_motor_control.MotorSpec("yam_joint3", damiao.MotorType.DM4340, 0x03, 0x13), + can_motor_control.MotorSpec("yam_joint4", damiao.MotorType.DM4310, 0x04, 0x14), + can_motor_control.MotorSpec("yam_joint5", damiao.MotorType.DM4310, 0x05, 0x15), + can_motor_control.MotorSpec("yam_joint6", damiao.MotorType.DM4310, 0x06, 0x16), + ] + gripper_motor = can_motor_control.MotorSpec( + "yam_gripper", + damiao.MotorType.DM4310, + 0x08, + 0x18, + ) + return ( + can_motor_control.Robot.builder() + .add_bus( + self.bus_name, + can_motor_control.SocketCanBus(self.bus_address(self.bus_name)), + damiao.DamiaoCodec(), + ) + .add_arm("arm", bus=self.bus_name, motors=arm_motors) + .add_gripper( + "gripper", + bus=self.bus_name, + motor=gripper_motor, + opening_direction="decreasing_position", + default_current=0.15, + ) + .build() + ) diff --git a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py new file mode 100644 index 0000000000..386fe27af6 --- /dev/null +++ b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py @@ -0,0 +1,55 @@ +# 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.openyam_damiao import adapter as adapter_module +from dimos.hardware.whole_body.openyam_damiao.adapter import OpenYamDamiaoAdapter +from dimos.robot.manipulators.openyam.config import OPENYAM_DOF + + +@pytest.fixture +def openyam_adapter(mocker: MockerFixture) -> Iterator[OpenYamDamiaoAdapter]: + mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) + adapter = OpenYamDamiaoAdapter( + 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_openyam_topology_connects_arm_and_gripper( + openyam_adapter: OpenYamDamiaoAdapter, +) -> None: + robot = openyam_adapter._build_robot() + + assert robot.group_names() == ["arm", "gripper"] + assert isinstance(robot["arm"], can_motor_control.Arm) + assert len(robot["arm"]) == OPENYAM_DOF + assert isinstance(robot["gripper"], can_motor_control.Gripper) + assert openyam_adapter.connect() diff --git a/dimos/hardware/whole_body/spec.py b/dimos/hardware/whole_body/spec.py index f725d51403..9fe808f8cf 100644 --- a/dimos/hardware/whole_body/spec.py +++ b/dimos/hardware/whole_body/spec.py @@ -26,10 +26,14 @@ @dataclass(frozen=True) class MotorCommand: - """Command for a single motor.""" + """Command for one joint in that joint's declared coordinate system. - q: float = POS_STOP # target position (rad) - dq: float = VEL_STOP # target velocity (rad/s) + Angular joints use radians/radians per second/Nm. Other joints may define + another coordinate; for example, a gripper may use normalized opening. + """ + + q: float = POS_STOP # target position in the joint's coordinate + dq: float = VEL_STOP # target velocity in the joint's coordinate per second kp: float = 0.0 # position gain kd: float = 0.0 # velocity gain tau: float = 0.0 # feedforward torque (Nm) @@ -37,10 +41,10 @@ class MotorCommand: @dataclass(frozen=True) class MotorState: - """Feedback from a single motor.""" + """Feedback for one joint in that joint's declared coordinate system.""" - q: float = 0.0 # position (rad) - dq: float = 0.0 # velocity (rad/s) + q: float = 0.0 # position in the joint's coordinate + dq: float = 0.0 # velocity in the joint's coordinate per second tau: float = 0.0 # estimated torque (Nm) @@ -75,7 +79,7 @@ class WholeBodyConfig: @runtime_checkable class WholeBodyAdapter(Protocol): - """Joint-level whole-body motor IO. SI units (rad, rad/s, Nm).""" + """Joint-level whole-body IO using each joint's declared coordinate.""" def connect(self) -> bool: ... def disconnect(self) -> None: ... diff --git a/dimos/manipulation/planning/utils/mesh_utils.py b/dimos/manipulation/planning/utils/mesh_utils.py index 33fce6d6c8..14c430780c 100644 --- a/dimos/manipulation/planning/utils/mesh_utils.py +++ b/dimos/manipulation/planning/utils/mesh_utils.py @@ -231,9 +231,12 @@ def convert_mesh(match: re.Match[str]) -> str: # Load mesh mesh = trimesh.load(original_path, force="mesh") - # Generate output path + # Generate output path. Include a source-path hash: different + # meshes may share a stem (visual/link3.dae vs collision/link3.stl) + # and stem-only names would overwrite each other. mesh_name = Path(original_path).stem - obj_path = mesh_dir / f"{mesh_name}.obj" + path_tag = hashlib.md5(original_path.encode()).hexdigest()[:8] + obj_path = mesh_dir / f"{mesh_name}_{path_tag}.obj" # Export as OBJ (trimesh.export returns None, ignore) mesh.export(str(obj_path), file_type="obj") # type: ignore[no-untyped-call] diff --git a/dimos/manipulation/planning/utils/test_mesh_utils.py b/dimos/manipulation/planning/utils/test_mesh_utils.py new file mode 100644 index 0000000000..f376f854d2 --- /dev/null +++ b/dimos/manipulation/planning/utils/test_mesh_utils.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import re + +import trimesh + +from dimos.manipulation.planning.utils.mesh_utils import _convert_meshes + + +def _write_box_mesh(path: Path, extents: tuple[float, float, float]) -> None: + trimesh.creation.box(extents=extents).export(str(path)) + + +def test_convert_meshes_same_stem_different_dirs_stay_distinct(tmp_path: Path) -> None: + """Visual and collision meshes often share a file stem; converted OBJs + must not overwrite each other.""" + visual_dir = tmp_path / "visual" + collision_dir = tmp_path / "collision" + visual_dir.mkdir() + collision_dir.mkdir() + _write_box_mesh(visual_dir / "link3.stl", (1.0, 1.0, 1.0)) + _write_box_mesh(collision_dir / "link3.stl", (2.0, 2.0, 2.0)) + + urdf = ( + f'' + f'' + ) + converted = _convert_meshes(urdf, tmp_path) + + obj_paths = [Path(p) for p in re.findall(r'filename="([^"]+\.obj)"', converted)] + assert len(obj_paths) == 2 + assert obj_paths[0] != obj_paths[1] + sizes = sorted(trimesh.load(str(p), force="mesh").extents[0] for p in obj_paths) + assert sizes[0] == 1.0 + assert sizes[1] == 2.0 + + +def test_convert_meshes_same_file_referenced_twice_converts_once(tmp_path: Path) -> None: + mesh = tmp_path / "part.stl" + _write_box_mesh(mesh, (1.0, 1.0, 1.0)) + + urdf = f'' + converted = _convert_meshes(urdf, tmp_path) + + obj_paths = set(re.findall(r'filename="([^"]+\.obj)"', converted)) + assert len(obj_paths) == 1 diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index 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( 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] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ecf75ac543..12f947d0a4 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,8 +67,9 @@ "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", "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm6", "keyboard-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm7", @@ -86,8 +84,10 @@ "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", + "mini-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints.mini_teleop:mini_teleop_openarm", + "mini-teleop-openarm-left": "dimos.robot.manipulators.openarm.blueprints.mini_teleop:mini_teleop_openarm_left", + "mini-teleop-openarm-right": "dimos.robot.manipulators.openarm.blueprints.mini_teleop:mini_teleop_openarm_right", + "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", @@ -244,6 +244,7 @@ "object-tracker2-d": "dimos.perception.experimental.object_tracker_2d.ObjectTracker2D", "object-tracker3-d": "dimos.perception.experimental.object_tracker_3d.ObjectTracker3D", "object-tracking": "dimos.perception.experimental.object_tracker.ObjectTracking", + "open-arm-mini-teleop-module": "dimos.teleop.openarm_mini.teleop_module.OpenArmMiniTeleopModule", "osm-skill": "dimos.agents.skills.osm.OsmSkill", "path-follower": "dimos.navigation.cmu_nav.modules.path_follower.path_follower.PathFollower", "path-following-coordinator": "dimos.control.path_following_coordinator.PathFollowingCoordinator", diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index 012d3ceb97..36afa57c52 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -16,53 +16,40 @@ from __future__ import annotations -from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.robot.manipulators.common.blueprints import trajectory_task +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( - LEFT_CAN, - OPENARM_ADAPTER_KWARGS, - RIGHT_CAN, + OPENARM_ARM_JOINTS, + openarm_bimanual_model_config, openarm_hardware, ) -def openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: - return trajectory_task(hw, name=name) +def _trajectory_task() -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(OPENARM_ARM_JOINTS), + priority=10, + params={"start_position_tolerance": 0.05}, + ) -mock_left = openarm_hardware(side="left") -mock_right = openarm_hardware(side="right") +_openarm_planner_hw = openarm_hardware() -coordinator_openarm_mock = ControlCoordinator.blueprint( - hardware=[mock_left, mock_right], - tasks=[trajectory_task(mock_left, mock_right)], +openarm_planner_coordinator = autoconnect( + planner(robots=[openarm_bimanual_model_config()]), + coordinator( + hardware=[_openarm_planner_hw], + tasks=[_trajectory_task()], + ), ) -left_hw = openarm_hardware( - side="left", - address=LEFT_CAN, - adapter_type="openarm", - adapter_kwargs=OPENARM_ADAPTER_KWARGS, -) -right_hw = openarm_hardware( - side="right", - address=RIGHT_CAN, - adapter_type="openarm", - adapter_kwargs=OPENARM_ADAPTER_KWARGS, -) - -coordinator_openarm_left = ControlCoordinator.blueprint( - hardware=[left_hw], - tasks=[openarm_task(left_hw)], -) - -coordinator_openarm_right = ControlCoordinator.blueprint( - hardware=[right_hw], - tasks=[openarm_task(right_hw)], -) +_openarm_hw = openarm_hardware() -coordinator_openarm_bimanual = ControlCoordinator.blueprint( - hardware=[left_hw, right_hw], - tasks=[trajectory_task(left_hw, right_hw)], +coordinator_openarm = ControlCoordinator.blueprint( + hardware=[_openarm_hw], + tasks=[_trajectory_task()], ) diff --git a/dimos/robot/manipulators/openarm/blueprints/mini_teleop.py b/dimos/robot/manipulators/openarm/blueprints/mini_teleop.py new file mode 100644 index 0000000000..a7cfb8d37f --- /dev/null +++ b/dimos/robot/manipulators/openarm/blueprints/mini_teleop.py @@ -0,0 +1,74 @@ +# 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 Mini leader teleop blueprints for the bimanual OpenArm follower.""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.manipulators.openarm.config import ( + openarm_arm_joints, + openarm_bimanual_model_config, + openarm_hardware, +) +from dimos.teleop.openarm_mini.calibration import OpenArmMiniSide +from dimos.teleop.openarm_mini.teleop_module import OpenArmMiniTeleopModule + + +def _servo_task(side: OpenArmMiniSide) -> TaskConfig: + return TaskConfig( + name=f"servo_{side}_arm", + type="servo", + joint_names=openarm_arm_joints(side), + priority=10, + ) + + +mini_teleop_openarm = autoconnect( + OpenArmMiniTeleopModule.blueprint(enabled_sides=("left", "right")), + ControlCoordinator.blueprint( + hardware=[openarm_hardware()], + tasks=[_servo_task("left"), _servo_task("right")], + ), + ManipulationModule.blueprint( + robots=[openarm_bimanual_model_config()], + visualization={"backend": "viser"}, + ), +) + +mini_teleop_openarm_left = autoconnect( + OpenArmMiniTeleopModule.blueprint(enabled_sides=("left",)), + ControlCoordinator.blueprint( + hardware=[openarm_hardware()], + tasks=[_servo_task("left")], + ), + ManipulationModule.blueprint( + robots=[openarm_bimanual_model_config()], + visualization={"backend": "viser"}, + ), +) + +mini_teleop_openarm_right = autoconnect( + OpenArmMiniTeleopModule.blueprint(enabled_sides=("right",)), + ControlCoordinator.blueprint( + hardware=[openarm_hardware()], + tasks=[_servo_task("right")], + ), + ManipulationModule.blueprint( + robots=[openarm_bimanual_model_config()], + visualization={"backend": "viser"}, + ), +) 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..435f61a457 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -16,48 +16,78 @@ 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_ARM_JOINTS, + OPENARM_DOF, + OPENARM_LEFT_MODEL, + OPENARM_RIGHT_MODEL, + openarm_arm_joints, + openarm_bimanual_model_config, + openarm_hardware, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_teleop_hw = openarm_single_hardware() +# 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" -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=list(OPENARM_ARM_JOINTS), + priority=priority, + params={"start_position_tolerance": 0.05}, + ) + + +keyboard_teleop_openarm = autoconnect( + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), ControlCoordinator.blueprint( - hardware=[_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"), + ], ), ManipulationModule.blueprint( - robots=[openarm_single_model_config()], - visualization={"backend": "meshcat"}, + robots=[openarm_bimanual_model_config()], + 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), + planner(robots=[openarm_bimanual_model_config()]), + 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), + _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..9cbfc9fa6d 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -12,132 +12,124 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""OpenArm hardware and planning model configuration helpers.""" +"""OpenArm hardware and planning model configuration.""" from __future__ import annotations from pathlib import Path -from typing import Any from dimos.control.components import HardwareComponent, HardwareType +from dimos.core.global_config import global_config +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import base_pose from dimos.utils.data import LfsPath -OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("openarm_left_link5", "openarm_left_link7"), - ("openarm_right_link5", "openarm_right_link7"), -] +OPENARM_DOF = 7 +OPENARM_HARDWARE_ID = "openarm" +OPENARM_SIDES = ("left", "right") +# Order must match OpenArmDamiaoAdapter.joint_names: all arm groups in +# declaration order (left then right), then all grippers. +OPENARM_LEFT_ARM_JOINTS = [f"left_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +OPENARM_RIGHT_ARM_JOINTS = [f"right_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +OPENARM_ARM_JOINTS = [*OPENARM_LEFT_ARM_JOINTS, *OPENARM_RIGHT_ARM_JOINTS] +OPENARM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] +# LOCAL EDIT for first-power bring-up: grippers out of the loop entirely +# (no enable, no calibration sweep). Restore before commit: +# OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] +OPENARM_JOINTS = [*OPENARM_ARM_JOINTS] OPENARM_PKG = LfsPath("openarm_description") -OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" -OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" -OPENARM_V10_FK_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" +OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_left.urdf" +OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_right.urdf" +OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_bimanual.urdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# Linux assigns can0/can1 in USB enumeration order, which is not guaranteed stable. -# Flip these if physical arms come up swapped. -LEFT_CAN = "can1" -RIGHT_CAN = "can0" - -# Leave true for normal operation; it is idempotent and ensures motors are in -# the expected CTRL_MODE=MIT mode at connect time. -AUTO_SET_MIT_MODE = True -OPENARM_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} +# MIT gains measured on v1.0 hardware, carried over as the v2.0 starting +# point: with gravity compensation active the PD terms only handle transient +# tracking, and high kd excites gearbox buzz. Gripper slots bypass MIT +# control, so their gains are 0. +# LOCAL EDIT for first-power bring-up: soft gains, restore before commit. +# Validated v1.0 values: kp (100,100,80,80,60,60,60), kd (1.5,1.5,1,1,.8,.8,.8) +_ARM_KP = (25.0, 25.0, 20.0, 20.0, 10.0, 10.0, 10.0) +_ARM_KD = (1.0, 1.0, 0.8, 0.8, 0.5, 0.5, 0.5) def validate_side(side: str) -> None: - if side not in ("left", "right"): + if side not in OPENARM_SIDES: raise ValueError(f"side must be 'left' or 'right', got {side!r}") -def openarm_joints(side: str) -> list[str]: +def openarm_arm_joints(side: str) -> list[str]: validate_side(side) - return [f"openarm_{side}_joint{i}" for i in range(1, 8)] + return [f"{side}_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] -def openarm_hardware( - side: str, - name: str | None = None, - *, - adapter_type: str = "mock", - address: str | None = None, - adapter_kwargs: dict[str, Any] | None = None, -) -> HardwareComponent: +def openarm_urdf_joints(side: str) -> list[str]: validate_side(side) - kwargs = {"side": side} - if adapter_kwargs: - kwargs.update(adapter_kwargs) + return [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] + + +def openarm_hardware() -> HardwareComponent: + """Select the physical or in-memory whole-body adapter for OpenArm.""" + adapter_type = "mock_whole_body" if global_config.simulation else "openarm_damiao" + adapter_kwargs: dict[str, object] = {} + if not global_config.simulation: + adapter_kwargs["runtime_config"] = DamiaoRuntimeConfig(gravity_comp=True) return HardwareComponent( - hardware_id=name or f"{side}_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=openarm_joints(side), + hardware_id=OPENARM_HARDWARE_ID, + hardware_type=HardwareType.WHOLE_BODY, + joints=list(OPENARM_JOINTS), adapter_type=adapter_type, - address=address, - adapter_kwargs=kwargs, + auto_enable=True, + adapter_kwargs=adapter_kwargs, + # LOCAL EDIT: 14 wide while grippers are out; restore the two + # trailing 0.0 gripper entries together with OPENARM_JOINTS. + wb_config=WholeBodyConfig( + kp=(*_ARM_KP, *_ARM_KP), + kd=(*_ARM_KD, *_ARM_KD), + ), ) -def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: - validate_side(side) - resolved_name = name or f"{side}_arm" - local_joint_names = openarm_joints(side) +def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModelConfig: + """Build the single fourteen-joint planning model with one group per arm. + + Collision exclusions cannot span robots, so both arms plan as one robot. + """ + local_joint_names = [*openarm_urdf_joints("left"), *openarm_urdf_joints("right")] return RobotModelConfig( - name=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", - ) - ], - package_paths=OPENARM_PACKAGE_PATHS, - collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - home_joints=[0.0] * 7, - ) - - -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=[ + tip_link="openarm_left_grasp_frame", + ), PlanningGroupDefinition( - name="manipulator", - joint_names=tuple(local_joint_names), + name="right_manipulator", + joint_names=tuple(openarm_urdf_joints("right")), base_link="openarm_body_link0", - tip_link="openarm_left_link7", - ) + tip_link="openarm_right_grasp_frame", + ), ], package_paths=OPENARM_PACKAGE_PATHS, auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, - home_joints=[0.0] * 7, + joint_name_mapping={ + coordinator_name: urdf_name + for side in OPENARM_SIDES + for coordinator_name, urdf_name in zip( + openarm_arm_joints(side), openarm_urdf_joints(side), strict=True + ) + }, + home_joints=[0.0] * (2 * OPENARM_DOF), ) diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py b/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py deleted file mode 100755 index 9c740ef485..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Probe an OpenArm on a SocketCAN interface. - -Enumerates all 8 expected Damiao motors (7 arm joints + gripper) on one CAN bus -(classical by default, use --fd for CAN-FD), enables each, reads back one state -frame, then disables. Phase-0 hardware-verification script. - -Run AFTER bringing the bus up with dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh. - -Usage: - python dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can0 - python dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can1 --ids 1,2,3,4,5,6,7 -""" - -from __future__ import annotations - -import argparse -import sys -import time - -try: - import can -except ImportError: - sys.exit("python-can not installed. Run: pip install 'python-can>=4.3'") - -# ---- Damiao motor limit tables (from enactic/openarm_can dm_motor_constants.hpp) -# [p_max rad, v_max rad/s, t_max Nm] -LIMITS: dict[str, tuple[float, float, float]] = { - "DM4310": (12.5, 30.0, 10.0), - "DM4340": (12.5, 8.0, 28.0), - "DM8006": (12.5, 45.0, 40.0), -} - -# OpenArm v10 per-joint motor assignment (derived from joint_limits.yaml effort column) -DEFAULT_MOTORS: list[tuple[int, str]] = [ - (0x01, "DM8006"), # joint1 - (0x02, "DM8006"), # joint2 - (0x03, "DM4340"), # joint3 - (0x04, "DM4340"), # joint4 - (0x05, "DM4310"), # joint5 - (0x06, "DM4310"), # joint6 - (0x07, "DM4310"), # joint7 - (0x08, "DM4310"), # gripper -] - -ENABLE = bytes([0xFF] * 7 + [0xFC]) -DISABLE = bytes([0xFF] * 7 + [0xFD]) - -FD = False # set by --fd at runtime; defaults to classical CAN @ 1 Mbit - - -def uint_to_float(x: int, lo: float, hi: float, bits: int) -> float: - return x / ((1 << bits) - 1) * (hi - lo) + lo - - -def parse_state(motor_type: str, data: bytes) -> tuple[float, float, float, int, int] | None: - """Decode an 8-byte DM motor state reply. Returns (q, dq, tau, t_mos, t_rotor).""" - if len(data) < 8: - return None - p_max, v_max, t_max = LIMITS[motor_type] - q_u = (data[1] << 8) | data[2] - dq_u = (data[3] << 4) | (data[4] >> 4) - tau_u = ((data[4] & 0x0F) << 8) | data[5] - q = uint_to_float(q_u, -p_max, p_max, 16) - dq = uint_to_float(dq_u, -v_max, v_max, 12) - tau = uint_to_float(tau_u, -t_max, t_max, 12) - return q, dq, tau, data[6], data[7] - - -def probe_motor( - bus: can.BusABC, send_id: int, recv_id: int, motor_type: str, timeout: float = 0.2 -) -> bool: - """Enable motor, wait for state reply on recv_id, print result, disable.""" - # Flush any stale frames - while bus.recv(0.0) is not None: - pass - - bus.send( - can.Message( - arbitration_id=send_id, data=ENABLE, is_extended_id=False, is_fd=FD, bitrate_switch=FD - ) - ) - t0 = time.monotonic() - while time.monotonic() - t0 < timeout: - msg = bus.recv(timeout - (time.monotonic() - t0)) - if msg is None: - break - if msg.arbitration_id != recv_id: - continue - parsed = parse_state(motor_type, bytes(msg.data)) - if parsed is None: - print(f" 0x{send_id:02X} ({motor_type}): short reply {list(msg.data)}") - bus.send( - can.Message( - arbitration_id=send_id, - data=DISABLE, - is_extended_id=False, - is_fd=FD, - bitrate_switch=FD, - ) - ) - return False - q, dq, tau, t_mos, t_rot = parsed - print( - f" 0x{send_id:02X} ({motor_type:>6}): " - f"q={q:+.3f} rad dq={dq:+.3f} rad/s tau={tau:+.3f} Nm " - f"T_mos={t_mos}C T_rotor={t_rot}C" - ) - bus.send( - can.Message( - arbitration_id=send_id, - data=DISABLE, - is_extended_id=False, - is_fd=FD, - bitrate_switch=FD, - ) - ) - return True - - print( - f" 0x{send_id:02X} ({motor_type:>6}): NO REPLY on 0x{recv_id:02X} within {timeout * 1e3:.0f}ms" - ) - bus.send( - can.Message( - arbitration_id=send_id, data=DISABLE, is_extended_id=False, is_fd=FD, bitrate_switch=FD - ) - ) - return False - - -def main() -> int: - ap = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - ap.add_argument("--channel", default="can0", help="SocketCAN interface (default: can0)") - ap.add_argument( - "--fd", - action="store_true", - help="Use CAN-FD (requires FD-capable adapter). Default is classical CAN @ 1 Mbit, which is what most gs_usb adapters support.", - ) - ap.add_argument("--ids", default=None, help="Comma-separated send IDs to probe (default: 1..8)") - ap.add_argument("--timeout", type=float, default=0.2, help="Reply timeout per motor (s)") - args = ap.parse_args() - - global FD - FD = args.fd - motors = DEFAULT_MOTORS - if args.ids: - wanted = {int(x, 0) for x in args.ids.split(",")} - motors = [m for m in DEFAULT_MOTORS if m[0] in wanted] - - # Preflight: is the interface up? - try: - flags = int(open(f"/sys/class/net/{args.channel}/flags").read().strip(), 16) - iface_up = bool(flags & 0x1) - except OSError: - print(f"ERROR: interface '{args.channel}' not found", file=sys.stderr) - return 1 - if not iface_up: - print(f"ERROR: SocketCAN interface '{args.channel}' is DOWN.", file=sys.stderr) - print( - f" Run: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {args.channel}", - file=sys.stderr, - ) - return 1 - - print(f"Opening {args.channel} ({'CAN-FD' if FD else 'classical CAN'})...") - try: - bus = can.Bus(interface="socketcan", channel=args.channel, fd=FD) - except Exception as e: - print(f"ERROR opening {args.channel}: {e}", file=sys.stderr) - print( - " Did you run 'sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh' first?", - file=sys.stderr, - ) - return 1 - - try: - print(f"Probing {len(motors)} motor(s) on {args.channel}:") - ok = 0 - for send_id, motor_type in motors: - recv_id = send_id | 0x10 - if probe_motor(bus, send_id, recv_id, motor_type, args.timeout): - ok += 1 - print(f"\n{ok}/{len(motors)} motors replied.") - return 0 if ok == len(motors) else 2 - finally: - bus.shutdown() - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh b/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh deleted file mode 100755 index d25fc41e43..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env bash -# Bring up CAN interfaces for OpenArm. Default is classical CAN @ 1 Mbit, -# which is what most gs_usb (OpenMoko / Geschwister Schneider) USB-CAN -# adapters support. Use MODE=fd if you have a CAN-FD-capable adapter. -# Run with sudo or as root. -# -# Usage: -# sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh # classical 1M, can0 and can1 -# sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 # single interface -# sudo MODE=fd ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 # CAN-FD 1M/5M -set -euo pipefail - -BITRATE=1000000 -DBITRATE=5000000 -MODE="${MODE:-classical}" # classical | fd -IFACES_ARG="${*:-can0 can1}" -# shellcheck disable=SC2206 -IFACES=(${IFACES_ARG[@]}) - -for IF in "${IFACES[@]}"; do - if ! ip link show "$IF" >/dev/null 2>&1; then - echo "[skip] $IF not present" - continue - fi - ip link set "$IF" down || true - if [ "$MODE" = "classical" ]; then - echo "[up ] $IF ${BITRATE} (classical CAN)" - ip link set "$IF" type can bitrate "$BITRATE" - else - echo "[up ] $IF ${BITRATE}/${DBITRATE} fd on" - ip link set "$IF" type can bitrate "$BITRATE" dbitrate "$DBITRATE" fd on - fi - ip link set "$IF" up - ip link set "$IF" txqueuelen 1000 - ip -details link show "$IF" | grep -E "can |bitrate" || true -done diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py b/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py deleted file mode 100755 index 04bf3912a3..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Write CTRL_MODE = MIT (1) to one or all OpenArm motors. - -Damiao motors have a persistent CTRL_MODE register (RID=10). If a motor was -previously configured in POS_VEL (2) / VEL (3) / POS_FORCE (4) mode, it will -respond to enable/disable but IGNORE MIT control frames — exactly the -"motor doesn't move, error grows" symptom. - -This script writes CTRL_MODE=1 (MIT) via the 0x7FF broadcast-write frame -format used by enactic/openarm_can: - - ID=0x7FF data = [id_lo, id_hi, 0x55, RID=10, val[0], val[1], val[2], val[3]] - -Run once per motor after CAN bring-up. The value is persistent across power -cycles. - -Usage: - # All 8 motors on can0 (classical CAN @ 1 Mbit, default) - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 - - # Single motor - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 --id 0x05 - - # CAN-FD (only if your adapter supports it) - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 --fd -""" - -from __future__ import annotations - -import argparse -import struct -import sys -import time - -try: - import can -except ImportError: - sys.exit("python-can not installed") - -RID_CTRL_MODE = 10 -MIT_MODE = 1 -DEFAULT_IDS = [0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08] - - -def write_ctrl_mode(bus: can.BusABC, send_id: int, fd: bool) -> bool: - val = struct.pack("> 8) & 0xFF, 0x55, RID_CTRL_MODE, val[0], val[1], val[2], val[3]] - ) - # Flush - while bus.recv(0.0) is not None: - pass - bus.send( - can.Message( - arbitration_id=0x7FF, data=data, is_extended_id=False, is_fd=fd, bitrate_switch=fd - ) - ) - # Wait for ack on 0x7FF (per openarm_can param response) - t0 = time.monotonic() - while time.monotonic() - t0 < 0.2: - msg = bus.recv(0.2 - (time.monotonic() - t0)) - if msg is None: - break - # Reply on 0x7FF: [id_lo, id_hi, 0x33|0x55, rid, value[0..3]] - if msg.arbitration_id != 0x7FF or len(msg.data) < 8: - continue - if msg.data[2] not in (0x33, 0x55): - continue - if msg.data[0] != (send_id & 0xFF) or msg.data[1] != ((send_id >> 8) & 0xFF): - continue # ack from a different motor - rid = msg.data[3] - if rid == RID_CTRL_MODE: - echoed = int(struct.unpack(" int: - ap = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - ap.add_argument("--channel", default="can0") - ap.add_argument("--fd", action="store_true", help="Use CAN-FD (default: classical CAN)") - ap.add_argument( - "--id", type=lambda s: int(s, 0), default=None, help="Single send ID (default: all 8)" - ) - args = ap.parse_args() - - fd = args.fd - ids = [args.id] if args.id is not None else DEFAULT_IDS - - # Preflight: is the interface up? - try: - flags = int(open(f"/sys/class/net/{args.channel}/flags").read().strip(), 16) - except OSError: - print(f"ERROR: interface '{args.channel}' not found", file=sys.stderr) - return 1 - if not (flags & 0x1): - print(f"ERROR: SocketCAN interface '{args.channel}' is DOWN.", file=sys.stderr) - print( - f" Run: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {args.channel}", - file=sys.stderr, - ) - return 1 - - print(f"Opening {args.channel} ({'CAN-FD' if fd else 'classical'})") - bus = can.Bus(interface="socketcan", channel=args.channel, fd=fd) - try: - ok = 0 - for i in ids: - if write_ctrl_mode(bus, i, fd): - ok += 1 - time.sleep(0.05) - print(f"\n{ok}/{len(ids)} motors set to MIT mode.") - return 0 if ok == len(ids) else 2 - finally: - bus.shutdown() - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/dimos/robot/manipulators/openyam/blueprints/basic.py b/dimos/robot/manipulators/openyam/blueprints/basic.py index cd9fbbcbb5..709ecae571 100644 --- a/dimos/robot/manipulators/openyam/blueprints/basic.py +++ b/dimos/robot/manipulators/openyam/blueprints/basic.py @@ -16,27 +16,40 @@ 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.robot.manipulators.common.blueprints import coordinator, planner, trajectory_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.openyam.config import ( - make_openyam_hardware, + OPENYAM_ARM_JOINTS, make_openyam_model_config, + openyam_hardware, ) -_openyam_planner_hw = make_openyam_hardware("arm") + +def _trajectory_task() -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(OPENYAM_ARM_JOINTS), + priority=10, + params={"start_position_tolerance": 0.05}, + ) + + +_openyam_planner_hw = openyam_hardware() openyam_planner_coordinator = autoconnect( planner(robots=[make_openyam_model_config(name="arm")]), coordinator( hardware=[_openyam_planner_hw], - tasks=[trajectory_task(_openyam_planner_hw)], + tasks=[_trajectory_task()], ), ) -_openyam_hw = make_openyam_hardware("arm") +_openyam_hw = openyam_hardware() coordinator_openyam = ControlCoordinator.blueprint( hardware=[_openyam_hw], - tasks=[trajectory_task(_openyam_hw)], + tasks=[_trajectory_task()], ) diff --git a/dimos/robot/manipulators/openyam/blueprints/teleop.py b/dimos/robot/manipulators/openyam/blueprints/teleop.py index 5c0fb521e1..1ef8937228 100644 --- a/dimos/robot/manipulators/openyam/blueprints/teleop.py +++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py @@ -16,30 +16,70 @@ 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, + EEF_TWIST_TASK_NAME, +) from dimos.robot.manipulators.openyam.config import ( + OPENYAM_ARM_JOINTS, OPENYAM_DOF, - OPENYAM_MODEL_PATH, - make_openyam_hardware, + OPENYAM_GRAVITY_MODEL_PATH, + OPENYAM_GRIPPER_JOINT, make_openyam_model_config, + openyam_hardware, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_openyam_keyboard_hw = make_openyam_hardware("arm") +_openyam_keyboard_hw = openyam_hardware() + + +def _eef_twist_task(*, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=EEF_TWIST_TASK_NAME, + type="eef_twist", + joint_names=list(OPENYAM_ARM_JOINTS), + priority=priority, + params={ + "model_path": OPENYAM_GRAVITY_MODEL_PATH, + "ee_joint_id": OPENYAM_DOF, + }, + ) + + +def _trajectory_task(*, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(OPENYAM_ARM_JOINTS), + priority=priority, + params={"start_position_tolerance": 0.05}, + ) + + +def _gripper_task() -> TaskConfig: + return TaskConfig( + name="servo_gripper", + type="servo", + joint_names=[OPENYAM_GRIPPER_JOINT], + priority=20, + params={"timeout": 0.0}, + ) + keyboard_teleop_openyam = autoconnect( KeyboardTeleopModule.blueprint(), ControlCoordinator.blueprint( hardware=[_openyam_keyboard_hw], tasks=[ - eef_twist_task( - _openyam_keyboard_hw, - model_path=OPENYAM_MODEL_PATH, - ee_joint_id=OPENYAM_DOF, - ) + _eef_twist_task(), + _gripper_task(), ], ), ManipulationModule.blueprint( @@ -47,3 +87,18 @@ visualization={"backend": "viser"}, ), ) + +_openyam_keyboard_planner_hw = openyam_hardware() + +keyboard_teleop_openyam_planner = autoconnect( + KeyboardTeleopModule.blueprint(), + planner(robots=[make_openyam_model_config(name="arm")]), + coordinator( + hardware=[_openyam_keyboard_planner_hw], + tasks=[ + _eef_twist_task(priority=10), + _gripper_task(), + _trajectory_task(priority=20), + ], + ), +) diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index a2972eacdb..f303e43586 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -12,13 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""OpenYAM hardware and planning model configuration helpers.""" +"""OpenYAM hardware and planning model configuration.""" from __future__ import annotations from pathlib import Path -from dimos.control.components import HardwareComponent, HardwareType, make_joints +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 ( @@ -29,62 +32,59 @@ from dimos.utils.data import LfsPath OPENYAM_DOF = 6 +OPENYAM_HARDWARE_ID = "openyam" +OPENYAM_ARM_JOINTS = [f"arm/joint{index}" for index in range(1, OPENYAM_DOF + 1)] +OPENYAM_GRIPPER_JOINT = "arm/gripper" +OPENYAM_JOINTS = [*OPENYAM_ARM_JOINTS, OPENYAM_GRIPPER_JOINT] OPENYAM_PACKAGE = LfsPath("yam_description") -OPENYAM_MODEL_PATH = OPENYAM_PACKAGE / "urdf/yam_gripper.urdf.xacro" +OPENYAM_MODEL_PATH = OPENYAM_PACKAGE / "i2rt/yam.urdf" +OPENYAM_GRAVITY_MODEL_PATH = OPENYAM_PACKAGE / "urdf/yam_gripper_gravity.urdf" OPENYAM_PACKAGE_PATHS: dict[str, Path] = {"yam_description": OPENYAM_PACKAGE} -def make_openyam_hardware( - hw_id: str = "arm", - *, - auto_enable: bool = True, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Create OpenYAM hardware, defaulting to the generic mock adapter.""" +def openyam_hardware() -> HardwareComponent: + """Select the physical or in-memory whole-body adapter for OpenYAM.""" + adapter_type = "mock_whole_body" if global_config.simulation else "openyam_damiao" adapter_kwargs: dict[str, object] = {} - if home_joints is not None: - adapter_kwargs["initial_positions"] = home_joints + if not global_config.simulation: + adapter_kwargs["runtime_config"] = DamiaoRuntimeConfig( + bus_addresses={"openyam": global_config.can_port or "can0"}, + gravity_comp=True, + ) return HardwareComponent( - hardware_id=hw_id, - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints(hw_id, OPENYAM_DOF), - adapter_type="mock", - address=None, - auto_enable=auto_enable, - gripper_joints=[f"{hw_id}/gripper"], + hardware_id=OPENYAM_HARDWARE_ID, + hardware_type=HardwareType.WHOLE_BODY, + joints=list(OPENYAM_JOINTS), + adapter_type=adapter_type, + auto_enable=True, adapter_kwargs=adapter_kwargs, + wb_config=WholeBodyConfig( + kp=(80.0, 80.0, 80.0, 10.0, 10.0, 10.0, 0.0), + kd=(5.0, 5.0, 5.0, 1.5, 1.5, 1.5, 0.0), + ), ) -def openyam_hardware( - hw_id: str = "arm", - *, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Create mock OpenYAM hardware for simulation and configuration checks.""" - return make_openyam_hardware(hw_id, home_joints=home_joints) - - def make_openyam_model_config( name: str = "arm", *, joint_prefix: str | None = None, home_joints: list[float] | None = None, ) -> RobotModelConfig: - """Build a planning config for the gripper-equipped OpenYAM.""" - local_joint_names = joint_names(OPENYAM_DOF, prefix="yam_joint") + """Build the six-arm-joint planning model for OpenYAM.""" + local_joint_names = joint_names(OPENYAM_DOF) return RobotModelConfig( name=name, model_path=OPENYAM_MODEL_PATH, base_pose=base_pose(), joint_names=local_joint_names, - base_link="yam_base_link", + base_link="base", planning_groups=[ PlanningGroupDefinition( name="manipulator", joint_names=tuple(local_joint_names), - base_link="yam_base_link", - tip_link="yam_hand_tcp", + base_link="base", + tip_link="gripper_tip", ) ], package_paths=OPENYAM_PACKAGE_PATHS, @@ -94,8 +94,7 @@ def make_openyam_model_config( name, OPENYAM_DOF, joint_prefix=joint_prefix, - urdf_joint_prefix="yam_", + urdf_joint_prefix="", ), - gripper_hardware_id=name, home_joints=home_joints or [0.0] * OPENYAM_DOF, ) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 092f347b99..cbb67bef4b 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -14,19 +14,28 @@ from typing import Any +import pytest + +from dimos.control.components import HardwareType from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import Blueprint -from dimos.hardware.manipulators.mock.adapter import MockAdapter -from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig +from dimos.core.global_config import global_config from dimos.robot.manipulators.openyam.blueprints.basic import ( coordinator_openyam, openyam_planner_coordinator, ) +from dimos.robot.manipulators.openyam.blueprints.teleop import ( + keyboard_teleop_openyam, + keyboard_teleop_openyam_planner, +) from dimos.robot.manipulators.openyam.config import ( + OPENYAM_ARM_JOINTS, OPENYAM_DOF, - OPENYAM_PACKAGE_PATHS, - make_openyam_hardware, + OPENYAM_GRIPPER_JOINT, + OPENYAM_HARDWARE_ID, + OPENYAM_JOINTS, make_openyam_model_config, + openyam_hardware, ) @@ -38,56 +47,80 @@ def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]: return _module_kwargs(blueprint, ControlCoordinator) -def test_openyam_model_config_has_expected_links_and_mapping() -> None: +def test_make_openyam_model_config_maps_only_arm_joints() -> None: config = make_openyam_model_config(name="arm") - assert config.joint_names == [f"yam_joint{i}" for i in range(1, OPENYAM_DOF + 1)] - assert config.joint_name_mapping == { - f"arm/joint{i}": f"yam_joint{i}" for i in range(1, OPENYAM_DOF + 1) - } - assert config.base_link == "yam_base_link" - assert config.end_effector_link == "yam_hand_tcp" - assert list(config.package_paths) == list(OPENYAM_PACKAGE_PATHS) - assert config.gripper_hardware_id == "arm" + assert len(config.joint_names) == OPENYAM_DOF + assert set(config.joint_name_mapping) == set(OPENYAM_ARM_JOINTS) + assert OPENYAM_GRIPPER_JOINT not in config.joint_name_mapping + assert config.base_link == "base" + assert config.end_effector_link == "gripper_tip" -def test_openyam_mock_hardware_has_gripper() -> None: - hardware = make_openyam_hardware("arm") +def test_openyam_hardware_physical_mode_returns_one_whole_body( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.setattr(global_config, "simulation", "") + monkeypatch.setattr(global_config, "can_port", "can1") - assert hardware.adapter_type == "mock" - assert hardware.joints == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] - assert hardware.gripper_joints == ["arm/gripper"] + hardware = openyam_hardware() + assert (hardware.hardware_id, hardware.hardware_type, hardware.adapter_type) == ( + OPENYAM_HARDWARE_ID, + HardwareType.WHOLE_BODY, + "openyam_damiao", + ) + assert hardware.adapter_kwargs["runtime_config"].bus_addresses == {"openyam": "can1"} -def test_openyam_mock_adapter_set_get_behavior() -> None: - positions = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] - adapter = MockAdapter(dof=OPENYAM_DOF, initial_positions=positions) - assert adapter.read_joint_positions() == positions - updated_positions = [-0.1, -0.2, -0.3, -0.4, -0.5, -0.6] - assert adapter.write_joint_positions(updated_positions) - assert adapter.read_joint_positions() == updated_positions - assert adapter.write_gripper_position(0.25) - assert adapter.read_gripper_position() == 0.25 +def test_openyam_hardware_simulation_mode_returns_generic_whole_body_mock( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.setattr(global_config, "simulation", "mujoco") + hardware = openyam_hardware() -def test_openyam_planner_blueprint_preserves_model_config() -> None: - blueprint = openyam_planner_coordinator - kwargs = _module_kwargs(blueprint, ManipulationModule) - config = ManipulationModuleConfig(**kwargs).robots[0] + assert hardware.adapter_type == "mock_whole_body" - assert config.name == "arm" - assert config.joint_names == [f"yam_joint{i}" for i in range(1, OPENYAM_DOF + 1)] - assert config.end_effector_link == "yam_hand_tcp" - assert config.gripper_hardware_id == "arm" - task = _coordinator_kwargs(blueprint)["tasks"][0] - assert task.type == "trajectory" - assert task.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] - -def test_openyam_coordinator_blueprint_uses_six_arm_joints() -> None: - blueprint = coordinator_openyam +@pytest.mark.parametrize( + "blueprint", + [ + coordinator_openyam, + openyam_planner_coordinator, + keyboard_teleop_openyam, + keyboard_teleop_openyam_planner, + ], +) +def test_openyam_blueprints_partition_arm_and_gripper_tasks(blueprint: Blueprint) -> None: kwargs = _coordinator_kwargs(blueprint) - assert len(kwargs["hardware"]) == 1 - assert len(kwargs["hardware"][0].joints) == OPENYAM_DOF - assert kwargs["tasks"][0].joint_names == kwargs["hardware"][0].joints + claimed_joints = [task.joint_names for task in kwargs["tasks"]] + + assert kwargs["hardware"][0].joints == OPENYAM_JOINTS + assert OPENYAM_ARM_JOINTS in claimed_joints + assert all(joints in (OPENYAM_ARM_JOINTS, [OPENYAM_GRIPPER_JOINT]) for joints in claimed_joints) + + +def test_keyboard_teleop_gripper_control_is_independent() -> None: + tasks = _coordinator_kwargs(keyboard_teleop_openyam)["tasks"] + gripper = next(task for task in tasks if task.name == "servo_gripper") + + assert gripper.joint_names == [OPENYAM_GRIPPER_JOINT] + + +def test_keyboard_teleop_openyam_planner_trajectory_has_priority_over_eef_task() -> None: + tasks = _coordinator_kwargs(keyboard_teleop_openyam_planner)["tasks"] + trajectory = next(task for task in tasks if task.type == "trajectory") + eef_twist = next(task for task in tasks if task.type == "eef_twist") + + assert trajectory.joint_names == OPENYAM_ARM_JOINTS + assert trajectory.priority == 20 + assert eef_twist.priority == 10 + + +def test_keyboard_teleop_openyam_gripper_task_has_no_default_position() -> None: + tasks = _coordinator_kwargs(keyboard_teleop_openyam)["tasks"] + gripper = next(task for task in tasks if task.name == "servo_gripper") + + assert gripper.joint_names == [OPENYAM_GRIPPER_JOINT] + assert "default_positions" not in gripper.params diff --git a/dimos/robot/manipulators/piper/cli.py b/dimos/robot/manipulators/piper/cli.py deleted file mode 100644 index 0e22fd5eea..0000000000 --- a/dimos/robot/manipulators/piper/cli.py +++ /dev/null @@ -1,43 +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. - -from __future__ import annotations - -import subprocess - -import typer - -app = typer.Typer(help="Piper robot commands") - - -@app.command("can-activate") -def can_activate( - interface: str = typer.Argument(..., help="CAN interface to configure"), - bitrate: int = typer.Option(1_000_000, "--bitrate", help="CAN bitrate"), -) -> None: - """Configure an existing Piper SocketCAN interface.""" - if not typer.confirm( - "This will request sudo to configure CAN. Continue?", - default=False, - ): - typer.echo("Aborted.") - raise typer.Exit(1) - - commands = [ - ["sudo", "ip", "link", "set", interface, "down"], - ["sudo", "ip", "link", "set", interface, "type", "can", "bitrate", str(bitrate)], - ["sudo", "ip", "link", "set", interface, "up"], - ] - for command in commands: - subprocess.run(command, check=True) diff --git a/dimos/robot/manipulators/piper/test_cli.py b/dimos/robot/manipulators/piper/test_cli.py deleted file mode 100644 index 2e7b1fd5f1..0000000000 --- a/dimos/robot/manipulators/piper/test_cli.py +++ /dev/null @@ -1,68 +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. - -from unittest.mock import Mock, call - -from typer.testing import CliRunner - -from dimos.robot.manipulators.piper import cli as piper - -runner = CliRunner() - - -def test_can_activate_confirms_before_spawning(monkeypatch): - confirm = Mock(return_value=True) - run = Mock() - monkeypatch.setattr(piper.typer, "confirm", confirm) - monkeypatch.setattr(piper.subprocess, "run", run) - - result = runner.invoke(piper.app, ["can1", "--bitrate", "500000"]) - - assert result.exit_code == 0, result.output - confirm.assert_called_once() - assert run.call_args_list == [ - call(["sudo", "ip", "link", "set", "can1", "down"], check=True), - call( - ["sudo", "ip", "link", "set", "can1", "type", "can", "bitrate", "500000"], - check=True, - ), - call(["sudo", "ip", "link", "set", "can1", "up"], check=True), - ] - - -def test_can_activate_rejection_does_not_spawn(monkeypatch): - confirm = Mock(return_value=False) - run = Mock() - monkeypatch.setattr(piper.typer, "confirm", confirm) - monkeypatch.setattr(piper.subprocess, "run", run) - - result = runner.invoke(piper.app, ["can0"]) - - assert result.exit_code == 1 - assert "Aborted." in result.output - run.assert_not_called() - - -def test_can_activate_uses_default_bitrate(monkeypatch): - monkeypatch.setattr(piper.typer, "confirm", Mock(return_value=True)) - run = Mock() - monkeypatch.setattr(piper.subprocess, "run", run) - - result = runner.invoke(piper.app, ["can0"]) - - assert result.exit_code == 0, result.output - assert run.call_args_list[1] == call( - ["sudo", "ip", "link", "set", "can0", "type", "can", "bitrate", "1000000"], - check=True, - ) diff --git a/dimos/teleop/openarm_mini/calibration.py b/dimos/teleop/openarm_mini/calibration.py new file mode 100644 index 0000000000..0e8d94ebe6 --- /dev/null +++ b/dimos/teleop/openarm_mini/calibration.py @@ -0,0 +1,125 @@ +# 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 Mini calibration artifact loading and validation.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Literal, Self + +from pydantic import StrictBool, StrictInt, ValidationError, model_validator + +from dimos.constants import STATE_DIR +from dimos.protocol.service.spec import BaseConfig + +CALIBRATION_FILENAME = "calibration.json" +FEETECH_RAW_MIN = 0 +FEETECH_RAW_MAX = 4095 +FEETECH_POSITION_SPAN = FEETECH_RAW_MAX - FEETECH_RAW_MIN +OPENARM_MINI_STATE_DIR = STATE_DIR / "teleop" / "openarm_mini" +OpenArmMiniSide = Literal["left", "right"] +OPENARM_MINI_ARM_JOINT_NAMES = ( + "joint_1", + "joint_2", + "joint_3", + "joint_4", + "joint_5", + "joint_6", + "joint_7", +) +OPENARM_MINI_MOTOR_NAMES = OPENARM_MINI_ARM_JOINT_NAMES + + +def default_calibration_path(side: OpenArmMiniSide) -> Path: + """Return the default persistent calibration directory for an OpenArm Mini side.""" + return OPENARM_MINI_STATE_DIR / side + + +class OpenArmMiniCalibrationError(RuntimeError): + """Raised when OpenArm Mini calibration is missing or invalid.""" + + +class OpenArmMiniMotorCalibration(BaseConfig): + """Calibration values for one arm-joint Feetech motor.""" + + id: StrictInt + homing_offset: StrictInt + flip: StrictBool = False + + @model_validator(mode="after") + def _validate_motor(self) -> Self: + if self.id <= 0: + raise OpenArmMiniCalibrationError(f"motor has invalid id {self.id}") + return self + + +class OpenArmMiniCalibration(BaseConfig): + """Side-specific OpenArm Mini calibration artifact.""" + + side: OpenArmMiniSide + motors: dict[str, OpenArmMiniMotorCalibration] + schema_version: Literal[1] = 1 + + @model_validator(mode="after") + def _validate_calibration(self) -> Self: + missing = set(OPENARM_MINI_ARM_JOINT_NAMES) - set(self.motors) + extra = set(self.motors) - set(OPENARM_MINI_ARM_JOINT_NAMES) + if missing or extra: + raise OpenArmMiniCalibrationError( + "OpenArm Mini calibration must contain exactly arm joints " + f"{list(OPENARM_MINI_ARM_JOINT_NAMES)}; missing={sorted(missing)}, extra={sorted(extra)}" + ) + for motor_name, motor in self.motors.items(): + if motor.id <= 0: + raise OpenArmMiniCalibrationError(f"{motor_name} has invalid id {motor.id}") + return self + + +def calibration_file(path: Path) -> Path: + """Resolve a calibration directory or file to the artifact file path.""" + if path.suffix == ".json": + return path + return path / CALIBRATION_FILENAME + + +def load_calibration(path: Path, side: OpenArmMiniSide) -> OpenArmMiniCalibration: + """Load and validate a side-specific calibration artifact.""" + artifact_path = calibration_file(path) + if not artifact_path.exists(): + raise OpenArmMiniCalibrationError( + f"Missing OpenArm Mini {side} calibration at {artifact_path}. " + "Run `dimos hardware openarm-mini calibrate` " + "to create calibration artifacts before starting teleop." + ) + try: + calibration = OpenArmMiniCalibration.model_validate_json(artifact_path.read_text()) + except (OpenArmMiniCalibrationError, ValidationError, ValueError) as exc: + raise OpenArmMiniCalibrationError( + f"Invalid OpenArm Mini {side} calibration at {artifact_path}: {exc}" + ) from exc + if calibration.side != side: + raise OpenArmMiniCalibrationError( + f"OpenArm Mini calibration side mismatch at {artifact_path}: " + f"expected {side!r}, got {calibration.side!r}" + ) + return calibration + + +def save_calibration(path: Path, calibration: OpenArmMiniCalibration) -> Path: + """Write a side-specific calibration artifact and return its file path.""" + artifact_path = calibration_file(path) + artifact_path.parent.mkdir(parents=True, exist_ok=True) + artifact_path.write_text(calibration.model_dump_json(indent=2) + "\n") + return artifact_path diff --git a/dimos/teleop/openarm_mini/cli/_errors.py b/dimos/teleop/openarm_mini/cli/_errors.py new file mode 100644 index 0000000000..38adeed945 --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/_errors.py @@ -0,0 +1,27 @@ +# 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. + +"""Shared error presentation for OpenArm Mini commands.""" + +from typing import NoReturn + +import typer + +from dimos.teleop.openarm_mini.feetech import OpenArmMiniDependencyError + + +def exit_for_missing_dependency(error: OpenArmMiniDependencyError) -> NoReturn: + """Print one actionable dependency error and exit without a traceback.""" + typer.echo(str(error), err=True) + raise typer.Exit(1) diff --git a/dimos/teleop/openarm_mini/cli/app.py b/dimos/teleop/openarm_mini/cli/app.py new file mode 100644 index 0000000000..ae0fec6095 --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/app.py @@ -0,0 +1,28 @@ +# 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 Mini hardware commands. + +This module is imported by the global hardware CLI. Keep command modules safe +to import without Rich, NumPy, control, manipulation, or hardware SDK imports. +""" + +import typer + +from dimos.teleop.openarm_mini.cli import calibrate, joint_tui, setup_motor_id + +app = typer.Typer(help="Configure and inspect OpenArm Mini leader hardware", no_args_is_help=True) +app.command("calibrate")(calibrate.main) +app.command("joint-tui")(joint_tui.main) +app.command("setup-motor-id")(setup_motor_id.main) diff --git a/dimos/teleop/openarm_mini/cli/calibrate.py b/dimos/teleop/openarm_mini/cli/calibrate.py new file mode 100644 index 0000000000..bd4f35317e --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/calibrate.py @@ -0,0 +1,246 @@ +# 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. + +"""Manual OpenArm Mini leader zero-calibration utility. + +This script intentionally talks only to the OpenArm Mini leader Feetech bus. It +does not import or start ControlCoordinator, ManipulationModule, or follower +OpenArm hardware. +""" + +from __future__ import annotations + +from collections.abc import Callable +from pathlib import Path +import time +from typing import Any, Literal + +import typer + +from dimos.teleop.openarm_mini.calibration import ( + OPENARM_MINI_ARM_JOINT_NAMES, + OpenArmMiniCalibration, + OpenArmMiniMotorCalibration, + OpenArmMiniSide, + default_calibration_path, + load_calibration, + save_calibration, +) +from dimos.teleop.openarm_mini.cli._errors import exit_for_missing_dependency +from dimos.teleop.openarm_mini.feetech import ( + FeetechLeaderReader, + OpenArmMiniDependencyError, + _calibrated_motor_radians, +) + +DEFAULT_MOTOR_IDS = { + joint_name: index + 1 for index, joint_name in enumerate(OPENARM_MINI_ARM_JOINT_NAMES) +} +DEFAULT_FLIPS_BY_SIDE: dict[OpenArmMiniSide, frozenset[str]] = { + "left": frozenset(("joint_1", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7")), + "right": frozenset(("joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6")), +} + + +def main( + side: Literal["left", "right", "both"] = typer.Option("both"), + port_left: str = typer.Option(..., help="Left leader Feetech serial port."), + port_right: str = typer.Option(..., help="Right leader Feetech serial port."), + baudrate: int = typer.Option(..., help="Feetech serial baudrate."), + left_calibration_path: Path = typer.Option(default_calibration_path("left")), + right_calibration_path: Path = typer.Option(default_calibration_path("right")), + left_flips: str | None = typer.Option( + None, + help=( + "Comma-separated left-side semantic joints to flip. Defaults to the " + "known OpenArm Mini left leader orientation. Use 'none' for no flips." + ), + ), + right_flips: str | None = typer.Option( + None, + help=( + "Comma-separated right-side semantic joints to flip. Defaults to the " + "known OpenArm Mini right leader orientation. Use 'none' for no flips." + ), + ), + live_readout: bool = typer.Option( + False, + help="Print calibrated arm-joint radians using existing calibration artifacts.", + ), +) -> None: + """Zero-calibrate OpenArm Mini leader teleop.""" + try: + _run( + side=side, + port_left=port_left, + port_right=port_right, + baudrate=baudrate, + left_calibration_path=left_calibration_path, + right_calibration_path=right_calibration_path, + left_flips=left_flips, + right_flips=right_flips, + live_readout=live_readout, + ) + except OpenArmMiniDependencyError as error: + exit_for_missing_dependency(error) + + +def _run( + *, + side: Literal["left", "right", "both"], + port_left: str, + port_right: str, + baudrate: int, + left_calibration_path: Path, + right_calibration_path: Path, + left_flips: str | None, + right_flips: str | None, + live_readout: bool, +) -> None: + sides: tuple[OpenArmMiniSide, ...] + if side == "both": + sides = ("left", "right") + elif side == "left": + sides = ("left",) + else: + sides = ("right",) + print("OpenArm Mini leader calibration only connects to Feetech leader ports.") + print("It never starts ControlCoordinator or connects follower OpenArm hardware.") + print("Place each selected leader side in its natural zero pose before calibration.") + for selected_side in sides: + port = port_left if selected_side == "left" else port_right + path = left_calibration_path if selected_side == "left" else right_calibration_path + flip_arg = left_flips if selected_side == "left" else right_flips + flips = _parse_flip_overrides(flip_arg, selected_side) + if live_readout: + _live_readout(selected_side, port, path, baudrate) + else: + _calibrate_side(selected_side, port, path, baudrate, flips=flips) + + +def _calibrate_side( + side: OpenArmMiniSide, + port: str, + path: Path, + baudrate: int, + *, + flips: set[str] | frozenset[str] | None = None, + reader_factory: Callable[[str, int], Any] = FeetechLeaderReader, +) -> None: + reader = reader_factory(port, baudrate) + reader.connect() + try: + print(f"\nCalibrating {side} OpenArm Mini leader on {port}") + print("Place the leader in its natural zero pose; reading arm-joint motors now.") + raw_positions = reader.read_raw_positions(DEFAULT_MOTOR_IDS) + calibration = _capture_zero_calibration( + side, + raw_positions, + flips if flips is not None else DEFAULT_FLIPS_BY_SIDE[side], + ) + artifact_path = save_calibration(path, calibration) + print(_format_calibration_confirmation(calibration)) + print(f"Wrote {side} calibration to {artifact_path}") + finally: + reader.disconnect() + + +def _live_readout(side: OpenArmMiniSide, port: str, path: Path, baudrate: int) -> None: + # Deferred because this command module is imported by the global hardware CLI. + from dimos.teleop.openarm_mini.mapping import map_side_readings + + calibration = load_calibration(path, side) + reader = FeetechLeaderReader(port, baudrate) + reader.connect() + try: + print(f"\nLive calibrated {side} arm readout from {port}; press Ctrl-C to stop.") + while True: + raw_positions = reader.read_raw_positions(DEFAULT_MOTOR_IDS) + calibrated_readings = { + joint_name: _calibrated_motor_radians(raw_position, calibration.motors[joint_name]) + for joint_name, raw_position in raw_positions.items() + } + command = map_side_readings(side, calibrated_readings) + print( + " ".join( + f"{joint_name}={position:+.3f}rad" + for joint_name, position in command.positions_by_joint.items() + ) + ) + time.sleep(0.25) + except KeyboardInterrupt: + print("\nStopped live readout.") + finally: + reader.disconnect() + + +def _capture_zero_calibration( + side: OpenArmMiniSide, + raw_positions: dict[str, int], + flips: set[str] | frozenset[str], +) -> OpenArmMiniCalibration: + _validate_raw_positions(raw_positions) + invalid_flips = set(flips) - set(OPENARM_MINI_ARM_JOINT_NAMES) + if invalid_flips: + raise RuntimeError(f"unknown OpenArm Mini flip joints: {sorted(invalid_flips)}") + return OpenArmMiniCalibration( + side=side, + motors={ + joint_name: OpenArmMiniMotorCalibration( + id=DEFAULT_MOTOR_IDS[joint_name], + homing_offset=raw_positions[joint_name], + flip=joint_name in flips, + ) + for joint_name in OPENARM_MINI_ARM_JOINT_NAMES + }, + ) + + +def _format_calibration_confirmation(calibration: OpenArmMiniCalibration) -> str: + lines = [ + f"Captured {calibration.side} OpenArm Mini leader zero offsets:", + f"{'Joint':<10} {'ID':>2} {'Zero Raw':>8} {'Flip':>5}", + "-" * 31, + ] + for joint_name in OPENARM_MINI_ARM_JOINT_NAMES: + motor = calibration.motors[joint_name] + lines.append(f"{joint_name:<10} {motor.id:>2} {motor.homing_offset:>8} {motor.flip!s:>5}") + return "\n".join(lines) + + +def _parse_flip_overrides(value: str | None, side: OpenArmMiniSide) -> set[str]: + if value is None: + return set(DEFAULT_FLIPS_BY_SIDE[side]) + stripped = value.strip() + if not stripped or stripped.lower() == "none": + return set() + flips = {entry.strip() for entry in stripped.split(",") if entry.strip()} + invalid = flips - set(OPENARM_MINI_ARM_JOINT_NAMES) + if invalid: + raise RuntimeError(f"unknown OpenArm Mini flip joints: {sorted(invalid)}") + return flips + + +def _validate_raw_positions(raw_positions: dict[str, int]) -> None: + missing = set(OPENARM_MINI_ARM_JOINT_NAMES) - set(raw_positions) + extra = set(raw_positions) - set(OPENARM_MINI_ARM_JOINT_NAMES) + if missing or extra: + raise RuntimeError( + "OpenArm Mini raw readings must contain exactly arm joints " + f"{list(OPENARM_MINI_ARM_JOINT_NAMES)}; missing={sorted(missing)}, extra={sorted(extra)}" + ) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/teleop/openarm_mini/cli/joint_tui.py b/dimos/teleop/openarm_mini/cli/joint_tui.py new file mode 100644 index 0000000000..dd0d24228e --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/joint_tui.py @@ -0,0 +1,203 @@ +# 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. + +"""Rich TUI for inspecting calibrated OpenArm Mini leader arm joints. + +This helper only connects to OpenArm Mini leader Feetech ports. It does not start +ControlCoordinator and does not connect follower OpenArm hardware. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +import time +from typing import TYPE_CHECKING + +import typer + +if TYPE_CHECKING: + from rich.console import Group + +from dimos.teleop.openarm_mini.calibration import ( + OPENARM_MINI_ARM_JOINT_NAMES, + OpenArmMiniCalibration, + OpenArmMiniSide, + default_calibration_path, + load_calibration, +) +from dimos.teleop.openarm_mini.cli._errors import exit_for_missing_dependency +from dimos.teleop.openarm_mini.cli.calibrate import DEFAULT_MOTOR_IDS +from dimos.teleop.openarm_mini.feetech import ( + OPENARM_MINI_DEFAULT_BAUDRATE, + FeetechLeaderReader, + OpenArmMiniDependencyError, + _calibrated_motor_radians, +) + + +@dataclass(frozen=True) +class OpenArmMiniJointRow: + side: str + joint: str + follower_joint: str + motor_id: int + raw: int + radians: float + clamped_radians: float + flip: bool + + +def main( + side: OpenArmMiniSide = typer.Option(..., help="Leader side to inspect."), + port: str = typer.Option(..., help="Leader Feetech serial port."), + baudrate: int = typer.Option( + OPENARM_MINI_DEFAULT_BAUDRATE, + help="Feetech serial baudrate.", + ), + calibration_path: Path | None = typer.Option( + None, + help="Calibration directory or JSON file. Defaults to the selected side calibration.", + ), + refresh_hz: float = typer.Option(10.0), +) -> None: + """Display one OpenArm Mini leader side in a Rich TUI.""" + try: + _run( + side=side, + port=port, + baudrate=baudrate, + calibration_path=calibration_path, + refresh_hz=refresh_hz, + ) + except OpenArmMiniDependencyError as error: + exit_for_missing_dependency(error) + + +def _run( + *, + side: OpenArmMiniSide, + port: str, + baudrate: int, + calibration_path: Path | None, + refresh_hz: float, +) -> None: + # Deferred because this command module is imported by the global hardware CLI. + from rich.live import Live + + refresh_seconds = 1.0 / refresh_hz + calibration = _load_tui_calibration(side, _resolve_calibration_path(side, calibration_path)) + reader = FeetechLeaderReader(port, baudrate) + try: + reader.connect() + + with Live(refresh_per_second=refresh_hz, screen=True) as live: + while True: + rows = _read_side_rows( + calibration, + reader.read_raw_positions(DEFAULT_MOTOR_IDS), + ) + live.update(_build_joint_dashboard(rows)) + time.sleep(refresh_seconds) + except KeyboardInterrupt: + pass + finally: + reader.disconnect() + + +def _resolve_calibration_path(side: OpenArmMiniSide, calibration_path: Path | None) -> Path: + if calibration_path is not None: + return calibration_path + return default_calibration_path(side) + + +def _load_tui_calibration( + side: OpenArmMiniSide, + calibration_path: Path, +) -> OpenArmMiniCalibration: + return load_calibration(calibration_path, side) + + +def _read_side_rows( + calibration: OpenArmMiniCalibration, + raw_positions: dict[str, int], +) -> list[OpenArmMiniJointRow]: + # Deferred because this command module is imported by the global hardware CLI. + from dimos.teleop.openarm_mini.mapping import map_side_readings + + side = calibration.side + calibrated_readings = { + joint_name: _calibrated_motor_radians( + raw_positions[joint_name], calibration.motors[joint_name] + ) + for joint_name in OPENARM_MINI_ARM_JOINT_NAMES + } + command = map_side_readings(side, calibrated_readings) + return [ + OpenArmMiniJointRow( + side=side, + joint=joint_name, + follower_joint=follower_joint, + motor_id=calibration.motors[joint_name].id, + raw=raw_positions[joint_name], + radians=calibrated_readings[joint_name], + clamped_radians=command.positions_by_joint[follower_joint], + flip=calibration.motors[joint_name].flip, + ) + for joint_name, follower_joint in zip( + OPENARM_MINI_ARM_JOINT_NAMES, + command.positions_by_joint, + strict=True, + ) + ] + + +def _build_joint_dashboard(rows: list[OpenArmMiniJointRow]) -> Group: + # Deferred because this command module is imported by the global hardware CLI. + from rich.console import Group + from rich.panel import Panel + from rich.table import Table + from rich.text import Text + + table = Table(title="OpenArm Mini leader joint readout", expand=True) + table.add_column("Side", style="cyan", no_wrap=True) + table.add_column("Joint", no_wrap=True) + table.add_column("Follower Joint", no_wrap=True) + table.add_column("ID", justify="right") + table.add_column("Raw", justify="right") + table.add_column("Rad", justify="right") + table.add_column("Clamped Rad", justify="right") + table.add_column("Flip", justify="center") + for row in rows: + clamp_style = "yellow" if abs(row.radians - row.clamped_radians) > 1e-9 else "green" + table.add_row( + row.side, + row.joint, + row.follower_joint, + str(row.motor_id), + str(row.raw), + f"{row.radians:+.3f}", + f"[{clamp_style}]{row.clamped_radians:+.3f}[/{clamp_style}]", + "yes" if row.flip else "no", + ) + help_text = Text( + "Leader only: reads Feetech arm joints from calibration, displays raw ticks, " + "calibrated radians, and sender-side clamped follower radians. Ctrl-C to exit.", + style="dim", + ) + return Group(Panel(help_text, title="OpenArm Mini Joint TUI"), table) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/teleop/openarm_mini/cli/setup_motor_id.py b/dimos/teleop/openarm_mini/cli/setup_motor_id.py new file mode 100644 index 0000000000..1a0cc6f9fe --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/setup_motor_id.py @@ -0,0 +1,208 @@ +# 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. + +"""One-shot Feetech motor ID setup for an OpenArm Mini leader motor. + +Connect exactly one Feetech motor to the USB controller before running this +script. Writing IDs while multiple motors are attached can address the wrong +device when IDs collide. +""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import Any + +import typer + +from dimos.teleop.openarm_mini.cli._errors import exit_for_missing_dependency +from dimos.teleop.openarm_mini.feetech import ( + OpenArmMiniDependencyError, + _create_sdk_handlers, +) + +FEETECH_ID_ADDRESS = 5 +FEETECH_TORQUE_ENABLE_ADDRESS = 40 +FEETECH_MIN_MOTOR_ID = 1 +FEETECH_MAX_MOTOR_ID = 253 +FEETECH_TORQUE_ENABLE = 1 +FEETECH_TORQUE_DISABLE = 0 +FEETECH_COMM_SUCCESS = 0 + + +def main( + port: str = typer.Option(..., help="Feetech serial port, e.g. /dev/ttyUSB1"), + new_id: int = typer.Option(..., "--new-id", help="Target Feetech motor ID"), + old_id: int | None = typer.Option( + None, + "--old-id", + help="Current motor ID. If omitted, scan for exactly one connected motor.", + ), + baudrate: int = typer.Option(..., help="Feetech serial baudrate."), + yes: bool = typer.Option(False, "--yes", help="Skip the safety confirmation prompt."), +) -> None: + """Discover or select one motor, safely rewrite its ID, and verify the change.""" + try: + _run(port=port, new_id=new_id, old_id=old_id, baudrate=baudrate, yes=yes) + except OpenArmMiniDependencyError as error: + exit_for_missing_dependency(error) + + +def _run(*, port: str, new_id: int, old_id: int | None, baudrate: int, yes: bool) -> None: + _validate_motor_id(new_id, "new-id") + if old_id is not None: + _validate_motor_id(old_id, "old-id") + + if not yes: + print("Connect exactly ONE Feetech motor to the controller before continuing.") + print("If multiple motors share an ID, this write can affect the wrong motor(s).") + input("Press Enter to continue or Ctrl-C to abort.") + + setup_motor_id(port=port, baudrate=baudrate, new_id=new_id, old_id=old_id) + + +def setup_motor_id(port: str, baudrate: int, new_id: int, old_id: int | None = None) -> int: + """Set one connected Feetech motor to ``new_id``. + + Returns the detected or provided previous motor ID. + """ + _validate_motor_id(new_id, "new-id") + if old_id is not None: + _validate_motor_id(old_id, "old-id") + + port_handler, packet_handler = _create_sdk_handlers(port) + if not port_handler.openPort(): + raise RuntimeError(f"failed to open Feetech port {port}") + try: + if not port_handler.setBaudRate(baudrate): + raise RuntimeError(f"failed to set Feetech baudrate {baudrate}") + motor_id = old_id if old_id is not None else find_single_motor_id(packet_handler) + write_motor_id(packet_handler, motor_id, new_id) + finally: + port_handler.closePort() + + print(f"Feetech motor ID set: {motor_id} -> {new_id}") + return motor_id + + +def find_single_motor_id(packet_handler: Any) -> int: + """Scan the Feetech bus and return the only responding motor ID.""" + found_ids = [ + motor_id + for motor_id in range(FEETECH_MIN_MOTOR_ID, FEETECH_MAX_MOTOR_ID + 1) + if ping_motor_id(packet_handler, motor_id) + ] + if not found_ids: + raise RuntimeError("no Feetech motor responded during ID scan") + if len(found_ids) > 1: + raise RuntimeError( + "multiple Feetech motors responded during ID scan: " + f"{found_ids}. Connect exactly one motor before setting IDs." + ) + return found_ids[0] + + +def ping_motor_id(packet_handler: Any, motor_id: int) -> bool: + """Return whether ``motor_id`` responds successfully to Feetech ping.""" + _validate_motor_id(motor_id, "motor-id") + model_number, comm_result, error = packet_handler.ping(motor_id) + return bool(comm_result == FEETECH_COMM_SUCCESS and error == 0 and model_number != 0) + + +def write_motor_id(packet_handler: Any, old_id: int, new_id: int) -> None: + """Disable torque, unlock EEPROM, write the new ID, lock, and verify.""" + _validate_motor_id(old_id, "old-id") + _validate_motor_id(new_id, "new-id") + if not ping_motor_id(packet_handler, old_id): + raise RuntimeError(f"Feetech motor {old_id} did not respond before ID write") + if old_id == new_id: + print(f"Feetech motor is already ID {new_id}; no write needed.") + return + + torque_disabled = False + eeprom_unlocked = False + try: + _ensure_success( + "disable torque", + packet_handler.write1ByteTxRx( + old_id, FEETECH_TORQUE_ENABLE_ADDRESS, FEETECH_TORQUE_DISABLE + ), + ) + torque_disabled = True + _ensure_success("unlock EEPROM", packet_handler.unLockEprom(old_id)) + eeprom_unlocked = True + _ensure_success( + "write motor ID", + packet_handler.write1ByteTxRx(old_id, FEETECH_ID_ADDRESS, new_id), + ) + _ensure_success("lock EEPROM", packet_handler.LockEprom(new_id)) + eeprom_unlocked = False + if not ping_motor_id(packet_handler, new_id): + raise RuntimeError(f"Feetech motor {new_id} did not respond after ID write") + except Exception: + if eeprom_unlocked: + _best_effort_lock_eeprom(packet_handler, candidate_ids=(new_id, old_id)) + if torque_disabled: + _best_effort_enable_torque(packet_handler, candidate_ids=(new_id, old_id)) + raise + + +def _best_effort_lock_eeprom(packet_handler: Any, candidate_ids: tuple[int, int]) -> None: + for motor_id in candidate_ids: + try: + if _is_success_result(packet_handler.LockEprom(motor_id)): + return + except Exception: + continue + + +def _best_effort_enable_torque(packet_handler: Any, candidate_ids: tuple[int, int]) -> None: + for motor_id in candidate_ids: + try: + if _is_success_result( + packet_handler.write1ByteTxRx( + motor_id, + FEETECH_TORQUE_ENABLE_ADDRESS, + FEETECH_TORQUE_ENABLE, + ) + ): + return + except Exception: + continue + + +def _ensure_success(operation: str, result: object) -> None: + if not _is_success_result(result): + raise RuntimeError(f"Feetech {operation} failed with result {result!r}") + + +def _is_success_result(result: object) -> bool: + if not isinstance(result, Sequence) or isinstance(result, (str, bytes)): + return False + if len(result) < 2: + return False + comm_result = result[-2] + error = result[-1] + return bool(comm_result == FEETECH_COMM_SUCCESS and error == 0) + + +def _validate_motor_id(motor_id: int, label: str) -> None: + if not FEETECH_MIN_MOTOR_ID <= motor_id <= FEETECH_MAX_MOTOR_ID: + raise ValueError( + f"{label} must be in [{FEETECH_MIN_MOTOR_ID}, {FEETECH_MAX_MOTOR_ID}], got {motor_id}" + ) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/teleop/openarm_mini/cli/test_calibrate.py b/dimos/teleop/openarm_mini/cli/test_calibrate.py new file mode 100644 index 0000000000..079e49ea02 --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/test_calibrate.py @@ -0,0 +1,110 @@ +# 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 __future__ import annotations + +from pathlib import Path + +import pytest + +from dimos.teleop.openarm_mini.calibration import OPENARM_MINI_ARM_JOINT_NAMES, load_calibration +from dimos.teleop.openarm_mini.cli.calibrate import ( + DEFAULT_FLIPS_BY_SIDE, + _calibrate_side, + _capture_zero_calibration, + _format_calibration_confirmation, + _parse_flip_overrides, +) + + +def _raw_positions(value: int) -> dict[str, int]: + return {joint: value for joint in OPENARM_MINI_ARM_JOINT_NAMES} + + +def test_zero_capture_records_offsets_and_flips() -> None: + raw_positions = _raw_positions(2048) + raw_positions["joint_6"] = 1234 + + calibration = _capture_zero_calibration("left", raw_positions, {"joint_1", "joint_6"}) + + assert set(calibration.motors) == set(OPENARM_MINI_ARM_JOINT_NAMES) + assert calibration.motors["joint_6"].homing_offset == 1234 + assert calibration.motors["joint_1"].flip is True + assert "gripper" not in calibration.motors + + +def test_confirmation_table_shows_zero_offsets_not_limits() -> None: + rendered = _format_calibration_confirmation( + _capture_zero_calibration("right", _raw_positions(100), {"joint_2"}) + ) + + assert "Zero Raw" in rendered + assert "Flip" in rendered + assert "Max" not in rendered + assert "gripper" not in rendered + + +def test_parse_flip_overrides_defaults_none_and_validation() -> None: + assert _parse_flip_overrides(None, "left") == set(DEFAULT_FLIPS_BY_SIDE["left"]) + assert _parse_flip_overrides("none", "left") == set() + assert _parse_flip_overrides("joint_1,joint_7", "right") == {"joint_1", "joint_7"} + + with pytest.raises(RuntimeError, match="unknown"): + _parse_flip_overrides("gripper", "right") + + +def test_calibrate_side_writes_artifact_and_disconnects(tmp_path: Path) -> None: + reader = _FakeRawReader(_raw_positions(2048)) + + _calibrate_side( + "left", + "/dev/fake-left", + tmp_path / "left", + 1_000_000, + flips={"joint_3"}, + reader_factory=lambda _port, _baudrate: reader, + ) + + calibration = load_calibration(tmp_path / "left", "left") + assert reader.connected and reader.disconnected + assert calibration.motors["joint_1"].homing_offset == 2048 + assert calibration.motors["joint_3"].flip is True + assert calibration.motors["joint_4"].flip is False + + +def test_zero_capture_rejects_extra_or_missing_joint() -> None: + with_extra = _raw_positions(2048) | {"gripper": 1} + without_joint = _raw_positions(2048) + del without_joint["joint_7"] + + with pytest.raises(RuntimeError, match="extra"): + _capture_zero_calibration("left", with_extra, set()) + with pytest.raises(RuntimeError, match="missing"): + _capture_zero_calibration("left", without_joint, set()) + + +class _FakeRawReader: + def __init__(self, snapshot: dict[str, int]) -> None: + self._snapshot = snapshot + self.connected = False + self.disconnected = False + + def connect(self) -> None: + self.connected = True + + def disconnect(self) -> None: + self.disconnected = True + + def read_raw_positions(self, _motor_ids_by_name: object) -> dict[str, int]: + return self._snapshot diff --git a/dimos/teleop/openarm_mini/cli/test_cli.py b/dimos/teleop/openarm_mini/cli/test_cli.py new file mode 100644 index 0000000000..a56bef1422 --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/test_cli.py @@ -0,0 +1,216 @@ +# 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 __future__ import annotations + +import subprocess +import sys + +import pytest +from typer.testing import CliRunner + +from dimos.teleop.openarm_mini.calibration import default_calibration_path +from dimos.teleop.openarm_mini.cli import calibrate, joint_tui, setup_motor_id +from dimos.teleop.openarm_mini.cli.app import app +from dimos.teleop.openarm_mini.feetech import ( + OPENARM_MINI_DEFAULT_BAUDRATE, + OpenArmMiniDependencyError, +) + +runner = CliRunner() + + +def test_openarm_mini_cli_lists_every_operator_command() -> None: + result = runner.invoke(app, ["--help"]) + + assert result.exit_code == 0, result.output + assert "calibrate" in result.output + assert "joint-tui" in result.output + assert "setup-motor-id" in result.output + + +@pytest.mark.parametrize("command", ["calibrate", "joint-tui", "setup-motor-id"]) +def test_openarm_mini_command_help_needs_no_hardware(command: str) -> None: + result = runner.invoke(app, [command, "--help"]) + + assert result.exit_code == 0, result.output + + +def test_calibrate_delegates_parsed_options(mocker) -> None: + run = mocker.patch.object(calibrate, "_run") + + result = runner.invoke( + app, + [ + "calibrate", + "--side", + "left", + "--port-left", + "/dev/left", + "--port-right", + "/dev/right", + "--baudrate", + "1000000", + "--live-readout", + ], + ) + + assert result.exit_code == 0, result.output + run.assert_called_once_with( + side="left", + port_left="/dev/left", + port_right="/dev/right", + baudrate=1_000_000, + left_calibration_path=default_calibration_path("left"), + right_calibration_path=default_calibration_path("right"), + left_flips=None, + right_flips=None, + live_readout=True, + ) + + +def test_joint_tui_delegates_parsed_options(mocker) -> None: + run = mocker.patch.object(joint_tui, "_run") + + result = runner.invoke( + app, + ["joint-tui", "--side", "right", "--port", "/dev/right"], + ) + + assert result.exit_code == 0, result.output + run.assert_called_once_with( + side="right", + port="/dev/right", + baudrate=OPENARM_MINI_DEFAULT_BAUDRATE, + calibration_path=None, + refresh_hz=10.0, + ) + + +def test_setup_motor_id_delegates_parsed_options(mocker) -> None: + run = mocker.patch.object(setup_motor_id, "_run") + + result = runner.invoke( + app, + [ + "setup-motor-id", + "--port", + "/dev/motor", + "--new-id", + "3", + "--old-id", + "1", + "--baudrate", + "1000000", + "--yes", + ], + ) + + assert result.exit_code == 0, result.output + run.assert_called_once_with( + port="/dev/motor", + new_id=3, + old_id=1, + baudrate=1_000_000, + yes=True, + ) + + +def test_missing_sdk_is_a_clean_actionable_cli_error(mocker) -> None: + mocker.patch.object( + setup_motor_id, + "_run", + side_effect=OpenArmMiniDependencyError("Install the OpenArm Mini extra."), + ) + + result = runner.invoke( + app, + [ + "setup-motor-id", + "--port", + "/dev/motor", + "--new-id", + "3", + "--baudrate", + "1000000", + "--yes", + ], + ) + + assert result.exit_code == 1 + assert result.output == "Install the OpenArm Mini extra.\n" + assert result.exception is not None + assert "Traceback" not in result.output + + +def test_importing_openarm_mini_cli_app_is_lightweight() -> None: + script = ( + "import sys; " + "import dimos.teleop.openarm_mini.cli.app; " + "bad = [m for m in " + "('scservo_sdk', 'numpy', 'rich', 'dimos.control', 'dimos.manipulation') " + "if m in sys.modules]; " + "assert not bad, f'Heavy imports: {bad}'" + ) + + result = subprocess.run( + [sys.executable, "-c", script], + capture_output=True, + text=True, + timeout=30, + ) + + assert result.returncode == 0, result.stderr + + +@pytest.mark.parametrize( + "module", + [ + "dimos.teleop.openarm_mini.cli.calibrate", + "dimos.teleop.openarm_mini.cli.joint_tui", + "dimos.teleop.openarm_mini.cli.setup_motor_id", + ], +) +def test_direct_module_help_remains_supported(module: str) -> None: + result = subprocess.run( + [sys.executable, "-m", module, "--help"], + capture_output=True, + text=True, + timeout=30, + ) + + assert result.returncode == 0, result.stderr + + +def test_help_does_not_load_openarm_execution_dependencies() -> None: + script = ( + "import sys; " + "from typer.testing import CliRunner; " + "from dimos.teleop.openarm_mini.cli.app import app; " + "result = CliRunner().invoke(app, ['--help']); " + "assert result.exit_code == 0, result.output; " + "bad = [m for m in " + "('scservo_sdk', 'numpy', 'dimos.control', 'dimos.manipulation') " + "if m in sys.modules]; " + "assert not bad, f'Heavy imports: {bad}'" + ) + + result = subprocess.run( + [sys.executable, "-c", script], + capture_output=True, + text=True, + timeout=30, + ) + + assert result.returncode == 0, result.stderr diff --git a/dimos/teleop/openarm_mini/cli/test_joint_tui.py b/dimos/teleop/openarm_mini/cli/test_joint_tui.py new file mode 100644 index 0000000000..5f5980e140 --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/test_joint_tui.py @@ -0,0 +1,131 @@ +# 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 __future__ import annotations + +from io import StringIO +import math +from pathlib import Path +import re + +import pytest +from rich.console import Console +import typer +from typer.testing import CliRunner + +from dimos.teleop.openarm_mini.calibration import ( + FEETECH_POSITION_SPAN, + OPENARM_MINI_ARM_JOINT_NAMES, + OpenArmMiniCalibration, + OpenArmMiniMotorCalibration, + save_calibration, +) +from dimos.teleop.openarm_mini.cli.joint_tui import ( + OpenArmMiniJointRow, + _build_joint_dashboard, + _load_tui_calibration, + _read_side_rows, + _resolve_calibration_path, + main, +) + + +def _joint_tui_app() -> typer.Typer: + app = typer.Typer() + app.command()(main) + return app + + +def _calibration(side: str = "left") -> OpenArmMiniCalibration: + return OpenArmMiniCalibration( + side=side, + motors={ + joint: OpenArmMiniMotorCalibration( + id=index + 1, + homing_offset=2048, + flip=joint == "joint_1", + ) + for index, joint in enumerate(OPENARM_MINI_ARM_JOINT_NAMES) + }, + ) + + +def test_read_side_rows_displays_calibrated_and_clamped_values(tmp_path: Path) -> None: + calibration_path = tmp_path / "left" + save_calibration(calibration_path, _calibration()) + raw_positions: dict[str, int] = {joint: 2048 for joint in OPENARM_MINI_ARM_JOINT_NAMES} + raw_positions["joint_1"] = 2049 + raw_positions["joint_4"] = 0 + + rows = _read_side_rows(_load_tui_calibration("left", calibration_path), raw_positions) + + assert ( + rows[0].side, + rows[0].joint, + rows[0].follower_joint, + rows[0].raw, + rows[0].flip, + ) == ("left", "joint_1", "left_arm/joint1", 2049, True) + assert rows[0].radians == pytest.approx(-(math.tau / (FEETECH_POSITION_SPAN + 1))) + assert rows[3].clamped_radians == 2.4435 + + +def test_build_joint_dashboard_contains_title_columns_and_joint() -> None: + rows = [ + OpenArmMiniJointRow( + side="right", + joint="joint_7", + follower_joint="openarm_right_joint7", + motor_id=7, + raw=100, + radians=0.0, + clamped_radians=0.0, + flip=False, + ) + ] + console = Console(record=True, width=140, file=StringIO()) + + console.print(_build_joint_dashboard(rows)) + rendered = console.export_text() + + assert "OpenArm Mini leader joint readout" in rendered + assert "Follower Joint" in rendered + assert "openarm_right_joint7" in rendered + + +def test_resolve_calibration_path_uses_side_default( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + def fake_default_calibration_path(side: str) -> Path: + return tmp_path / side + + monkeypatch.setattr( + "dimos.teleop.openarm_mini.cli.joint_tui.default_calibration_path", + fake_default_calibration_path, + ) + + assert _resolve_calibration_path("left", None) == tmp_path / "left" + assert _resolve_calibration_path("right", None) == tmp_path / "right" + + +def test_joint_tui_cli_uses_side_and_single_port_options() -> None: + result = CliRunner().invoke(_joint_tui_app(), ["--help"]) + output = re.sub(r"\x1b\[[0-9;]*m", "", result.output) + + assert result.exit_code == 0 + assert "--side" in output + assert "--port" in output + assert "--port-left" not in output + assert "--port-right" not in output diff --git a/dimos/teleop/openarm_mini/cli/test_setup_motor_id.py b/dimos/teleop/openarm_mini/cli/test_setup_motor_id.py new file mode 100644 index 0000000000..8b8ee3e261 --- /dev/null +++ b/dimos/teleop/openarm_mini/cli/test_setup_motor_id.py @@ -0,0 +1,134 @@ +# 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 __future__ import annotations + +import pytest + +import dimos.teleop.openarm_mini.cli.setup_motor_id as setup_motor_id_module +from dimos.teleop.openarm_mini.cli.setup_motor_id import ( + FEETECH_ID_ADDRESS, + FEETECH_TORQUE_ENABLE, + FEETECH_TORQUE_ENABLE_ADDRESS, + find_single_motor_id, + setup_motor_id, + write_motor_id, +) + + +class _FakePacketHandler: + def __init__(self, responding_ids: set[int]) -> None: + self.responding_ids = responding_ids + self.calls: list[tuple[str, int, int | None, int | None]] = [] + self.fail_id_write = False + + def ping(self, scs_id: int) -> tuple[int, int, int]: + self.calls.append(("ping", scs_id, None, None)) + return (1234, 0, 0) if scs_id in self.responding_ids else (0, -1, 0) + + def write1ByteTxRx(self, scs_id: int, address: int, value: int) -> tuple[int, int]: + self.calls.append(("write1", scs_id, address, value)) + if scs_id not in self.responding_ids or ( + address == FEETECH_ID_ADDRESS and self.fail_id_write + ): + return (-1, 0) + if address == FEETECH_ID_ADDRESS: + self.responding_ids = (self.responding_ids - {scs_id}) | {value} + return (0, 0) + + def unLockEprom(self, scs_id: int) -> tuple[int, int]: + self.calls.append(("unlock", scs_id, None, None)) + return (0, 0) + + def LockEprom(self, scs_id: int) -> tuple[int, int]: + self.calls.append(("lock", scs_id, None, None)) + return (0, 0) if scs_id in self.responding_ids else (-1, 0) + + +class _FakePortHandler: + def __init__(self) -> None: + self.opened = False + self.closed = False + self.baudrate: int | None = None + + def openPort(self) -> bool: + self.opened = True + return True + + def setBaudRate(self, baudrate: int) -> bool: + self.baudrate = baudrate + return True + + def closePort(self) -> None: + self.closed = True + + +def test_write_motor_id_writes_sequence_and_verifies_new_id() -> None: + packet_handler = _FakePacketHandler({3}) + + write_motor_id(packet_handler, old_id=3, new_id=7) + + assert packet_handler.calls == [ + ("ping", 3, None, None), + ("write1", 3, FEETECH_TORQUE_ENABLE_ADDRESS, 0), + ("unlock", 3, None, None), + ("write1", 3, FEETECH_ID_ADDRESS, 7), + ("lock", 7, None, None), + ("ping", 7, None, None), + ] + assert packet_handler.responding_ids == {7} + + +def test_write_motor_id_locks_and_restores_torque_after_failure() -> None: + packet_handler = _FakePacketHandler({3}) + packet_handler.fail_id_write = True + + with pytest.raises(RuntimeError, match="write motor ID"): + write_motor_id(packet_handler, old_id=3, new_id=7) + + assert ("lock", 3, None, None) in packet_handler.calls + assert ( + "write1", + 3, + FEETECH_TORQUE_ENABLE_ADDRESS, + FEETECH_TORQUE_ENABLE, + ) in packet_handler.calls + assert packet_handler.responding_ids == {3} + + +def test_find_single_motor_id_rejects_multiple_connected_motors() -> None: + with pytest.raises(RuntimeError, match="multiple Feetech motors"): + find_single_motor_id(_FakePacketHandler({2, 4})) + + +def test_setup_motor_id_scans_and_closes_port(monkeypatch: pytest.MonkeyPatch) -> None: + packet_handler = _FakePacketHandler({5}) + port_handler = _FakePortHandler() + + def create_handlers(_port: str) -> tuple[_FakePortHandler, _FakePacketHandler]: + return port_handler, packet_handler + + monkeypatch.setattr(setup_motor_id_module, "_create_sdk_handlers", create_handlers) + + previous_id = setup_motor_id("/dev/test-feetech", baudrate=123456, new_id=9) + + assert previous_id == 5 + assert port_handler.opened and port_handler.closed + assert port_handler.baudrate == 123456 + assert packet_handler.responding_ids == {9} + + +def test_setup_motor_id_rejects_invalid_ids() -> None: + with pytest.raises(ValueError, match="new-id"): + setup_motor_id("/dev/test-feetech", baudrate=1_000_000, new_id=254, old_id=1) diff --git a/dimos/teleop/openarm_mini/feetech.py b/dimos/teleop/openarm_mini/feetech.py new file mode 100644 index 0000000000..d0745ef032 --- /dev/null +++ b/dimos/teleop/openarm_mini/feetech.py @@ -0,0 +1,170 @@ +# 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. + +"""Shared Feetech SDK helpers for OpenArm Mini leader tools and adapters.""" + +from __future__ import annotations + +from collections.abc import Mapping +import math +from typing import Any + +from dimos.teleop.openarm_mini.calibration import ( + FEETECH_POSITION_SPAN, + OPENARM_MINI_ARM_JOINT_NAMES, + OpenArmMiniCalibration, + OpenArmMiniMotorCalibration, + OpenArmMiniSide, +) + +OPENARM_MINI_TELEOP_EXTRA = "openarm-mini-teleop" +OPENARM_MINI_DEFAULT_BAUDRATE = 1_000_000 +FEETECH_COMM_SUCCESS = 0 +_FEETECH_ENCODER_TICKS = FEETECH_POSITION_SPAN + 1 + + +class OpenArmMiniDependencyError(ImportError): + """Raised when the optional Feetech SDK dependency is unavailable.""" + + +def missing_dependency_error() -> OpenArmMiniDependencyError: + """Build the localized missing dependency error for OpenArm Mini teleop.""" + return OpenArmMiniDependencyError( + "OpenArm Mini teleop requires the Feetech SDK. Install it with " + f"`uv sync --extra {OPENARM_MINI_TELEOP_EXTRA}`, or " + f"`pip install 'dimos[{OPENARM_MINI_TELEOP_EXTRA}]'`." + ) + + +def _create_sdk_handlers(port: str) -> tuple[Any, Any]: + """Create optional Feetech SDK port and packet handlers at the hardware boundary.""" + try: + from scservo_sdk import PortHandler, sms_sts # type: ignore[import-untyped] + except ImportError as exc: + raise missing_dependency_error() from exc + port_handler = PortHandler(port) + return port_handler, sms_sts(port_handler) + + +def _read_motor_position(packet_handler: Any, motor_id: int) -> int: + result = packet_handler.ReadPos(motor_id) + if isinstance(result, tuple): + values: list[Any] = list(result) + if not values: + raise RuntimeError(f"Feetech motor {motor_id} position read returned no data") + if len(values) >= 3: + comm_result = values[-2] + error = values[-1] + if comm_result != FEETECH_COMM_SUCCESS or error != 0: + raise RuntimeError( + f"Feetech motor {motor_id} position read failed with result {values!r}" + ) + position = values[0] + else: + position = result + # STS3215 firmware accumulates multi-turn ticks, so reads legitimately + # leave 0..4095 whenever a joint crosses the encoder boundary; wrap to + # one turn instead of rejecting. + return int(position) % _FEETECH_ENCODER_TICKS + + +class FeetechLeaderReader: + """Concrete reader for raw Feetech positions on one OpenArm Mini leader bus.""" + + def __init__(self, port: str, baudrate: int, *, label: str = "Feetech") -> None: + self._port = port + self._baudrate = baudrate + self._label = label + self._port_handler: Any | None = None + self._packet_handler: Any | None = None + + def connect(self) -> None: + port_handler, packet_handler = _create_sdk_handlers(self._port) + if not port_handler.openPort(): + raise RuntimeError(f"failed to open {self._label} port {self._port}") + if not port_handler.setBaudRate(self._baudrate): + port_handler.closePort() + raise RuntimeError(f"failed to set {self._label} baudrate {self._baudrate}") + self._port_handler = port_handler + self._packet_handler = packet_handler + + def disconnect(self) -> None: + if self._port_handler is None: + return + close_port = getattr(self._port_handler, "closePort", None) + if callable(close_port): + close_port() + self._port_handler = None + self._packet_handler = None + + def read_raw_positions(self, motor_ids_by_name: Mapping[str, int]) -> dict[str, int]: + if self._packet_handler is None: + raise RuntimeError(f"{self._label} reader is not connected") + return { + joint_name: _read_motor_position(self._packet_handler, motor_id) + for joint_name, motor_id in motor_ids_by_name.items() + } + + +class OpenArmMiniLeaderReader: + """Concrete calibrated reader for one OpenArm Mini leader side.""" + + def __init__( + self, + side: OpenArmMiniSide, + port: str, + calibration: OpenArmMiniCalibration, + baudrate: int, + ) -> None: + self._calibration = calibration + self._reader = FeetechLeaderReader( + port, + baudrate, + label=f"OpenArm Mini {side} Feetech", + ) + + def connect(self) -> None: + self._reader.connect() + + def disconnect(self) -> None: + self._reader.disconnect() + + def read_positions(self) -> dict[str, float]: + motor_ids_by_name = { + joint_name: self._calibration.motors[joint_name].id + for joint_name in OPENARM_MINI_ARM_JOINT_NAMES + } + raw_positions = self._reader.read_raw_positions(motor_ids_by_name) + return { + joint_name: _calibrated_motor_radians( + raw_positions[joint_name], + self._calibration.motors[joint_name], + ) + for joint_name in OPENARM_MINI_ARM_JOINT_NAMES + } + + +def _calibrated_motor_radians(raw_position: int, calibration: OpenArmMiniMotorCalibration) -> float: + centered = (raw_position - calibration.homing_offset) % _FEETECH_ENCODER_TICKS + if centered > _FEETECH_ENCODER_TICKS / 2: + centered -= _FEETECH_ENCODER_TICKS + radians = centered * math.tau / _FEETECH_ENCODER_TICKS + if calibration.flip: + radians = -radians + return radians + + +def _normalize_motor_position(raw_position: int, calibration: OpenArmMiniMotorCalibration) -> float: + """Backward-compatible helper for tests; returns calibrated radians.""" + return _calibrated_motor_radians(raw_position, calibration) diff --git a/dimos/teleop/openarm_mini/mapping.py b/dimos/teleop/openarm_mini/mapping.py new file mode 100644 index 0000000000..c80e3833fb --- /dev/null +++ b/dimos/teleop/openarm_mini/mapping.py @@ -0,0 +1,132 @@ +# 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 Mini leader to OpenArm follower joint mapping.""" + +from __future__ import annotations + +from collections.abc import Sequence +from dataclasses import dataclass + +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.robot.manipulators.openarm.config import openarm_arm_joints +from dimos.teleop.openarm_mini.calibration import OPENARM_MINI_ARM_JOINT_NAMES, OpenArmMiniSide + +LEADER_JOINT_NAMES = OPENARM_MINI_ARM_JOINT_NAMES +LEADER_MOTOR_NAMES = LEADER_JOINT_NAMES + +# Mirrors the OpenArm v2.0 limits from openarm_v20_bimanual.urdf. The +# sender-side clamp improves teleop behavior; the follower/control stack +# remains defensive. +OPENARM_FOLLOWER_JOINT_LIMITS: dict[OpenArmMiniSide, tuple[tuple[float, float], ...]] = { + "left": ( + (-3.4907, 1.3963), + (-3.3161, 0.1745), + (-1.5708, 1.5708), + (0.0, 2.4435), + (-1.5708, 1.5708), + (-0.7854, 0.7854), + (-1.5708, 1.5708), + ), + "right": ( + (-1.3963, 3.4907), + (-0.1745, 3.3161), + (-1.5708, 1.5708), + (0.0, 2.4435), + (-1.5708, 1.5708), + (-0.7854, 0.7854), + (-1.5708, 1.5708), + ), +} + + +@dataclass(frozen=True) +class OpenArmMiniSideCommand: + """Mapped command for one OpenArm follower side.""" + + side: OpenArmMiniSide + positions_by_joint: dict[str, float] + + +def map_side_readings( + side: OpenArmMiniSide, + readings: dict[str, float], + *, + target_joint_names: Sequence[str] | None = None, + previous_positions_by_joint: dict[str, float] | None = None, + max_joint_jump_radians: float | None = None, +) -> OpenArmMiniSideCommand: + """Map calibrated leader arm radians into OpenArm follower joint positions.""" + _validate_readings(readings) + + follower_joint_names = tuple(target_joint_names or openarm_arm_joints(side)) + if len(follower_joint_names) != len(LEADER_JOINT_NAMES): + raise ValueError( + f"target_joint_names must contain {len(LEADER_JOINT_NAMES)} names, " + f"got {len(follower_joint_names)}" + ) + side_limits = OPENARM_FOLLOWER_JOINT_LIMITS[side] + positions_by_joint = { + follower_joint: _clamp(readings[f"joint_{index}"], *side_limits[index - 1]) + for index, follower_joint in enumerate(follower_joint_names, start=1) + } + _validate_jump_threshold( + positions_by_joint, + previous_positions_by_joint, + max_joint_jump_radians, + ) + return OpenArmMiniSideCommand( + side=side, + positions_by_joint=positions_by_joint, + ) + + +def combine_side_commands(commands: list[OpenArmMiniSideCommand]) -> JointState: + """Combine side commands into a coordinator-facing OpenArm JointState.""" + names: list[str] = [] + positions: list[float] = [] + for command in commands: + for name, position in command.positions_by_joint.items(): + names.append(name) + positions.append(position) + return JointState({"name": names, "position": positions}) + + +def _validate_readings(readings: dict[str, float]) -> None: + missing = set(LEADER_MOTOR_NAMES) - set(readings) + if missing: + raise ValueError(f"OpenArm Mini readings missing arm joints: {sorted(missing)}") + + +def _clamp(position: float, lower: float, upper: float) -> float: + return max(lower, min(upper, position)) + + +def _validate_jump_threshold( + positions_by_joint: dict[str, float], + previous_positions_by_joint: dict[str, float] | None, + max_joint_jump_radians: float | None, +) -> None: + if previous_positions_by_joint is None or max_joint_jump_radians is None: + return + for joint_name, position in positions_by_joint.items(): + previous_position = previous_positions_by_joint.get(joint_name) + if previous_position is None: + continue + jump = abs(position - previous_position) + if jump > max_joint_jump_radians: + raise ValueError( + f"Mapped OpenArm Mini {joint_name} jump {jump:.3f} rad exceeds " + f"threshold {max_joint_jump_radians:.3f} rad" + ) diff --git a/dimos/teleop/openarm_mini/teleop_module.py b/dimos/teleop/openarm_mini/teleop_module.py new file mode 100644 index 0000000000..86e7ea224c --- /dev/null +++ b/dimos/teleop/openarm_mini/teleop_module.py @@ -0,0 +1,248 @@ +# 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 Mini teleop module.""" + +from __future__ import annotations + +from collections.abc import Mapping +from pathlib import Path +import threading +import time +from typing import Annotated, Literal, Self + +from pydantic import Field, model_validator + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import Out +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.robot.manipulators.openarm.config import openarm_arm_joints +from dimos.teleop.openarm_mini.calibration import ( + OpenArmMiniCalibrationError, + OpenArmMiniSide, + default_calibration_path, + load_calibration, +) +from dimos.teleop.openarm_mini.feetech import ( + OPENARM_MINI_DEFAULT_BAUDRATE, + OpenArmMiniDependencyError, + OpenArmMiniLeaderReader, +) +from dimos.teleop.openarm_mini.mapping import combine_side_commands, map_side_readings +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() +OPENARM_MINI_UNCONFIGURED_PORT = "" +OpenArmMiniTargetJointNames = Annotated[tuple[str, ...], Field(min_length=7, max_length=7)] + + +class OpenArmMiniTeleopModuleConfig(ModuleConfig): + """Config for OpenArm Mini leader teleoperation. + + Runtime startup is intentionally non-interactive: calibration paths point to + pre-existing side-specific calibration directories created by the package + calibration utility. + """ + + # Default to one side so running the concrete module directly only requires + # one leader calibration/port override. Dual-arm blueprints opt into both. + backend: Literal["openarm_mini"] = "openarm_mini" + tick_period_s: float = Field(default=0.02, gt=0.0) + port_left: str = OPENARM_MINI_UNCONFIGURED_PORT + port_right: str = OPENARM_MINI_UNCONFIGURED_PORT + left_calibration_path: Path | None = None + right_calibration_path: Path | None = None + baudrate: int = Field(default=OPENARM_MINI_DEFAULT_BAUDRATE, gt=0) + max_joint_jump_radians: float = 0.75 + authority_active: bool = True + enabled_sides: tuple[OpenArmMiniSide, ...] = Field(default=("left",), min_length=1) + target_joint_names_by_side: Mapping[OpenArmMiniSide, OpenArmMiniTargetJointNames] | None = None + + @model_validator(mode="after") + def _validate_openarm_mini_config(self) -> Self: + """Validate OpenArm Mini-specific configuration.""" + if len(set(self.enabled_sides)) != len(self.enabled_sides): + raise ValueError("enabled_sides must not contain duplicate sides") + return self + + def calibration_path(self, side: OpenArmMiniSide) -> Path: + """Return the configured or default calibration directory for a side.""" + if side == "left" and self.left_calibration_path is not None: + return self.left_calibration_path + if side == "right" and self.right_calibration_path is not None: + return self.right_calibration_path + return default_calibration_path(side) + + def port(self, side: OpenArmMiniSide) -> str: + """Return the configured serial port for a side.""" + port = self.port_left if side == "left" else self.port_right + if not port: + raise ValueError(f"port_{side} must be configured for OpenArm Mini teleop") + return port + + def connection_baudrate(self) -> int: + """Return the configured Feetech serial baudrate.""" + return self.baudrate + + def sides(self) -> tuple[OpenArmMiniSide, ...]: + """Return the selected leader sides in runtime order.""" + return self.enabled_sides + + def target_joint_names(self, side: OpenArmMiniSide) -> tuple[str, ...]: + """Return the follower joint names emitted for a leader side.""" + if self.target_joint_names_by_side is None: + return tuple(openarm_arm_joints(side)) + configured = self.target_joint_names_by_side.get(side) + if configured is None: + return tuple(openarm_arm_joints(side)) + return tuple(configured) + + +class OpenArmMiniTeleopModule(Module): + """Teleop module for OpenArm Mini leader devices.""" + + config: OpenArmMiniTeleopModuleConfig # type: ignore[assignment] + joint_command: Out[JointState] + + def __init__(self, **kwargs: object) -> None: + super().__init__(**kwargs) + self._buses: dict[OpenArmMiniSide, OpenArmMiniLeaderReader] = {} + self._previous_positions_by_side: dict[OpenArmMiniSide, dict[str, float]] = {} + self._last_read_error: str | None = None + self._teleop_connected = False + self._stop_event = threading.Event() + self._thread: threading.Thread | None = None + + @property + def openarm_mini_config(self) -> OpenArmMiniTeleopModuleConfig: + return self.config + + @rpc + def start(self) -> None: + if self._thread is not None and self._thread.is_alive(): + logger.warning("OpenArm Mini teleop polling worker is already running") + return + super().start() + self._stop_event.clear() + try: + self.connect_teleop() + self._thread = threading.Thread(target=self._run_loop, daemon=True) + self._thread.start() + except Exception: + self._stop_event.set() + self._thread = None + self.disconnect_teleop() + raise + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._thread is not None: + self._thread.join(DEFAULT_THREAD_JOIN_TIMEOUT) + self._thread = None + self.disconnect_teleop() + super().stop() + + def connect_teleop(self) -> None: + if self._teleop_connected: + return + openarm_mini = self.openarm_mini_config + buses: dict[OpenArmMiniSide, OpenArmMiniLeaderReader] = {} + try: + baudrate = openarm_mini.connection_baudrate() + for side in openarm_mini.sides(): + calibration = load_calibration(openarm_mini.calibration_path(side), side) + bus = OpenArmMiniLeaderReader( + side, + openarm_mini.port(side), + calibration, + baudrate, + ) + bus.connect() + buses[side] = bus + except ( + OpenArmMiniCalibrationError, + OpenArmMiniDependencyError, + ValueError, + RuntimeError, + OSError, + ): + for bus in buses.values(): + bus.disconnect() + raise + + self._buses = buses + self._teleop_connected = True + + def disconnect_teleop(self) -> None: + for bus in self._buses.values(): + bus.disconnect() + self._buses = {} + self._previous_positions_by_side = {} + self._last_read_error = None + self._teleop_connected = False + + def get_current_command(self) -> JointState | None: + openarm_mini = self.openarm_mini_config + if not self._teleop_connected or not openarm_mini.authority_active: + return None + + side_commands = [] + next_previous_positions_by_side: dict[OpenArmMiniSide, dict[str, float]] = {} + try: + for side in openarm_mini.sides(): + bus = self._buses[side] + side_command = map_side_readings( + side, + bus.read_positions(), + target_joint_names=openarm_mini.target_joint_names(side), + previous_positions_by_joint=self._previous_positions_by_side.get(side), + max_joint_jump_radians=openarm_mini.max_joint_jump_radians, + ) + side_commands.append(side_command) + next_previous_positions_by_side[side] = side_command.positions_by_joint + except (KeyError, ValueError, RuntimeError, OSError) as exc: + error_message = str(exc) + if error_message != self._last_read_error: + logger.warning( + "OpenArm Mini teleop read failed; dropping command: %s", + error_message, + ) + self._last_read_error = error_message + return None + + self._last_read_error = None + self._previous_positions_by_side = next_previous_positions_by_side + return combine_side_commands(side_commands) + + def tick(self) -> None: + """Run one synchronous OpenArm Mini polling iteration.""" + if self._stop_event.is_set(): + return + command = self.get_current_command() + if command is not None: + self.joint_command.publish(command) + + def _run_loop(self) -> None: + next_tick_time = time.monotonic() + while not self._stop_event.is_set(): + try: + self.tick() + except Exception: + logger.exception("Unexpected OpenArm Mini teleop polling worker error") + next_tick_time += self.openarm_mini_config.tick_period_s + sleep_s = max(0.0, next_tick_time - time.monotonic()) + self._stop_event.wait(sleep_s) diff --git a/dimos/teleop/openarm_mini/test_calibration.py b/dimos/teleop/openarm_mini/test_calibration.py new file mode 100644 index 0000000000..3b30a48238 --- /dev/null +++ b/dimos/teleop/openarm_mini/test_calibration.py @@ -0,0 +1,124 @@ +# 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 __future__ import annotations + +from pathlib import Path + +from pydantic import ValidationError +import pytest + +from dimos.constants import STATE_DIR +from dimos.teleop.openarm_mini.calibration import ( + OPENARM_MINI_ARM_JOINT_NAMES, + OpenArmMiniCalibration, + OpenArmMiniCalibrationError, + OpenArmMiniMotorCalibration, + default_calibration_path, + load_calibration, + save_calibration, +) +from dimos.teleop.openarm_mini.feetech import missing_dependency_error +from dimos.teleop.openarm_mini.teleop_module import OpenArmMiniTeleopModuleConfig + + +def _valid_calibration(side: str = "left") -> OpenArmMiniCalibration: + return OpenArmMiniCalibration( + side=side, + motors={ + motor_name: OpenArmMiniMotorCalibration( + id=index + 1, + homing_offset=100 + index, + flip=index % 2 == 0, + ) + for index, motor_name in enumerate(OPENARM_MINI_ARM_JOINT_NAMES) + }, + ) + + +def test_default_calibration_paths_use_dimos_state_dir() -> None: + config = OpenArmMiniTeleopModuleConfig() + + assert default_calibration_path("left") == STATE_DIR / "teleop" / "openarm_mini" / "left" + assert config.calibration_path("right") == STATE_DIR / "teleop" / "openarm_mini" / "right" + + +def test_explicit_calibration_paths_override_defaults(tmp_path: Path) -> None: + left_path = tmp_path / "left-cal" + config = OpenArmMiniTeleopModuleConfig(left_calibration_path=left_path) + + assert config.calibration_path("left") == left_path + assert config.calibration_path("right") == STATE_DIR / "teleop" / "openarm_mini" / "right" + + +def test_save_and_load_side_specific_calibration(tmp_path: Path) -> None: + calibration = _valid_calibration("right") + + artifact_path = save_calibration(tmp_path / "right", calibration) + loaded = load_calibration(tmp_path / "right", "right") + + assert artifact_path == tmp_path / "right" / "calibration.json" + assert loaded == calibration + assert set(loaded.motors) == set(OPENARM_MINI_ARM_JOINT_NAMES) + assert "gripper" not in loaded.motors + + +def test_missing_calibration_error_mentions_calibration_utility(tmp_path: Path) -> None: + with pytest.raises(OpenArmMiniCalibrationError, match="hardware openarm-mini calibrate"): + load_calibration(tmp_path / "missing", "left") + + +def test_invalid_calibration_rejects_missing_motor() -> None: + motors = _valid_calibration().motors.copy() + del motors["joint_7"] + + with pytest.raises(OpenArmMiniCalibrationError, match="missing"): + OpenArmMiniCalibration(side="left", motors=motors) + + +def test_invalid_calibration_rejects_gripper_or_legacy_fields() -> None: + motors = _valid_calibration().motors.copy() + motors["gripper"] = OpenArmMiniMotorCalibration(id=8, homing_offset=2048, flip=False) + + with pytest.raises(OpenArmMiniCalibrationError, match="extra"): + OpenArmMiniCalibration(side="left", motors=motors) + + data = _valid_calibration().model_dump(mode="json") + data["motors"]["joint_1"]["drive_mode"] = 0 # type: ignore[index] + + with pytest.raises(ValidationError, match="extra"): + OpenArmMiniCalibration.model_validate(data) + + +def test_invalid_calibration_rejects_non_bool_flip() -> None: + data = _valid_calibration().model_dump(mode="json") + data["motors"]["joint_1"]["flip"] = 0 # type: ignore[index] + + with pytest.raises(ValidationError, match="flip"): + OpenArmMiniCalibration.model_validate(data) + + +def test_invalid_calibration_rejects_side_mismatch(tmp_path: Path) -> None: + save_calibration(tmp_path / "left", _valid_calibration("right")) + + with pytest.raises(OpenArmMiniCalibrationError, match="side mismatch"): + load_calibration(tmp_path / "left", "left") + + +def test_missing_dependency_error_names_optional_extra() -> None: + error = missing_dependency_error() + + assert "openarm-mini-teleop" in str(error) + assert "Feetech" in str(error) + assert "--extra openarm`" not in str(error) diff --git a/dimos/teleop/openarm_mini/test_feetech.py b/dimos/teleop/openarm_mini/test_feetech.py new file mode 100644 index 0000000000..ce544576f6 --- /dev/null +++ b/dimos/teleop/openarm_mini/test_feetech.py @@ -0,0 +1,133 @@ +# 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 __future__ import annotations + +import builtins +import sys +from types import ModuleType + +import pytest + +from dimos.teleop.openarm_mini.calibration import FEETECH_POSITION_SPAN +from dimos.teleop.openarm_mini.feetech import ( + FeetechLeaderReader, + OpenArmMiniDependencyError, + _create_sdk_handlers, + _read_motor_position, +) + + +class _FakePortHandler: + def __init__(self, port: str) -> None: + self.port = port + self.closed = False + self.baudrate: int | None = None + + def openPort(self) -> bool: + return True + + def setBaudRate(self, baudrate: int) -> bool: + self.baudrate = baudrate + return True + + def closePort(self) -> None: + self.closed = True + + +class _FakePacketHandler: + def __init__(self, port_handler: _FakePortHandler) -> None: + self.port_handler = port_handler + + def ReadPos(self, motor_id: int) -> tuple[int, int, int]: + return (1000 + motor_id, 0, 0) + + +class _FailingPacketHandler: + def ReadPos(self, motor_id: int) -> tuple[int, int, int]: + return (1000 + motor_id, -1, 2) + + +class _PositionPacketHandler: + def __init__(self, result: int | tuple[int, int, int]) -> None: + self._result = result + + def ReadPos(self, motor_id: int) -> int | tuple[int, int, int]: + return self._result + + +def _install_fake_sdk(monkeypatch: pytest.MonkeyPatch) -> None: + sdk = ModuleType("scservo_sdk") + sdk.__dict__.update({"PortHandler": _FakePortHandler, "sms_sts": _FakePacketHandler}) + monkeypatch.setitem(sys.modules, "scservo_sdk", sdk) + + +def test_feetech_reader_uses_direct_optional_sdk_import( + monkeypatch: pytest.MonkeyPatch, +) -> None: + _install_fake_sdk(monkeypatch) + reader = FeetechLeaderReader("/dev/fake", 123456) + + reader.connect() + try: + raw_positions = reader.read_raw_positions({"joint_1": 1, "joint_2": 7}) + finally: + reader.disconnect() + + assert raw_positions == {"joint_1": 1001, "joint_2": 1007} + + +def test_create_sdk_handlers_raises_openarm_mini_dependency_error( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.delitem(sys.modules, "scservo_sdk", raising=False) + + real_import = builtins.__import__ + + def fake_import( + name: str, + globals: dict[str, object] | None = None, + locals: dict[str, object] | None = None, + fromlist: tuple[str, ...] = (), + level: int = 0, + ) -> object: + if name == "scservo_sdk": + raise ImportError("missing scservo_sdk") + return real_import(name, globals, locals, fromlist, level) + + monkeypatch.setattr(builtins, "__import__", fake_import) + + with pytest.raises(OpenArmMiniDependencyError): + _create_sdk_handlers("/dev/missing") + + +def test_read_motor_position_rejects_sdk_error_tuple() -> None: + with pytest.raises(RuntimeError, match="position read failed"): + _read_motor_position(_FailingPacketHandler(), 3) + + +@pytest.mark.parametrize( + ("result", "expected"), + [ + (-1, FEETECH_POSITION_SPAN), + (FEETECH_POSITION_SPAN + 82, 81), + ((-82, 0, 0), FEETECH_POSITION_SPAN - 81), + ((FEETECH_POSITION_SPAN + 1, 0, 0), 0), + ], +) +def test_read_motor_position_wraps_multi_turn_encoder_ticks( + result: int | tuple[int, int, int], + expected: int, +) -> None: + assert _read_motor_position(_PositionPacketHandler(result), 3) == expected diff --git a/dimos/teleop/openarm_mini/test_mapping.py b/dimos/teleop/openarm_mini/test_mapping.py new file mode 100644 index 0000000000..20b7857ccc --- /dev/null +++ b/dimos/teleop/openarm_mini/test_mapping.py @@ -0,0 +1,108 @@ +# 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 __future__ import annotations + +import pytest + +from dimos.teleop.openarm_mini.mapping import ( + combine_side_commands, + map_side_readings, +) + + +def _readings() -> dict[str, float]: + return { + "joint_1": 0.1, + "joint_2": 0.2, + "joint_3": 0.3, + "joint_4": 0.4, + "joint_5": 0.5, + "joint_6": 0.6, + "joint_7": 0.7, + } + + +def test_mapping_uses_direct_arm_joint_assignment_and_follower_names() -> None: + command = map_side_readings("left", _readings()) + + assert list(command.positions_by_joint) == [f"left_arm/joint{i}" for i in range(1, 8)] + assert command.positions_by_joint["left_arm/joint1"] == pytest.approx(0.1) + assert command.positions_by_joint["left_arm/joint6"] == pytest.approx(0.6) + assert command.positions_by_joint["left_arm/joint7"] == pytest.approx(0.7) + assert not hasattr(command, "gripper_position") + + +def test_combined_command_uses_openarm_follower_joint_names() -> None: + left = map_side_readings("left", _readings()) + right = map_side_readings("right", _readings()) + + joint_state = combine_side_commands([left, right]) + + assert joint_state.name == [ + *[f"left_arm/joint{i}" for i in range(1, 8)], + *[f"right_arm/joint{i}" for i in range(1, 8)], + ] + assert len(joint_state.position) == 14 + + +def test_mapping_can_emit_configured_target_joint_names() -> None: + target_names = [f"right_arm/openarm_right_joint{i}" for i in range(1, 8)] + + command = map_side_readings("right", _readings(), target_joint_names=target_names) + + assert list(command.positions_by_joint) == target_names + assert command.positions_by_joint["right_arm/openarm_right_joint1"] == pytest.approx(0.1) + assert not hasattr(command, "gripper_position") + + +def test_follower_joint_limits_clamp_sender_side() -> None: + readings = _readings() + readings["joint_1"] = 5.0 + readings["joint_4"] = -1.0 + + left = map_side_readings("left", readings) + right = map_side_readings("right", readings) + + assert left.positions_by_joint["left_arm/joint1"] == pytest.approx(1.3963) + assert right.positions_by_joint["right_arm/joint1"] == pytest.approx(3.4907) + assert left.positions_by_joint["left_arm/joint4"] == pytest.approx(0.0) + + +def test_jump_threshold_rejects_large_leader_discontinuity_after_clamp() -> None: + previous = map_side_readings("left", _readings()).positions_by_joint + readings = _readings() + readings["joint_2"] = -1.0 + + with pytest.raises(ValueError, match="exceeds"): + map_side_readings( + "left", + readings, + previous_positions_by_joint=previous, + max_joint_jump_radians=0.5, + ) + + +def test_missing_leader_arm_joint_reading_is_rejected() -> None: + readings = _readings() + del readings["joint_4"] + + with pytest.raises(ValueError, match="missing"): + map_side_readings("left", readings) + + +def test_gripper_reading_is_not_required() -> None: + command = map_side_readings("right", _readings()) + + assert list(command.positions_by_joint) == [f"right_arm/joint{i}" for i in range(1, 8)] diff --git a/dimos/teleop/openarm_mini/test_teleop_module.py b/dimos/teleop/openarm_mini/test_teleop_module.py new file mode 100644 index 0000000000..e1744399f0 --- /dev/null +++ b/dimos/teleop/openarm_mini/test_teleop_module.py @@ -0,0 +1,482 @@ +# 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 __future__ import annotations + +from collections.abc import Iterator, Mapping +from contextlib import contextmanager +import math +from pathlib import Path +import threading +from typing import Any + +import pytest + +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.teleop.openarm_mini import teleop_module +from dimos.teleop.openarm_mini.calibration import ( + FEETECH_POSITION_SPAN, + OPENARM_MINI_ARM_JOINT_NAMES, + OpenArmMiniCalibration, + OpenArmMiniMotorCalibration, + OpenArmMiniSide, + save_calibration, +) +from dimos.teleop.openarm_mini.feetech import ( + _calibrated_motor_radians, + _normalize_motor_position, +) +from dimos.teleop.openarm_mini.teleop_module import ( + OpenArmMiniTeleopModule, + OpenArmMiniTeleopModuleConfig, +) + + +class _FakeBus: + def __init__(self, readings: dict[str, float]) -> None: + self.readings = readings + self.connected = False + self.disconnected = False + + def connect(self) -> None: + self.connected = True + + def disconnect(self) -> None: + self.disconnected = True + + def read_positions(self) -> dict[str, float]: + return self.readings + + +class _FailingBus: + def __init__(self, exc: Exception | None = None) -> None: + self._exc = exc if exc is not None else ValueError("read failure") + + def connect(self) -> None: + pass + + def disconnect(self) -> None: + pass + + def read_positions(self) -> dict[str, float]: + raise self._exc + + +def _payload(command: JointState | None) -> JointState: + assert command is not None + return command + + +def _calibration(side: OpenArmMiniSide) -> OpenArmMiniCalibration: + return OpenArmMiniCalibration( + side=side, + motors={ + motor_name: OpenArmMiniMotorCalibration( + id=index + 1, + homing_offset=0, + flip=False, + ) + for index, motor_name in enumerate(OPENARM_MINI_ARM_JOINT_NAMES) + }, + ) + + +def _write_calibrations(tmp_path: Path) -> tuple[Path, Path]: + left_path = tmp_path / "left" + right_path = tmp_path / "right" + save_calibration(left_path, _calibration("left")) + save_calibration(right_path, _calibration("right")) + return left_path, right_path + + +def _configured_config( + left_path: Path, + right_path: Path, + **kwargs: Any, +) -> OpenArmMiniTeleopModuleConfig: + return OpenArmMiniTeleopModuleConfig( + port_left="left-port", + port_right="right-port", + left_calibration_path=left_path, + right_calibration_path=right_path, + baudrate=123, + **kwargs, + ) + + +def _readings() -> dict[str, float]: + return { + "joint_1": 1.0, + "joint_2": 2.0, + "joint_3": 3.0, + "joint_4": 4.0, + "joint_5": 5.0, + "joint_6": 0.6, + "joint_7": 0.7, + } + + +def _patch_buses( + monkeypatch: pytest.MonkeyPatch, + buses: Mapping[str, _FakeBus | _FailingBus], +) -> list[tuple[str, str, str, int]]: + created: list[tuple[str, str, str, int]] = [] + + def factory( + side: str, + port: str, + calibration: OpenArmMiniCalibration, + baudrate: int, + ) -> _FakeBus | _FailingBus: + created.append((side, port, calibration.side, baudrate)) + return buses[side] + + monkeypatch.setattr(teleop_module, "OpenArmMiniLeaderReader", factory) + return created + + +def _module(config: OpenArmMiniTeleopModuleConfig) -> OpenArmMiniTeleopModule: + return OpenArmMiniTeleopModule(**config.model_dump()) + + +@contextmanager +def _connected_module( + config: OpenArmMiniTeleopModuleConfig, +) -> Iterator[OpenArmMiniTeleopModule]: + module = _module(config) + try: + module.connect_teleop() + yield module + finally: + module.stop() + + +def test_teleop_module_loads_calibration_connects_both_buses_and_returns_joint_command( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + buses = {"left": _FakeBus(_readings()), "right": _FakeBus(_readings())} + created = _patch_buses(monkeypatch, buses) + + with _connected_module( + OpenArmMiniTeleopModuleConfig( + port_left="left-port", + port_right="right-port", + left_calibration_path=left_path, + right_calibration_path=right_path, + baudrate=123, + enabled_sides=("left", "right"), + ) + ) as module: + command = module.get_current_command() + + joint = _payload(command) + assert joint.name == [ + *[f"left_arm/joint{i}" for i in range(1, 8)], + *[f"right_arm/joint{i}" for i in range(1, 8)], + ] + assert created == [("left", "left-port", "left", 123), ("right", "right-port", "right", 123)] + assert buses["left"].connected + assert buses["right"].connected + assert buses["left"].disconnected + assert buses["right"].disconnected + + +def test_teleop_module_left_only_connects_left_bus_and_emits_left_joints( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + left_bus = _FakeBus(_readings()) + created = _patch_buses(monkeypatch, {"left": left_bus}) + + with _connected_module( + _configured_config(left_path, right_path, enabled_sides=("left",)) + ) as module: + command = module.get_current_command() + + assert created == [("left", "left-port", "left", 123)] + joint = _payload(command) + assert joint.name == [f"left_arm/joint{i}" for i in range(1, 8)] + assert left_bus.connected + assert left_bus.disconnected + + +def test_config_rejects_invalid_or_duplicate_enabled_sides() -> None: + with pytest.raises(ValueError, match="at least 1"): + OpenArmMiniTeleopModuleConfig(enabled_sides=()) + with pytest.raises(ValueError, match="Input should be 'left' or 'right'"): + OpenArmMiniTeleopModuleConfig.model_validate({"enabled_sides": ("center",)}) + with pytest.raises(ValueError, match="duplicate"): + OpenArmMiniTeleopModuleConfig(enabled_sides=("left", "left")) + + +def test_config_rejects_non_positive_tick_period() -> None: + with pytest.raises(ValueError, match="greater than 0"): + OpenArmMiniTeleopModuleConfig(tick_period_s=0.0) + with pytest.raises(ValueError, match="greater than 0"): + OpenArmMiniTeleopModuleConfig(tick_period_s=-0.1) + + +def test_config_resolves_default_and_configured_target_joint_names() -> None: + right_target_names = tuple(f"right_arm/openarm_right_joint{i}" for i in range(1, 8)) + config = OpenArmMiniTeleopModuleConfig(target_joint_names_by_side={"right": right_target_names}) + + assert config.target_joint_names("left") == tuple(f"left_arm/joint{i}" for i in range(1, 8)) + assert config.target_joint_names("right") == right_target_names + + +def test_config_rejects_wrong_target_joint_name_count() -> None: + with pytest.raises(ValueError, match="at least 7"): + OpenArmMiniTeleopModuleConfig(target_joint_names_by_side={"right": ("only_one",)}) + + +def test_teleop_module_returns_none_without_authority( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + buses = {"left": _FakeBus(_readings()), "right": _FakeBus(_readings())} + _patch_buses(monkeypatch, buses) + + with _connected_module( + _configured_config(left_path, right_path, authority_active=False) + ) as module: + command = module.get_current_command() + + assert command is None + + +def test_teleop_module_emits_configured_global_target_joint_names( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + right_bus = _FakeBus(_readings()) + target_names = tuple(f"right_arm/openarm_right_joint{i}" for i in range(1, 8)) + created = _patch_buses(monkeypatch, {"right": right_bus}) + + with _connected_module( + _configured_config( + left_path, + right_path, + enabled_sides=("right",), + target_joint_names_by_side={"right": target_names}, + ) + ) as module: + command = module.get_current_command() + + joint = _payload(command) + assert joint.name == list(target_names) + assert created == [("right", "right-port", "right", 123)] + + +def test_teleop_module_rejects_jump_threshold_by_returning_no_command( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + left_bus = _FakeBus(_readings()) + right_bus = _FakeBus(_readings()) + buses = {"left": left_bus, "right": right_bus} + _patch_buses(monkeypatch, buses) + + with _connected_module( + _configured_config(left_path, right_path, max_joint_jump_radians=0.1) + ) as module: + first = module.get_current_command() + left_bus.readings = {**_readings(), "joint_2": -1.0} + second = module.get_current_command() + + assert first is not None + assert second is None + + +def test_calibrated_motor_radians_uses_zero_offset_full_encoder_span_and_flip() -> None: + calibration = OpenArmMiniMotorCalibration( + id=1, + homing_offset=2048, + flip=True, + ) + + assert _calibrated_motor_radians(2200, calibration) == pytest.approx( + -(2200 - 2048) * math.tau / (FEETECH_POSITION_SPAN + 1) + ) + assert _normalize_motor_position(2200, calibration) == pytest.approx( + _calibrated_motor_radians(2200, calibration) + ) + + +def test_teleop_module_clamps_over_limit_sender_side( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + buses = {"left": _FakeBus({**_readings(), "joint_1": 5.0}), "right": _FakeBus(_readings())} + _patch_buses(monkeypatch, buses) + + with _connected_module(_configured_config(left_path, right_path)) as module: + command = module.get_current_command() + + joint = _payload(command) + assert joint.position[0] == pytest.approx(1.3963) + + +def test_calibration_can_assign_semantic_joint_to_nondefault_motor_id() -> None: + calibration = OpenArmMiniMotorCalibration( + id=42, + homing_offset=1000, + flip=False, + ) + + assert calibration.id == 42 + assert _calibrated_motor_radians(1001, calibration) == pytest.approx( + math.tau / (FEETECH_POSITION_SPAN + 1) + ) + + +def test_calibrated_motor_radians_wraps_short_way_across_encoder_boundary() -> None: + calibration = OpenArmMiniMotorCalibration(id=1, homing_offset=4090, flip=False) + + assert _calibrated_motor_radians(3, calibration) == pytest.approx( + 9 * math.tau / (FEETECH_POSITION_SPAN + 1) + ) + + +def test_teleop_module_returns_none_when_bus_reports_invalid_reading( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + _patch_buses(monkeypatch, {"left": _FailingBus(), "right": _FakeBus(_readings())}) + + with _connected_module(_configured_config(left_path, right_path)) as module: + command = module.get_current_command() + + assert command is None + + +def test_teleop_module_returns_none_when_bus_read_raises_runtime_error( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + _patch_buses( + monkeypatch, + { + "left": _FailingBus(RuntimeError("Feetech motor read failed")), + "right": _FakeBus(_readings()), + }, + ) + + with _connected_module(_configured_config(left_path, right_path)) as module: + command = module.get_current_command() + + assert command is None + + +def test_tick_publishes_direct_joint_command( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + mocker: Any, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + _patch_buses(monkeypatch, {"left": _FakeBus(_readings())}) + + with _connected_module(_configured_config(left_path, right_path)) as module: + publish = mocker.patch.object(module.joint_command, "publish") + module.tick() + + published = publish.call_args.args[0] + assert isinstance(published, JointState) + assert published.name == [f"left_arm/joint{i}" for i in range(1, 8)] + + +def test_tick_suppresses_failed_read_and_recovers( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + mocker: Any, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + left_bus = _FakeBus(_readings()) + _patch_buses(monkeypatch, {"left": left_bus}) + + with _connected_module(_configured_config(left_path, right_path)) as module: + publish = mocker.patch.object(module.joint_command, "publish") + mocker.patch.object( + left_bus, "read_positions", side_effect=[RuntimeError("read"), _readings()] + ) + + module.tick() + module.tick() + + publish.assert_called_once() + + +def test_start_is_idempotent_and_stop_cleans_worker_and_bus( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + mocker: Any, +) -> None: + left_path, right_path = _write_calibrations(tmp_path) + left_bus = _FakeBus(_readings()) + _patch_buses(monkeypatch, {"left": left_bus}) + module = _module(_configured_config(left_path, right_path, tick_period_s=10.0)) + starts: list[threading.Thread] = [] + original_start = threading.Thread.start + mocker.patch.object(module, "tick") + + def record_start(thread: threading.Thread) -> None: + starts.append(thread) + original_start(thread) + + monkeypatch.setattr(threading.Thread, "start", record_start) + + try: + module.start() + module.start() + assert len(starts) == 1 + assert left_bus.connected + + module.stop() + + assert module._thread is None + assert left_bus.disconnected + finally: + module.stop() + + +def test_polling_loop_logs_unexpected_exceptions_without_tight_loop( + mocker: Any, +) -> None: + module = OpenArmMiniTeleopModule(tick_period_s=0.01) + try: + waits: list[float] = [] + mocker.patch.object(module, "tick", side_effect=RuntimeError("boom")) + logged = mocker.patch.object(teleop_module.logger, "exception") + + def wait_once(timeout: float) -> bool: + waits.append(timeout) + module._stop_event.set() + return True + + mocker.patch.object(module._stop_event, "wait", side_effect=wait_once) + + module._run_loop() + + logged.assert_called_once() + assert waits == [pytest.approx(0.01, abs=0.01)] + finally: + module.stop() diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index d6dc3a403f..7aaadd3c24 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -19,11 +19,26 @@ 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 dimos run keyboard-teleop-xarm7 # XArm7 7-DOF ``` +OpenYAM is exposed as one whole-body device with six angular arm joints and a +normalized gripper joint. `arm/gripper` uses `0.0` for fully closed and `1.0` +for fully open; it does not use meters. Hardware activation calibrates both +mechanical endpoints, so clear the gripper jaws and workspace before startup. +The gripper has no default startup target and moves only after joint control has +an explicit target. + +OpenArm follows the same whole-body model with both arms and both grippers in +one device: fourteen angular joints (`left_arm/joint1..7`, +`right_arm/joint1..7`) plus two normalized gripper joints (`left_arm/gripper`, +`right_arm/gripper`). The keyboard jogs the left arm while the right arm holds +its pose; keyboard gripper bindings are a follow-up. + Open the Meshcat URL printed in the terminal (default `http://localhost:7000`) to see the robot. Keyboard controls: @@ -193,6 +208,15 @@ uv run dimos run xarm7-planner-coordinator \ --visualization.backend=viser ``` +Viser binds to `127.0.0.1` by default. To expose it on the network, opt in +explicitly with the nested host override: + +```bash +uv run dimos run xarm7-planner-coordinator \ + -o manipulationmodule.visualization.backend=viser \ + -o manipulationmodule.visualization.host=0.0.0.0 +``` + Blueprint example: ```python skip diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 4dd5b587dd..ed8b1d07f1 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -1,387 +1,84 @@ --- title: "OpenArm Integration" --- -Guide for running the **OpenArm** — an open-source bimanual 7-DOF research arm built from Damiao DM-J quasi-direct-drive motors — under the dimos manipulation + control stack. -**If you're standing in front of the hardware and just want to run it, skip to [Quick start](#quick-start).** +DimOS drives the [OpenArm](https://openarm.dev) bimanual platform (two 7-DOF +arms + grippers, Damiao motors, one CAN bus per arm) as a single whole-body +device through the generic Damiao adapter stack introduced for OpenYAM. Related: - Upstream hardware + C++ reference: [enactic/openarm_can](https://github.com/enactic/openarm_can) - How to integrate any new arm: [adding_a_custom_arm.md](/docs/capabilities/manipulation/adding_a_custom_arm.md) ---- - -## Why this integration is different - -Every other arm in dimos wraps a vendor Python SDK: - -| Arm | Transport | Python SDK | -|---|---|---| -| xArm | TCP/IP | `xarm-python-sdk` | -| Piper | CAN (via SDK) | `piper_sdk` | -| R1 Pro | Galaxea | Galaxea SDK | -| Go2 / G1 | WebRTC | Unitree SDK | -| Panda | FCI | `panda-py` | - -**OpenArm ships no Python SDK.** The only interface is raw CAN frames on the wire, speaking the Damiao MIT-mode protocol. So dimos includes a from-scratch driver that encodes/decodes the protocol directly on a SocketCAN bus. The reference implementation is the Enactic C++ library at [enactic/openarm_can](https://github.com/enactic/openarm_can) — we port the frame layout from there. - ## Architecture ``` -ManipulationModule → ControlCoordinator → OpenArmAdapter → OpenArmBus → SocketCAN → arm - (Drake plan) (100Hz tick loop) (dimos protocol) (CAN driver) +ControlCoordinator (100 Hz) + └── HardwareComponent "openarm" (WHOLE_BODY, 16 joints) + └── OpenArmDamiaoAdapter # dimos/hardware/whole_body/openarm_damiao/ + └── DamiaoWholeBodyAdapter # generic Damiao lifecycle + gravity comp + └── can-motor-control # Rust CAN transport + Damiao codec (PyPI) ``` -Code layout: +One adapter owns both arms: bus `left` (default `can1`) and bus `right` +(default `can0`) are commanded together in one synchronized tick per control +cycle. The command vector order is `left_arm/joint1..7`, `right_arm/joint1..7`, +`left_arm/gripper`, `right_arm/gripper`; gripper joints are normalized +(`0.0` closed, `1.0` open). -``` -dimos/hardware/manipulators/openarm/ -├── driver.py # OpenArmBus, DamiaoMotor — pure CAN driver, no dimos deps -├── adapter.py # OpenArmAdapter — implements dimos ManipulatorAdapter protocol -├── test_driver.py # 13 unit tests (virtual CAN loopback, no hardware) -└── test_adapter.py # 11 unit tests (virtual CAN + mock state frames) +Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): +2x DM8009, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. -dimos/robot/manipulators/openarm/ -├── blueprints.py # coordinator-*, planner-*, keyboard-teleop-* blueprints and model config -└── scripts/ # bring-up + diagnostic scripts (run manually by humans) - ├── openarm_can_up.sh # bring SocketCAN interfaces up (needs sudo) - ├── openarm_can_probe.py # enumerate & read state from all 8 motors - ├── openarm_set_mit_mode.py # one-time CTRL_MODE=MIT write per motor - └── ... (diagnostics) +Gravity compensation uses the bimanual URDF +(`openarm_description/urdf/robot/openarm_v20_bimanual.urdf`, resolved lazily +from LFS at connect time) and is preflighted against the declared joint order +before the motors enable. -data/openarm_description/ # URDF + meshes (in-tree; may migrate to LFS) -└── urdf/robot/ - ├── openarm_v10_bimanual.urdf # both arms (14 DOF, used by coordinator) - ├── openarm_v10_left.urdf # left arm + torso (7 DOF, per-side planning) - ├── openarm_v10_right.urdf # right arm + torso (7 DOF) - └── openarm_v10_single.urdf # standalone arm (Pinocchio FK for teleop) -``` +Planning also uses the bimanual URDF: one robot model with a +`left_manipulator` and a `right_manipulator` planning group, since collision +exclusions cannot span robots. -Workspace analysis is generic and lives in [dimos/utils/workspace.py](/dimos/utils/workspace.py) — works for any URDF, not just OpenArm. - ---- - -## Quick start - -You need: - -- 2× **OpenArm v10** arms, wired to USB-CAN adapters -- 2× **USB-CAN adapters** (we used gs_usb family, VID:PID `1d50:606f`, e.g. CANable 2.0). Classical CAN @ 1 Mbit is enough; CAN-FD not required -- **Python 3.12 venv with dimos installed** plus `python-can >= 4.3` and `pinocchio` -- **sudo** on first run (to bring up the CAN interfaces) - -### 1. Bring up the CAN buses +## Bring-up ```bash -sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 can1 +dimos hardware can setup can0 +dimos hardware can setup can1 +dimos run keyboard-teleop-openarm ``` -This sets both interfaces to classical CAN @ 1 Mbit with a 1000-frame TX queue (enough headroom for the 100 Hz tick loop). If only one bus is present, pass just that one: `sudo ... openarm_can_up.sh can0`. - -**Troubleshooting:** -- `Operation not permitted` → you forgot `sudo`. -- `Operation not supported` on `fd on` → your adapter doesn't support CAN-FD. The script defaults to classical, so this shouldn't happen unless you set `MODE=fd`. -- Only one `can*` interface appears → the other adapter isn't enumerating. On gs_usb boards, the **blue LED** indicates USB enumeration. If one adapter only shows red/green, swap the USB cable (many USB-C cables are charge-only). +Linux assigns `can0`/`can1` in USB enumeration order. If the arms come up +swapped, override the mapping through +`DamiaoRuntimeConfig(bus_addresses={"left": ..., "right": ...})` rather than +editing the adapter topology. -### 2. Verify all 16 motors are alive +## Blueprints -```bash -python ./dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can0 -python ./dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can1 -``` - -Expected: `8/8 motors replied` on each bus, with plausible joint positions and rotor temps around 25–30 °C. - -### 3. (First time only) Put motors in MIT mode - -Damiao motors have a persistent `CTRL_MODE` register. They ship in POS_VEL mode by default, which means they will reply to enable/state queries but **silently ignore** any MIT control frames — the "motor doesn't move, error grows" failure. The adapter writes MIT on every `connect()` by default, so this step is usually automatic. If you want to set it explicitly once: - -```bash -python ./dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 -python ./dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can1 -``` - -The register is persistent across power cycles, so you only need this once per motor (or after a firmware reset). - -### 4. Run a blueprint - -| Blueprint | What it does | +| Blueprint | Contents | |---|---| -| `coordinator-openarm-mock` | Bimanual, mock adapters. No hardware. | -| `openarm-mock-planner-coordinator` | Drake planner + bimanual mock, Meshcat viz. Great smoke test. | -| `coordinator-openarm-left` / `coordinator-openarm-right` | Single arm, real hardware on can0 / can1. | -| `coordinator-openarm-bimanual` | Both arms, real hardware, no planner. | -| `openarm-planner-coordinator` | **Main usable blueprint** — Drake planner + both arms on real hardware. | -| `keyboard-teleop-openarm-mock` / `keyboard-teleop-openarm` | Single-arm Cartesian IK + pygame keyboard, mock / real. | - -**Safety before hot-plugging hardware:** hold the arms before starting. On connect, the adapter enables all motors and sends gravity-comp holds — the arms go slightly stiff but don't leap. Ctrl-C to cleanly disable and exit. - -First-time recommendation: mock planner to verify everything wires up, then real single-arm, then bimanual. - -```bash -# smoke test (no hardware) -dimos run openarm-mock-planner-coordinator - -# single-arm bring-up (hold the arm physically first) -dimos run coordinator-openarm-left - -# full bimanual with planner -dimos run openarm-planner-coordinator -``` +| `coordinator-openarm` | coordinator + trajectory task over both arms | +| `openarm-planner-coordinator` | planner (bimanual model) + coordinator | +| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + viser | +| `keyboard-teleop-openarm-planner` | teleop + planner + preempting trajectory task | -Meshcat will appear at http://localhost:7000. +All blueprints run against the in-memory whole-body adapter under +`--simulation`; the physical adapter is selected automatically otherwise. -### 5. Drive the arms from the manipulation client +The keyboard jogs the left arm (`eef_twist_left_arm`); the right arm's twist +task holds its anchor pose. Keyboard gripper bindings for the two grippers are +a follow-up; the gripper joints accept normalized `/joint_command` targets in +the meantime. -With `openarm-planner-coordinator` running in one terminal, open a second terminal and start the REPL client: +## Files -```bash -python -i -m dimos.manipulation.planning.examples.manipulation_client -``` - -This gives you an interactive Python prompt with these functions: - -| Function | Purpose | +| Path | Role | |---|---| -| `robots()` | List configured robots (here: `["left_arm", "right_arm"]`) | -| `joints(robot_name)` | Read current joint positions (7 floats) | -| `ee(robot_name)` | Read current end-effector pose | -| `state()` | Module state: `IDLE`, `PLANNING`, `EXECUTING`, `FAULT`, etc. | -| `plan([q1..q7], robot_name)` | Plan a collision-free trajectory to a joint configuration | -| `plan_pose(x, y, z, robot_name=...)` | Plan to a Cartesian EE pose (preserves current orientation) | -| `preview(robot_name)` | Animate the planned path in Meshcat without executing | -| `execute()` | Send the complete planned trajectory to the coordinator | -| `home(robot_name)` | Plan + execute to home joints | -| `commands()` | Print all available functions | - -#### Example session — simple joint moves - -```python skip ->>> robots() -['left_arm', 'right_arm'] - ->>> joints(robot_name="left_arm") -[0.02, -0.01, -0.13, 0.15, 0.17, -0.07, 0.10] - ->>> # One-liner: plan → preview in Meshcat → execute on hardware ->>> plan([0.3, 0, 0, 0, 0, 0, 0], robot_name="left_arm") and preview(robot_name="left_arm") and execute() -True - ->>> joints(robot_name="left_arm") -[0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00] # arm is now at the commanded pose -``` - -`plan()` returns `True` on success, `False` if planning failed (check the coordinator terminal for `COLLISION_AT_GOAL`, `INVALID_START`, `NO_SOLUTION`, etc). The `and` chaining is an idiom — if any step fails, the next one is short-circuited. - -If you ever get stuck in a `FAULT` state (e.g. an invalid plan was sent), reset the state machine: - -```python skip ->>> _client.reset() -'Reset to IDLE — ready for new commands' -``` - -#### Example session — bimanual - -```python skip ->>> # Move both arms to mirrored poses ->>> plan([0.5, 0, 0, 0, 0, 0, 0], robot_name="left_arm") and execute() -True ->>> plan([-0.5, 0, 0, 0, 0, 0, 0], robot_name="right_arm") and execute() -True -``` - -Each arm plans and executes independently — the coordinator runs both trajectories simultaneously on separate tick-loop tasks. - -#### Example session — Cartesian target - -```python skip ->>> ee(robot_name="left_arm") # see where the EE currently is ->>> plan_pose(0.1, 0.3, 0.5, robot_name="left_arm") and preview(robot_name="left_arm") -True ->>> execute() -True -``` - -If you don't know which Cartesian targets are reachable, check first with the workspace tool — see [Workspace analysis](#workspace-analysis) below. `plan_pose` will fail with `NO_SOLUTION` if the IK can't find a configuration reaching the target. - -#### Adding obstacles - -```python skip ->>> add_box("table", 0.4, 0.0, 0.1, w=0.6, h=0.4, d=0.05) # rectangular obstacle ->>> add_sphere("ball", 0.3, 0.2, 0.4, radius=0.05) ->>> plan_pose(0.4, 0.0, 0.3, robot_name="left_arm") # now plans around it ->>> remove("table") # id returned by add_* -``` - ---- - -## Configuration - -### Which CAN bus is which arm - -Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants in [config.py](/dimos/robot/manipulators/openarm/config.py): - -```python -LEFT_CAN = "can0" -RIGHT_CAN = "can1" -``` - -No other code changes are needed. +| `dimos/hardware/whole_body/openarm_damiao/adapter.py` | physical topology (motors, buses, gravity URDF) | +| `dimos/robot/manipulators/openarm/config.py` | joints, gains, hardware + planning model configs | +| `dimos/robot/manipulators/openarm/blueprints/` | coordinator/planner/teleop blueprints | -### Gain tuning (MIT kp/kd) - -Defaults live in [adapter.py](/dimos/hardware/manipulators/openarm/adapter.py). Gains are per-joint because the shoulder motors (DM8006, 40 Nm) tolerate higher kp than the wrist motors (DM4310, 10 Nm): - -```python -_DEFAULT_KP = [100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0] -_DEFAULT_KD = [1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8] -``` - -Guidelines: -- `kp ∈ [0, 500]` in MIT mode. Higher kp = stiffer position tracking; too high → oscillation. -- `kd ∈ [0, 5]`. Higher kd = more damping, but values above ~2 on these gearboxes cause high-frequency buzz/grinding. -- Gravity compensation is on by default (`gravity_comp=True`) — the adapter uses Pinocchio to compute `G(q)` and adds it as feedforward torque. This removes the need for very high kp to fight gravity, so prefer low kp + gravity comp over high kp. - -### Physical joint limits - -The URDFs use the xacro-generated limits (which include per-side offsets for mirroring). The adapter's `get_limits()` reports the same per-side limits. If you measure tighter physical limits and want to enforce them, edit the URDFs directly — the planner will respect them. - -### Disabling auto MIT-mode write - -The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [config.py](/dimos/robot/manipulators/openarm/config.py) and restart — the arms should still respond. - ---- - -## Motor mapping (OpenArm v10) - -Derived from the URDF's `joint_limits.yaml` (effort column) cross-checked against the Damiao torque tables. Both arms are identical. - -| Send ID | Recv ID | Joint | Motor | vMax [rad/s] | tMax [Nm] | -|---|---|---|---|---|---| -| 0x01 | 0x11 | joint1 | DM8006 | 45 | 40 | -| 0x02 | 0x12 | joint2 | DM8006 | 45 | 40 | -| 0x03 | 0x13 | joint3 | DM4340 | 8 | 28 | -| 0x04 | 0x14 | joint4 | DM4340 | 8 | 28 | -| 0x05 | 0x15 | joint5 | DM4310 | 30 | 10 | -| 0x06 | 0x16 | joint6 | DM4310 | 30 | 10 | -| 0x07 | 0x17 | joint7 | DM4310 | 30 | 10 | -| 0x08 | 0x18 | gripper | DM4310 | 30 | 10 | - -Convention: `recv_id = send_id | 0x10`. - ---- - -## Damiao protocol essentials - -Ported from `enactic/openarm_can/src/openarm/damiao_motor/dm_motor_control.cpp`. You shouldn't need these unless you're modifying the driver. - -### Enable / disable / zero-position - -Send to the motor's send_id. 8-byte payload: - -``` -[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, CMD] - where CMD = 0xFC (enable) | 0xFD (disable) | 0xFE (zero current pose) -``` - -### MIT control frame (8 bytes) - -Bit layout: `q[16] | dq[12] | kp[12] | kd[12] | tau[12]`. Each float quantized via: - -```python -def float_to_uint(x, lo, hi, bits): - x = clamp(x, lo, hi) - return round((x - lo) / (hi - lo) * ((1 << bits) - 1)) -``` - -Gain ranges: `kp ∈ [0, 500]`, `kd ∈ [0, 5]`. Position/velocity/torque ranges come from the motor-type table above. - -Byte layout: -``` -byte0 = q_u >> 8 -byte1 = q_u & 0xFF -byte2 = dq_u >> 4 -byte3 = ((dq_u & 0xF) << 4) | ((kp_u >> 8) & 0xF) -byte4 = kp_u & 0xFF -byte5 = kd_u >> 4 -byte6 = ((kd_u & 0xF) << 4) | ((tau_u >> 8) & 0xF) -byte7 = tau_u & 0xFF -``` - -### State reply (8 bytes, on recv_id) - -Same `q | dq | tau` layout + 2 temperature bytes: - -``` -byte0 = motor_id_echo -byte1..5 = q | dq | tau (same packing as above) -byte6 = t_mos (°C) -byte7 = t_rotor (°C) -``` - -### CTRL_MODE register write - -Broadcast frame on CAN ID `0x7FF`: - -``` -data = [send_id_lo, send_id_hi, 0x55, RID=10, val[0..3]] - where val = 1 (MIT) | 2 (POS_VEL) | 3 (VEL) | 4 (POS_FORCE), little-endian uint32 -``` - -Persistent across power cycles. - ---- - -## Known gotchas - -- **`ip link ... fd on` → `Operation not supported`.** gs_usb firmware doesn't support CAN-FD. Use classical CAN @ 1 Mbit (our bringup script's default). -- **Motors reply to probes but commands do nothing.** CTRL_MODE is not MIT. The adapter now writes MIT on connect, but if you disabled that and motors got reset, run `openarm_set_mit_mode.py`. -- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm config module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. -- **`INVALID_START` during planning.** Hardware encoder noise pushed a joint 1 mrad past a URDF limit. Joint4 used to be exactly `lower=0.0` which tripped this — it's now `-0.01` to give breathing room. If you see it on a different joint, widen that limit by ~10 mrad. -- **"Transmit buffer full" (ENOBUFS) at 100 Hz.** Kernel TX queue too small. The bringup script sets `txqueuelen 1000`; the driver also retries on ENOBUFS. If you still see the error, check `ip -details link show canX | grep qlen`. -- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [config.py](/dimos/robot/manipulators/openarm/config.py). - ---- - -## Design decisions - -- **Driver separate from adapter.** `driver.py` has zero dimos deps → unit-testable with a virtual CAN bus, reusable outside dimos. -- **MIT mode for everything.** MIT can emulate position (high kp), velocity (kp=0, nonzero kd+dq), and torque (kp=kd=0, nonzero tau). One code path. -- **Gravity compensation on by default.** Eliminates steady-state position error without needing high kp. Needs Pinocchio + the per-side URDFs. -- **One adapter per CAN bus, keyed by `address`.** Matches the Piper adapter pattern. Bimanual = two adapters with different `address` values. -- **Per-side URDFs for Drake planning.** Loading the full 14-DOF bimanual URDF twice (once per robot instance) creates phantom-arm collisions with the "other" arm frozen at zero. The per-side URDFs keep only one arm's links + the torso, avoiding the phantom collisions while matching the bimanual kinematics exactly. -- **URDF stays in-tree (`data/openarm_description/`) for now.** Can migrate to LFS later — only the path constants in the OpenArm blueprint module change. -- **CAN bringup stays manual (`sudo`).** Auto-bringup from `connect()` would need sudo-in-a-library or a systemd unit; the explicit script is clearer and testable. For production, add a oneshot systemd unit that runs the script at boot. - ---- - -## Workspace analysis - -For figuring out which targets are reachable before planning, use the generic workspace tool: - -```bash -# Visualize the left arm's reachable workspace as a point cloud -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf - -# Check if a specific target is reachable -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf query 0.1 0.3 0.5 - -# Get a list of reachable poses near a target, ranked by manipulability -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf suggest 0.1 0.3 0.5 - -# Interactive: visualize + type targets to query -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf interactive -``` - -Points are colored by Yoshikawa manipulability index: green = dexterous, red = near singularity. Avoid planning targets in the red regions. - ---- - -## Testing +## Validation ```bash -# Unit tests (no hardware, use virtual CAN) -.venv/bin/python -m pytest dimos/hardware/manipulators/openarm/ -v +uv run pytest dimos/hardware/whole_body/openarm_damiao \ + dimos/hardware/test_adapter_registries.py ``` - -Expected: 24 passed (13 driver + 11 adapter). All tests use `can.Bus(interface="virtual")` loopback — no real hardware needed. diff --git a/docs/capabilities/manipulation/piper_integration.md b/docs/capabilities/manipulation/piper_integration.md index 4df8656ba3..2a2483380f 100644 --- a/docs/capabilities/manipulation/piper_integration.md +++ b/docs/capabilities/manipulation/piper_integration.md @@ -21,20 +21,20 @@ Piper uses SocketCAN at 1,000,000 bit/s. For the default vendor setup, use the DimOS CLI to configure an existing CAN interface and bring it up: ```bash -dimos piper can-activate can0 +dimos hardware can setup can0 ``` For a non-default bitrate, pass `--bitrate` explicitly: ```bash -dimos piper can-activate can0 --bitrate 500000 +dimos hardware can setup can0 --bitrate 500000 ``` -The command asks for confirmation before requesting sudo. Verify the interface -before starting a blueprint: +The command prints each privileged operation before requesting sudo. Verify the +interface before starting a blueprint: ```bash -ip link show can0 +dimos hardware can status can0 ``` ## Run a Piper blueprint diff --git a/pyproject.toml b/pyproject.toml index 6441ea3202..8b2482b347 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -280,11 +280,14 @@ manipulation = [ "xarm-python-sdk>=1.17.0", "a750-control; sys_platform == 'linux' and platform_machine == 'x86_64'", + "can-motor-control>=0.0.5; sys_platform == 'linux'", + # Mesh conversion (STL/DAE → OBJ for Drake collision geometry) "trimesh", "pycollada", # Visualization (Optional) + "pygame>=2.6.1", "viser[urdf]>=1.0.29", "yourdfpy>=0.0.60", "xacro", @@ -295,6 +298,11 @@ manipulation = [ "roboplan>=0.5.1", ] +openarm-mini-teleop = [ + "ftservo-python-sdk", + "rich", +] + cpu = [ # CPU inference backends "onnxruntime", @@ -395,6 +403,8 @@ project-deps = [ "lap>=0.5.12", "langchain-openai>=1,<2", "ollama>=0.6.0", + # The generic Damiao whole-body adapter imports upstream types directly. + "can-motor-control>=0.0.5; sys_platform == 'linux'", ] tests = [ @@ -489,7 +499,7 @@ tests-self-hosted = [ required-version = ">=0.9.17" default-groups = ["tests"] exclude-newer = "7 days" -exclude-newer-package = { dimos-viewer = false, pyrealsense2-extended = false, dimos-lcm = false, lcm-dimos-fork = false, roboplan = false, md-babel-py = false } +exclude-newer-package = { dimos-viewer = false, pyrealsense2-extended = false, dimos-lcm = false, lcm-dimos-fork = false, roboplan = false, can-motor-control = false, md-babel-py = false } override-dependencies = [ # ultralytics, unitree-sdk2py-dimos and unitree-webrtc-connect depend on # opencv-python, which ships the same cv2/ tree as our opencv-contrib-python diff --git a/uv.lock b/uv.lock index 799831a286..96b20cb994 100644 --- a/uv.lock +++ b/uv.lock @@ -30,6 +30,7 @@ exclude-newer = "0001-01-01T00:00:00Z" # This has no effect and is included for exclude-newer-span = "P7D" [options.exclude-newer-package] +can-motor-control = false dimos-viewer = false dimos-lcm = false lcm-dimos-fork = false @@ -596,6 +597,19 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/0d/fe/6bea5c9162869c5beba5d9c8abbed835ec85bf1ec1fba05a3822325c45f3/build-1.5.0-py3-none-any.whl", hash = "sha256:13f3eecb844759ab66efec90ca17639bbf14dc06cb2fdf37a9010322d9c50a6f", size = 26018, upload-time = "2026-04-30T03:18:23.644Z" }, ] +[[package]] +name = "can-motor-control" +version = "0.0.5" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64' and sys_platform != 'darwin' and sys_platform != 'win32') or (python_full_version < '3.11' and sys_platform == 'linux')" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform != 'darwin' and sys_platform != 'win32') or (python_full_version >= '3.11' and sys_platform == 'linux')" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/89/df/4f57da2ecf0022b58e69c84ce49eae8e3c00863f18cefd73298c973917f8/can_motor_control-0.0.5.tar.gz", hash = "sha256:96914bcff093c9f90aca799fc9c83627ed67d48db6a55223e04aca756b0bfea5", size = 128944, upload-time = "2026-08-01T23:04:52.496Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/97/15/6a70b6296b52777d1688e493c53b8e505a5569075f8ec4b3f6b0fd07fe67/can_motor_control-0.0.5-cp310-abi3-manylinux_2_34_x86_64.whl", hash = "sha256:e423f5d3eb5749dd20e3cb510a3f3661865842b02b64066ca4dce50a66db11ca", size = 528991, upload-time = "2026-08-01T23:04:50.992Z" }, +] + [[package]] name = "cattrs" version = "25.3.0" @@ -1602,6 +1616,7 @@ all = [ { name = "aiohttp" }, { name = "aioquic" }, { name = "aiortc" }, + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "coacd" }, { name = "cupy-cuda12x", marker = "platform_machine == 'x86_64'" }, @@ -1723,12 +1738,14 @@ learning = [ ] manipulation = [ { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "drake", version = "1.45.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, { name = "drake", version = "1.49.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, { name = "matplotlib" }, { name = "pin-pink" }, { name = "piper-sdk" }, { name = "pycollada" }, + { name = "pygame" }, { name = "pyrealsense2-extended", marker = "sys_platform != 'darwin'" }, { name = "pyyaml" }, { name = "qpsolvers", extra = ["proxqp"] }, @@ -1755,6 +1772,10 @@ misc = [ { name = "torchreid" }, { name = "xarm-python-sdk" }, ] +openarm-mini-teleop = [ + { name = "ftservo-python-sdk" }, + { name = "rich" }, +] perception = [ { name = "chromadb" }, { name = "einops" }, @@ -1871,6 +1892,7 @@ browser-tests = [ ] lint = [ { name = "aiortc" }, + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "dimos", extra = ["visualization", "web", "webrtc"] }, { name = "einops" }, @@ -1910,6 +1932,7 @@ lint = [ { name = "xacro" }, ] project-deps = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "dimos", extra = ["visualization", "web", "webrtc"] }, { name = "einops" }, @@ -1932,6 +1955,7 @@ project-deps = [ { name = "xacro" }, ] tests = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "coacd" }, { name = "coverage" }, @@ -1979,6 +2003,7 @@ tests = [ { name = "xacro" }, ] tests-self-hosted = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "coacd" }, { name = "coverage" }, @@ -2037,6 +2062,7 @@ requires-dist = [ { name = "annotation-protocol", specifier = ">=1.4.0" }, { name = "attrs", specifier = ">=25.4.0" }, { name = "bleak", specifier = ">=3.0.2" }, + { name = "can-motor-control", marker = "sys_platform == 'linux' and extra == 'manipulation'", specifier = ">=0.0.5" }, { name = "chromadb", marker = "extra == 'perception'", specifier = ">=1.0.0" }, { name = "cmeel-tinyxml2", specifier = ">=11,<12" }, { name = "coacd", marker = "extra == 'scene'", specifier = ">=1.0.0" }, @@ -2060,6 +2086,7 @@ requires-dist = [ { name = "faster-whisper", marker = "extra == 'agents'", specifier = ">=1.0.0" }, { name = "ffmpeg-python", marker = "extra == 'web'" }, { name = "filelock", specifier = ">=3.16,<4" }, + { name = "ftservo-python-sdk", marker = "extra == 'openarm-mini-teleop'" }, { name = "gdown", marker = "extra == 'misc'", specifier = ">=5.2.2" }, { name = "googlemaps", marker = "extra == 'misc'", specifier = ">=4.10.0" }, { name = "gtsam-extended", marker = "extra == 'mapping'", specifier = ">=4.3a1.post1" }, @@ -2110,6 +2137,7 @@ requires-dist = [ { name = "pycollada", marker = "extra == 'manipulation'" }, { name = "pydantic" }, { name = "pydantic-settings", specifier = ">=2.11.0,<3" }, + { name = "pygame", marker = "extra == 'manipulation'", specifier = ">=2.6.1" }, { name = "pygame", marker = "extra == 'sim'", specifier = ">=2.6.1" }, { name = "pymavlink", marker = "extra == 'drone'" }, { name = "pyrealsense2-extended", marker = "sys_platform != 'darwin' and extra == 'manipulation'" }, @@ -2122,6 +2150,7 @@ requires-dist = [ { name = "reportlab", marker = "extra == 'apriltag'", specifier = ">=4.5.0" }, { name = "rerun-sdk", specifier = "==0.32.0" }, { name = "rerun-sdk", marker = "extra == 'visualization'", specifier = "==0.32.0" }, + { name = "rich", marker = "extra == 'openarm-mini-teleop'" }, { name = "roboplan", marker = "extra == 'manipulation'", specifier = ">=0.5.1" }, { name = "scipy", specifier = ">=1.15.1" }, { name = "sortedcontainers", specifier = "==2.4.0" }, @@ -2156,13 +2185,14 @@ requires-dist = [ { name = "yourdfpy", marker = "(platform_machine != 'aarch64' and extra == 'visualization') or (sys_platform != 'linux' and extra == 'visualization')", specifier = ">=0.0.60" }, { name = "yourdfpy", marker = "extra == 'manipulation'", specifier = ">=0.0.60" }, ] -provides-extras = ["misc", "visualization", "learning", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "cpu", "cuda", "sim", "mapping", "drone", "dds", "webrtc", "base", "apriltag", "scene", "all"] +provides-extras = ["misc", "visualization", "learning", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "openarm-mini-teleop", "cpu", "cuda", "sim", "mapping", "drone", "dds", "webrtc", "base", "apriltag", "scene", "all"] [package.metadata.requires-dev] autofix = [{ name = "ruff", specifier = "==0.14.3" }] browser-tests = [{ name = "playwright", specifier = ">=1.55" }] lint = [ { name = "aiortc", specifier = ">=1.14.0" }, + { name = "can-motor-control", marker = "sys_platform == 'linux'", specifier = ">=0.0.5" }, { name = "chromadb", specifier = ">=1.0.0" }, { name = "dimos", extras = ["web", "visualization", "webrtc"] }, { name = "einops", specifier = ">=0.8.1" }, @@ -2201,6 +2231,7 @@ lint = [ { name = "xacro" }, ] project-deps = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'", specifier = ">=0.0.5" }, { name = "chromadb", specifier = ">=1.0.0" }, { name = "dimos", extras = ["web", "visualization", "webrtc"] }, { name = "einops", specifier = ">=0.8.1" }, @@ -2223,6 +2254,7 @@ project-deps = [ { name = "xacro" }, ] tests = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'", specifier = ">=0.0.5" }, { name = "chromadb", specifier = ">=1.0.0" }, { name = "coacd", specifier = ">=1.0.0" }, { name = "coverage", specifier = ">=7.0" }, @@ -2271,6 +2303,7 @@ tests = [ { name = "xacro" }, ] tests-self-hosted = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'", specifier = ">=0.0.5" }, { name = "chromadb", specifier = ">=1.0.0" }, { name = "coacd", specifier = ">=1.0.0" }, { name = "coverage", specifier = ">=7.0" }, @@ -2995,6 +3028,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/ab/6e/81d47999aebc1b155f81eca4477a616a70f238a2549848c38983f3c22a82/ftfy-6.3.1-py3-none-any.whl", hash = "sha256:7c70eb532015cd2f9adb53f101fb6c7945988d023a085d127d1573dc49dd0083", size = 44821, upload-time = "2024-10-26T00:50:33.425Z" }, ] +[[package]] +name = "ftservo-python-sdk" +version = "2.0.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "pyserial" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/fe/da/d3f07d88136d338d075df06eaee4e4ada90da03f37dbdea63d339b3341d0/ftservo_python_sdk-2.0.0.tar.gz", hash = "sha256:4ffad15e4d31ecd386fe941abcb8bb23b9f50f2825e5681786c5170c34dc9a5f", size = 13354, upload-time = "2025-04-17T12:31:07.401Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/a2/a5/a7dad40ae9ae48180c7c595b4af62df63fcf0ce83c097bf4307f4ebfaa16/ftservo_python_sdk-2.0.0-py3-none-any.whl", hash = "sha256:c8303df01b2c772f3e1dffbb3b789e2d39545f6fb187ec45032c7737356be2a4", size = 12172, upload-time = "2025-04-17T12:31:05.78Z" }, +] + [[package]] name = "future" version = "1.0.0" @@ -7265,6 +7310,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/a6/9b/f81c8009a3bf8cd2b1d1ce74321c6f8bdb7d7075895fb04800f3795b431d/pyrealsense2_extended-2.58.1.10581.post1-cp312-cp312-win_amd64.whl", hash = "sha256:76ddf1dadd4dd8c542d4249d50dc4507962808f9ae3b6e807f317f319abeead3", size = 8754299, upload-time = "2026-05-31T20:50:09.02Z" }, ] +[[package]] +name = "pyserial" +version = "3.5" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/1e/7d/ae3f0a63f41e4d2f6cb66a5b57197850f919f59e558159a4dd3a818f5082/pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb", size = 159125, upload-time = "2020-11-23T03:59:15.045Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/07/bc/587a445451b253b285629263eb51c2d8e9bcea4fc97826266d186f96f558/pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0", size = 90585, upload-time = "2020-11-23T03:59:13.41Z" }, +] + [[package]] name = "pysocks" version = "1.7.1"