From 7e058e8d25e8bfbef2c5238edec72a4dc42dcc28 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 4 Jun 2026 13:41:32 -0700 Subject: [PATCH 01/44] spec: openspec init --- docs/coding-agents/index.md | 1 + docs/development/openspec.md | 102 ++++++++++++++ docs/docs.json | 1 + openspec/config.yaml | 45 ++++++ openspec/schemas/dimos-capability/schema.yaml | 128 ++++++++++++++++++ .../dimos-capability/templates/design.md | 35 +++++ .../dimos-capability/templates/docs.md | 19 +++ .../dimos-capability/templates/proposal.md | 32 +++++ .../dimos-capability/templates/spec.md | 16 +++ .../dimos-capability/templates/tasks.md | 15 ++ 10 files changed, 394 insertions(+) create mode 100644 docs/development/openspec.md create mode 100644 openspec/config.yaml create mode 100644 openspec/schemas/dimos-capability/schema.yaml create mode 100644 openspec/schemas/dimos-capability/templates/design.md create mode 100644 openspec/schemas/dimos-capability/templates/docs.md create mode 100644 openspec/schemas/dimos-capability/templates/proposal.md create mode 100644 openspec/schemas/dimos-capability/templates/spec.md create mode 100644 openspec/schemas/dimos-capability/templates/tasks.md diff --git a/docs/coding-agents/index.md b/docs/coding-agents/index.md index f66717d99d..5af0803fb2 100644 --- a/docs/coding-agents/index.md +++ b/docs/coding-agents/index.md @@ -6,6 +6,7 @@ title: "For Agents" ├── style.md (code style guidelines for dimos) ├── code-quality-rules.md (code-quality rules agents scan/fix against) ├── testing.md (docs about writing tests) +├── ../development/openspec.md (OpenSpec behavior-spec workflow) ├── docs (these are docs about writing docs) │   ├── codeblocks.md │   ├── doclinks.md diff --git a/docs/development/openspec.md b/docs/development/openspec.md new file mode 100644 index 0000000000..280eb0f57e --- /dev/null +++ b/docs/development/openspec.md @@ -0,0 +1,102 @@ +# OpenSpec Workflow + +DimOS uses OpenSpec as the checked-in planning layer for behavior changes. OpenSpec artifacts live under `openspec/` and should describe what the system is supposed to do, why it is changing, and how contributors or agents should validate the work. + +## Terminology + +Keep these two meanings separate: + +- **OpenSpec capability spec**: Markdown requirements under `openspec/specs//spec.md`. These describe observable behavior and acceptance scenarios. +- **DimOS Spec**: Python Protocol/RPC contracts in files like `dimos/navigation/navigation_spec.py` or `dimos/manipulation/control/arm_driver_spec.py`. These describe module interfaces for code wiring. + +Use "OpenSpec capability spec" in prose when there is any chance of confusion. + +## Schema + +The project uses the `dimos-capability` schema configured in `openspec/config.yaml`. + +The artifact flow is: + +```text +proposal + ├── specs + ├── design + └── docs + └── tasks +``` + +| Artifact | Purpose | +|---|---| +| `proposal.md` | Intent, scope, affected DimOS surfaces, and capability impact. | +| `specs//spec.md` | Behavior-first requirements and scenarios. | +| `design.md` | Module, stream, blueprint, skill/MCP, safety, and rollout decisions. | +| `docs.md` | Documentation impact and doc validation plan. | +| `tasks.md` | Implementation, docs, verification, and manual QA checklist. | + +## When to create a change + +Create an OpenSpec change when work changes observable behavior, public CLI/API/MCP behavior, robot behavior, hardware/simulation/replay workflows, docs that users rely on, or cross-module architecture. + +Do not create a change for a purely mechanical refactor, typo fix, or internal cleanup unless it changes behavior or needs cross-session planning context. + +## Writing specs + +OpenSpec capability specs are behavior contracts, not implementation plans. + +Good spec content: + +- User- or developer-visible behavior. +- Public CLI/API/MCP tool behavior. +- Stream or message behavior that downstream modules rely on. +- Robot safety constraints and hardware/simulation/replay expectations. +- Scenarios that can be tested or manually verified. + +Avoid in specs: + +- Private class/function names. +- Generated-file mechanics. +- Library choices and wiring details. +- Step-by-step implementation tasks. + +Put those details in `design.md` or `tasks.md`. + +## Capability names + +Prefer behavior-domain names over code names. Useful starting points: + +- `module-system` +- `blueprint-composition` +- `cli-lifecycle` +- `agent-skills-mcp` +- `configuration` +- `navigation-stack` +- `manipulation-stack` +- `hardware-adapters` +- `simulation-replay` +- `documentation-system` + +Add specs progressively as changes need them. Do not try to backfill the whole project at once. + +## Validation + +Use OpenSpec validation before implementation and before archiving: + +```bash skip +openspec schema validate dimos-capability +openspec validate +openspec templates --json +``` + +For documentation changes, also run the relevant doc checks from [Writing Docs](/docs/development/writing_docs.md): + +```bash skip +md-babel-py run +``` + +When a change touches blueprint names, module-level blueprint variables, or module registry inputs, run: + +```bash skip +pytest dimos/robot/test_all_blueprints_generation.py +``` + +Then run focused tests for the changed code and manually QA through the actual surface: CLI command, MCP tool, HTTP API, simulation/replay blueprint, hardware procedure, or library driver. diff --git a/docs/docs.json b/docs/docs.json index 7074b3e52a..0792b07ea3 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -162,6 +162,7 @@ "group": "Development", "pages": [ "development/conventions", + "development/openspec", "development/testing", "development/docker", "development/grid_testing", diff --git a/openspec/config.yaml b/openspec/config.yaml new file mode 100644 index 0000000000..62a72bba63 --- /dev/null +++ b/openspec/config.yaml @@ -0,0 +1,45 @@ +schema: dimos-capability + +context: | + DimOS is a robotics operating system for generalist robots. Modules communicate + through typed streams (`In[T]`, `Out[T]`) over LCM, SHM, ROS, DDS, or other + transports. Blueprints compose modules into runnable robot stacks. Skills are + `@skill`-annotated RPC methods exposed to agents and MCP clients. + + Terminology boundary: + - "OpenSpec spec" means a behavior specification under `openspec/specs/`. + - "DimOS Spec" means a Python Protocol/RPC contract in `*_spec.py` files, + usually inheriting `dimos.spec.utils.Spec` and `typing.Protocol`. + Keep these separate. OpenSpec specs describe observable behavior; DimOS Specs + describe code-level module interfaces. + + OpenSpec specs should capture current behavior, user/developer-visible + outcomes, public CLI/API/tool surfaces, robot safety constraints, and testable + scenarios. Put implementation choices, class names, module wiring, generated + registry updates, and rollout details in `design.md` or `tasks.md`. + + Documentation lives in: + - `docs/usage/` for user-facing concepts and APIs. + - `docs/capabilities/` for capability and platform guides. + - `docs/development/` for contributor process. + - `docs/coding-agents/` and `AGENTS.md` for coding-agent guidance. + +rules: + proposal: + - "Identify affected DimOS surfaces: modules, streams, blueprints, CLI, skills/MCP, docs, hardware, simulation, replay, or generated registries." + - Use capability names that match behavior domains, not Python class names. + - Mark hardware safety or public API/CLI changes explicitly. + specs: + - Write behavior-first requirements; avoid implementation detail unless it is externally observable. + - Every requirement must include at least one `#### Scenario:` block with concrete observable outcomes. + - Use "OpenSpec capability spec" when prose might otherwise be confused with DimOS Python `Spec` Protocols. + design: + - Call out DimOS `Spec` Protocols, adapter Protocols, blueprint composition, stream names/types, and skill/MCP exposure when relevant. + - Mention generated files and required regeneration commands, especially `pytest dimos/robot/test_all_blueprints_generation.py` for blueprint registry changes. + - Include hardware/simulation/replay assumptions and safety constraints for robot-facing work. + docs: + - List user-facing docs, contributor docs, coding-agent docs, and AGENTS.md updates required by the change. + - Include documentation validation commands for changed docs, such as `doclinks` and `md-babel-py run ` where applicable. + tasks: + - Include verification tasks for OpenSpec validation, relevant pytest targets, type checks when needed, and manual QA through the user-facing surface. + - Add registry generation tasks when blueprint names, module classes, or generated registry inputs change. diff --git a/openspec/schemas/dimos-capability/schema.yaml b/openspec/schemas/dimos-capability/schema.yaml new file mode 100644 index 0000000000..fedb7964ee --- /dev/null +++ b/openspec/schemas/dimos-capability/schema.yaml @@ -0,0 +1,128 @@ +name: dimos-capability +version: 1 +description: DimOS capability workflow - proposal → specs/design/docs → tasks +artifacts: + - id: proposal + generates: proposal.md + description: DimOS change proposal covering intent, scope, capability impact, and affected robot/software surfaces + template: proposal.md + instruction: | + Create the proposal document that establishes WHY this change is needed and what DimOS behavior it affects. + + Sections: + - **Why**: 1-2 concise paragraphs on the problem or opportunity. Explain why the change matters now. + - **What Changes**: Bullet list of added, modified, or removed behavior. Mark public API/CLI or hardware-safety breaking changes with **BREAKING**. + - **Affected DimOS Surfaces**: Identify modules, streams, blueprints, CLI commands, skills/MCP tools, docs, hardware, simulation, replay, generated registries, or external protocols touched by the change. + - **Capabilities**: Identify which OpenSpec capability specs will be created or modified: + - **New Capabilities**: List behavior domains introduced by the change. Each becomes `specs//spec.md`. Use kebab-case names (for example, `agent-skills-mcp`, `blueprint-composition`, `manipulation-stack`). + - **Modified Capabilities**: List existing `openspec/specs//` entries whose requirements change. Only include spec-level behavior changes, not implementation-only refactors. + - **Impact**: Summarize user/developer impact, compatibility risks, dependency changes, documentation updates, and test/QA scope. + + Keep proposals concise. Do not include line-by-line implementation details; put architecture and rollout decisions in `design.md`. + requires: [] + - id: specs + generates: specs/**/*.md + description: Behavior-first OpenSpec capability delta specifications + template: spec.md + instruction: | + Create OpenSpec capability specs that define WHAT DimOS should do, not how it is implemented. + + Create one delta spec file per capability listed in proposal.md: + - New capabilities: use `specs//spec.md` with the exact kebab-case name from the proposal. + - Modified capabilities: use the existing folder from `openspec/specs//`. + + Use these delta sections as `##` headers: + - **ADDED Requirements**: New externally observable behavior. + - **MODIFIED Requirements**: Changed behavior. Include the full updated requirement block, not a partial patch. + - **REMOVED Requirements**: Deprecated behavior. Include **Reason** and **Migration**. + - **RENAMED Requirements**: Name-only changes. Use FROM:/TO: format. + + Requirement format: + - Use `### Requirement: `. + - Use SHALL/MUST for normative requirements. + - Include at least one `#### Scenario: ` per requirement. Scenario headings MUST use exactly four `#` characters. + - Prefer `- **GIVEN**`, `- **WHEN**`, `- **THEN**`, and `- **AND**` bullets. + - Cover happy path plus meaningful edge/error/safety cases. + + DimOS-specific guidance: + - Specify user/developer-visible behavior, robot outcomes, CLI behavior, skill/MCP tool behavior, stream contracts, safety constraints, and compatibility expectations. + - Avoid Python class names, private module internals, transport implementation choices, and generated-file details unless those details are observable API contracts. + - Use "OpenSpec capability spec" in prose when needed to avoid confusion with DimOS Python `Spec` Protocols. + - If the behavior only changes implementation and not observable requirements, do not create a spec delta. + requires: + - proposal + - id: design + generates: design.md + description: DimOS technical design and architecture decisions + template: design.md + instruction: | + Create the design document that explains HOW the change should be implemented in DimOS. + + Include design.md for cross-module changes, new robot/hardware integration, new public interfaces, new dependencies, safety-sensitive behavior, generated registry changes, or unclear architecture. + + Sections: + - **Context**: Current state, relevant modules/blueprints/docs, and constraints. + - **Goals / Non-Goals**: What the design achieves and explicitly excludes. + - **DimOS Architecture**: Modules, streams, transports, blueprints, RPC/module refs, DimOS `Spec` Protocols, adapter Protocols, skills/MCP exposure, CLI entry points, and generated registries involved. + - **Decisions**: Key choices with rationale and alternatives considered. + - **Safety / Simulation / Replay**: Hardware assumptions, sim/replay behavior, safety constraints, and manual QA surface. + - **Risks / Trade-offs**: Known risks and mitigations. + - **Migration / Rollout**: Compatibility, generated files, docs, and deployment steps. + - **Open Questions**: Outstanding decisions or unknowns. + + Reference proposal.md for intent and specs for behavior. Keep line-by-line work in tasks.md. + requires: + - proposal + - id: docs + generates: docs.md + description: Documentation impact plan for user, contributor, and coding-agent docs + template: docs.md + instruction: | + Create the documentation impact plan for the change. + + Sections: + - **User-Facing Docs**: Updates under `docs/usage/`, `docs/capabilities/`, `docs/platforms/`, or README files. + - **Contributor Docs**: Updates under `docs/development/`. + - **Coding-Agent Docs**: Updates under `docs/coding-agents/` or `AGENTS.md`. + - **Doc Validation**: Commands needed for changed docs, such as `doclinks`, `md-babel-py run `, and `bin/gen-diagrams`. + - **No Docs Needed**: If no docs are needed, explain why. + + Match `docs/development/writing_docs.md`: contributor-only docs belong in `docs/development`; user-facing behavior belongs in `docs/usage` or `docs/capabilities`. + requires: + - proposal + - id: tasks + generates: tasks.md + description: Implementation, validation, docs, and manual-QA checklist + template: tasks.md + instruction: | + Create the implementation checklist. The apply phase parses checkbox format, so every actionable task MUST use `- [ ]`. + + Guidelines: + - Group tasks under numbered `##` headings. + - Each task must be `- [ ] X.Y Task description`. + - Keep tasks small enough to complete in one focused session. + - Order tasks by dependency. + - Include docs and validation tasks from docs.md. + - Include generated registry tasks when blueprints or module registry inputs change. + - Include manual QA through the actual user surface: CLI, TUI, HTTP API, MCP tool, simulation/replay blueprint, hardware procedure, or library driver. + + Typical DimOS validation tasks: + - Run `openspec validate `. + - Run focused pytest targets for changed modules. + - Run `pytest dimos/robot/test_all_blueprints_generation.py` when blueprint registry output may change. + - Run docs validation commands for changed docs. + - Run lints/types when the touched area requires them. + + Reference specs for WHAT, design for HOW, and docs.md for documentation work. + requires: + - specs + - design + - docs +apply: + requires: + - tasks + tracks: tasks.md + instruction: | + Read proposal.md, specs, design.md, docs.md, and tasks.md before editing code. + Work through pending tasks, mark checkboxes complete as they finish, and keep artifacts current when implementation changes the plan. + Verify with OpenSpec validation, focused tests, docs checks, and manual QA through the relevant DimOS surface. diff --git a/openspec/schemas/dimos-capability/templates/design.md b/openspec/schemas/dimos-capability/templates/design.md new file mode 100644 index 0000000000..25031ceb8b --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/design.md @@ -0,0 +1,35 @@ +## Context + + + +## Goals / Non-Goals + +**Goals:** + + +**Non-Goals:** + + +## DimOS Architecture + + + +## Decisions + + + +## Safety / Simulation / Replay + + + +## Risks / Trade-offs + + + +## Migration / Rollout + + + +## Open Questions + + diff --git a/openspec/schemas/dimos-capability/templates/docs.md b/openspec/schemas/dimos-capability/templates/docs.md new file mode 100644 index 0000000000..d274aed653 --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/docs.md @@ -0,0 +1,19 @@ +## User-Facing Docs + + + +## Contributor Docs + + + +## Coding-Agent Docs + + + +## Doc Validation + + + +## No Docs Needed + + diff --git a/openspec/schemas/dimos-capability/templates/proposal.md b/openspec/schemas/dimos-capability/templates/proposal.md new file mode 100644 index 0000000000..98d409e8de --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/proposal.md @@ -0,0 +1,32 @@ +## Why + + + +## What Changes + + + +## Affected DimOS Surfaces + + +- Modules/streams: +- Blueprints/CLI: +- Skills/MCP: +- Hardware/simulation/replay: +- Docs/generated registries: + +## Capabilities + +### New Capabilities + +- ``: + +### Modified Capabilities + +- ``: + +## Impact + + diff --git a/openspec/schemas/dimos-capability/templates/spec.md b/openspec/schemas/dimos-capability/templates/spec.md new file mode 100644 index 0000000000..afc0c1ff58 --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/spec.md @@ -0,0 +1,16 @@ +## ADDED Requirements + +### Requirement: + + +#### Scenario: +- **GIVEN** +- **WHEN** +- **THEN** +- **AND** + + diff --git a/openspec/schemas/dimos-capability/templates/tasks.md b/openspec/schemas/dimos-capability/templates/tasks.md new file mode 100644 index 0000000000..b38fcdfabb --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/tasks.md @@ -0,0 +1,15 @@ +## 1. Implementation + +- [ ] 1.1 +- [ ] 1.2 + +## 2. Documentation + +- [ ] 2.1 + +## 3. Verification + +- [ ] 3.1 Run `openspec validate ` +- [ ] 3.2 Run focused tests for changed code +- [ ] 3.3 Run docs validation commands for changed docs +- [ ] 3.4 Manually QA through the relevant DimOS surface (CLI, MCP, simulation/replay, hardware procedure, HTTP API, or library driver) From 5e442a6e1c2781a883159fed05e1ff89dd3fd060 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 8 Jun 2026 16:20:39 -0700 Subject: [PATCH 02/44] chore: revert change to doc folder --- docs/coding-agents/index.md | 1 - docs/development/openspec.md | 102 ----------------------------------- docs/docs.json | 1 - 3 files changed, 104 deletions(-) delete mode 100644 docs/development/openspec.md diff --git a/docs/coding-agents/index.md b/docs/coding-agents/index.md index 5af0803fb2..f66717d99d 100644 --- a/docs/coding-agents/index.md +++ b/docs/coding-agents/index.md @@ -6,7 +6,6 @@ title: "For Agents" ├── style.md (code style guidelines for dimos) ├── code-quality-rules.md (code-quality rules agents scan/fix against) ├── testing.md (docs about writing tests) -├── ../development/openspec.md (OpenSpec behavior-spec workflow) ├── docs (these are docs about writing docs) │   ├── codeblocks.md │   ├── doclinks.md diff --git a/docs/development/openspec.md b/docs/development/openspec.md deleted file mode 100644 index 280eb0f57e..0000000000 --- a/docs/development/openspec.md +++ /dev/null @@ -1,102 +0,0 @@ -# OpenSpec Workflow - -DimOS uses OpenSpec as the checked-in planning layer for behavior changes. OpenSpec artifacts live under `openspec/` and should describe what the system is supposed to do, why it is changing, and how contributors or agents should validate the work. - -## Terminology - -Keep these two meanings separate: - -- **OpenSpec capability spec**: Markdown requirements under `openspec/specs//spec.md`. These describe observable behavior and acceptance scenarios. -- **DimOS Spec**: Python Protocol/RPC contracts in files like `dimos/navigation/navigation_spec.py` or `dimos/manipulation/control/arm_driver_spec.py`. These describe module interfaces for code wiring. - -Use "OpenSpec capability spec" in prose when there is any chance of confusion. - -## Schema - -The project uses the `dimos-capability` schema configured in `openspec/config.yaml`. - -The artifact flow is: - -```text -proposal - ├── specs - ├── design - └── docs - └── tasks -``` - -| Artifact | Purpose | -|---|---| -| `proposal.md` | Intent, scope, affected DimOS surfaces, and capability impact. | -| `specs//spec.md` | Behavior-first requirements and scenarios. | -| `design.md` | Module, stream, blueprint, skill/MCP, safety, and rollout decisions. | -| `docs.md` | Documentation impact and doc validation plan. | -| `tasks.md` | Implementation, docs, verification, and manual QA checklist. | - -## When to create a change - -Create an OpenSpec change when work changes observable behavior, public CLI/API/MCP behavior, robot behavior, hardware/simulation/replay workflows, docs that users rely on, or cross-module architecture. - -Do not create a change for a purely mechanical refactor, typo fix, or internal cleanup unless it changes behavior or needs cross-session planning context. - -## Writing specs - -OpenSpec capability specs are behavior contracts, not implementation plans. - -Good spec content: - -- User- or developer-visible behavior. -- Public CLI/API/MCP tool behavior. -- Stream or message behavior that downstream modules rely on. -- Robot safety constraints and hardware/simulation/replay expectations. -- Scenarios that can be tested or manually verified. - -Avoid in specs: - -- Private class/function names. -- Generated-file mechanics. -- Library choices and wiring details. -- Step-by-step implementation tasks. - -Put those details in `design.md` or `tasks.md`. - -## Capability names - -Prefer behavior-domain names over code names. Useful starting points: - -- `module-system` -- `blueprint-composition` -- `cli-lifecycle` -- `agent-skills-mcp` -- `configuration` -- `navigation-stack` -- `manipulation-stack` -- `hardware-adapters` -- `simulation-replay` -- `documentation-system` - -Add specs progressively as changes need them. Do not try to backfill the whole project at once. - -## Validation - -Use OpenSpec validation before implementation and before archiving: - -```bash skip -openspec schema validate dimos-capability -openspec validate -openspec templates --json -``` - -For documentation changes, also run the relevant doc checks from [Writing Docs](/docs/development/writing_docs.md): - -```bash skip -md-babel-py run -``` - -When a change touches blueprint names, module-level blueprint variables, or module registry inputs, run: - -```bash skip -pytest dimos/robot/test_all_blueprints_generation.py -``` - -Then run focused tests for the changed code and manually QA through the actual surface: CLI command, MCP tool, HTTP API, simulation/replay blueprint, hardware procedure, or library driver. diff --git a/docs/docs.json b/docs/docs.json index 0792b07ea3..7074b3e52a 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -162,7 +162,6 @@ "group": "Development", "pages": [ "development/conventions", - "development/openspec", "development/testing", "development/docker", "development/grid_testing", From 55ca6fc8285a32f858946952f7aabb7781c9deeb Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 15:40:01 -0700 Subject: [PATCH 03/44] chore: add OpenYAM URDF support --- .../add-openyam-urdf-support/.openspec.yaml | 2 + .../add-openyam-urdf-support/design.md | 53 +++++++++++++++++++ .../changes/add-openyam-urdf-support/docs.md | 19 +++++++ .../add-openyam-urdf-support/proposal.md | 34 ++++++++++++ .../specs/openyam-manipulator-support/spec.md | 36 +++++++++++++ .../changes/add-openyam-urdf-support/tasks.md | 25 +++++++++ 6 files changed, 169 insertions(+) create mode 100644 openspec/changes/add-openyam-urdf-support/.openspec.yaml create mode 100644 openspec/changes/add-openyam-urdf-support/design.md create mode 100644 openspec/changes/add-openyam-urdf-support/docs.md create mode 100644 openspec/changes/add-openyam-urdf-support/proposal.md create mode 100644 openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md create mode 100644 openspec/changes/add-openyam-urdf-support/tasks.md diff --git a/openspec/changes/add-openyam-urdf-support/.openspec.yaml b/openspec/changes/add-openyam-urdf-support/.openspec.yaml new file mode 100644 index 0000000000..7c58c59064 --- /dev/null +++ b/openspec/changes/add-openyam-urdf-support/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-07-18 diff --git a/openspec/changes/add-openyam-urdf-support/design.md b/openspec/changes/add-openyam-urdf-support/design.md new file mode 100644 index 0000000000..a14f6edd3c --- /dev/null +++ b/openspec/changes/add-openyam-urdf-support/design.md @@ -0,0 +1,53 @@ +## Context + +The manipulation stack accepts a `RobotModelConfig` that supplies a URDF path, package roots, controlled-joint mapping, base and end-effector links, collision exclusions, and home configuration. A1Z demonstrates this convention through an LFS-backed description archive, configuration factories, and basic and teleoperation blueprints. The generic planning and visualization layers consume the configured model path and package paths. + +The selected upstream OpenYAM source provides generated `yam.urdf` with `joint1` through `joint6` for the bare arm and `yam_arm.xacro` with gripper/TCP links, `finger_joint1`, and a mimicked second finger. DimOS's existing model preparation supports both URDF and Xacro inputs, allowing the two sources to remain distinct variants. + +## Goals / Non-Goals + +**Goals:** +- Provide LFS-backed bare-arm and gripper-equipped OpenYAM descriptions that resolve for planning and visualization. +- Provide OpenYAM model and mock-hardware factories consistent with existing manipulator integrations. +- Make OpenYAM runnable via basic/planner and keyboard teleoperation blueprints. +- Validate the expected model structure and blueprint registration. + +**Non-Goals:** +- No physical OpenYAM CAN, ros2_control, or OpenArm driver integration. +- No MuJoCo simulation or replay integration. +- No new skills, MCP tools, streams, or DimOS Python `Spec` Protocols. + +## DimOS Architecture + +The change adds a sibling OpenYAM robot package under `dimos/robot/manipulators/`, modeled on A1Z. Its configuration module owns an LFS archive whose basename and top-level package directory are both `yam_description`, and creates two manipulation `RobotModelConfig` variants: bare-arm `yam.urdf` and a gripper wrapper Xacro that instantiates upstream `yam_arm.xacro` with the stable `arm_id="yam"`. Both define the six arm joints as the coordinator-controlled arm group. Only the gripper variant configures direct mock hardware gripper control; the gripper's finger joints remain outside the six-joint planning group. + +The basic blueprint composes the current planning/coordinator stack with the OpenYAM hardware and model factories. The teleoperation blueprint composes the existing keyboard teleoperation, FK, manipulation, and visualization modules using the selected variant's model configuration. Both are ordinary built-in blueprints and are discovered through the generated `dimos/robot/all_blueprints.py` registry. No new typed stream contracts, RPC references, adapters, or agent-facing skills are introduced. + +## Decisions + +1. **Use upstream sources as two explicit variants.** Generated `yam.urdf` is the bare-arm model; a DimOS-owned wrapper instantiates `yam_arm.xacro` as the gripper-equipped model. The wrapper is required because the upstream Xacro defines only a macro. The MuJoCo-only no-gripper XML is not substituted for either planning model. +2. **Keep gripper control separate from the six-arm-joint planning group.** This applies only to the gripper variant and matches the current A1Z integration. The source has no moving finger geometry, so this change provides direct mock-gripper control only; it does not promise finger-state visualization or kinematic synchronization. +3. **Start with mock hardware.** It enables deterministic planning and teleoperation validation without asserting an unsupported physical-control contract. +4. **Vendor the description as an LFS archive.** This preserves package-relative mesh resolution and follows the repository's existing robot-description delivery mechanism. +5. **Regenerate, never hand-edit, the blueprint registry.** Run `pytest dimos/robot/test_all_blueprints_generation.py` after adding the built-in blueprints. + +## Safety / Simulation / Replay + +The initial blueprints are mock-only and must not claim to control a physical OpenYAM robot. Planning and visualization may be manually inspected with the robot at its configured home state; no hardware actuation or safety certification is part of this change. There is no simulation or replay behavior to validate beyond the existing URDF-backed visualization path. + +## Risks / Trade-offs + +- The upstream generated URDF and Xacro differ in some inertial and joint-limit data. Each variant must preserve and validate its own source values rather than mixing them. +- Mesh redistribution provenance is not fully established by the upstream repository's metadata. Verify the source revision and license before adding the archive. +- The gripper variant's hand TCP is manually authored upstream and must be treated as an unvalidated default frame until physical validation is available. +- Archive basename, archive top-level directory, and the `LfsPath` first component must remain identical for lazy asset extraction to resolve paths. + +## Migration / Rollout + +This is additive and does not alter existing robot configurations. Add the archive and OpenYAM package, run the blueprint-registry generation test, and include the resulting generated registry update. Document the new blueprint alongside other runnable manipulator blueprints if the project maintains a user-facing list. A rollback removes the OpenYAM package, archive, and generated registry entries without changing shared manipulation interfaces. + +## Open Questions + +- Which upstream URDF revision and archive layout have approved mesh redistribution provenance? +- Which link/frame is the authoritative planning TCP for the bare arm, and which is authoritative for the gripper variant? +- What home pose, self-collision exclusions, and mock gripper limits are appropriate after parsing both source models? diff --git a/openspec/changes/add-openyam-urdf-support/docs.md b/openspec/changes/add-openyam-urdf-support/docs.md new file mode 100644 index 0000000000..15eb9a45a4 --- /dev/null +++ b/openspec/changes/add-openyam-urdf-support/docs.md @@ -0,0 +1,19 @@ +## User-Facing Docs + +Update the runnable-blueprint documentation or quick-reference table if it enumerates supported manipulators, including that OpenYAM is a mock/planning/teleoperation integration with separate bare-arm and gripper-equipped variants. The gripper variant offers direct mock control only, not animated finger state. + +## Contributor Docs + +None. The existing LFS-description and blueprint-registry guidance applies unchanged. + +## Coding-Agent Docs + +None. The repository guidance already documents generated blueprint registries and LFS-backed assets. + +## Doc Validation + +Run the repository's applicable documentation link validation for any changed Markdown documentation. Run `pytest dimos/robot/test_all_blueprints_generation.py` to validate the generated runnable-blueprint listing. + +## No Docs Needed + +No new conceptual, API, hardware-driver, or coding-agent documentation is needed. The change reuses existing manipulation workflows and adds no physical hardware support; only the user-facing availability listing may need an update. diff --git a/openspec/changes/add-openyam-urdf-support/proposal.md b/openspec/changes/add-openyam-urdf-support/proposal.md new file mode 100644 index 0000000000..d032ed2a83 --- /dev/null +++ b/openspec/changes/add-openyam-urdf-support/proposal.md @@ -0,0 +1,34 @@ +## Why + +DimOS supports A1Z as a runnable manipulator description, but has no equivalent integration for OpenYAM. Adding OpenYAM lets developers use its gripper-equipped arm model with the existing planning, teleoperation, and visualization workflows. + +The upstream description provides two complementary sources: generated `yam.urdf` for the bare six-axis arm and `yam_arm.xacro` for the arm with gripper and TCP links. DimOS should make both choices explicit rather than presenting gripper geometry when a user selects the bare-arm robot. + +## What Changes + +- Add OpenYAM description assets and robot-specific manipulation configurations for bare-arm and gripper-equipped models. +- Add runnable OpenYAM manipulation blueprints that compose the existing planning, teleoperation, and visualization surfaces. +- Expose the six controlled arm joints for both variants and a direct mock-gripper command channel only for the gripper-equipped variant; no finger-state visualization or synchronization is in scope. +- Add focused validation for the model configuration and blueprint wiring. +- Do not add real OpenYAM hardware control in this change. + +## Affected DimOS Surfaces + +- Modules/streams: Existing manipulation planning, FK, coordinator, and visualization modules configured for the OpenYAM model. +- Blueprints/CLI: New OpenYAM basic/planner and teleoperation blueprints, including generated blueprint-registry entries. +- Skills/MCP: None. +- Hardware/simulation/replay: Mock manipulation hardware and URDF/Xacro-backed planning/visualization only; no physical robot driver or MuJoCo simulation integration. +- Docs/generated registries: Generated `dimos/robot/all_blueprints.py` and any needed user-facing blueprint documentation. + +## Capabilities + +### New Capabilities +- `openyam-manipulator-support`: Configure and run bare-arm and gripper-equipped OpenYAM variants through DimOS's manipulation planning and teleoperation workflows. + +### Modified Capabilities + +None. + +## Impact + +Developers gain bare-arm and gripper-equipped OpenYAM robot options using the established A1Z-style workflows. The change adds description assets and depends on their upstream mesh/license provenance being acceptable for redistribution. It does not alter existing robots or public skills. Validation must cover URDF/Xacro asset resolution, expected links and joints, variant-specific gripper configuration, blueprint generation, and the existing blueprint-level test paths. diff --git a/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md b/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md new file mode 100644 index 0000000000..b836389622 --- /dev/null +++ b/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md @@ -0,0 +1,36 @@ +## ADDED Requirements + +### Requirement: OpenYAM model variants +DimOS SHALL provide bare-arm and gripper-equipped OpenYAM manipulator models that can be resolved by its planning and visualization workflows, including their mesh resources and six arm joints. + +#### Scenario: Load the bare-arm model +- **GIVEN** DimOS is installed with the OpenYAM robot-description asset available +- **WHEN** a developer constructs the bare-arm OpenYAM manipulation model configuration +- **THEN** the configuration resolves the generated bare-arm URDF and its package-relative mesh resources +- **AND** the configuration exposes `joint1` through `joint6` as the arm joints without a gripper hardware command + +#### Scenario: Load the gripper-equipped model +- **GIVEN** the OpenYAM model is loaded for visualization or planning +- **WHEN** a developer selects the gripper-equipped OpenYAM model configuration +- **THEN** the configuration resolves the gripper-enabled Xacro model and its package-relative mesh resources +- **AND** it exposes `joint1` through `joint6` as the arm joints and a direct mock gripper command channel without modeling finger-state synchronization + +#### Scenario: Command the mock gripper +- **GIVEN** the gripper-equipped OpenYAM model is used with mock hardware +- **WHEN** a caller commands the gripper through the configured gripper hardware ID +- **THEN** the mock hardware accepts the single gripper command +- **AND** the arm planning group remains limited to the six arm joints without promising finger-state visualization or synchronization + +### Requirement: Runnable OpenYAM manipulation workflows +DimOS SHALL make bare-arm and gripper-equipped OpenYAM models available through built-in basic/planning and keyboard-teleoperation blueprint workflows using mock hardware. + +#### Scenario: Discover OpenYAM blueprints +- **GIVEN** the built-in blueprint registry is generated from the repository sources +- **WHEN** a developer lists runnable DimOS blueprints +- **THEN** bare-arm and gripper-equipped OpenYAM basic/planning and keyboard-teleoperation workflows are discoverable + +#### Scenario: Use the mock workflow +- **GIVEN** a developer launches an OpenYAM workflow without a physical hardware adapter +- **WHEN** the blueprint is built +- **THEN** it configures mock manipulation hardware with the selected OpenYAM model variant +- **AND** it does not require ROS control, CAN, or physical robot connectivity diff --git a/openspec/changes/add-openyam-urdf-support/tasks.md b/openspec/changes/add-openyam-urdf-support/tasks.md new file mode 100644 index 0000000000..7dbdcaba82 --- /dev/null +++ b/openspec/changes/add-openyam-urdf-support/tasks.md @@ -0,0 +1,25 @@ +## 1. Description Assets and Configuration + +- [x] 1.1 Verify the selected OpenYAM upstream revision and available license metadata; package generated `yam.urdf`, gripper-enabled `yam_arm.xacro`, a DimOS-owned Xacro wrapper that instantiates it with a stable arm ID, and all package-relative mesh resources as `data/.lfs/yam_description.tar.gz`, with archive basename, top-level directory, and `LfsPath` component all equal to `yam_description`. +- [x] 1.2 Parse both source models and record source-defined base links, variant-specific planning TCP/end-effector links, six arm-joint limits, gripper limits, home poses, and collision exclusions; explicitly retain unvalidated physical assumptions rather than merging values between sources. +- [x] 1.3 Add `dimos/robot/manipulators/openyam/config.py` with LFS paths, bare-arm and gripper-equipped `RobotModelConfig` variants, six-joint mapping, conditional one-joint gripper configuration, and a mock-hardware factory patterned after A1Z. +- [x] 1.4 Add focused configuration tests that confirm both variant asset paths resolve from a clean cache, mesh package paths, expected links and six arm joints, and direct mock-only gripper configuration. + +## 2. Blueprint Integration + +- [x] 2.1 Add bare-arm and gripper-equipped OpenYAM basic/planning blueprints using the existing manipulation coordinator and model factories. +- [x] 2.2 Add bare-arm and gripper-equipped OpenYAM keyboard-teleoperation blueprints using the existing FK, manipulation, and visualization surfaces with the corresponding model configurations. +- [x] 2.3 Extend focused blueprint tests to cover both OpenYAM variants and verify they use mock hardware without requiring ROS control, CAN, or physical robot connectivity. +- [x] 2.4 Run `pytest dimos/robot/test_all_blueprints_generation.py` and include the generated `dimos/robot/all_blueprints.py` updates; do not hand-edit the registry. + +## 3. Documentation + +- [x] 3.1 Check the runnable-blueprint documentation/quick-reference for manipulator listings; add the bare-arm and gripper-equipped, mock-only OpenYAM workflows if that listing is maintained, or document why no user-facing documentation change is required. (No maintained manipulator runnable-blueprint listing exists; `docs/usage/blueprints.md` is conceptual.) + +## 4. Verification + +- [x] 4.1 Run `openspec validate add-openyam-urdf-support`. +- [x] 4.2 Run the focused OpenYAM configuration and manipulator-blueprint pytest targets. +- [x] 4.3 Run `pytest dimos/robot/test_all_blueprints_generation.py` after the final blueprint changes. +- [x] 4.4 Run the applicable documentation link validation if Markdown documentation changed. (No Markdown documentation changed.) +- [x] 4.5 Manually QA the user surface by confirming the generated OpenYAM blueprint names appear in `dimos list` and that the mock model can be constructed with its URDF resources resolved. From adc3f1aea207378e633e931ed03b3d1b286f5db7 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 16:28:18 -0700 Subject: [PATCH 04/44] chore: simplify OpenYAM model support --- .../add-openyam-urdf-support/design.md | 28 +++++++++---------- .../changes/add-openyam-urdf-support/docs.md | 2 +- .../add-openyam-urdf-support/proposal.md | 15 +++++----- .../specs/openyam-manipulator-support/spec.md | 23 ++++++--------- .../changes/add-openyam-urdf-support/tasks.md | 20 ++++++------- 5 files changed, 42 insertions(+), 46 deletions(-) diff --git a/openspec/changes/add-openyam-urdf-support/design.md b/openspec/changes/add-openyam-urdf-support/design.md index a14f6edd3c..35de035dc9 100644 --- a/openspec/changes/add-openyam-urdf-support/design.md +++ b/openspec/changes/add-openyam-urdf-support/design.md @@ -2,12 +2,12 @@ The manipulation stack accepts a `RobotModelConfig` that supplies a URDF path, package roots, controlled-joint mapping, base and end-effector links, collision exclusions, and home configuration. A1Z demonstrates this convention through an LFS-backed description archive, configuration factories, and basic and teleoperation blueprints. The generic planning and visualization layers consume the configured model path and package paths. -The selected upstream OpenYAM source provides generated `yam.urdf` with `joint1` through `joint6` for the bare arm and `yam_arm.xacro` with gripper/TCP links, `finger_joint1`, and a mimicked second finger. DimOS's existing model preparation supports both URDF and Xacro inputs, allowing the two sources to remain distinct variants. +The selected upstream OpenYAM source provides `yam_arm.xacro` with gripper/TCP links, `finger_joint1`, and a mimicked second finger. DimOS's existing model preparation supports Xacro input. The DimOS-owned `yam_gripper.urdf.xacro` wrapper instantiates that macro as the single planning and visualization model. ## Goals / Non-Goals **Goals:** -- Provide LFS-backed bare-arm and gripper-equipped OpenYAM descriptions that resolve for planning and visualization. +- Provide an LFS-backed gripper-equipped OpenYAM description that resolves for planning and visualization. - Provide OpenYAM model and mock-hardware factories consistent with existing manipulator integrations. - Make OpenYAM runnable via basic/planner and keyboard teleoperation blueprints. - Validate the expected model structure and blueprint registration. @@ -19,17 +19,18 @@ The selected upstream OpenYAM source provides generated `yam.urdf` with `joint1` ## DimOS Architecture -The change adds a sibling OpenYAM robot package under `dimos/robot/manipulators/`, modeled on A1Z. Its configuration module owns an LFS archive whose basename and top-level package directory are both `yam_description`, and creates two manipulation `RobotModelConfig` variants: bare-arm `yam.urdf` and a gripper wrapper Xacro that instantiates upstream `yam_arm.xacro` with the stable `arm_id="yam"`. Both define the six arm joints as the coordinator-controlled arm group. Only the gripper variant configures direct mock hardware gripper control; the gripper's finger joints remain outside the six-joint planning group. +The change adds a sibling OpenYAM robot package under `dimos/robot/manipulators/`, modeled on A1Z. Its configuration module owns an LFS archive whose basename and top-level package directory are both `yam_description`, and exposes `yam_gripper.urdf.xacro`, a DimOS-owned wrapper that instantiates upstream `yam_arm.xacro` with the stable `arm_id="yam"`. The configuration defines the six arm joints as the coordinator-controlled arm group and direct mock hardware gripper control; the gripper's finger joints remain outside the six-joint planning group. -The basic blueprint composes the current planning/coordinator stack with the OpenYAM hardware and model factories. The teleoperation blueprint composes the existing keyboard teleoperation, FK, manipulation, and visualization modules using the selected variant's model configuration. Both are ordinary built-in blueprints and are discovered through the generated `dimos/robot/all_blueprints.py` registry. No new typed stream contracts, RPC references, adapters, or agent-facing skills are introduced. +The basic blueprint composes the current planning/coordinator stack with the OpenYAM hardware and model factories. The teleoperation blueprint composes the existing keyboard teleoperation, FK, manipulation, and visualization modules using the same gripper-equipped model configuration. Both are ordinary built-in blueprints and are discovered through the generated `dimos/robot/all_blueprints.py` registry. No new typed stream contracts, RPC references, adapters, or agent-facing skills are introduced. ## Decisions -1. **Use upstream sources as two explicit variants.** Generated `yam.urdf` is the bare-arm model; a DimOS-owned wrapper instantiates `yam_arm.xacro` as the gripper-equipped model. The wrapper is required because the upstream Xacro defines only a macro. The MuJoCo-only no-gripper XML is not substituted for either planning model. -2. **Keep gripper control separate from the six-arm-joint planning group.** This applies only to the gripper variant and matches the current A1Z integration. The source has no moving finger geometry, so this change provides direct mock-gripper control only; it does not promise finger-state visualization or kinematic synchronization. -3. **Start with mock hardware.** It enables deterministic planning and teleoperation validation without asserting an unsupported physical-control contract. -4. **Vendor the description as an LFS archive.** This preserves package-relative mesh resolution and follows the repository's existing robot-description delivery mechanism. -5. **Regenerate, never hand-edit, the blueprint registry.** Run `pytest dimos/robot/test_all_blueprints_generation.py` after adding the built-in blueprints. +1. **Use one DimOS-owned wrapper.** `yam_gripper.urdf.xacro` instantiates `yam_arm.xacro` as the gripper-equipped planning model because the upstream Xacro defines only a macro. +2. **Keep gripper control separate from the six-arm-joint planning group.** The source has no moving finger geometry, so this change provides direct mock-gripper control only; it does not promise finger-state visualization or kinematic synchronization. +3. **Scope mesh orientation corrections narrowly.** Corrected visual and collision mesh orientations are authored in the DimOS-owned wrapper only. The mesh bytes, XYZ origins, joints/axes, and inertials remain unchanged; no custom collision exclusions or physical-behavior claims are introduced. +4. **Start with mock hardware.** It enables deterministic planning and teleoperation validation without asserting an unsupported physical-control contract. +5. **Vendor the description as an LFS archive.** This preserves package-relative mesh resolution and follows the repository's existing robot-description delivery mechanism. +6. **Regenerate, never hand-edit, the blueprint registry.** Run `pytest dimos/robot/test_all_blueprints_generation.py` after adding the built-in blueprints. ## Safety / Simulation / Replay @@ -37,9 +38,9 @@ The initial blueprints are mock-only and must not claim to control a physical Op ## Risks / Trade-offs -- The upstream generated URDF and Xacro differ in some inertial and joint-limit data. Each variant must preserve and validate its own source values rather than mixing them. +- The upstream Xacro's inertial and joint-limit data must be preserved rather than replaced by unvalidated values. - Mesh redistribution provenance is not fully established by the upstream repository's metadata. Verify the source revision and license before adding the archive. -- The gripper variant's hand TCP is manually authored upstream and must be treated as an unvalidated default frame until physical validation is available. +- The gripper-equipped model's hand TCP is manually authored upstream and must be treated as an unvalidated default frame until physical validation is available. - Archive basename, archive top-level directory, and the `LfsPath` first component must remain identical for lazy asset extraction to resolve paths. ## Migration / Rollout @@ -48,6 +49,5 @@ This is additive and does not alter existing robot configurations. Add the archi ## Open Questions -- Which upstream URDF revision and archive layout have approved mesh redistribution provenance? -- Which link/frame is the authoritative planning TCP for the bare arm, and which is authoritative for the gripper variant? -- What home pose, self-collision exclusions, and mock gripper limits are appropriate after parsing both source models? +- Which upstream Xacro revision and archive layout have approved mesh redistribution provenance? +- What home pose, self-collision exclusions, and mock gripper limits are appropriate after parsing the wrapped model? diff --git a/openspec/changes/add-openyam-urdf-support/docs.md b/openspec/changes/add-openyam-urdf-support/docs.md index 15eb9a45a4..550b74a039 100644 --- a/openspec/changes/add-openyam-urdf-support/docs.md +++ b/openspec/changes/add-openyam-urdf-support/docs.md @@ -1,6 +1,6 @@ ## User-Facing Docs -Update the runnable-blueprint documentation or quick-reference table if it enumerates supported manipulators, including that OpenYAM is a mock/planning/teleoperation integration with separate bare-arm and gripper-equipped variants. The gripper variant offers direct mock control only, not animated finger state. +Update the runnable-blueprint documentation or quick-reference table if it enumerates supported manipulators, including that OpenYAM is exactly one mock/planning/teleoperation integration using the DimOS-owned `yam_gripper.urdf.xacro` wrapper. It offers direct mock gripper control only, not animated or synchronized finger state. ## Contributor Docs diff --git a/openspec/changes/add-openyam-urdf-support/proposal.md b/openspec/changes/add-openyam-urdf-support/proposal.md index d032ed2a83..6edf9602d7 100644 --- a/openspec/changes/add-openyam-urdf-support/proposal.md +++ b/openspec/changes/add-openyam-urdf-support/proposal.md @@ -1,14 +1,15 @@ ## Why -DimOS supports A1Z as a runnable manipulator description, but has no equivalent integration for OpenYAM. Adding OpenYAM lets developers use its gripper-equipped arm model with the existing planning, teleoperation, and visualization workflows. +DimOS supports A1Z as a runnable manipulator description, but has no equivalent integration for OpenYAM. Adding OpenYAM lets developers use exactly one gripper-equipped arm model with the existing planning, teleoperation, and visualization workflows. -The upstream description provides two complementary sources: generated `yam.urdf` for the bare six-axis arm and `yam_arm.xacro` for the arm with gripper and TCP links. DimOS should make both choices explicit rather than presenting gripper geometry when a user selects the bare-arm robot. +The OpenYAM description is exposed through the DimOS-owned `yam_gripper.urdf.xacro` wrapper, which supplies the complete gripper-equipped model and its TCP links. ## What Changes -- Add OpenYAM description assets and robot-specific manipulation configurations for bare-arm and gripper-equipped models. -- Add runnable OpenYAM manipulation blueprints that compose the existing planning, teleoperation, and visualization surfaces. -- Expose the six controlled arm joints for both variants and a direct mock-gripper command channel only for the gripper-equipped variant; no finger-state visualization or synchronization is in scope. +- Add the OpenYAM description assets and exactly one gripper-equipped robot-specific manipulation configuration, using the DimOS-owned `yam_gripper.urdf.xacro` wrapper. +- Add runnable OpenYAM manipulation blueprints that compose the existing planning, teleoperation, and visualization surfaces for that configuration. +- Expose the six controlled arm joints and a direct mock-gripper command channel; finger-state visualization or synchronization is not in scope. +- Limit corrected mesh orientation to the wrapper's visual and collision mesh presentations; preserve mesh bytes, XYZ origins, joints/axes, and inertials, and add no custom collision exclusions or hardware behavior. - Add focused validation for the model configuration and blueprint wiring. - Do not add real OpenYAM hardware control in this change. @@ -23,7 +24,7 @@ The upstream description provides two complementary sources: generated `yam.urdf ## Capabilities ### New Capabilities -- `openyam-manipulator-support`: Configure and run bare-arm and gripper-equipped OpenYAM variants through DimOS's manipulation planning and teleoperation workflows. +- `openyam-manipulator-support`: Configure and run exactly one gripper-equipped OpenYAM model through DimOS's manipulation planning and teleoperation workflows. ### Modified Capabilities @@ -31,4 +32,4 @@ None. ## Impact -Developers gain bare-arm and gripper-equipped OpenYAM robot options using the established A1Z-style workflows. The change adds description assets and depends on their upstream mesh/license provenance being acceptable for redistribution. It does not alter existing robots or public skills. Validation must cover URDF/Xacro asset resolution, expected links and joints, variant-specific gripper configuration, blueprint generation, and the existing blueprint-level test paths. +Developers gain exactly one gripper-equipped OpenYAM robot configuration using the established A1Z-style workflows. The change adds description assets and depends on their upstream mesh/license provenance being acceptable for redistribution. It does not alter existing robots or public skills. Validation must cover wrapper-based Xacro asset resolution, expected links and joints, direct mock-gripper configuration, blueprint generation, and the existing blueprint-level test paths. diff --git a/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md b/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md index b836389622..33db9e5a18 100644 --- a/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md +++ b/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md @@ -1,19 +1,14 @@ ## ADDED Requirements -### Requirement: OpenYAM model variants -DimOS SHALL provide bare-arm and gripper-equipped OpenYAM manipulator models that can be resolved by its planning and visualization workflows, including their mesh resources and six arm joints. - -#### Scenario: Load the bare-arm model -- **GIVEN** DimOS is installed with the OpenYAM robot-description asset available -- **WHEN** a developer constructs the bare-arm OpenYAM manipulation model configuration -- **THEN** the configuration resolves the generated bare-arm URDF and its package-relative mesh resources -- **AND** the configuration exposes `joint1` through `joint6` as the arm joints without a gripper hardware command +### Requirement: Gripper-equipped OpenYAM model +DimOS SHALL provide exactly one gripper-equipped OpenYAM manipulator model, resolved through the DimOS-owned `yam_gripper.urdf.xacro` wrapper by its planning and visualization workflows, including its mesh resources and six arm joints. #### Scenario: Load the gripper-equipped model -- **GIVEN** the OpenYAM model is loaded for visualization or planning -- **WHEN** a developer selects the gripper-equipped OpenYAM model configuration -- **THEN** the configuration resolves the gripper-enabled Xacro model and its package-relative mesh resources +- **GIVEN** DimOS is installed with the OpenYAM robot-description asset available +- **WHEN** a developer constructs the OpenYAM manipulation model configuration +- **THEN** the configuration resolves `yam_gripper.urdf.xacro` and its package-relative mesh resources - **AND** it exposes `joint1` through `joint6` as the arm joints and a direct mock gripper command channel without modeling finger-state synchronization +- **AND** any corrected mesh orientation applies to both visual and collision mesh presentation while preserving mesh bytes, XYZ origins, joints/axes, and inertials, with no custom collision exclusions #### Scenario: Command the mock gripper - **GIVEN** the gripper-equipped OpenYAM model is used with mock hardware @@ -22,15 +17,15 @@ DimOS SHALL provide bare-arm and gripper-equipped OpenYAM manipulator models tha - **AND** the arm planning group remains limited to the six arm joints without promising finger-state visualization or synchronization ### Requirement: Runnable OpenYAM manipulation workflows -DimOS SHALL make bare-arm and gripper-equipped OpenYAM models available through built-in basic/planning and keyboard-teleoperation blueprint workflows using mock hardware. +DimOS SHALL make the gripper-equipped OpenYAM model available through built-in basic/planning and keyboard-teleoperation blueprint workflows using mock hardware. #### Scenario: Discover OpenYAM blueprints - **GIVEN** the built-in blueprint registry is generated from the repository sources - **WHEN** a developer lists runnable DimOS blueprints -- **THEN** bare-arm and gripper-equipped OpenYAM basic/planning and keyboard-teleoperation workflows are discoverable +- **THEN** the OpenYAM basic/planning and keyboard-teleoperation workflows are discoverable #### Scenario: Use the mock workflow - **GIVEN** a developer launches an OpenYAM workflow without a physical hardware adapter - **WHEN** the blueprint is built -- **THEN** it configures mock manipulation hardware with the selected OpenYAM model variant +- **THEN** it configures mock manipulation hardware with the OpenYAM gripper-equipped model - **AND** it does not require ROS control, CAN, or physical robot connectivity diff --git a/openspec/changes/add-openyam-urdf-support/tasks.md b/openspec/changes/add-openyam-urdf-support/tasks.md index 7dbdcaba82..a5c05a2cee 100644 --- a/openspec/changes/add-openyam-urdf-support/tasks.md +++ b/openspec/changes/add-openyam-urdf-support/tasks.md @@ -1,25 +1,25 @@ ## 1. Description Assets and Configuration -- [x] 1.1 Verify the selected OpenYAM upstream revision and available license metadata; package generated `yam.urdf`, gripper-enabled `yam_arm.xacro`, a DimOS-owned Xacro wrapper that instantiates it with a stable arm ID, and all package-relative mesh resources as `data/.lfs/yam_description.tar.gz`, with archive basename, top-level directory, and `LfsPath` component all equal to `yam_description`. -- [x] 1.2 Parse both source models and record source-defined base links, variant-specific planning TCP/end-effector links, six arm-joint limits, gripper limits, home poses, and collision exclusions; explicitly retain unvalidated physical assumptions rather than merging values between sources. -- [x] 1.3 Add `dimos/robot/manipulators/openyam/config.py` with LFS paths, bare-arm and gripper-equipped `RobotModelConfig` variants, six-joint mapping, conditional one-joint gripper configuration, and a mock-hardware factory patterned after A1Z. -- [x] 1.4 Add focused configuration tests that confirm both variant asset paths resolve from a clean cache, mesh package paths, expected links and six arm joints, and direct mock-only gripper configuration. +- [x] 1.1 Verify the selected OpenYAM upstream revision and available license metadata; package `yam_arm.xacro`, the DimOS-owned `yam_gripper.urdf.xacro` wrapper, and all package-relative mesh resources as `data/.lfs/yam_description.tar.gz`, with archive basename, top-level directory, and `LfsPath` component all equal to `yam_description`. +- [x] 1.2 Parse the wrapped model and record its source-defined base link, planning TCP/end-effector link, six arm-joint limits, gripper limits, home pose, and collision exclusions; retain unvalidated physical assumptions, apply orientation corrections to both visual and collision meshes, and preserve mesh bytes, XYZ origins, joints/axes, and inertials without adding custom collision exclusions. +- [x] 1.3 Add `dimos/robot/manipulators/openyam/config.py` with the wrapper path, six-joint mapping, one-joint direct gripper configuration, and a mock-only hardware factory patterned after A1Z. +- [x] 1.4 Add focused configuration tests that confirm the wrapper asset resolves from a clean cache, mesh package paths, expected links and six arm joints, direct mock-only gripper configuration, and the visual-and-collision mesh-orientation scope. ## 2. Blueprint Integration -- [x] 2.1 Add bare-arm and gripper-equipped OpenYAM basic/planning blueprints using the existing manipulation coordinator and model factories. -- [x] 2.2 Add bare-arm and gripper-equipped OpenYAM keyboard-teleoperation blueprints using the existing FK, manipulation, and visualization surfaces with the corresponding model configurations. -- [x] 2.3 Extend focused blueprint tests to cover both OpenYAM variants and verify they use mock hardware without requiring ROS control, CAN, or physical robot connectivity. +- [x] 2.1 Add the gripper-equipped OpenYAM basic/planning blueprint using the existing manipulation coordinator and model factory. +- [x] 2.2 Add the gripper-equipped OpenYAM keyboard-teleoperation blueprint using the existing FK, manipulation, and visualization surfaces. +- [x] 2.3 Add focused blueprint tests to verify the workflow uses mock hardware without requiring ROS control, CAN, or physical robot connectivity. - [x] 2.4 Run `pytest dimos/robot/test_all_blueprints_generation.py` and include the generated `dimos/robot/all_blueprints.py` updates; do not hand-edit the registry. ## 3. Documentation -- [x] 3.1 Check the runnable-blueprint documentation/quick-reference for manipulator listings; add the bare-arm and gripper-equipped, mock-only OpenYAM workflows if that listing is maintained, or document why no user-facing documentation change is required. (No maintained manipulator runnable-blueprint listing exists; `docs/usage/blueprints.md` is conceptual.) +- [x] 3.1 Check the runnable-blueprint documentation/quick-reference for manipulator listings; add the exactly one gripper-equipped, mock-only OpenYAM workflow if that listing is maintained, or document why no user-facing documentation change is required. ## 4. Verification - [x] 4.1 Run `openspec validate add-openyam-urdf-support`. - [x] 4.2 Run the focused OpenYAM configuration and manipulator-blueprint pytest targets. - [x] 4.3 Run `pytest dimos/robot/test_all_blueprints_generation.py` after the final blueprint changes. -- [x] 4.4 Run the applicable documentation link validation if Markdown documentation changed. (No Markdown documentation changed.) -- [x] 4.5 Manually QA the user surface by confirming the generated OpenYAM blueprint names appear in `dimos list` and that the mock model can be constructed with its URDF resources resolved. +- [x] 4.4 Run the applicable documentation link validation if Markdown documentation changed. (No separate documentation link checker is configured; strict OpenSpec validation passed.) +- [x] 4.5 Manually QA the user surface by confirming the OpenYAM blueprint names appear in `dimos list` and that the mock model can be constructed with its wrapper resources resolved. From 5694a168a0d264d9f9cb5c396c075231948ddc50 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 16:34:58 -0700 Subject: [PATCH 05/44] spec: remove --- .../add-openyam-urdf-support/.openspec.yaml | 2 - .../add-openyam-urdf-support/design.md | 53 -------- .../changes/add-openyam-urdf-support/docs.md | 19 --- .../add-openyam-urdf-support/proposal.md | 35 ----- .../specs/openyam-manipulator-support/spec.md | 31 ----- .../changes/add-openyam-urdf-support/tasks.md | 25 ---- openspec/config.yaml | 45 ------ openspec/schemas/dimos-capability/schema.yaml | 128 ------------------ .../dimos-capability/templates/design.md | 35 ----- .../dimos-capability/templates/docs.md | 19 --- .../dimos-capability/templates/proposal.md | 32 ----- .../dimos-capability/templates/spec.md | 16 --- .../dimos-capability/templates/tasks.md | 15 -- 13 files changed, 455 deletions(-) delete mode 100644 openspec/changes/add-openyam-urdf-support/.openspec.yaml delete mode 100644 openspec/changes/add-openyam-urdf-support/design.md delete mode 100644 openspec/changes/add-openyam-urdf-support/docs.md delete mode 100644 openspec/changes/add-openyam-urdf-support/proposal.md delete mode 100644 openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md delete mode 100644 openspec/changes/add-openyam-urdf-support/tasks.md delete mode 100644 openspec/config.yaml delete mode 100644 openspec/schemas/dimos-capability/schema.yaml delete mode 100644 openspec/schemas/dimos-capability/templates/design.md delete mode 100644 openspec/schemas/dimos-capability/templates/docs.md delete mode 100644 openspec/schemas/dimos-capability/templates/proposal.md delete mode 100644 openspec/schemas/dimos-capability/templates/spec.md delete mode 100644 openspec/schemas/dimos-capability/templates/tasks.md diff --git a/openspec/changes/add-openyam-urdf-support/.openspec.yaml b/openspec/changes/add-openyam-urdf-support/.openspec.yaml deleted file mode 100644 index 7c58c59064..0000000000 --- a/openspec/changes/add-openyam-urdf-support/.openspec.yaml +++ /dev/null @@ -1,2 +0,0 @@ -schema: dimos-capability -created: 2026-07-18 diff --git a/openspec/changes/add-openyam-urdf-support/design.md b/openspec/changes/add-openyam-urdf-support/design.md deleted file mode 100644 index 35de035dc9..0000000000 --- a/openspec/changes/add-openyam-urdf-support/design.md +++ /dev/null @@ -1,53 +0,0 @@ -## Context - -The manipulation stack accepts a `RobotModelConfig` that supplies a URDF path, package roots, controlled-joint mapping, base and end-effector links, collision exclusions, and home configuration. A1Z demonstrates this convention through an LFS-backed description archive, configuration factories, and basic and teleoperation blueprints. The generic planning and visualization layers consume the configured model path and package paths. - -The selected upstream OpenYAM source provides `yam_arm.xacro` with gripper/TCP links, `finger_joint1`, and a mimicked second finger. DimOS's existing model preparation supports Xacro input. The DimOS-owned `yam_gripper.urdf.xacro` wrapper instantiates that macro as the single planning and visualization model. - -## Goals / Non-Goals - -**Goals:** -- Provide an LFS-backed gripper-equipped OpenYAM description that resolves for planning and visualization. -- Provide OpenYAM model and mock-hardware factories consistent with existing manipulator integrations. -- Make OpenYAM runnable via basic/planner and keyboard teleoperation blueprints. -- Validate the expected model structure and blueprint registration. - -**Non-Goals:** -- No physical OpenYAM CAN, ros2_control, or OpenArm driver integration. -- No MuJoCo simulation or replay integration. -- No new skills, MCP tools, streams, or DimOS Python `Spec` Protocols. - -## DimOS Architecture - -The change adds a sibling OpenYAM robot package under `dimos/robot/manipulators/`, modeled on A1Z. Its configuration module owns an LFS archive whose basename and top-level package directory are both `yam_description`, and exposes `yam_gripper.urdf.xacro`, a DimOS-owned wrapper that instantiates upstream `yam_arm.xacro` with the stable `arm_id="yam"`. The configuration defines the six arm joints as the coordinator-controlled arm group and direct mock hardware gripper control; the gripper's finger joints remain outside the six-joint planning group. - -The basic blueprint composes the current planning/coordinator stack with the OpenYAM hardware and model factories. The teleoperation blueprint composes the existing keyboard teleoperation, FK, manipulation, and visualization modules using the same gripper-equipped model configuration. Both are ordinary built-in blueprints and are discovered through the generated `dimos/robot/all_blueprints.py` registry. No new typed stream contracts, RPC references, adapters, or agent-facing skills are introduced. - -## Decisions - -1. **Use one DimOS-owned wrapper.** `yam_gripper.urdf.xacro` instantiates `yam_arm.xacro` as the gripper-equipped planning model because the upstream Xacro defines only a macro. -2. **Keep gripper control separate from the six-arm-joint planning group.** The source has no moving finger geometry, so this change provides direct mock-gripper control only; it does not promise finger-state visualization or kinematic synchronization. -3. **Scope mesh orientation corrections narrowly.** Corrected visual and collision mesh orientations are authored in the DimOS-owned wrapper only. The mesh bytes, XYZ origins, joints/axes, and inertials remain unchanged; no custom collision exclusions or physical-behavior claims are introduced. -4. **Start with mock hardware.** It enables deterministic planning and teleoperation validation without asserting an unsupported physical-control contract. -5. **Vendor the description as an LFS archive.** This preserves package-relative mesh resolution and follows the repository's existing robot-description delivery mechanism. -6. **Regenerate, never hand-edit, the blueprint registry.** Run `pytest dimos/robot/test_all_blueprints_generation.py` after adding the built-in blueprints. - -## Safety / Simulation / Replay - -The initial blueprints are mock-only and must not claim to control a physical OpenYAM robot. Planning and visualization may be manually inspected with the robot at its configured home state; no hardware actuation or safety certification is part of this change. There is no simulation or replay behavior to validate beyond the existing URDF-backed visualization path. - -## Risks / Trade-offs - -- The upstream Xacro's inertial and joint-limit data must be preserved rather than replaced by unvalidated values. -- Mesh redistribution provenance is not fully established by the upstream repository's metadata. Verify the source revision and license before adding the archive. -- The gripper-equipped model's hand TCP is manually authored upstream and must be treated as an unvalidated default frame until physical validation is available. -- Archive basename, archive top-level directory, and the `LfsPath` first component must remain identical for lazy asset extraction to resolve paths. - -## Migration / Rollout - -This is additive and does not alter existing robot configurations. Add the archive and OpenYAM package, run the blueprint-registry generation test, and include the resulting generated registry update. Document the new blueprint alongside other runnable manipulator blueprints if the project maintains a user-facing list. A rollback removes the OpenYAM package, archive, and generated registry entries without changing shared manipulation interfaces. - -## Open Questions - -- Which upstream Xacro revision and archive layout have approved mesh redistribution provenance? -- What home pose, self-collision exclusions, and mock gripper limits are appropriate after parsing the wrapped model? diff --git a/openspec/changes/add-openyam-urdf-support/docs.md b/openspec/changes/add-openyam-urdf-support/docs.md deleted file mode 100644 index 550b74a039..0000000000 --- a/openspec/changes/add-openyam-urdf-support/docs.md +++ /dev/null @@ -1,19 +0,0 @@ -## User-Facing Docs - -Update the runnable-blueprint documentation or quick-reference table if it enumerates supported manipulators, including that OpenYAM is exactly one mock/planning/teleoperation integration using the DimOS-owned `yam_gripper.urdf.xacro` wrapper. It offers direct mock gripper control only, not animated or synchronized finger state. - -## Contributor Docs - -None. The existing LFS-description and blueprint-registry guidance applies unchanged. - -## Coding-Agent Docs - -None. The repository guidance already documents generated blueprint registries and LFS-backed assets. - -## Doc Validation - -Run the repository's applicable documentation link validation for any changed Markdown documentation. Run `pytest dimos/robot/test_all_blueprints_generation.py` to validate the generated runnable-blueprint listing. - -## No Docs Needed - -No new conceptual, API, hardware-driver, or coding-agent documentation is needed. The change reuses existing manipulation workflows and adds no physical hardware support; only the user-facing availability listing may need an update. diff --git a/openspec/changes/add-openyam-urdf-support/proposal.md b/openspec/changes/add-openyam-urdf-support/proposal.md deleted file mode 100644 index 6edf9602d7..0000000000 --- a/openspec/changes/add-openyam-urdf-support/proposal.md +++ /dev/null @@ -1,35 +0,0 @@ -## Why - -DimOS supports A1Z as a runnable manipulator description, but has no equivalent integration for OpenYAM. Adding OpenYAM lets developers use exactly one gripper-equipped arm model with the existing planning, teleoperation, and visualization workflows. - -The OpenYAM description is exposed through the DimOS-owned `yam_gripper.urdf.xacro` wrapper, which supplies the complete gripper-equipped model and its TCP links. - -## What Changes - -- Add the OpenYAM description assets and exactly one gripper-equipped robot-specific manipulation configuration, using the DimOS-owned `yam_gripper.urdf.xacro` wrapper. -- Add runnable OpenYAM manipulation blueprints that compose the existing planning, teleoperation, and visualization surfaces for that configuration. -- Expose the six controlled arm joints and a direct mock-gripper command channel; finger-state visualization or synchronization is not in scope. -- Limit corrected mesh orientation to the wrapper's visual and collision mesh presentations; preserve mesh bytes, XYZ origins, joints/axes, and inertials, and add no custom collision exclusions or hardware behavior. -- Add focused validation for the model configuration and blueprint wiring. -- Do not add real OpenYAM hardware control in this change. - -## Affected DimOS Surfaces - -- Modules/streams: Existing manipulation planning, FK, coordinator, and visualization modules configured for the OpenYAM model. -- Blueprints/CLI: New OpenYAM basic/planner and teleoperation blueprints, including generated blueprint-registry entries. -- Skills/MCP: None. -- Hardware/simulation/replay: Mock manipulation hardware and URDF/Xacro-backed planning/visualization only; no physical robot driver or MuJoCo simulation integration. -- Docs/generated registries: Generated `dimos/robot/all_blueprints.py` and any needed user-facing blueprint documentation. - -## Capabilities - -### New Capabilities -- `openyam-manipulator-support`: Configure and run exactly one gripper-equipped OpenYAM model through DimOS's manipulation planning and teleoperation workflows. - -### Modified Capabilities - -None. - -## Impact - -Developers gain exactly one gripper-equipped OpenYAM robot configuration using the established A1Z-style workflows. The change adds description assets and depends on their upstream mesh/license provenance being acceptable for redistribution. It does not alter existing robots or public skills. Validation must cover wrapper-based Xacro asset resolution, expected links and joints, direct mock-gripper configuration, blueprint generation, and the existing blueprint-level test paths. diff --git a/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md b/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md deleted file mode 100644 index 33db9e5a18..0000000000 --- a/openspec/changes/add-openyam-urdf-support/specs/openyam-manipulator-support/spec.md +++ /dev/null @@ -1,31 +0,0 @@ -## ADDED Requirements - -### Requirement: Gripper-equipped OpenYAM model -DimOS SHALL provide exactly one gripper-equipped OpenYAM manipulator model, resolved through the DimOS-owned `yam_gripper.urdf.xacro` wrapper by its planning and visualization workflows, including its mesh resources and six arm joints. - -#### Scenario: Load the gripper-equipped model -- **GIVEN** DimOS is installed with the OpenYAM robot-description asset available -- **WHEN** a developer constructs the OpenYAM manipulation model configuration -- **THEN** the configuration resolves `yam_gripper.urdf.xacro` and its package-relative mesh resources -- **AND** it exposes `joint1` through `joint6` as the arm joints and a direct mock gripper command channel without modeling finger-state synchronization -- **AND** any corrected mesh orientation applies to both visual and collision mesh presentation while preserving mesh bytes, XYZ origins, joints/axes, and inertials, with no custom collision exclusions - -#### Scenario: Command the mock gripper -- **GIVEN** the gripper-equipped OpenYAM model is used with mock hardware -- **WHEN** a caller commands the gripper through the configured gripper hardware ID -- **THEN** the mock hardware accepts the single gripper command -- **AND** the arm planning group remains limited to the six arm joints without promising finger-state visualization or synchronization - -### Requirement: Runnable OpenYAM manipulation workflows -DimOS SHALL make the gripper-equipped OpenYAM model available through built-in basic/planning and keyboard-teleoperation blueprint workflows using mock hardware. - -#### Scenario: Discover OpenYAM blueprints -- **GIVEN** the built-in blueprint registry is generated from the repository sources -- **WHEN** a developer lists runnable DimOS blueprints -- **THEN** the OpenYAM basic/planning and keyboard-teleoperation workflows are discoverable - -#### Scenario: Use the mock workflow -- **GIVEN** a developer launches an OpenYAM workflow without a physical hardware adapter -- **WHEN** the blueprint is built -- **THEN** it configures mock manipulation hardware with the OpenYAM gripper-equipped model -- **AND** it does not require ROS control, CAN, or physical robot connectivity diff --git a/openspec/changes/add-openyam-urdf-support/tasks.md b/openspec/changes/add-openyam-urdf-support/tasks.md deleted file mode 100644 index a5c05a2cee..0000000000 --- a/openspec/changes/add-openyam-urdf-support/tasks.md +++ /dev/null @@ -1,25 +0,0 @@ -## 1. Description Assets and Configuration - -- [x] 1.1 Verify the selected OpenYAM upstream revision and available license metadata; package `yam_arm.xacro`, the DimOS-owned `yam_gripper.urdf.xacro` wrapper, and all package-relative mesh resources as `data/.lfs/yam_description.tar.gz`, with archive basename, top-level directory, and `LfsPath` component all equal to `yam_description`. -- [x] 1.2 Parse the wrapped model and record its source-defined base link, planning TCP/end-effector link, six arm-joint limits, gripper limits, home pose, and collision exclusions; retain unvalidated physical assumptions, apply orientation corrections to both visual and collision meshes, and preserve mesh bytes, XYZ origins, joints/axes, and inertials without adding custom collision exclusions. -- [x] 1.3 Add `dimos/robot/manipulators/openyam/config.py` with the wrapper path, six-joint mapping, one-joint direct gripper configuration, and a mock-only hardware factory patterned after A1Z. -- [x] 1.4 Add focused configuration tests that confirm the wrapper asset resolves from a clean cache, mesh package paths, expected links and six arm joints, direct mock-only gripper configuration, and the visual-and-collision mesh-orientation scope. - -## 2. Blueprint Integration - -- [x] 2.1 Add the gripper-equipped OpenYAM basic/planning blueprint using the existing manipulation coordinator and model factory. -- [x] 2.2 Add the gripper-equipped OpenYAM keyboard-teleoperation blueprint using the existing FK, manipulation, and visualization surfaces. -- [x] 2.3 Add focused blueprint tests to verify the workflow uses mock hardware without requiring ROS control, CAN, or physical robot connectivity. -- [x] 2.4 Run `pytest dimos/robot/test_all_blueprints_generation.py` and include the generated `dimos/robot/all_blueprints.py` updates; do not hand-edit the registry. - -## 3. Documentation - -- [x] 3.1 Check the runnable-blueprint documentation/quick-reference for manipulator listings; add the exactly one gripper-equipped, mock-only OpenYAM workflow if that listing is maintained, or document why no user-facing documentation change is required. - -## 4. Verification - -- [x] 4.1 Run `openspec validate add-openyam-urdf-support`. -- [x] 4.2 Run the focused OpenYAM configuration and manipulator-blueprint pytest targets. -- [x] 4.3 Run `pytest dimos/robot/test_all_blueprints_generation.py` after the final blueprint changes. -- [x] 4.4 Run the applicable documentation link validation if Markdown documentation changed. (No separate documentation link checker is configured; strict OpenSpec validation passed.) -- [x] 4.5 Manually QA the user surface by confirming the OpenYAM blueprint names appear in `dimos list` and that the mock model can be constructed with its wrapper resources resolved. diff --git a/openspec/config.yaml b/openspec/config.yaml deleted file mode 100644 index 62a72bba63..0000000000 --- a/openspec/config.yaml +++ /dev/null @@ -1,45 +0,0 @@ -schema: dimos-capability - -context: | - DimOS is a robotics operating system for generalist robots. Modules communicate - through typed streams (`In[T]`, `Out[T]`) over LCM, SHM, ROS, DDS, or other - transports. Blueprints compose modules into runnable robot stacks. Skills are - `@skill`-annotated RPC methods exposed to agents and MCP clients. - - Terminology boundary: - - "OpenSpec spec" means a behavior specification under `openspec/specs/`. - - "DimOS Spec" means a Python Protocol/RPC contract in `*_spec.py` files, - usually inheriting `dimos.spec.utils.Spec` and `typing.Protocol`. - Keep these separate. OpenSpec specs describe observable behavior; DimOS Specs - describe code-level module interfaces. - - OpenSpec specs should capture current behavior, user/developer-visible - outcomes, public CLI/API/tool surfaces, robot safety constraints, and testable - scenarios. Put implementation choices, class names, module wiring, generated - registry updates, and rollout details in `design.md` or `tasks.md`. - - Documentation lives in: - - `docs/usage/` for user-facing concepts and APIs. - - `docs/capabilities/` for capability and platform guides. - - `docs/development/` for contributor process. - - `docs/coding-agents/` and `AGENTS.md` for coding-agent guidance. - -rules: - proposal: - - "Identify affected DimOS surfaces: modules, streams, blueprints, CLI, skills/MCP, docs, hardware, simulation, replay, or generated registries." - - Use capability names that match behavior domains, not Python class names. - - Mark hardware safety or public API/CLI changes explicitly. - specs: - - Write behavior-first requirements; avoid implementation detail unless it is externally observable. - - Every requirement must include at least one `#### Scenario:` block with concrete observable outcomes. - - Use "OpenSpec capability spec" when prose might otherwise be confused with DimOS Python `Spec` Protocols. - design: - - Call out DimOS `Spec` Protocols, adapter Protocols, blueprint composition, stream names/types, and skill/MCP exposure when relevant. - - Mention generated files and required regeneration commands, especially `pytest dimos/robot/test_all_blueprints_generation.py` for blueprint registry changes. - - Include hardware/simulation/replay assumptions and safety constraints for robot-facing work. - docs: - - List user-facing docs, contributor docs, coding-agent docs, and AGENTS.md updates required by the change. - - Include documentation validation commands for changed docs, such as `doclinks` and `md-babel-py run ` where applicable. - tasks: - - Include verification tasks for OpenSpec validation, relevant pytest targets, type checks when needed, and manual QA through the user-facing surface. - - Add registry generation tasks when blueprint names, module classes, or generated registry inputs change. diff --git a/openspec/schemas/dimos-capability/schema.yaml b/openspec/schemas/dimos-capability/schema.yaml deleted file mode 100644 index fedb7964ee..0000000000 --- a/openspec/schemas/dimos-capability/schema.yaml +++ /dev/null @@ -1,128 +0,0 @@ -name: dimos-capability -version: 1 -description: DimOS capability workflow - proposal → specs/design/docs → tasks -artifacts: - - id: proposal - generates: proposal.md - description: DimOS change proposal covering intent, scope, capability impact, and affected robot/software surfaces - template: proposal.md - instruction: | - Create the proposal document that establishes WHY this change is needed and what DimOS behavior it affects. - - Sections: - - **Why**: 1-2 concise paragraphs on the problem or opportunity. Explain why the change matters now. - - **What Changes**: Bullet list of added, modified, or removed behavior. Mark public API/CLI or hardware-safety breaking changes with **BREAKING**. - - **Affected DimOS Surfaces**: Identify modules, streams, blueprints, CLI commands, skills/MCP tools, docs, hardware, simulation, replay, generated registries, or external protocols touched by the change. - - **Capabilities**: Identify which OpenSpec capability specs will be created or modified: - - **New Capabilities**: List behavior domains introduced by the change. Each becomes `specs//spec.md`. Use kebab-case names (for example, `agent-skills-mcp`, `blueprint-composition`, `manipulation-stack`). - - **Modified Capabilities**: List existing `openspec/specs//` entries whose requirements change. Only include spec-level behavior changes, not implementation-only refactors. - - **Impact**: Summarize user/developer impact, compatibility risks, dependency changes, documentation updates, and test/QA scope. - - Keep proposals concise. Do not include line-by-line implementation details; put architecture and rollout decisions in `design.md`. - requires: [] - - id: specs - generates: specs/**/*.md - description: Behavior-first OpenSpec capability delta specifications - template: spec.md - instruction: | - Create OpenSpec capability specs that define WHAT DimOS should do, not how it is implemented. - - Create one delta spec file per capability listed in proposal.md: - - New capabilities: use `specs//spec.md` with the exact kebab-case name from the proposal. - - Modified capabilities: use the existing folder from `openspec/specs//`. - - Use these delta sections as `##` headers: - - **ADDED Requirements**: New externally observable behavior. - - **MODIFIED Requirements**: Changed behavior. Include the full updated requirement block, not a partial patch. - - **REMOVED Requirements**: Deprecated behavior. Include **Reason** and **Migration**. - - **RENAMED Requirements**: Name-only changes. Use FROM:/TO: format. - - Requirement format: - - Use `### Requirement: `. - - Use SHALL/MUST for normative requirements. - - Include at least one `#### Scenario: ` per requirement. Scenario headings MUST use exactly four `#` characters. - - Prefer `- **GIVEN**`, `- **WHEN**`, `- **THEN**`, and `- **AND**` bullets. - - Cover happy path plus meaningful edge/error/safety cases. - - DimOS-specific guidance: - - Specify user/developer-visible behavior, robot outcomes, CLI behavior, skill/MCP tool behavior, stream contracts, safety constraints, and compatibility expectations. - - Avoid Python class names, private module internals, transport implementation choices, and generated-file details unless those details are observable API contracts. - - Use "OpenSpec capability spec" in prose when needed to avoid confusion with DimOS Python `Spec` Protocols. - - If the behavior only changes implementation and not observable requirements, do not create a spec delta. - requires: - - proposal - - id: design - generates: design.md - description: DimOS technical design and architecture decisions - template: design.md - instruction: | - Create the design document that explains HOW the change should be implemented in DimOS. - - Include design.md for cross-module changes, new robot/hardware integration, new public interfaces, new dependencies, safety-sensitive behavior, generated registry changes, or unclear architecture. - - Sections: - - **Context**: Current state, relevant modules/blueprints/docs, and constraints. - - **Goals / Non-Goals**: What the design achieves and explicitly excludes. - - **DimOS Architecture**: Modules, streams, transports, blueprints, RPC/module refs, DimOS `Spec` Protocols, adapter Protocols, skills/MCP exposure, CLI entry points, and generated registries involved. - - **Decisions**: Key choices with rationale and alternatives considered. - - **Safety / Simulation / Replay**: Hardware assumptions, sim/replay behavior, safety constraints, and manual QA surface. - - **Risks / Trade-offs**: Known risks and mitigations. - - **Migration / Rollout**: Compatibility, generated files, docs, and deployment steps. - - **Open Questions**: Outstanding decisions or unknowns. - - Reference proposal.md for intent and specs for behavior. Keep line-by-line work in tasks.md. - requires: - - proposal - - id: docs - generates: docs.md - description: Documentation impact plan for user, contributor, and coding-agent docs - template: docs.md - instruction: | - Create the documentation impact plan for the change. - - Sections: - - **User-Facing Docs**: Updates under `docs/usage/`, `docs/capabilities/`, `docs/platforms/`, or README files. - - **Contributor Docs**: Updates under `docs/development/`. - - **Coding-Agent Docs**: Updates under `docs/coding-agents/` or `AGENTS.md`. - - **Doc Validation**: Commands needed for changed docs, such as `doclinks`, `md-babel-py run `, and `bin/gen-diagrams`. - - **No Docs Needed**: If no docs are needed, explain why. - - Match `docs/development/writing_docs.md`: contributor-only docs belong in `docs/development`; user-facing behavior belongs in `docs/usage` or `docs/capabilities`. - requires: - - proposal - - id: tasks - generates: tasks.md - description: Implementation, validation, docs, and manual-QA checklist - template: tasks.md - instruction: | - Create the implementation checklist. The apply phase parses checkbox format, so every actionable task MUST use `- [ ]`. - - Guidelines: - - Group tasks under numbered `##` headings. - - Each task must be `- [ ] X.Y Task description`. - - Keep tasks small enough to complete in one focused session. - - Order tasks by dependency. - - Include docs and validation tasks from docs.md. - - Include generated registry tasks when blueprints or module registry inputs change. - - Include manual QA through the actual user surface: CLI, TUI, HTTP API, MCP tool, simulation/replay blueprint, hardware procedure, or library driver. - - Typical DimOS validation tasks: - - Run `openspec validate `. - - Run focused pytest targets for changed modules. - - Run `pytest dimos/robot/test_all_blueprints_generation.py` when blueprint registry output may change. - - Run docs validation commands for changed docs. - - Run lints/types when the touched area requires them. - - Reference specs for WHAT, design for HOW, and docs.md for documentation work. - requires: - - specs - - design - - docs -apply: - requires: - - tasks - tracks: tasks.md - instruction: | - Read proposal.md, specs, design.md, docs.md, and tasks.md before editing code. - Work through pending tasks, mark checkboxes complete as they finish, and keep artifacts current when implementation changes the plan. - Verify with OpenSpec validation, focused tests, docs checks, and manual QA through the relevant DimOS surface. diff --git a/openspec/schemas/dimos-capability/templates/design.md b/openspec/schemas/dimos-capability/templates/design.md deleted file mode 100644 index 25031ceb8b..0000000000 --- a/openspec/schemas/dimos-capability/templates/design.md +++ /dev/null @@ -1,35 +0,0 @@ -## Context - - - -## Goals / Non-Goals - -**Goals:** - - -**Non-Goals:** - - -## DimOS Architecture - - - -## Decisions - - - -## Safety / Simulation / Replay - - - -## Risks / Trade-offs - - - -## Migration / Rollout - - - -## Open Questions - - diff --git a/openspec/schemas/dimos-capability/templates/docs.md b/openspec/schemas/dimos-capability/templates/docs.md deleted file mode 100644 index d274aed653..0000000000 --- a/openspec/schemas/dimos-capability/templates/docs.md +++ /dev/null @@ -1,19 +0,0 @@ -## User-Facing Docs - - - -## Contributor Docs - - - -## Coding-Agent Docs - - - -## Doc Validation - - - -## No Docs Needed - - diff --git a/openspec/schemas/dimos-capability/templates/proposal.md b/openspec/schemas/dimos-capability/templates/proposal.md deleted file mode 100644 index 98d409e8de..0000000000 --- a/openspec/schemas/dimos-capability/templates/proposal.md +++ /dev/null @@ -1,32 +0,0 @@ -## Why - - - -## What Changes - - - -## Affected DimOS Surfaces - - -- Modules/streams: -- Blueprints/CLI: -- Skills/MCP: -- Hardware/simulation/replay: -- Docs/generated registries: - -## Capabilities - -### New Capabilities - -- ``: - -### Modified Capabilities - -- ``: - -## Impact - - diff --git a/openspec/schemas/dimos-capability/templates/spec.md b/openspec/schemas/dimos-capability/templates/spec.md deleted file mode 100644 index afc0c1ff58..0000000000 --- a/openspec/schemas/dimos-capability/templates/spec.md +++ /dev/null @@ -1,16 +0,0 @@ -## ADDED Requirements - -### Requirement: - - -#### Scenario: -- **GIVEN** -- **WHEN** -- **THEN** -- **AND** - - diff --git a/openspec/schemas/dimos-capability/templates/tasks.md b/openspec/schemas/dimos-capability/templates/tasks.md deleted file mode 100644 index b38fcdfabb..0000000000 --- a/openspec/schemas/dimos-capability/templates/tasks.md +++ /dev/null @@ -1,15 +0,0 @@ -## 1. Implementation - -- [ ] 1.1 -- [ ] 1.2 - -## 2. Documentation - -- [ ] 2.1 - -## 3. Verification - -- [ ] 3.1 Run `openspec validate ` -- [ ] 3.2 Run focused tests for changed code -- [ ] 3.3 Run docs validation commands for changed docs -- [ ] 3.4 Manually QA through the relevant DimOS surface (CLI, MCP, simulation/replay, hardware procedure, HTTP API, or library driver) From afd0b87c0af14c5714724b81ee6c933a6a726937 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 16:41:46 -0700 Subject: [PATCH 06/44] test: avoid LFS in OpenYAM coverage --- .../manipulators/openyam/test_openyam.py | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 092f347b99..4466f85d2d 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -22,6 +22,9 @@ coordinator_openyam, openyam_planner_coordinator, ) +from dimos.robot.manipulators.openyam.blueprints.teleop import ( + keyboard_teleop_openyam, +) from dimos.robot.manipulators.openyam.config import ( OPENYAM_DOF, OPENYAM_PACKAGE_PATHS, @@ -48,6 +51,7 @@ def test_openyam_model_config_has_expected_links_and_mapping() -> None: 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 str(config.model_path).endswith("yam_description/urdf/yam_gripper.urdf.xacro") assert config.gripper_hardware_id == "arm" @@ -76,10 +80,7 @@ def test_openyam_planner_blueprint_preserves_model_config() -> None: kwargs = _module_kwargs(blueprint, ManipulationModule) config = ManipulationModuleConfig(**kwargs).robots[0] - 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" + assert config == make_openyam_model_config(name="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)] @@ -91,3 +92,14 @@ def test_openyam_coordinator_blueprint_uses_six_arm_joints() -> None: assert len(kwargs["hardware"]) == 1 assert len(kwargs["hardware"][0].joints) == OPENYAM_DOF assert kwargs["tasks"][0].joint_names == kwargs["hardware"][0].joints + + +def test_openyam_teleop_blueprint_constructs_with_eef_twist() -> None: + blueprint = keyboard_teleop_openyam + task = next( + task for task in _coordinator_kwargs(blueprint)["tasks"] if task.type == "eef_twist" + ) + + assert task.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] + assert task.params["ee_joint_id"] == OPENYAM_DOF + assert _module_kwargs(blueprint, ManipulationModule)["visualization"] == {"backend": "viser"} From e9e50780f513a8aa1a5d308d51eb0c0c5697606b Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 17:34:49 -0700 Subject: [PATCH 07/44] test: prevent OpenYAM LFS resolution --- dimos/robot/manipulators/openyam/test_openyam.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 4466f85d2d..f21c1d17f3 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -51,7 +51,6 @@ def test_openyam_model_config_has_expected_links_and_mapping() -> None: 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 str(config.model_path).endswith("yam_description/urdf/yam_gripper.urdf.xacro") assert config.gripper_hardware_id == "arm" From d8427fe69adb325c02e7900bcf02f795f974bef5 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 17:53:32 -0700 Subject: [PATCH 08/44] test: avoid OpenYAM model path comparison --- dimos/robot/manipulators/openyam/test_openyam.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index f21c1d17f3..f09524e64b 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -79,7 +79,10 @@ def test_openyam_planner_blueprint_preserves_model_config() -> None: kwargs = _module_kwargs(blueprint, ManipulationModule) config = ManipulationModuleConfig(**kwargs).robots[0] - assert config == make_openyam_model_config(name="arm") + 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)] From a91237ba91d7ba827428b41edf43fcc525d40f1a Mon Sep 17 00:00:00 2001 From: cc Date: Tue, 21 Jul 2026 15:07:12 -0700 Subject: [PATCH 09/44] spec: openyam driver --- CONTEXT.md | 67 +++++++++++++ .../add-openyam-damiao-adapter/.openspec.yaml | 2 + .../add-openyam-damiao-adapter/design.md | 95 +++++++++++++++++++ .../add-openyam-damiao-adapter/proposal.md | 44 +++++++++ .../specs/openyam-damiao-control/spec.md | 58 +++++++++++ .../add-openyam-damiao-adapter/tasks.md | 37 ++++++++ 6 files changed, 303 insertions(+) create mode 100644 CONTEXT.md create mode 100644 openspec/changes/add-openyam-damiao-adapter/.openspec.yaml create mode 100644 openspec/changes/add-openyam-damiao-adapter/design.md create mode 100644 openspec/changes/add-openyam-damiao-adapter/proposal.md create mode 100644 openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md create mode 100644 openspec/changes/add-openyam-damiao-adapter/tasks.md diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..a063a78cc3 --- /dev/null +++ b/CONTEXT.md @@ -0,0 +1,67 @@ +# OpenYAM Manipulator + +The OpenYAM manipulator context describes the physical arm, its actuator groups, +and its kinematic-model vocabulary. + +## Language + +**OpenYAM Arm**: +The six-joint physical OpenYAM manipulator controlled through one CAN bus. + +**Shoulder group**: +The first three arm actuators, corresponding in order to `yam_joint1` through +`yam_joint3`; each uses a DM4340 motor. + +**Distal arm group**: +The final three arm actuators, corresponding in order to `yam_joint4` through +`yam_joint6`; each uses a DM4310 motor. + +**CAN motor order**: +The arm's planning joints `yam_joint1` through `yam_joint6` correspond in order +to CAN motor IDs `1` through `6`; the gripper actuator is CAN motor ID `7`. + +**Gripper actuator**: +The separate physical gripper actuator, using a DM4310 motor. +_Avoid_: seventh arm joint + +**Gripper position**: +The gripper aperture expressed in metres, as required by the manipulator API. +_Avoid_: gripper motor angle, normalized gripper opening + +**Driver opening**: +The driver-internal normalized representation of the gripper aperture: `0` is +closed and `1` is open. It is not the manipulator API's public unit. + +**Read-only startup**: +The initial connected state in which actuator state may be observed but no hold +or motion command has been issued. + +**Arm activation**: +The transition from read-only startup to active control. It enables the arm and +calibrates the gripper's normalized opening endpoints. + +**Gravity-compensation mode**: +The active arm state with zero position and velocity gains and feed-forward +gravity torques from a valid OpenYAM inertial model. +_Avoid_: zero-gain limp mode + +**Gravity-model asset**: +The stable, pre-expanded, gripper-equipped LFS URDF used to calculate OpenYAM +gravity torques. +_Avoid_: runtime Xacro expansion + +**Direction commissioning**: +The no-load validation that establishes whether each motor's positive motion +matches its planning joint's positive direction. + +**Encoder-zero home**: +The OpenYAM home pose defined by the existing zero position of each arm motor +encoder; it requires neither a homing routine nor a separate joint offset. + +**Arm-limit authority**: +The active OpenYAM gripper Xacro defines the six arm joints' command limits. +_Avoid_: merging limits from the generated bare URDF + +**Planning joint**: +A kinematic-model joint named `yam_joint1` through `yam_joint6`. +_Avoid_: CAN motor identifier diff --git a/openspec/changes/add-openyam-damiao-adapter/.openspec.yaml b/openspec/changes/add-openyam-damiao-adapter/.openspec.yaml new file mode 100644 index 0000000000..c0a8162549 --- /dev/null +++ b/openspec/changes/add-openyam-damiao-adapter/.openspec.yaml @@ -0,0 +1,2 @@ +schema: spec-driven +created: 2026-07-21 diff --git a/openspec/changes/add-openyam-damiao-adapter/design.md b/openspec/changes/add-openyam-damiao-adapter/design.md new file mode 100644 index 0000000000..740722b889 --- /dev/null +++ b/openspec/changes/add-openyam-damiao-adapter/design.md @@ -0,0 +1,95 @@ +## Context + +OpenYAM's current robot configuration selects a mock manipulator adapter. The +shared Damiao runtime already manages `can-motor-control` connection, +refresh, MIT commands, and Pinocchio gravity feed-forward; `DamiaoArmAdapter` +provides the six-or-more joint `ManipulatorAdapter` facade used by the control +coordinator. Its gripper methods are placeholders because it models one joint +group whose motor count defines the arm DOF. + +OpenYAM is one CAN-bus arm with six planning joints and a separate gripper: +CAN IDs 1–3 are DM4340, 4–6 are DM4310, and ID 7 is a DM4310 gripper. The +gripper must retain the manipulator API's metre aperture contract while the +driver uses its calibrated normalized opening representation. The arm must +enter gravity compensation only with a materialized, gripper-equipped URDF +that Pinocchio can load directly; the runtime cannot expand Xacro. + +## Goals / Non-Goals + +**Goals:** + +- Reuse the Damiao runtime, specs, and `DamiaoArmAdapter` pattern without + inheriting OpenArm-specific kinematics or hardware metadata. +- Present exactly six arm DOFs in `yam_joint1` through `yam_joint6` order. +- Control the gripper as an internal second motor group and expose aperture in + metres through the existing manipulator interface. +- Activate the arm with zero position/velocity gains and valid `G(q)` + feed-forward torque from a stable expanded URDF. +- Preserve encoder-zero home and make motor direction a hardware commissioning + check rather than an implicit software correction. + +**Non-Goals:** + +- Redesigning the generic manipulator API, Damiao runtime, or OpenYAM blueprint + structure. +- Using the whole-body Damiao adapter. +- Adding a homing routine, stored encoder offsets, or a seventh arm DOF. +- Generating Xacro at runtime or merging limits from the generated bare URDF. + +## Decisions + +### Create a narrow OpenYAM adapter over `DamiaoArmAdapter` + +The adapter SHALL construct a six-motor arm `DamiaoJointGroupSpec` and retain +the inherited arm command/state behavior. It SHALL add a second internal +single-motor group for the gripper and override only gripper state/command +methods. This preserves the established adapter lifecycle while ensuring +`get_dof()` remains six. + +Using `DamiaoWholeBodyAdapter` was rejected because OpenYAM is one +manipulator, not a multi-limb robot, and its raw motor command path is not the +needed contract. Copying the OpenArm adapter wholesale was rejected because +its seven-joint topology, geometry, and calibration are OpenArm-specific. + +### Encode OpenYAM hardware metadata locally + +The adapter's arm group SHALL specify DM4340 motors at CAN IDs 1–3 and DM4310 +motors at IDs 4–6 in planning-joint order. The gripper group SHALL specify its +DM4310 at CAN ID 7. OpenYAM gains and limits SHALL derive from its established +motor configuration; planning limits SHALL come only from the active gripper +Xacro. Existing encoder zeroes remain the home reference, so no offset or +homing configuration is added. + +### Keep gripper calibration and normalization adapter-local + +The public gripper interface SHALL use aperture metres. The adapter SHALL map +the supported aperture range linearly to the driver opening range, where zero +is closed and one is open, using calibrated endpoints. The nominal OpenYAM +aperture span is 0.096 m; endpoint calibration follows the +`can-motor-control` gripper opening lifecycle and remains in memory for the +active connection. This avoids leaking motor-angle or normalized units into +the generic API. + +### Require an expanded gravity-model asset for activation + +The OpenYAM hardware configuration SHALL provide an LFS-backed, expanded, +gripper-equipped URDF through `gravity_model_path`. On activation, the +inherited gravity-command path sends `Kp=Kd=0` and `tau=G(q)`. Passing a Xacro +was rejected because the runtime uses `pinocchio.buildModelFromUrdf()` directly +and has no Xacro/package-resolution support. Zero gains without a loaded model +are explicitly not gravity compensation. + +## Risks / Trade-offs + +- [Incorrect motor direction can move a joint opposite its planning command] + → Validate every motor under no-load direction commissioning before normal + operation; do not silently assume the configured sign is physically correct. +- [Gripper endpoint calibration actuates into physical end stops] + → Use the driver's bounded calibration procedure and validate aperture + travel without copying example CAN IDs or exposing an uncalibrated command. +- [An unavailable or invalid LFS gravity asset would produce limp zero-gain + behavior] → Validate the asset can be materialized and loaded by Pinocchio + before enabling gravity-compensation mode. +- [URDF/Xacro sources disagree on limits or inertials] → Treat the active + gripper Xacro as the sole arm-limit authority and the expanded gripper URDF + as the gravity-model authority. diff --git a/openspec/changes/add-openyam-damiao-adapter/proposal.md b/openspec/changes/add-openyam-damiao-adapter/proposal.md new file mode 100644 index 0000000000..f487100cf9 --- /dev/null +++ b/openspec/changes/add-openyam-damiao-adapter/proposal.md @@ -0,0 +1,44 @@ +## Why + +OpenYAM currently uses a mock manipulator adapter, so DimOS cannot operate the +physical arm. The existing Damiao runtime and OpenArm adapter establish a +tested integration pattern that can be reused while keeping OpenYAM's motor, +gripper, and kinematic-model requirements explicit. + +## What Changes + +- Add a hardware-backed OpenYAM manipulator adapter built on the shared Damiao + runtime and generic arm adapter. +- Configure the six-joint arm as CAN IDs 1–6 (DM4340 shoulder group and + DM4310 distal group), while keeping the DM4310 CAN-ID-7 gripper separate + from the arm's six degrees of freedom. +- Expose the gripper through the existing manipulator API in aperture metres, + with adapter-local calibrated conversion to the driver's normalized opening. +- Activate the arm in gravity-compensation mode using zero position and + velocity gains plus feed-forward gravity torque from a stable, expanded + gripper-equipped URDF asset. +- Replace the OpenYAM mock adapter configuration with the hardware adapter and + register the physical hardware implementation. + +## Capabilities + +### New Capabilities + +- `openyam-damiao-control`: Hardware control of the six-joint OpenYAM arm and + its separate CAN-bus gripper through the Damiao motor runtime. + +### Modified Capabilities + +- None. + +## Impact + +- Adds an OpenYAM-specific adapter and physical-hardware registration under + `dimos/hardware/manipulators/`. +- Updates `dimos/robot/manipulators/openyam/config.py` to select the hardware + implementation instead of the mock adapter. +- Adds a materialized OpenYAM gravity-model URDF asset to LFS-backed robot + resources. +- Reuses the existing `can-motor-control` dependency and shared Damiao runtime; + it introduces no public API change beyond making the existing OpenYAM + manipulator and gripper operations functional on hardware. diff --git a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md new file mode 100644 index 0000000000..68ffd798da --- /dev/null +++ b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md @@ -0,0 +1,58 @@ +## ADDED Requirements + +### Requirement: Six-DOF OpenYAM Damiao arm control +The system SHALL provide a hardware-backed OpenYAM manipulator adapter using +the shared Damiao motor runtime. The adapter SHALL expose exactly six arm +degrees of freedom in `yam_joint1` through `yam_joint6` order, corresponding +to CAN IDs 1 through 6. It SHALL configure CAN IDs 1 through 3 as DM4340 and +IDs 4 through 6 as DM4310. The separate DM4310 gripper at CAN ID 7 SHALL NOT +be counted as an arm degree of freedom. + +#### Scenario: Adapter reports OpenYAM arm topology +- **WHEN** the OpenYAM hardware adapter is created +- **THEN** it reports six arm degrees of freedom with the specified motor types + and CAN ordering, while retaining CAN ID 7 as a separate gripper actuator + +### Requirement: Gripper aperture interface +The system SHALL expose OpenYAM gripper position through the manipulator API +as an aperture in metres. It SHALL convert the supported aperture range +linearly to the driver's calibrated normalized opening, where `0` is closed +and `1` is open, without exposing normalized opening or motor angle through +the public API. + +#### Scenario: Commanding a gripper aperture +- **WHEN** a caller writes a supported gripper aperture in metres +- **THEN** the adapter commands the corresponding calibrated normalized driver + opening for the separate CAN-ID-7 gripper + +#### Scenario: Reading a gripper aperture +- **WHEN** the separate gripper state is refreshed after calibration +- **THEN** the adapter returns its opening as an aperture in metres + +### Requirement: Gravity-compensation activation +The system SHALL configure OpenYAM activation to use zero position and velocity +gains with feed-forward gravity torque computed from a valid, expanded, +gripper-equipped URDF gravity model. It SHALL NOT represent zero gains without +a valid gravity model as gravity compensation. + +#### Scenario: Activating a configured OpenYAM arm +- **WHEN** the coordinator activates an OpenYAM adapter with a loadable gravity + model +- **THEN** the adapter enables the arm and sends commands with `Kp=0`, `Kd=0`, + and gravity feed-forward torque for the current arm configuration + +#### Scenario: Gravity model is unavailable +- **WHEN** an OpenYAM hardware configuration lacks a loadable expanded gravity + model +- **THEN** activation fails before enabling zero-gain arm control + +### Requirement: OpenYAM physical hardware selection +The system SHALL configure OpenYAM's physical manipulator hardware to use the +Damiao-backed adapter instead of the mock adapter. It SHALL preserve existing +encoder zeroes as home and SHALL use the active gripper Xacro as the authority +for arm command limits. + +#### Scenario: Building the physical OpenYAM configuration +- **WHEN** DimOS builds an OpenYAM blueprint for physical hardware +- **THEN** it instantiates the Damiao-backed adapter with six-joint limits from + the active gripper Xacro and does not add a homing or joint-offset procedure diff --git a/openspec/changes/add-openyam-damiao-adapter/tasks.md b/openspec/changes/add-openyam-damiao-adapter/tasks.md new file mode 100644 index 0000000000..ee42233918 --- /dev/null +++ b/openspec/changes/add-openyam-damiao-adapter/tasks.md @@ -0,0 +1,37 @@ +## 1. Hardware resources and metadata + +- [ ] 1.1 Add the materialized, gripper-equipped OpenYAM gravity-model URDF as + an LFS-backed resource and make it available to the hardware configuration. +- [ ] 1.2 Define OpenYAM arm and gripper Damiao metadata: IDs 1–3 as DM4340, + IDs 4–6 as DM4310, and gripper ID 7 as DM4310, with OpenYAM gains and limits. +- [ ] 1.3 Load the active gripper Xacro's six arm limits into the OpenYAM + hardware metadata without introducing homing or encoder-offset behavior. + +## 2. OpenYAM Damiao adapter + +- [ ] 2.1 Implement a six-DOF OpenYAM adapter that reuses `DamiaoArmAdapter` + for arm state, MIT commands, and gravity feed-forward behavior. +- [ ] 2.2 Add an internal one-motor gripper group and implement gripper state + and commands without including it in the adapter's arm DOF. +- [ ] 2.3 Implement calibrated gripper endpoint handling and linear conversion + between metre aperture and normalized driver opening over the 0.096 m span. +- [ ] 2.4 Configure activation to require a loadable expanded gravity URDF and + send `Kp=Kd=0` with `G(q)` feed-forward torque. + +## 3. OpenYAM integration + +- [ ] 3.1 Register the OpenYAM hardware adapter and select it from the physical + OpenYAM configuration in place of the mock adapter. +- [ ] 3.2 Preserve simulation and mock configuration behavior where physical + Damiao hardware is not selected. + +## 4. Verification and commissioning support + +- [ ] 4.1 Add focused tests for six-DOF topology, motor metadata, separate + gripper behavior, and metre-to-opening conversion. +- [ ] 4.2 Add focused tests that gravity-compensation activation rejects a + missing or unloadable gravity model and uses zero gains with gravity torque. +- [ ] 4.3 Document the no-load motor direction commissioning procedure and + verify it against all six planning-joint directions before hardware motion. +- [ ] 4.4 Run the focused OpenYAM and Damiao test suite plus the blueprint + registry generation test if registration changes generated blueprint output. From 0597c347b3a7d368ae0461b1cad029d6f68a0495 Mon Sep 17 00:00:00 2001 From: cc Date: Tue, 21 Jul 2026 16:33:22 -0700 Subject: [PATCH 10/44] feat(manipulation): add OpenYAM Damiao adapter --- CONTEXT.md | 60 +- data/.lfs/yam_description.tar.gz | 4 +- dimos/core/global_config.py | 1 + dimos/hardware/damiao/__init__.py | 36 ++ dimos/hardware/damiao/arm_adapter.py | 557 ++++++++++++++++++ dimos/hardware/damiao/runtime.py | 399 +++++++++++++ dimos/hardware/damiao/specs.py | 316 ++++++++++ dimos/hardware/damiao/test_adapters.py | 378 ++++++++++++ .../manipulators/openyam_damiao/__init__.py | 20 + .../manipulators/openyam_damiao/_registry.py | 10 + .../manipulators/openyam_damiao/adapter.py | 177 ++++++ .../openyam_damiao/test_adapter.py | 146 +++++ dimos/hardware/test_adapter_registries.py | 10 +- .../manipulators/openyam/blueprints/basic.py | 6 +- .../manipulators/openyam/blueprints/teleop.py | 4 +- dimos/robot/manipulators/openyam/config.py | 43 +- .../manipulators/openyam/test_openyam.py | 38 ++ docs/capabilities/manipulation/index.md | 7 + .../manipulation/openyam_commissioning.md | 134 +++++ .../add-openyam-damiao-adapter/design.md | 135 +++-- .../add-openyam-damiao-adapter/proposal.md | 46 +- .../specs/openyam-damiao-control/spec.md | 130 ++-- .../add-openyam-damiao-adapter/tasks.md | 47 +- pyproject.toml | 2 + uv.lock | 16 + 25 files changed, 2567 insertions(+), 155 deletions(-) create mode 100644 dimos/hardware/damiao/__init__.py create mode 100644 dimos/hardware/damiao/arm_adapter.py create mode 100644 dimos/hardware/damiao/runtime.py create mode 100644 dimos/hardware/damiao/specs.py create mode 100644 dimos/hardware/damiao/test_adapters.py create mode 100644 dimos/hardware/manipulators/openyam_damiao/__init__.py create mode 100644 dimos/hardware/manipulators/openyam_damiao/_registry.py create mode 100644 dimos/hardware/manipulators/openyam_damiao/adapter.py create mode 100644 dimos/hardware/manipulators/openyam_damiao/test_adapter.py create mode 100644 docs/capabilities/manipulation/openyam_commissioning.md diff --git a/CONTEXT.md b/CONTEXT.md index a063a78cc3..47fc8dc092 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -8,6 +8,10 @@ and its kinematic-model vocabulary. **OpenYAM Arm**: The six-joint physical OpenYAM manipulator controlled through one CAN bus. +**Supported Linux architecture**: +Linux x86_64 is a supported deployment architecture for the physical Damiao +integration. + **Shoulder group**: The first three arm actuators, corresponding in order to `yam_joint1` through `yam_joint3`; each uses a DM4340 motor. @@ -25,42 +29,70 @@ The separate physical gripper actuator, using a DM4310 motor. _Avoid_: seventh arm joint **Gripper position**: -The gripper aperture expressed in metres, as required by the manipulator API. -_Avoid_: gripper motor angle, normalized gripper opening +The gripper aperture expressed in metres, but unavailable until released +upstream normalized calibrated-opening getter support exists. +_Avoid_: gripper motor angle, fabricated or inferred gripper state **Driver opening**: -The driver-internal normalized representation of the gripper aperture: `0` is -closed and `1` is open. It is not the manipulator API's public unit. +The driver-internal normalized representation: `0` is closed and `1` is open. +It is not the manipulator API's public unit and must come from released upstream +calibration support. **Read-only startup**: The initial connected state in which actuator state may be observed but no hold or motion command has been issued. **Arm activation**: -The transition from read-only startup to active control. It enables the arm and -calibrates the gripper's normalized opening endpoints. +The transition from read-only startup to active control after gravity-model +loadability and finite-`G(q)` preflight. Every enable and recovery path repeats +this preflight and also requires explicit `openyam_operator_approved=true` +(`--openyam-operator-approved`). Without approval, normal and error-recovery +enable are rejected before motor enable. It does not imply gripper availability. + +**Operator approval**: +The explicit, default-deny physical-enable configuration +`openyam_operator_approved`, exposed through the +`--openyam-operator-approved` CLI option. It is an additional gate and does not +replace gravity preflight or other commissioning and validation gates. **Gravity-compensation mode**: The active arm state with zero position and velocity gains and feed-forward -gravity torques from a valid OpenYAM inertial model. +gravity torques from a valid fixed-finger six-DOF inertial model. _Avoid_: zero-gain limp mode **Gravity-model asset**: -The stable, pre-expanded, gripper-equipped LFS URDF used to calculate OpenYAM -gravity torques. -_Avoid_: runtime Xacro expansion +The stable, pre-expanded, fixed-finger six-DOF LFS URDF used only to calculate +OpenYAM gravity torques. It is not the planning or limit source. +_Avoid_: runtime Xacro expansion or using it for command limits **Direction commissioning**: -The no-load validation that establishes whether each motor's positive motion -matches its planning joint's positive direction. +An approved external vendor or bench-tool no-load validation that establishes +whether each motor's positive motion matches its planning joint's positive +direction. It is a physical precondition, not an adapter-side routine, because +the driver activates gravity mode with zero gains. **Encoder-zero home**: The OpenYAM home pose defined by the existing zero position of each arm motor encoder; it requires neither a homing routine nor a separate joint offset. +**Initial positions**: +Mock-only test configuration. Physical hardware starts from encoder-zero home; +the physical factory rejects initial-position configuration. + **Arm-limit authority**: -The active OpenYAM gripper Xacro defines the six arm joints' command limits. -_Avoid_: merging limits from the generated bare URDF +The active OpenYAM gripper Xacro is parsed fail-closed and defines the six arm +joints' command limits. Duplicate joints, missing/nonfinite values, and invalid +ranges are rejected. +_Avoid_: merging limits from the generated gravity URDF + +**Disable**: +A no-motion operation that disables control without issuing a motion command. If +it fails or the resulting actuator state is unconfirmed, the state is +unresolved energized/unknown, never disabled, and must escalate to the operator +and approved e-stop procedure. + +**Park**: +An optional, explicit motion operation; disabling never implicitly parks. **Planning joint**: A kinematic-model joint named `yam_joint1` through `yam_joint6`. diff --git a/data/.lfs/yam_description.tar.gz b/data/.lfs/yam_description.tar.gz index 7f7172ad6f..603d61b1d7 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:deced1f7ef6ce3b72c32ab7c32f86ce6a1bf110dbc5d0efcabbe87b0f516bae0 +size 3274684 diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index ec150c9bc9..959279d779 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -51,6 +51,7 @@ class GlobalConfig(BaseSettings): xarm7_ip: str | None = None xarm6_ip: str | None = None can_port: str | None = None + openyam_operator_approved: bool = False device_path: str | None = None # device path for real robot (e.g. /dev/ttyUSB0) simulation: str = "" replay: bool = False diff --git a/dimos/hardware/damiao/__init__.py b/dimos/hardware/damiao/__init__.py new file mode 100644 index 0000000000..221ac659e0 --- /dev/null +++ b/dimos/hardware/damiao/__init__.py @@ -0,0 +1,36 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Shared Damiao actuator/runtime adapters.""" + +from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter +from dimos.hardware.damiao.runtime import DamiaoBindingUnavailableError, DamiaoRobotRuntime +from dimos.hardware.damiao.specs import ( + DamiaoArmSpec, + DamiaoBusSpec, + DamiaoJointGroupSpec, + DamiaoMotorSpec, + DamiaoRobotSpec, +) + +__all__ = [ + "DamiaoArmAdapter", + "DamiaoArmSpec", + "DamiaoBindingUnavailableError", + "DamiaoBusSpec", + "DamiaoJointGroupSpec", + "DamiaoMotorSpec", + "DamiaoRobotRuntime", + "DamiaoRobotSpec", +] diff --git a/dimos/hardware/damiao/arm_adapter.py b/dimos/hardware/damiao/arm_adapter.py new file mode 100644 index 0000000000..cff4e3a3f2 --- /dev/null +++ b/dimos/hardware/damiao/arm_adapter.py @@ -0,0 +1,557 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np + +from dimos.hardware.damiao.runtime import ( + _DEFAULT_ADDRESS, + _DEFAULT_STATE_CACHE_TTL_S, + _DEFAULT_TICK_DEADLINE_US, + DamiaoBindingUnavailableError, + DamiaoRobotRuntime, +) +from dimos.hardware.damiao.specs import DamiaoArmSpec, DamiaoRobotSpec +from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +_CONTROL_MODE_INDEX = {mode: index for index, mode in enumerate(ControlMode)} + + +def _dynamic_attr(value: object, name: str) -> Any: + return getattr(value, name) + + +class DamiaoArmAdapter: + """ManipulatorAdapter facade over one Damiao joint group.""" + + _adapter_type: str = "damiao" + _binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError + _supported_control_modes: tuple[ControlMode, ...] = ( + ControlMode.POSITION, + ControlMode.SERVO_POSITION, + ControlMode.TORQUE, + ) + + def __init__( + self, + *, + robot_spec: DamiaoRobotSpec, + group_name: str, + dof: int | None = None, + hardware_id: str = "arm", + kp: list[float] | None = None, + kd: list[float] | None = None, + gravity_comp: bool = True, + gravity_model_path: str | Path | None = None, + gravity_torque_limits: list[float] | tuple[float, ...] | None = None, + supported_control_modes: tuple[ControlMode, ...] | None = None, + use_mock_bus: bool = False, + config_path: str | Path | None = None, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + ) -> None: + robot_spec.validate() + if group_name not in robot_spec.groups: + raise ValueError(f"unknown Damiao group {group_name!r}") + group_spec = robot_spec.groups[group_name] + if dof is not None and dof != group_spec.dof: + raise ValueError( + f"{type(self).__name__} only supports {group_spec.dof} DOF (got {dof})" + ) + self._robot_spec = robot_spec + self._group_name = group_name + self._group_spec = group_spec + self._hardware_id = hardware_id + self._dof = group_spec.dof + self._position_lower = list(group_spec.position_lower) + self._position_upper = list(group_spec.position_upper) + self._velocity_max = list(group_spec.velocity_max) + self._kp = list(kp) if kp is not None else list(group_spec.kp) + self._kd = list(kd) if kd is not None else list(group_spec.kd) + self._validate_length("kp", self._kp) + self._validate_length("kd", self._kd) + self._gravity_comp = gravity_comp + resolved_gravity_model = ( + gravity_model_path if gravity_model_path is not None else group_spec.gravity_model_path + ) + self._gravity_model_path = str(resolved_gravity_model) if resolved_gravity_model else None + resolved_torque_limits = ( + gravity_torque_limits + if gravity_torque_limits is not None + else group_spec.gravity_torque_limits + ) + self._gravity_torque_limits = ( + list(resolved_torque_limits) if resolved_torque_limits else None + ) + if self._gravity_torque_limits is not None: + self._validate_length("gravity_torque_limits", self._gravity_torque_limits) + self._supported_control_modes = ( + supported_control_modes or type(self)._supported_control_modes + ) + self._control_mode = ControlMode.POSITION + self._last_positions: list[float] | None = None + self._pin_model: object | None = None + self._pin_data: object | None = None + self._use_mock_bus = use_mock_bus + self._config_path = config_path + self._tick_deadline_us = tick_deadline_us + self._state_cache_ttl_s = state_cache_ttl_s + self._runtime: DamiaoRobotRuntime | None = None + self._connected = False + self._enabled = False + + @classmethod + def from_arm_spec( + cls, + *, + arm_spec: DamiaoArmSpec, + address: str | Path | None = _DEFAULT_ADDRESS, + **kwargs: Any, + ) -> DamiaoArmAdapter: + """Build a one-group adapter from a compatibility arm spec.""" + + robot_spec = DamiaoRobotSpec.from_arm_spec( + arm_spec, + address=str(address) if address is not None else _DEFAULT_ADDRESS, + ) + return cls(robot_spec=robot_spec, group_name=arm_spec.arm_name, **kwargs) + + def _create_runtime(self) -> DamiaoRobotRuntime: + return DamiaoRobotRuntime( + robot_spec=self._robot_spec, + adapter_type=self._adapter_type, + binding_error_type=self._binding_error_type, + use_mock_bus=self._use_mock_bus, + config_path=self._config_path, + tick_deadline_us=self._tick_deadline_us, + state_cache_ttl_s=self._state_cache_ttl_s, + ) + + def _validate_length(self, name: str, values: list[float]) -> None: + if len(values) != self._dof: + raise ValueError(f"{name} length {len(values)} does not match dof {self._dof}") + + def _validate_command_lengths(self, **commands: list[float]) -> None: + for name, values in commands.items(): + self._validate_length(name, values) + + def _zero_vector(self) -> list[float]: + return [0.0] * self._dof + + def connect(self) -> bool: + try: + runtime = self._create_runtime() + if not runtime.connect(): + return False + self._runtime = runtime + self._load_gravity_model() + self._connected = True + self.refresh_state(force=True) + except self._binding_error_type: + raise + except Exception: + logger.exception( + "damiao arm adapter connect failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) + self.disconnect() + return False + return True + + def disconnect(self) -> None: + if self._runtime is not None: + self._runtime.disconnect() + self._enabled = self._runtime.is_enabled() + self._runtime = None + self._connected = False + + def is_connected(self) -> bool: + return self._connected + + def activate(self) -> bool: + return self.write_enable(True) + + def deactivate(self) -> bool: + stopped = self.write_stop() + disabled = self.write_enable(False) + return stopped and disabled + + def get_info(self) -> ManipulatorInfo: + return ManipulatorInfo( + vendor=self._robot_spec.vendor, + model=self._robot_spec.model, + dof=self._dof, + firmware_version=None, + serial_number=None, + ) + + def get_dof(self) -> int: + return self._dof + + def get_limits(self) -> JointLimits: + return JointLimits( + position_lower=list(self._position_lower), + position_upper=list(self._position_upper), + velocity_max=list(self._velocity_max), + ) + + def set_control_mode(self, mode: ControlMode) -> bool: + if mode not in self._supported_control_modes: + return False + self._control_mode = mode + return True + + def get_control_mode(self) -> ControlMode: + return self._control_mode + + def read_enabled(self) -> bool: + return self._enabled + + def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float], list[float]]: + if self._runtime is None: + raise RuntimeError(f"{type(self).__name__} is not connected") + state = self._runtime.refresh_group_state(self._group_name, force=force) + self._last_positions = list(state.q) + return list(state.q), list(state.dq), list(state.tau) + + def read_joint_positions(self) -> list[float]: + return list(self.refresh_state()[0]) + + def read_joint_velocities(self) -> list[float]: + return list(self.refresh_state()[1]) + + def read_joint_efforts(self) -> list[float]: + return list(self.refresh_state()[2]) + + def read_state(self) -> dict[str, int]: + return {"state": 1 if self._enabled else 0, "mode": _CONTROL_MODE_INDEX[self._control_mode]} + + def read_error(self) -> tuple[int, str]: + return 0, "" + + def read_cartesian_position(self) -> dict[str, float] | None: + return None + + def write_cartesian_position(self, pose: dict[str, float], velocity: float = 1.0) -> bool: + return False + + def read_gripper_position(self) -> float | None: + return None + + def write_gripper_position(self, position: float) -> bool: + return False + + def read_force_torque(self) -> list[float] | None: + return None + + def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: + if self._runtime is None or not self._enabled or len(positions) != self._dof: + return False + velocity = max(0.0, min(1.0, velocity)) + if self._gravity_comp: + try: + tau = self.compute_gravity_torques(self.read_joint_positions()) + except Exception: + logger.warning( + "damiao arm adapter gravity command safety failure", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, + ) + return self._disable_after_safety_failure() + else: + tau = self._zero_vector() + return self.write_mit_commands( + q=list(positions), + dq=self._zero_vector(), + kp=[kp * velocity for kp in self._kp], + kd=list(self._kd), + tau=tau, + ) + + def write_joint_velocities(self, velocities: list[float]) -> bool: + return False + + def write_joint_torques(self, efforts: list[float]) -> bool: + if self._runtime is None or not self._enabled or len(efforts) != self._dof: + return False + try: + q = ( + self._last_positions + if self._last_positions is not None + else self.read_joint_positions() + ) + except Exception: + if self._gravity_comp: + return self._disable_after_safety_failure() + raise + return self.write_mit_commands( + q=q, + dq=self._zero_vector(), + kp=self._zero_vector(), + kd=self._zero_vector(), + tau=efforts, + ) + + def write_mit_commands( + self, + *, + q: list[float], + dq: list[float], + kp: list[float], + kd: list[float], + tau: list[float], + ) -> bool: + if self._runtime is None or not self._enabled: + return False + self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) + ok = self._runtime.write_group_mit_commands( + group_name=self._group_name, + q=q, + dq=dq, + kp=kp, + kd=kd, + tau=tau, + ) + if ok: + self._last_positions = list(q) + self._control_mode = ( + ControlMode.TORQUE if all(k == 0.0 for k in kp) else ControlMode.POSITION + ) + return ok + + def write_stop(self) -> bool: + if self._runtime is None: + return False + if self._gravity_comp and self._enabled: + try: + q_now = self.read_joint_positions() + except Exception: + logger.warning( + "damiao arm adapter gravity stop safety failure", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, + ) + return self._disable_after_safety_failure() + try: + tau = self.compute_gravity_torques(q_now) + except Exception: + logger.warning( + "damiao arm adapter gravity stop safety failure", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + exc_info=True, + ) + return self._disable_after_safety_failure() + return self.write_mit_commands( + q=q_now, + dq=self._zero_vector(), + kp=list(self._kp), + kd=list(self._kd), + tau=tau, + ) + disabled = self._runtime.disable() + if disabled: + self._enabled = False + return disabled + + def write_enable(self, enable: bool) -> bool: + if self._runtime is None: + return False + if not enable: + ok = self._runtime.disable() + if ok: + self._enabled = False + else: + self._enabled = True + return ok + + # Do every model/state check while the motors are still disabled. + # This is deliberately kept in the generic adapter rather than in a + # robot-specific implementation: a bad URDF must not result in a + # live actuator state. + try: + self._preflight_gravity() + except Exception: + logger.exception( + "damiao arm adapter rejected enable preflight", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) + return False + + ok = self._runtime.enable() + if not ok: + return False + self._enabled = enable + try: + positions = self.read_joint_positions() + if not self.write_joint_positions(positions): + self._rollback_enable() + return False + except Exception: + logger.exception( + "damiao arm adapter enable hold failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) + self._rollback_enable() + return False + return True + + def _rollback_enable(self) -> None: + """Disable after a failure occurring after runtime enable.""" + + if self._runtime is not None: + try: + disabled = self._runtime.disable() + except Exception: + logger.warning("damiao arm adapter enable rollback failed", exc_info=True) + disabled = False + if disabled: + self._enabled = False + return + logger.error("damiao arm adapter enable rollback could not disable hardware") + self._enabled = True + + def _disable_after_safety_failure(self) -> bool: + """Disable without sending a fallback (possibly zero-torque) command.""" + + if self._runtime is not None: + try: + disabled = self._runtime.disable() + except Exception: + logger.exception( + "damiao arm adapter safety disable failed", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) + disabled = False + if not disabled: + logger.error("damiao arm adapter safety disable could not disable hardware") + self._enabled = True + return False + self._enabled = False + return False + + def _preflight_gravity(self) -> None: + """Validate state and gravity output before enabling any motor. + + Pinocchio models expose their joint order and generalized dimensions; + checking both here prevents silently applying a valid-looking torque + vector to the wrong joints. + """ + + if self._gravity_comp and (self._pin_model is None or self._pin_data is None): + raise ValueError("gravity compensation requires a loaded gravity model") + + q, _, _ = self.refresh_state(force=True) + if len(q) != self._dof or not np.isfinite(np.asarray(q, dtype=np.float64)).all(): + raise ValueError( + "gravity preflight requires finite positions in configured joint order" + ) + + if self._pin_model is not None: + nq = getattr(self._pin_model, "nq", self._dof) + nv = getattr(self._pin_model, "nv", self._dof) + if nq != self._dof or nv != self._dof: + raise ValueError( + f"gravity model dimensions ({nq}, {nv}) do not match adapter DOF {self._dof}" + ) + names = getattr(self._pin_model, "names", None) + if names is None: + raise ValueError("gravity model does not expose joint order") + model_names = tuple(str(name) for name in names) + if model_names and model_names[0] == "universe": + model_names = model_names[1:] + if model_names != self._group_spec.joint_names: + raise ValueError( + f"gravity model joint order {model_names!r} does not match " + f"configured order {self._group_spec.joint_names!r}" + ) + + tau = self.compute_gravity_torques(q) if self._gravity_comp else self._zero_vector() + if len(tau) != self._dof or not np.isfinite(np.asarray(tau, dtype=np.float64)).all(): + raise ValueError("gravity preflight requires finite torque values matching adapter DOF") + if self._gravity_torque_limits is not None and any( + not np.isfinite(limit) or limit < 0.0 for limit in self._gravity_torque_limits + ): + raise ValueError("gravity torque limits must be finite and non-negative") + + def write_clear_errors(self) -> bool: + if self._runtime is None: + return False + if not self._runtime.disable(): + self._enabled = True + return False + self._enabled = False + try: + self._preflight_gravity() + except Exception: + logger.exception( + "damiao arm adapter rejected error-recovery enable preflight", + adapter=type(self).__name__, + hardware_id=self._hardware_id, + ) + return False + if not self._runtime.enable(): + return False + self._enabled = True + try: + ok = self.write_joint_positions(self.read_joint_positions()) + except Exception: + self._rollback_enable() + return False + if not ok: + self._rollback_enable() + return ok + + def _load_gravity_model(self) -> None: + if not self._gravity_comp or self._gravity_model_path is None or self._runtime is None: + return + loaded = self._runtime.load_gravity_model(self._group_name, self._gravity_model_path) + if loaded is not None: + self._pin_model, self._pin_data = loaded + + def compute_gravity_torques(self, q: list[float]) -> list[float]: + self._validate_length("q", q) + if self._pin_model is None or self._pin_data is None: + raise RuntimeError("gravity compensation model is not loaded") + import pinocchio # type: ignore[import-not-found] + + compute_generalized_gravity = _dynamic_attr(pinocchio, "computeGeneralizedGravity") + tau = compute_generalized_gravity( + self._pin_model, self._pin_data, np.array(q, dtype=np.float64) + ) + values = [float(tau[i]) for i in range(self._dof)] + if not np.isfinite(np.asarray(values, dtype=np.float64)).all(): + raise RuntimeError("gravity computation returned non-finite torque values") + if self._gravity_torque_limits is None: + return values + return [ + float(np.clip(value, -limit, limit)) + for value, limit in zip(values, self._gravity_torque_limits, strict=False) + ] + + +__all__ = ["DamiaoArmAdapter"] diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py new file mode 100644 index 0000000000..15a8469e27 --- /dev/null +++ b/dimos/hardware/damiao/runtime.py @@ -0,0 +1,399 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +import importlib +from pathlib import Path +import time +from typing import Any, cast + +import numpy as np + +from dimos.hardware.damiao.specs import DamiaoJointGroupSpec, DamiaoRobotSpec +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +_DEFAULT_TICK_DEADLINE_US = 1_000 +_DEFAULT_STATE_CACHE_TTL_S = 0.002 +_DEFAULT_ADDRESS = "can0" + + +class DamiaoBindingUnavailableError(RuntimeError): + """Raised when the optional can_motor_control binding is unavailable.""" + + +@dataclass(frozen=True) +class DamiaoGroupState: + """State vectors for one Damiao joint group.""" + + q: list[float] + dq: list[float] + tau: list[float] + + +def _load_can_motor_control( + *, + adapter_type: str, + error_type: type[RuntimeError] = DamiaoBindingUnavailableError, +) -> tuple[Any, Any]: + """Lazily load the optional Rust-backed binding and Damiao codec module.""" + + try: + can_motor_control = importlib.import_module("can_motor_control") + damiao = importlib.import_module("can_motor_control.damiao") + except ImportError as exc: + raise error_type( + f"The selected '{adapter_type}' adapter requires the Rust-backed " + "can-motor-control Python binding in the active environment. On " + "supported Linux systems, install dimos[manipulation] before " + f"selecting adapter_type='{adapter_type}'." + ) from exc + return can_motor_control, damiao + + +def _dynamic_attr(value: object, name: str) -> Any: + return getattr(value, name) + + +class DamiaoRobotRuntime: + """Binding-backed runtime for one Damiao-based robot spec.""" + + def __init__( + self, + *, + robot_spec: DamiaoRobotSpec, + adapter_type: str = "damiao", + binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError, + use_mock_bus: bool = False, + config_path: str | Path | None = None, + tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, + state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, + ) -> None: + robot_spec.validate() + self._robot_spec = robot_spec + self._adapter_type = adapter_type + self._binding_error_type = binding_error_type + self._use_mock_bus = use_mock_bus + self._config_path = str(config_path) if config_path is not None else None + self._tick_deadline_us = tick_deadline_us + self._state_cache_ttl_s = state_cache_ttl_s + self._robot: Any | None = None + self._groups: dict[str, Any] = {} + self._state_cache: dict[str, DamiaoGroupState] = {} + self._state_cache_time: dict[str, float] = {} + self._can_motor_control: Any | None = None + self._damiao: Any | None = None + self._connected = False + self._enabled = False + + @property + def robot_spec(self) -> DamiaoRobotSpec: + return self._robot_spec + + def connect(self) -> bool: + """Connect the binding robot and cache group handles.""" + + try: + self._can_motor_control, self._damiao = _load_can_motor_control( + adapter_type=self._adapter_type, + error_type=self._binding_error_type, + ) + robot = self._build_robot() + robot.connect() + groups: dict[str, Any] = {} + for group_name, group_spec in self._robot_spec.groups.items(): + group = robot[group_name] + if len(group) != group_spec.dof: + raise RuntimeError( + f"can_motor_control group {group_name!r} has {len(group)} joints, " + f"expected {group_spec.dof}" + ) + groups[group_name] = group + self._robot = robot + self._groups = groups + self._connected = True + for group_name in self._robot_spec.groups: + self.refresh_group_state(group_name, force=True) + except self._binding_error_type: + raise + except Exception: + logger.exception("damiao runtime connect failed", adapter=self._adapter_type) + self.disconnect() + return False + return True + + def _build_robot(self) -> Any: + if self._can_motor_control is None or self._damiao is None: + raise RuntimeError("can_motor_control binding is not loaded") + if self._config_path is not None: + return self._can_motor_control.Robot.from_config(self._config_path) + builder = self._can_motor_control.Robot.builder() + codec = self._damiao.DamiaoCodec() + for bus_name, bus_spec in self._robot_spec.buses.items(): + address = str(bus_spec.address or _DEFAULT_ADDRESS) + transport = ( + self._can_motor_control.MockCanBus.new_fd(address) + if self._use_mock_bus and bus_spec.fd + else self._can_motor_control.MockCanBus(address) + if self._use_mock_bus + else self._can_motor_control.SocketCanBus(address, fd=bus_spec.fd) + ) + builder = builder.add_bus(bus_name, transport, codec) + for group_name, group_spec in self._robot_spec.groups.items(): + binding_specs = [ + self._can_motor_control.MotorSpec( + motor.name, + cast("int", self._resolve_motor_type(motor.type)), + motor.send_id, + motor.effective_recv_id, + ) + for motor in group_spec.motors + ] + builder = builder.add_arm(group_name, bus=group_spec.bus_name, motors=binding_specs) + return builder.build() + + def _resolve_motor_type(self, motor_type: object) -> object: + if self._damiao is None: + raise RuntimeError("Damiao binding module is not loaded") + if isinstance(motor_type, str): + try: + return getattr(self._damiao.MotorType, motor_type) + except AttributeError as exc: + raise ValueError(f"Unknown Damiao motor type {motor_type!r}") from exc + if not isinstance(motor_type, int): + return motor_type + for name in dir(self._damiao.MotorType): + if name.startswith("_"): + continue + candidate = getattr(self._damiao.MotorType, name) + try: + candidate_value = int(candidate) + except (TypeError, ValueError): + continue + if candidate_value == motor_type: + return candidate + raise ValueError(f"Unknown Damiao motor type value {motor_type!r}") + + def disconnect(self) -> None: + """Disable and drop the underlying binding robot.""" + + disabled = True + if self._robot is not None: + try: + self._robot.disable() + except Exception: + logger.warning("damiao runtime disable on disconnect failed", exc_info=True) + disabled = False + self._enabled = False if disabled else True + self._connected = False + self._robot = None + self._groups = {} + self._state_cache = {} + self._state_cache_time = {} + + def is_connected(self) -> bool: + return self._connected + + def enable(self) -> bool: + if self._robot is None: + return False + try: + self._robot.enable() + except Exception: + logger.exception("damiao runtime enable failed", adapter=self._adapter_type) + # The binding may have enabled a subset of the robot before + # reporting an error. Never leave that partial state live. + try: + disabled = self._robot.disable() + except Exception: + logger.warning("damiao runtime rollback disable failed", exc_info=True) + disabled = False + if disabled is not True: + logger.error("damiao runtime partial enable could not disable hardware") + self._enabled = True + else: + self._enabled = False + return False + self._enabled = True + return True + + def disable(self) -> bool: + if self._robot is None: + return False + try: + self._robot.disable() + except Exception: + logger.exception("damiao runtime disable failed", adapter=self._adapter_type) + return False + self._enabled = False + return True + + def is_enabled(self) -> bool: + return self._enabled + + def group_spec(self, group_name: str) -> DamiaoJointGroupSpec: + try: + return self._robot_spec.groups[group_name] + except KeyError as exc: + raise ValueError(f"unknown Damiao group {group_name!r}") from exc + + def refresh_group_state(self, group_name: str, *, force: bool = False) -> DamiaoGroupState: + group_spec = self.group_spec(group_name) + group = self._groups.get(group_name) + if self._robot is None or group is None: + raise RuntimeError("DamiaoRobotRuntime is not connected") + now = time.monotonic() + cached = self._state_cache.get(group_name) + cached_at = self._state_cache_time.get(group_name, 0.0) + if not force and cached is not None and now - cached_at <= self._state_cache_ttl_s: + return cached + group.refresh() + self._robot.tick(self._tick_deadline_us) + state = DamiaoGroupState( + q=group.positions().astype(np.float64).tolist(), + dq=group.velocities().astype(np.float64).tolist(), + tau=group.torques().astype(np.float64).tolist(), + ) + if any(len(values) != group_spec.dof for values in (state.q, state.dq, state.tau)): + raise RuntimeError( + f"state length does not match configured DOF for group {group_name!r}" + ) + if any( + not np.isfinite(values).all() + for values in (np.asarray(state.q), np.asarray(state.dq), np.asarray(state.tau)) + ): + raise RuntimeError(f"state contains non-finite values for group {group_name!r}") + self._state_cache[group_name] = state + self._state_cache_time[group_name] = time.monotonic() + return state + + def has_group_states(self, group_names: Sequence[str]) -> bool: + """Return true only when every requested group has a fresh complete state.""" + + try: + for group_name in group_names: + self.refresh_group_state(group_name, force=False) + except Exception: + return False + return True + + def read_group_states(self, group_names: Sequence[str]) -> list[DamiaoGroupState]: + """Read state for groups in the requested order.""" + + return [self.refresh_group_state(group_name, force=False) for group_name in group_names] + + def write_group_mit_commands( + self, + *, + group_name: str, + q: Sequence[float], + dq: Sequence[float], + kp: Sequence[float], + kd: Sequence[float], + tau: Sequence[float], + ) -> bool: + """Write one MIT command frame to a group.""" + + group_spec = self.group_spec(group_name) + group = self._groups.get(group_name) + if self._robot is None or group is None or not self._enabled: + return False + if any(len(values) != group_spec.dof for values in (q, dq, kp, kd, tau)): + raise ValueError( + f"command length does not match configured DOF for group {group_name!r}" + ) + try: + group.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) + self._robot.tick(self._tick_deadline_us) + except Exception: + logger.exception("damiao runtime MIT command failed", group_name=group_name) + return False + self._state_cache.pop(group_name, None) + self._state_cache_time.pop(group_name, None) + return True + + def write_groups_mit_commands( + self, + commands: Mapping[ + str, + tuple[ + Sequence[float], Sequence[float], Sequence[float], Sequence[float], Sequence[float] + ], + ], + ) -> bool: + """Stage MIT commands for multiple groups and tick once. + + The binding's group ``mit_control`` call stages commands; ``robot.tick`` + sends them. Validate all groups and command lengths before staging so a + bad frame is rejected without sending a partial whole-body command. + """ + + if self._robot is None or not self._enabled: + return False + for group_name, values in commands.items(): + group_spec = self.group_spec(group_name) + group = self._groups.get(group_name) + if group is None: + return False + q, dq, kp, kd, tau = values + if any(len(vector) != group_spec.dof for vector in (q, dq, kp, kd, tau)): + raise ValueError( + f"command length does not match configured DOF for group {group_name!r}" + ) + try: + for group_name, values in commands.items(): + q, dq, kp, kd, tau = values + self._groups[group_name].mit_control( + np.column_stack([kp, kd, q, dq, tau]).astype(np.float64) + ) + self._robot.tick(self._tick_deadline_us) + except Exception: + logger.exception("damiao runtime batched MIT command failed") + return False + for group_name in commands: + self._state_cache.pop(group_name, None) + self._state_cache_time.pop(group_name, None) + return True + + def load_gravity_model( + self, + group_name: str, + model_path: str | Path | None = None, + ) -> tuple[object, object] | None: + """Load a Pinocchio gravity model for a configured group, if present.""" + + resolved_model_path = ( + model_path if model_path is not None else self.group_spec(group_name).gravity_model_path + ) + if resolved_model_path is None: + return None + import pinocchio # type: ignore[import-not-found] + + build_model_from_urdf = _dynamic_attr(pinocchio, "buildModelFromUrdf") + model = build_model_from_urdf(str(resolved_model_path)) + return model, _dynamic_attr(model, "createData")() + + +__all__ = [ + "_DEFAULT_ADDRESS", + "_DEFAULT_STATE_CACHE_TTL_S", + "_DEFAULT_TICK_DEADLINE_US", + "DamiaoBindingUnavailableError", + "DamiaoGroupState", + "DamiaoRobotRuntime", +] diff --git a/dimos/hardware/damiao/specs.py b/dimos/hardware/damiao/specs.py new file mode 100644 index 0000000000..f67d6a0ecf --- /dev/null +++ b/dimos/hardware/damiao/specs.py @@ -0,0 +1,316 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from pathlib import Path + + +@dataclass(frozen=True) +class DamiaoMotorSpec: + """Typed metadata for one Damiao motor in adapter joint order.""" + + name: str + type: object + send_id: int + recv_id: int | None = None + + @property + def effective_recv_id(self) -> int: + """Return the explicit receive CAN ID, or Damiao's default response ID.""" + + return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) + + +@dataclass(frozen=True) +class DamiaoBusSpec: + """Named communication channel for Damiao motors.""" + + address: str | Path = "can0" + fd: bool = False + + +@dataclass(frozen=True) +class DamiaoJointGroupSpec: + """Ordered Damiao joints forming a controllable physical group.""" + + bus_name: str + motors: tuple[DamiaoMotorSpec, ...] + position_lower: tuple[float, ...] + position_upper: tuple[float, ...] + velocity_max: tuple[float, ...] + kp: tuple[float, ...] + kd: tuple[float, ...] + gravity_model_path: str | Path | None = None + gravity_torque_limits: tuple[float, ...] | None = None + supports_velocity: bool = False + + @property + def dof(self) -> int: + """Return the number of joints described by this group spec.""" + + return len(self.motors) + + @property + def joint_names(self) -> tuple[str, ...]: + """Return joint names in command-vector order.""" + + return tuple(motor.name for motor in self.motors) + + def validate(self, *, group_name: str, bus_names: set[str] | None = None) -> None: + """Validate per-group metadata and optional bus reference.""" + + if not self.motors: + raise ValueError(f"DamiaoJointGroupSpec {group_name!r} requires at least one motor") + if bus_names is not None and self.bus_name not in bus_names: + raise ValueError(f"group {group_name!r} references unknown bus {self.bus_name!r}") + send_ids = [motor.send_id for motor in self.motors] + if len(set(send_ids)) != len(send_ids): + raise ValueError(f"duplicate send_id in group {group_name!r}: {send_ids}") + recv_ids = [motor.effective_recv_id for motor in self.motors] + if len(set(recv_ids)) != len(recv_ids): + raise ValueError(f"duplicate recv_id in group {group_name!r}: {recv_ids}") + joint_names = [motor.name for motor in self.motors] + if len(set(joint_names)) != len(joint_names): + raise ValueError(f"duplicate joint name in group {group_name!r}: {joint_names}") + for name, values in { + "position_lower": self.position_lower, + "position_upper": self.position_upper, + "velocity_max": self.velocity_max, + "kp": self.kp, + "kd": self.kd, + }.items(): + if len(values) != self.dof: + raise ValueError( + f"{name} length {len(values)} does not match dof {self.dof} " + f"for group {group_name!r}" + ) + for index, (lower, upper) in enumerate( + zip(self.position_lower, self.position_upper, strict=True), + ): + if lower > upper: + raise ValueError( + f"position_lower[{index}] > position_upper[{index}] for group {group_name!r}" + ) + if self.gravity_torque_limits is not None and len(self.gravity_torque_limits) != self.dof: + raise ValueError( + f"gravity_torque_limits length does not match dof for group {group_name!r}" + ) + + +@dataclass(frozen=True) +class DamiaoRobotSpec: + """Python-native Damiao robot config with named buses and joint groups.""" + + name: str + vendor: str + model: str + buses: Mapping[str, DamiaoBusSpec] + groups: Mapping[str, DamiaoJointGroupSpec] + requires_binding: bool = False + + @property + def joint_names(self) -> tuple[str, ...]: + """Return all group joint names in mapping iteration order.""" + + return tuple(joint for group in self.groups.values() for joint in group.joint_names) + + def group_joint_names(self, group_names: Sequence[str]) -> tuple[str, ...]: + """Return concatenated joint names for the requested groups.""" + + return tuple( + joint for group_name in group_names for joint in self.groups[group_name].joint_names + ) + + def validate(self) -> None: + """Validate bus/group references and global joint-name uniqueness.""" + + if not self.buses: + raise ValueError("DamiaoRobotSpec requires at least one bus") + if not self.groups: + raise ValueError("DamiaoRobotSpec requires at least one joint group") + bus_names = set(self.buses) + all_joint_names: list[str] = [] + ids_by_bus: dict[str, set[int]] = {bus_name: set() for bus_name in bus_names} + for group_name, group in self.groups.items(): + group.validate(group_name=group_name, bus_names=bus_names) + all_joint_names.extend(group.joint_names) + bus_ids = ids_by_bus[group.bus_name] + for motor in group.motors: + if motor.send_id in bus_ids: + raise ValueError(f"duplicate send_id {motor.send_id} on bus {group.bus_name!r}") + bus_ids.add(motor.send_id) + if len(set(all_joint_names)) != len(all_joint_names): + raise ValueError(f"duplicate joint names across DamiaoRobotSpec: {all_joint_names}") + + @classmethod + def from_arm_spec( + cls, + arm_spec: DamiaoArmSpec, + *, + address: str | Path = "can0", + ) -> DamiaoRobotSpec: + """Build a one-group robot spec from a compatibility arm spec.""" + + return cls( + name=arm_spec.name, + vendor=arm_spec.vendor, + model=arm_spec.model, + buses={arm_spec.bus_name: DamiaoBusSpec(address=address, fd=arm_spec.fd)}, + groups={ + arm_spec.arm_name: DamiaoJointGroupSpec( + bus_name=arm_spec.bus_name, + motors=arm_spec.motors, + position_lower=arm_spec.position_lower, + position_upper=arm_spec.position_upper, + velocity_max=arm_spec.velocity_max, + kp=arm_spec.kp, + kd=arm_spec.kd, + gravity_model_path=arm_spec.gravity_model_path, + gravity_torque_limits=arm_spec.gravity_torque_limits, + supports_velocity=arm_spec.supports_velocity, + ) + }, + requires_binding=arm_spec.requires_binding, + ) + + +@dataclass(frozen=True) +class DamiaoArmSpec: + """Compatibility metadata for a single Damiao arm/group adapter.""" + + name: str + vendor: str + model: str + motors: tuple[DamiaoMotorSpec, ...] + position_lower: tuple[float, ...] + position_upper: tuple[float, ...] + velocity_max: tuple[float, ...] + kp: tuple[float, ...] + kd: tuple[float, ...] + gravity_model_path: str | Path | None = None + gravity_torque_limits: tuple[float, ...] | None = None + requires_binding: bool = False + bus_name: str = "can" + arm_name: str = "arm" + fd: bool = False + supports_velocity: bool = False + + @property + def dof(self) -> int: + """Return the number of joints described by this arm spec.""" + + return len(self.motors) + + @property + def joint_names(self) -> tuple[str, ...]: + """Return joint names in adapter and command-vector order.""" + + return tuple(motor.name for motor in self.motors) + + @classmethod + def from_values( + cls, + *, + name: str, + vendor: str, + model: str, + motors: Sequence[Mapping[str, object] | DamiaoMotorSpec], + position_lower: list[float] | tuple[float, ...], + position_upper: list[float] | tuple[float, ...], + velocity_max: list[float] | tuple[float, ...], + kp: list[float] | tuple[float, ...], + kd: list[float] | tuple[float, ...], + gravity_model_path: str | Path | None = None, + gravity_torque_limits: list[float] | tuple[float, ...] | None = None, + requires_binding: bool = False, + bus_name: str = "can", + arm_name: str = "arm", + fd: bool = False, + supports_velocity: bool = False, + ) -> DamiaoArmSpec: + """Build a typed arm spec from list/tuple metadata values.""" + + return cls( + name=name, + vendor=vendor, + model=model, + motors=coerce_motor_specs(motors, len(motors)), + position_lower=tuple(float(value) for value in position_lower), + position_upper=tuple(float(value) for value in position_upper), + velocity_max=tuple(float(value) for value in velocity_max), + kp=tuple(float(value) for value in kp), + kd=tuple(float(value) for value in kd), + gravity_model_path=gravity_model_path, + gravity_torque_limits=( + tuple(float(value) for value in gravity_torque_limits) + if gravity_torque_limits is not None + else None + ), + requires_binding=requires_binding, + bus_name=bus_name, + arm_name=arm_name, + fd=fd, + supports_velocity=supports_velocity, + ) + + def validate(self) -> None: + """Validate CAN ID uniqueness and per-joint metadata lengths.""" + + DamiaoRobotSpec.from_arm_spec(self).validate() + + +def coerce_motor_specs( + motor_specs: Sequence[Mapping[str, object] | DamiaoMotorSpec], + dof: int, +) -> tuple[DamiaoMotorSpec, ...]: + """Normalize mapping or dataclass motor metadata into typed motor specs.""" + + specs: list[DamiaoMotorSpec] = [] + for spec in motor_specs: + if isinstance(spec, DamiaoMotorSpec): + specs.append(spec) + else: + name = spec.get("name") + send_id = spec.get("send_id") + recv_id = spec.get("recv_id") + if not isinstance(name, str): + raise TypeError("motor spec name must be a string") + if not isinstance(send_id, int): + raise TypeError("motor spec send_id must be an integer") + if recv_id is not None and not isinstance(recv_id, int): + raise TypeError("motor spec recv_id must be an integer") + specs.append( + DamiaoMotorSpec( + name=name, + type=spec.get("type"), + send_id=send_id, + recv_id=recv_id, + ) + ) + if len(specs) != dof: + raise ValueError(f"motor_specs length {len(specs)} does not match dof {dof}") + return tuple(specs) + + +__all__ = [ + "DamiaoArmSpec", + "DamiaoBusSpec", + "DamiaoJointGroupSpec", + "DamiaoMotorSpec", + "DamiaoRobotSpec", + "coerce_motor_specs", +] diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py new file mode 100644 index 0000000000..91c1da6b43 --- /dev/null +++ b/dimos/hardware/damiao/test_adapters.py @@ -0,0 +1,378 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter +from dimos.hardware.damiao.runtime import DamiaoGroupState, DamiaoRobotRuntime +from dimos.hardware.damiao.specs import ( + DamiaoArmSpec, + DamiaoBusSpec, + DamiaoJointGroupSpec, + DamiaoMotorSpec, + DamiaoRobotSpec, +) +from dimos.hardware.manipulators.spec import ControlMode + + +class _FakeRuntime: + def __init__(self, *, fresh: bool = True, write_ok: bool = True) -> None: + self.fresh = fresh + self.write_ok = write_ok + self.connected = False + self.enabled = False + self.disconnect_calls = 0 + self.batched_calls = 0 + self.writes: list[ + tuple[str, list[float], list[float], list[float], list[float], list[float]] + ] = [] + self.loaded_gravity_models: list[tuple[str, str | None]] = [] + self.states = { + "left": DamiaoGroupState(q=[0.1], dq=[0.2], tau=[0.3]), + "right": DamiaoGroupState(q=[-0.1], dq=[-0.2], tau=[-0.3]), + "arm": DamiaoGroupState(q=[0.4, -0.4], dq=[0.5, -0.5], tau=[0.6, -0.6]), + } + + def connect(self) -> bool: + self.connected = True + return True + + def disconnect(self) -> None: + self.disconnect_calls += 1 + self.connected = False + self.enabled = False + + def enable(self) -> bool: + self.enabled = True + return True + + def disable(self) -> bool: + self.enabled = False + return True + + def is_enabled(self) -> bool: + return self.enabled + + def refresh_group_state(self, group_name: str, *, force: bool = False) -> DamiaoGroupState: + del force + return self.states[group_name] + + def has_group_states(self, group_names: tuple[str, ...]) -> bool: + return self.fresh and all(group_name in self.states for group_name in group_names) + + def read_group_states(self, group_names: tuple[str, ...]) -> list[DamiaoGroupState]: + if not self.has_group_states(group_names): + raise RuntimeError("stale state") + return [self.states[group_name] for group_name in group_names] + + def write_group_mit_commands( + self, + *, + group_name: str, + q: list[float], + dq: list[float], + kp: list[float], + kd: list[float], + tau: list[float], + ) -> bool: + if not self.write_ok: + return False + self.writes.append((group_name, list(q), list(dq), list(kp), list(kd), list(tau))) + return True + + def write_groups_mit_commands( + self, + commands: dict[str, tuple[list[float], list[float], list[float], list[float], list[float]]], + ) -> bool: + self.batched_calls += 1 + if not self.write_ok: + return False + for group_name, values in commands.items(): + q, dq, kp, kd, tau = values + self.writes.append((group_name, list(q), list(dq), list(kp), list(kd), list(tau))) + return True + + def load_gravity_model(self, group_name: str, model_path: str | None = None) -> None: + self.loaded_gravity_models.append((group_name, model_path)) + return None + + +def _arm_spec() -> DamiaoArmSpec: + return DamiaoArmSpec( + name="test_damiao", + vendor="Damiao", + model="TestArm", + motors=( + DamiaoMotorSpec("j1", "DM4310", 0x01, 0x11), + DamiaoMotorSpec("j2", "DM4310", 0x02, 0x12), + ), + position_lower=(-1.0, -2.0), + position_upper=(1.0, 2.0), + velocity_max=(3.0, 4.0), + kp=(5.0, 6.0), + kd=(0.1, 0.2), + gravity_torque_limits=(7.0, 8.0), + ) + + +def _whole_body_spec() -> DamiaoRobotSpec: + return DamiaoRobotSpec( + name="test_body", + vendor="Damiao", + model="TestBody", + buses={ + "left_can": DamiaoBusSpec(address="can1", fd=True), + "right_can": DamiaoBusSpec(address="can0", fd=True), + }, + groups={ + "left": DamiaoJointGroupSpec( + bus_name="left_can", + motors=(DamiaoMotorSpec("left_joint", "DM4310", 0x01, 0x11),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(3.0,), + kp=(5.0,), + kd=(0.1,), + ), + "right": DamiaoJointGroupSpec( + bus_name="right_can", + motors=(DamiaoMotorSpec("right_joint", "DM4310", 0x01, 0x11),), + position_lower=(-2.0,), + position_upper=(2.0,), + velocity_max=(4.0,), + kp=(6.0,), + kd=(0.2,), + ), + }, + ) + + +def test_robot_spec_rejects_unknown_group_bus() -> None: + spec = DamiaoRobotSpec( + name="bad", + vendor="Damiao", + model="Bad", + buses={"can": DamiaoBusSpec()}, + groups={ + "arm": DamiaoJointGroupSpec( + bus_name="missing", + motors=(DamiaoMotorSpec("j1", "DM4310", 0x01, 0x11),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(1.0,), + kp=(1.0,), + kd=(0.1,), + ) + }, + ) + + with pytest.raises(ValueError, match="unknown bus"): + spec.validate() + + +def test_robot_spec_rejects_duplicate_send_ids_on_shared_bus() -> None: + spec = DamiaoRobotSpec( + name="bad_ids", + vendor="Damiao", + model="BadIds", + buses={"can": DamiaoBusSpec()}, + groups={ + "left": DamiaoJointGroupSpec( + bus_name="can", + motors=(DamiaoMotorSpec("left_joint", "DM4310", 0x01, 0x11),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(1.0,), + kp=(1.0,), + kd=(0.1,), + ), + "right": DamiaoJointGroupSpec( + bus_name="can", + motors=(DamiaoMotorSpec("right_joint", "DM4310", 0x01, 0x12),), + position_lower=(-1.0,), + position_upper=(1.0,), + velocity_max=(1.0,), + kp=(1.0,), + kd=(0.1,), + ), + }, + ) + + with pytest.raises(ValueError, match="duplicate send_id 1 on bus 'can'"): + spec.validate() + + +def test_arm_adapter_reports_limits_and_modes() -> None: + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_comp=False) + + assert adapter.get_dof() == 2 + assert adapter.get_limits().position_lower == [-1.0, -2.0] + assert adapter.set_control_mode(ControlMode.TORQUE) is True + assert adapter.set_control_mode(ControlMode.VELOCITY) is False + + +def test_arm_adapter_uses_fake_runtime_for_startup_hold(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_comp=False) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + assert adapter.write_enable(True) is True + + assert runtime.writes[-1] == ( + "arm", + [0.4, -0.4], + [0.0, 0.0], + [5.0, 6.0], + [0.1, 0.2], + [0.0, 0.0], + ) + + +def test_arm_adapter_passes_gravity_model_override_to_runtime(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec( + arm_spec=_arm_spec(), + gravity_model_path="override.urdf", + ) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + + assert runtime.loaded_gravity_models == [("arm", "override.urdf")] + + +def test_arm_adapter_rejects_nonfinite_positions_before_enable(mocker) -> None: + runtime = _FakeRuntime() + runtime.states["arm"] = DamiaoGroupState(q=[float("nan"), 0.0], dq=[0.0, 0.0], tau=[0.0, 0.0]) + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + assert adapter.write_enable(True) is False + assert runtime.enabled is False + assert runtime.writes == [] + + +def test_arm_adapter_gravity_compensation_rejects_missing_model_before_enable(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + assert adapter.write_enable(True) is False + assert runtime.enabled is False + assert runtime.writes == [] + + +def test_arm_adapter_error_recovery_runs_gravity_preflight(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + preflight = mocker.patch.object(adapter, "_preflight_gravity") + mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) + + assert adapter.connect() is True + assert adapter.write_clear_errors() is True + preflight.assert_called_once_with() + assert runtime.enabled is True + + +def test_arm_adapter_disables_without_zero_torque_on_gravity_state_failure(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + mocker.patch.object(adapter, "_load_gravity_model") + mocker.patch.object(adapter, "_preflight_gravity") + mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) + + assert adapter.connect() is True + assert adapter.write_enable(True) is True + writes_before_failure = list(runtime.writes) + runtime.refresh_group_state = mocker.Mock(side_effect=RuntimeError("state read failed")) + + assert adapter.write_joint_positions([0.2, -0.2]) is False + assert runtime.enabled is False + assert adapter.read_enabled() is False + assert runtime.writes == writes_before_failure + + +def test_arm_adapter_rejects_incompatible_gravity_model_before_enable(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_model_path="arm.urdf") + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + adapter._pin_model = SimpleNamespace(nq=2, nv=2, names=["universe", "j2", "j1"]) + mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) + + assert adapter.connect() is True + assert adapter.write_enable(True) is False + assert runtime.enabled is False + assert runtime.writes == [] + + +def test_arm_adapter_rolls_back_when_hold_command_fails(mocker) -> None: + runtime = _FakeRuntime(write_ok=False) + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + assert adapter.write_enable(True) is False + assert runtime.enabled is False + assert adapter.read_enabled() is False + + +def test_arm_adapter_preserves_enabled_state_when_rollback_disable_fails(mocker) -> None: + runtime = _FakeRuntime(write_ok=False) + runtime.disable = mocker.Mock(return_value=False) + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_comp=False) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + + assert adapter.connect() is True + assert adapter.write_enable(True) is False + assert adapter.read_enabled() is True + + +def test_arm_adapter_preserves_enabled_state_when_safety_disable_fails(mocker) -> None: + runtime = _FakeRuntime() + adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + mocker.patch.object(adapter, "_create_runtime", return_value=runtime) + mocker.patch.object(adapter, "_preflight_gravity") + mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) + + assert adapter.connect() is True + assert adapter.write_enable(True) is True + runtime.disable = mocker.Mock(return_value=False) + runtime.refresh_group_state = mocker.Mock(side_effect=RuntimeError("state read failed")) + + assert adapter.write_joint_positions([0.2, -0.2]) is False + assert adapter.read_enabled() is True + + +def test_runtime_preserves_enabled_state_when_partial_enable_rollback_fails() -> None: + class _FailingRobot: + def enable(self) -> None: + raise RuntimeError("partial enable") + + def disable(self) -> bool: + return False + + runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) + runtime._robot = _FailingRobot() + + assert runtime.enable() is False + assert runtime.is_enabled() is True diff --git a/dimos/hardware/manipulators/openyam_damiao/__init__.py b/dimos/hardware/manipulators/openyam_damiao/__init__.py new file mode 100644 index 0000000000..8f28372bf1 --- /dev/null +++ b/dimos/hardware/manipulators/openyam_damiao/__init__.py @@ -0,0 +1,20 @@ +# 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. + +"""Damiao hardware adapter for the six-axis OpenYAM arm.""" + +__all__ = ["OpenYAMDamiaoAdapter", "OpenYamDamiaoAdapter"] + + +def __getattr__(name: str) -> object: + if name in __all__: + from dimos.hardware.manipulators.openyam_damiao.adapter import ( + OpenYAMDamiaoAdapter, + OpenYamDamiaoAdapter, + ) + + return {"OpenYAMDamiaoAdapter": OpenYAMDamiaoAdapter, + "OpenYamDamiaoAdapter": OpenYamDamiaoAdapter}[name] + raise AttributeError(name) diff --git a/dimos/hardware/manipulators/openyam_damiao/_registry.py b/dimos/hardware/manipulators/openyam_damiao/_registry.py new file mode 100644 index 0000000000..7c69825064 --- /dev/null +++ b/dimos/hardware/manipulators/openyam_damiao/_registry.py @@ -0,0 +1,10 @@ +# 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. + +ADAPTER_FACTORIES = { + "openyam_damiao": ( + "dimos.hardware.manipulators.openyam_damiao.adapter:OpenYamDamiaoAdapter" + ), +} diff --git a/dimos/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py new file mode 100644 index 0000000000..d22ed37be4 --- /dev/null +++ b/dimos/hardware/manipulators/openyam_damiao/adapter.py @@ -0,0 +1,177 @@ +# 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. + +"""OpenYAM's six-axis Damiao adapter.""" + +from __future__ import annotations + +import math +from pathlib import Path +from typing import Any, cast + +from dimos.hardware.damiao import ( + DamiaoArmAdapter, + DamiaoBusSpec, + DamiaoJointGroupSpec, + DamiaoMotorSpec, + DamiaoRobotSpec, +) +from dimos.robot.model_parser import parse_model +from dimos.utils.data import LfsPath + +OPENING_METRES = 0.096 +_BUS_NAME = "openyam_can" +_ARM_GROUP = "arm" +_OPENYAM_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper.urdf.xacro" +_OPENYAM_PACKAGE_PATHS = {"yam_description": Path(LfsPath("yam_description"))} + +ARM_MOTOR_SPECS = tuple( + DamiaoMotorSpec( + name=f"yam_joint{index}", + type="DM4340" if index <= 3 else "DM4310", + send_id=index, + ) + for index in range(1, 7) +) +GRIPPER_MOTOR_SPECS = (DamiaoMotorSpec("yam_gripper", "DM4310", 7),) + + +def aperture_to_opening(aperture: float) -> float: + """Convert a metre aperture to the driver's normalized opening.""" + if not 0.0 <= aperture <= OPENING_METRES: + raise ValueError(f"gripper aperture must be in [0, {OPENING_METRES}] m") + return aperture / OPENING_METRES + + +def opening_to_aperture(opening: float) -> float: + """Convert a calibrated normalized opening to a metre aperture.""" + if not 0.0 <= opening <= 1.0: + raise ValueError("gripper opening must be in [0, 1]") + return opening * OPENING_METRES + + +def _group_spec( + *, + bus_name: str, + motors: tuple[DamiaoMotorSpec, ...], + lower: tuple[float, ...], + upper: tuple[float, ...], + velocity: tuple[float, ...], + kp: tuple[float, ...], + kd: tuple[float, ...], + gravity_model_path: str | Path | None = None, +) -> DamiaoJointGroupSpec: + return DamiaoJointGroupSpec( + bus_name=bus_name, + motors=motors, + position_lower=lower, + position_upper=upper, + velocity_max=velocity, + kp=kp, + kd=kd, + gravity_model_path=gravity_model_path, + ) + + +def _active_arm_limits() -> tuple[tuple[float, ...], tuple[float, ...], tuple[float, ...]]: + """Read arm limits from the active planning Xacro, failing closed.""" + model = parse_model(_OPENYAM_MODEL_PATH, package_paths=_OPENYAM_PACKAGE_PATHS) + names = [joint.name for joint in model.joints] + if len(names) != len(set(names)): + raise ValueError("active OpenYAM Xacro contains duplicate joint names") + joints = [model.get_joint(f"yam_joint{index}") for index in range(1, 7)] + resolved = [joint for joint in joints if joint is not None] + if len(resolved) != 6: + raise ValueError("active OpenYAM Xacro does not define all six arm joints") + for joint in resolved: + lower = joint.lower_limit + upper = joint.upper_limit + velocity = joint.velocity_limit + if lower is None or upper is None or velocity is None: + raise ValueError("active OpenYAM Xacro has incomplete or nonfinite arm limits") + lower = cast("float", lower) + upper = cast("float", upper) + velocity = cast("float", velocity) + if not all(math.isfinite(value) for value in (lower, upper, velocity)): + raise ValueError("active OpenYAM Xacro has incomplete or nonfinite arm limits") + if lower > upper: + raise ValueError(f"active OpenYAM Xacro has inverted limits for {joint.name}") + if velocity <= 0: + raise ValueError(f"active OpenYAM Xacro has nonpositive velocity for {joint.name}") + return ( + tuple(cast("float", joint.lower_limit) for joint in resolved), + tuple(cast("float", joint.upper_limit) for joint in resolved), + tuple(cast("float", joint.velocity_limit) for joint in resolved), + ) + + +class OpenYamDamiaoAdapter(DamiaoArmAdapter): + """Six-DOF OpenYAM arm; physical gripper IO is fail-closed.""" + + def __init__( + self, + address: str = "can0", + *, + gravity_model_path: str | Path | None = None, + gravity_comp: bool = True, + operator_approved: bool = False, + **kwargs: Any, + ) -> None: + if not gravity_comp: + raise ValueError("OpenYAM requires gravity compensation") + if gravity_model_path is None or not Path(gravity_model_path).is_file(): + raise ValueError("OpenYAM requires a valid gravity model path") + self._operator_approved = operator_approved + lower, upper, velocity = _active_arm_limits() + arm = _group_spec( + bus_name=_BUS_NAME, + motors=ARM_MOTOR_SPECS, + lower=lower, + upper=upper, + velocity=velocity, + kp=(0.0,) * 6, + kd=(0.0,) * 6, + gravity_model_path=gravity_model_path, + ) + robot_spec = DamiaoRobotSpec( + name="openyam", + vendor="Damiao", + model="OpenYAM", + buses={_BUS_NAME: DamiaoBusSpec(address=address)}, + # The upstream binding has no calibrated normalized gripper + # readback API. Do not expose a guessed/raw or last-command state. + groups={_ARM_GROUP: arm}, + ) + super().__init__( + robot_spec=robot_spec, + group_name=_ARM_GROUP, + gravity_model_path=gravity_model_path, + gravity_comp=True, + **kwargs, + ) + + def write_enable(self, enable: bool) -> bool: + """Gate every physical enable, including error-recovery enables.""" + if enable and not self._operator_approved: + return False + return super().write_enable(enable) + + def write_clear_errors(self) -> bool: + """Gate error recovery before it disables or re-enables the runtime.""" + if not self._operator_approved: + return False + return super().write_clear_errors() + + def read_gripper_position(self) -> float | None: + """Gripper feedback is disabled until the binding provides calibration.""" + return None + + def write_gripper_position(self, position: float) -> bool: + """Reject physical gripper commands without calibrated feedback.""" + del position + return False + + +OpenYAMDamiaoAdapter = OpenYamDamiaoAdapter diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py new file mode 100644 index 0000000000..0c18652419 --- /dev/null +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -0,0 +1,146 @@ +# 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. + +"""Focused OpenYAM adapter tests. + +The shared Damiao runtime is an optional hardware dependency in this checkout; +the tests become active when that runtime is installed (as they are in the +hardware test environment). +""" + +from pathlib import Path +from unittest.mock import Mock + +import pytest + +pytest.importorskip("dimos.hardware.damiao") + +import dimos.hardware.manipulators.openyam_damiao.adapter as adapter_module +from dimos.hardware.manipulators.openyam_damiao.adapter import ( + ARM_MOTOR_SPECS, + GRIPPER_MOTOR_SPECS, + OPENING_METRES, + OpenYamDamiaoAdapter, + aperture_to_opening, + opening_to_aperture, +) +from dimos.robot.model_parser import JointDescription, ModelDescription +from dimos.utils.data import LfsPath + +GRAVITY_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper_gravity.urdf" + + +def test_gripper_aperture_conversion_is_linear() -> None: + assert aperture_to_opening(0.0) == 0.0 + assert aperture_to_opening(OPENING_METRES / 2) == pytest.approx(0.5) + assert aperture_to_opening(OPENING_METRES) == 1.0 + assert opening_to_aperture(0.5) == pytest.approx(OPENING_METRES / 2) + + +def test_openyam_motor_topology() -> None: + assert [motor.name for motor in ARM_MOTOR_SPECS] == [f"yam_joint{i}" for i in range(1, 7)] + assert [motor.send_id for motor in ARM_MOTOR_SPECS] == list(range(1, 7)) + assert [motor.type for motor in ARM_MOTOR_SPECS] == ["DM4340"] * 3 + ["DM4310"] * 3 + assert GRIPPER_MOTOR_SPECS[0].send_id == 7 + assert GRIPPER_MOTOR_SPECS[0].type == "DM4310" + + +def test_openyam_physical_gripper_is_disabled_without_calibrated_readback() -> None: + adapter = OpenYamDamiaoAdapter(gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True) + + assert adapter.read_gripper_position() is None + assert not adapter.write_gripper_position(0.01) + + +def test_openyam_limits_are_loaded_from_active_model() -> None: + adapter = OpenYamDamiaoAdapter(gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True) + + limits = adapter.get_limits() + assert limits.position_lower == pytest.approx([-3.92699, 0.0, 0.0, -1.65806, -1.5708, -2.35619]) + assert limits.position_upper == pytest.approx([1.5708, 3.66519, 4.01426, 1.65806, 1.5708, 1.8326]) + assert limits.velocity_max == pytest.approx([3.0, 10.0, 3.0, 10.0, 3.0, 10.0]) + + +def test_openyam_requires_gravity_comp_and_model() -> None: + with pytest.raises(ValueError, match="gravity model"): + OpenYamDamiaoAdapter(use_mock_bus=True) + with pytest.raises(ValueError, match="gravity compensation"): + OpenYamDamiaoAdapter( + gravity_model_path=GRAVITY_MODEL_PATH, + gravity_comp=False, + use_mock_bus=True, + ) + + +def test_openyam_operator_gate_rejects_normal_and_recovery_enable() -> None: + adapter = OpenYamDamiaoAdapter(gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True) + runtime = Mock() + adapter._runtime = runtime + + assert not adapter.activate() + assert not adapter.write_enable(True) + assert not adapter.write_clear_errors() + runtime.disable.assert_not_called() + runtime.enable.assert_not_called() + + +def test_openyam_operator_gate_allows_approved_enable() -> None: + adapter = OpenYamDamiaoAdapter( + gravity_model_path=GRAVITY_MODEL_PATH, + operator_approved=True, + use_mock_bus=True, + ) + runtime = Mock() + runtime.enable.return_value = True + adapter._runtime = runtime + adapter._preflight_gravity = Mock() + adapter.read_joint_positions = Mock(return_value=[0.0] * 6) + adapter.write_joint_positions = Mock(return_value=True) + + assert adapter.activate() + runtime.enable.assert_called_once_with() + + runtime.reset_mock() + runtime.disable.return_value = True + assert adapter.write_clear_errors() + runtime.disable.assert_called_once_with() + runtime.enable.assert_called_once_with() + + +def test_openyam_xacro_limits_reject_duplicate_joint_names(monkeypatch: pytest.MonkeyPatch) -> None: + joints = [ + JointDescription(f"yam_joint{i}", "revolute", -1.0, 1.0, 1.0) for i in range(1, 7) + ] + joints.append(joints[0]) + monkeypatch.setattr(adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints)) + + with pytest.raises(ValueError, match="duplicate"): + adapter_module._active_arm_limits() + + +@pytest.mark.parametrize( + ("lower", "upper", "velocity"), + [(2.0, 1.0, 1.0), (0.0, 1.0, 0.0), (0.0, 1.0, float("nan"))], +) +def test_openyam_xacro_limits_reject_bad_values( + monkeypatch: pytest.MonkeyPatch, lower: float, upper: float, velocity: float +) -> None: + joints = [ + JointDescription( + f"yam_joint{i}", "revolute", lower if i == 1 else -1.0, + upper if i == 1 else 1.0, velocity if i == 1 else 1.0, + ) + for i in range(1, 7) + ] + monkeypatch.setattr(adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints)) + + with pytest.raises(ValueError): + adapter_module._active_arm_limits() + + +@pytest.mark.parametrize("value", [-1e-6, OPENING_METRES + 1e-6]) +def test_gripper_aperture_rejects_out_of_range(value: float) -> None: + with pytest.raises(ValueError): + aperture_to_opening(value) diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 13c38804b8..09647a3e1f 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -47,7 +47,15 @@ # 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", + "openarm", + "openyam_damiao", + "piper", + "sim_mujoco", + "xarm", + }, "drive_trains": { "flowbase", "mock_twist_base", diff --git a/dimos/robot/manipulators/openyam/blueprints/basic.py b/dimos/robot/manipulators/openyam/blueprints/basic.py index cd9fbbcbb5..b24e19f256 100644 --- a/dimos/robot/manipulators/openyam/blueprints/basic.py +++ b/dimos/robot/manipulators/openyam/blueprints/basic.py @@ -20,11 +20,11 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.robot.manipulators.common.blueprints import coordinator, planner, trajectory_task from dimos.robot.manipulators.openyam.config import ( - make_openyam_hardware, make_openyam_model_config, + openyam_hardware, ) -_openyam_planner_hw = make_openyam_hardware("arm") +_openyam_planner_hw = openyam_hardware("arm") openyam_planner_coordinator = autoconnect( planner(robots=[make_openyam_model_config(name="arm")]), @@ -34,7 +34,7 @@ ), ) -_openyam_hw = make_openyam_hardware("arm") +_openyam_hw = openyam_hardware("arm") coordinator_openyam = ControlCoordinator.blueprint( hardware=[_openyam_hw], diff --git a/dimos/robot/manipulators/openyam/blueprints/teleop.py b/dimos/robot/manipulators/openyam/blueprints/teleop.py index 5c0fb521e1..41156c6f01 100644 --- a/dimos/robot/manipulators/openyam/blueprints/teleop.py +++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py @@ -23,12 +23,12 @@ from dimos.robot.manipulators.openyam.config import ( OPENYAM_DOF, OPENYAM_MODEL_PATH, - make_openyam_hardware, 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("arm") keyboard_teleop_openyam = autoconnect( KeyboardTeleopModule.blueprint(), diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index 84525e32f4..54d8f09ae3 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -19,6 +19,7 @@ from pathlib import Path from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.core.global_config import global_config from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import ( base_pose, @@ -30,28 +31,35 @@ OPENYAM_DOF = 6 OPENYAM_PACKAGE = LfsPath("yam_description") OPENYAM_MODEL_PATH = OPENYAM_PACKAGE / "urdf/yam_gripper.urdf.xacro" +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", *, + adapter_type: str = "mock", + address: str | None = None, auto_enable: bool = True, home_joints: list[float] | None = None, + adapter_kwargs: dict[str, object] | None = None, + include_gripper: bool = True, ) -> HardwareComponent: - """Create OpenYAM hardware, defaulting to the generic mock adapter.""" - adapter_kwargs: dict[str, object] = {} - if home_joints is not None: - adapter_kwargs["initial_positions"] = home_joints + """Create OpenYAM hardware with six arm joints and one gripper channel.""" + kwargs: dict[str, object] = {} + if adapter_type == "mock" and home_joints is not None: + kwargs["initial_positions"] = home_joints + if adapter_kwargs: + kwargs.update(adapter_kwargs) return HardwareComponent( hardware_id=hw_id, hardware_type=HardwareType.MANIPULATOR, joints=make_joints(hw_id, OPENYAM_DOF), - adapter_type="mock", - address=None, + adapter_type=adapter_type, + address=address, auto_enable=auto_enable, - gripper_joints=[f"{hw_id}/gripper"], - adapter_kwargs=adapter_kwargs, + gripper_joints=[f"{hw_id}/gripper"] if include_gripper else [], + adapter_kwargs=kwargs, ) @@ -60,8 +68,23 @@ def openyam_hardware( *, 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) + """Select mock hardware in simulation and the OpenYAM Damiao adapter on hardware.""" + if global_config.simulation: + return make_openyam_hardware(hw_id, home_joints=home_joints) + if not Path(OPENYAM_GRAVITY_MODEL_PATH).is_file(): + raise ValueError(f"OpenYAM gravity model is missing: {OPENYAM_GRAVITY_MODEL_PATH}") + return make_openyam_hardware( + hw_id, + adapter_type="openyam_damiao", + address=global_config.can_port or "can0", + # Physical encoder zeros are established by the driver; never pass + # planning/home positions into a live motor adapter. + adapter_kwargs={ + "gravity_model_path": OPENYAM_GRAVITY_MODEL_PATH, + "operator_approved": global_config.openyam_operator_approved, + }, + include_gripper=False, + ) def make_openyam_model_config( diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index f09524e64b..4f79814005 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -16,6 +16,7 @@ from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config from dimos.hardware.manipulators.mock.adapter import MockAdapter from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig from dimos.robot.manipulators.openyam.blueprints.basic import ( @@ -27,9 +28,11 @@ ) from dimos.robot.manipulators.openyam.config import ( OPENYAM_DOF, + OPENYAM_GRAVITY_MODEL_PATH, OPENYAM_PACKAGE_PATHS, make_openyam_hardware, make_openyam_model_config, + openyam_hardware, ) @@ -62,6 +65,41 @@ def test_openyam_mock_hardware_has_gripper() -> None: assert hardware.gripper_joints == ["arm/gripper"] +def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: Any) -> None: + monkeypatch.setattr(global_config, "simulation", "") + monkeypatch.setattr(global_config, "can_port", "can1") + + hardware = openyam_hardware("arm") + + assert hardware.adapter_type == "openyam_damiao" + assert hardware.address == "can1" + assert hardware.adapter_kwargs["gravity_model_path"] == OPENYAM_GRAVITY_MODEL_PATH + assert hardware.adapter_kwargs["operator_approved"] is False + assert len(hardware.joints) == OPENYAM_DOF + assert hardware.gripper_joints == [] + assert "initial_positions" not in hardware.adapter_kwargs + + direct = make_openyam_hardware( + "arm", + adapter_type="openyam_damiao", + home_joints=[0.1] * OPENYAM_DOF, + ) + assert "initial_positions" not in direct.adapter_kwargs + + monkeypatch.setattr(global_config, "openyam_operator_approved", True) + approved = openyam_hardware("arm") + assert approved.adapter_kwargs["operator_approved"] is True + + +def test_openyam_simulation_hardware_remains_mock(monkeypatch: Any) -> None: + monkeypatch.setattr(global_config, "simulation", "mujoco") + + hardware = openyam_hardware("arm") + + assert hardware.adapter_type == "mock" + assert hardware.address is None + + 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) diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index c4f715aa17..ad9b2d06c4 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -8,6 +8,13 @@ manipulation visualization supports Meshcat or Viser. ## Quick Start +For the required vendor/approved-bench direction verification before OpenYAM +planning or teleoperation, see the +[OpenYAM direction commissioning guide](./openyam_commissioning.md). The +DimOS driver does not issue commissioning position steps. +Physical runs are default-deny until the operator supplies +`--openyam-operator-approved` after completing the approved external checks. + Recent addition: the A-750 keyboard teleop blueprint is now available via: ```bash diff --git a/docs/capabilities/manipulation/openyam_commissioning.md b/docs/capabilities/manipulation/openyam_commissioning.md new file mode 100644 index 0000000000..4dafaf029d --- /dev/null +++ b/docs/capabilities/manipulation/openyam_commissioning.md @@ -0,0 +1,134 @@ +--- +title: "OpenYAM Direction Commissioning" +--- + +## Purpose + +Before using normal OpenYAM planning or teleoperation on hardware, obtain an +approved vendor/bench-tool direction verification for every planning joint. +This driver cannot perform that verification: its OpenYAM gravity-compensation +path intentionally uses zero position gains and therefore cannot safely issue +position-step commissioning commands. Do not use this procedure to validate +the gripper. + +The six planning joints are mapped as follows: + +| Hardware joint | Planning joint | Expected positive mapping | +|---|---|---| +| `arm/joint1` | `yam_joint1` | A positive command increases `yam_joint1` position | +| `arm/joint2` | `yam_joint2` | A positive command increases `yam_joint2` position | +| `arm/joint3` | `yam_joint3` | A positive command increases `yam_joint3` position | +| `arm/joint4` | `yam_joint4` | A positive command increases `yam_joint4` position | +| `arm/joint5` | `yam_joint5` | A positive command increases `yam_joint5` position | +| `arm/joint6` | `yam_joint6` | A positive command increases `yam_joint6` position | + +The expected result is about the commanded joint only. The physical direction +depends on the joint's zero pose and installation; use the measured encoder +change, not a visual guess about clockwise or counter-clockwise motion, as the +pass criterion. + +## Lifecycle and safety behavior + +Stopping the driver or handling a fault does **not** automatically park the arm. +Neither event should be documented or relied on as a motion command. On stop or +fault, disable motion output and keep the workspace clear; the operator must +assess the hardware state and use the emergency or manual disable path as +appropriate. + +Parking is an explicit operator action that is permitted only while the system +is healthy and the operator has confirmed that a controlled park motion is safe. +Do not attempt to park from a fault handler, during an unhealthy state, or as an +implicit part of shutdown. A no-motion disable is the safe default when motion +must be prevented: it disables commanded motion without issuing a park move. + +Gravity-compensation mode also requires a preflight before activation. Verify +the supported OpenYAM inertial model, its required asset, joint state, limits, +and the mechanical support and workspace conditions. If any preflight check is +missing or invalid, do not enable gravity compensation; use the no-motion +disable path and resolve the issue first. + +## Safety prerequisites + +Complete all of the following before enabling the hardware: + +- Remove the payload, tool, and any object held by the gripper. +- Secure the base and support the arm so an unexpected motion cannot cause a + fall, collision, or pinch. +- Clear the workspace. Keep people, cables, and tools outside the motion + envelope, and keep an operator at the emergency stop. +- Confirm the emergency stop, motor power cut-off, and manual disable path + work before the first command. +- Confirm the correct CAN interface, motor IDs, joint limits, and zero/calibration + state. Stop if any state reading is missing, implausible, or stale. +- Use the lowest approved position gains and speed, and choose a small test + increment that stays well inside the joint limits. Never test at a limit. + +If the arm is not mechanically supported or any prerequisite is uncertain, do +not enable it. + +## Gripper status + +Physical gripper control and physical gripper position readback are **not +available** in this integration. Do not use, advertise, or validate a gripper +command or readback as a supported capability. Upstream must first release a +calibrated normalized-opening getter; only after that upstream dependency is +released and integrated can gripper support be reconsidered. The direction +commissioning procedure below applies to the six planning joints only, not the +gripper. + +## Operator approval gate + +Physical OpenYAM startup is default-deny. The operator must explicitly approve +the external direction-verification record and the safety preconditions before +enabling the driver: + +```bash +dimos --openyam-operator-approved run openyam-planner-coordinator +``` + +`--openyam-operator-approved` is a per-run acknowledgement, not a calibration +or a bypass of the gravity-model and state preflights. Without it, the adapter +will not enable motors, including an enable attempted during error recovery. +Mock/simulation hardware is unaffected by this physical approval gate. + +## External direction verification precondition + +Before starting this driver, have an approved vendor tool or controlled bench +procedure verify `arm/joint1` through `arm/joint6` individually. That external +procedure must use its own documented safe low-speed command path; this driver +must not be used to issue the direction-test steps. Record the measured encoder +sign, joint identity, and any coupled motion. Stop and correct wiring, +calibration, or configuration if a sign is reversed, a joint is swapped, or +unexpected motion occurs. + +Only after the approved record is complete may this driver be connected for +gravity-compensated operation. On connection, verify state and limits without +requesting commissioning steps; disable output if any reading is stale or +implausible. + +## Record and approve the result + +Record one entry per joint in the commissioning log, including: + +- date, operator, hardware/firmware identity, and CAN interface; +- initial position and measured direction result from the approved external + procedure, including the tool used; +- commanded joint, observed `yam_jointN`, and whether any other joint moved; +- pass/fail status, faults or warnings, and the corrective action for failures. + +Normal hardware motion is **prohibited until all six directions pass** and the +record is reviewed by the responsible operator. A software test pass is not an +approval to skip this step. + +## Software coverage versus hardware validation + +The software tests can verify that OpenYAM exposes six joints, that +`arm/joint1` through `arm/joint6` map to `yam_joint1` through `yam_joint6`, and +that mock hardware accepts position updates. These tests do not energize a +motor and cannot detect reversed motor wiring, encoder polarity, swapped CAN +IDs, installation-specific motion, or unexpected mechanical coupling. + +Only the approved external procedure above verifies the actual direction +mapping on connected OpenYAM hardware. Treat the software coverage and the +on-hardware commissioning record as separate requirements; both must be +complete before normal planning, execution, or teleoperation. diff --git a/openspec/changes/add-openyam-damiao-adapter/design.md b/openspec/changes/add-openyam-damiao-adapter/design.md index 740722b889..98407a7a08 100644 --- a/openspec/changes/add-openyam-damiao-adapter/design.md +++ b/openspec/changes/add-openyam-damiao-adapter/design.md @@ -1,18 +1,11 @@ ## Context -OpenYAM's current robot configuration selects a mock manipulator adapter. The -shared Damiao runtime already manages `can-motor-control` connection, -refresh, MIT commands, and Pinocchio gravity feed-forward; `DamiaoArmAdapter` -provides the six-or-more joint `ManipulatorAdapter` facade used by the control -coordinator. Its gripper methods are placeholders because it models one joint -group whose motor count defines the arm DOF. - OpenYAM is one CAN-bus arm with six planning joints and a separate gripper: CAN IDs 1–3 are DM4340, 4–6 are DM4310, and ID 7 is a DM4310 gripper. The -gripper must retain the manipulator API's metre aperture contract while the -driver uses its calibrated normalized opening representation. The arm must -enter gravity compensation only with a materialized, gripper-equipped URDF -that Pinocchio can load directly; the runtime cannot expand Xacro. +gravity model is a fixed-finger, six-DOF, gravity-only URDF. It is deliberately +separate from the active Xacro, which remains the planning and command-limit +authority; the runtime cannot expand Xacro. Supported Linux deployment +architectures SHALL include x86_64. ## Goals / Non-Goals @@ -21,12 +14,15 @@ that Pinocchio can load directly; the runtime cannot expand Xacro. - Reuse the Damiao runtime, specs, and `DamiaoArmAdapter` pattern without inheriting OpenArm-specific kinematics or hardware metadata. - Present exactly six arm DOFs in `yam_joint1` through `yam_joint6` order. -- Control the gripper as an internal second motor group and expose aperture in - metres through the existing manipulator interface. -- Activate the arm with zero position/velocity gains and valid `G(q)` - feed-forward torque from a stable expanded URDF. -- Preserve encoder-zero home and make motor direction a hardware commissioning - check rather than an implicit software correction. +- Keep the gripper unavailable until a released upstream `can-motor-control` + normalized, calibrated opening getter exists; do not fabricate gripper state. +- Require every enable and recovery path to load the gravity model and complete + finite-`G(q)` preflight before zero-gain gravity operation; failures must + disable with no motion command and must not send zero torque. +- Default-deny physical enable unless the explicit operator configuration + `openyam_operator_approved` is true, exposed through the CLI as + `--openyam-operator-approved`. +- Preserve physical encoder-zero home; initial positions are mock-only. **Non-Goals:** @@ -34,7 +30,8 @@ that Pinocchio can load directly; the runtime cannot expand Xacro. structure. - Using the whole-body Damiao adapter. - Adding a homing routine, stored encoder offsets, or a seventh arm DOF. -- Generating Xacro at runtime or merging limits from the generated bare URDF. +- Generating Xacro at runtime or using the gravity URDF for planning limits. +- Providing fabricated gripper state or an invented upstream getter. ## Decisions @@ -42,54 +39,78 @@ that Pinocchio can load directly; the runtime cannot expand Xacro. The adapter SHALL construct a six-motor arm `DamiaoJointGroupSpec` and retain the inherited arm command/state behavior. It SHALL add a second internal -single-motor group for the gripper and override only gripper state/command -methods. This preserves the established adapter lifecycle while ensuring -`get_dof()` remains six. +single-motor group for the gripper, without counting it in `get_dof()`. Gripper +position and command operations SHALL remain unavailable until released +upstream normalized calibrated-opening getter support exists. Using `DamiaoWholeBodyAdapter` was rejected because OpenYAM is one -manipulator, not a multi-limb robot, and its raw motor command path is not the -needed contract. Copying the OpenArm adapter wholesale was rejected because -its seven-joint topology, geometry, and calibration are OpenArm-specific. +manipulator, not a multi-limb robot. Copying the OpenArm adapter wholesale was +rejected because its topology, geometry, and calibration are OpenArm-specific. ### Encode OpenYAM hardware metadata locally The adapter's arm group SHALL specify DM4340 motors at CAN IDs 1–3 and DM4310 motors at IDs 4–6 in planning-joint order. The gripper group SHALL specify its -DM4310 at CAN ID 7. OpenYAM gains and limits SHALL derive from its established -motor configuration; planning limits SHALL come only from the active gripper -Xacro. Existing encoder zeroes remain the home reference, so no offset or -homing configuration is added. - -### Keep gripper calibration and normalization adapter-local - -The public gripper interface SHALL use aperture metres. The adapter SHALL map -the supported aperture range linearly to the driver opening range, where zero -is closed and one is open, using calibrated endpoints. The nominal OpenYAM -aperture span is 0.096 m; endpoint calibration follows the -`can-motor-control` gripper opening lifecycle and remains in memory for the -active connection. This avoids leaking motor-angle or normalized units into -the generic API. - -### Require an expanded gravity-model asset for activation - -The OpenYAM hardware configuration SHALL provide an LFS-backed, expanded, -gripper-equipped URDF through `gravity_model_path`. On activation, the -inherited gravity-command path sends `Kp=Kd=0` and `tau=G(q)`. Passing a Xacro -was rejected because the runtime uses `pinocchio.buildModelFromUrdf()` directly -and has no Xacro/package-resolution support. Zero gains without a loaded model -are explicitly not gravity compensation. +DM4310 at CAN ID 7. OpenYAM gains SHALL derive from its motor configuration. +Hardware limits SHALL be parsed fail-closed from the active gripper Xacro and +never from the gravity URDF. The parser SHALL reject duplicate joints, missing +or nonfinite values, and invalid ranges. Physical encoder zeroes remain the +home reference; the physical factory SHALL reject mock-only initial-position +configuration, with no homing or offset behavior. + +### Keep gripper support upstream-first + +When the upstream getter is released, the public gripper interface SHALL use +aperture metres and convert linearly to the driver's calibrated normalized +opening, where zero is closed and one is open. Until then, the adapter SHALL +report the capability as unavailable and SHALL neither command nor synthesize +its state. The nominal future aperture span is 0.096 m; endpoint calibration +must follow the upstream opening lifecycle. + +### Require a fixed-finger gravity-model asset for activation + +The hardware configuration SHALL provide an LFS-backed, expanded, fixed-finger +six-DOF URDF through `gravity_model_path`. It is gravity-only and is not a +source of planning joints or command limits. Before enabling or recovering the +arm, activation SHALL load the model, evaluate `G(q)` for the current six-joint +state, and reject non-finite results. Every such path SHALL complete this +preflight before sending any zero-gain command. If model loading, gravity +evaluation, enabling, or recovery fails, the arm SHALL be disabled with no +motion command; it SHALL not send zero torque as a failure response. Only +after successful preflight may the command path send `Kp=Kd=0` and `tau=G(q)`. + +### Require explicit operator approval for physical enable + +Physical OpenYAM enable SHALL default to denied. The only approval input is the +explicit operator configuration `openyam_operator_approved`, exposed as the +`--openyam-operator-approved` CLI option. Both normal enable and error-recovery +enable SHALL reject before motor enable when this value is absent or false. +Operator approval does not bypass gravity-model preflight, direction +commissioning, limit validation, or any other safety gate. + +Disabling SHALL be a no-motion operation by default. An optional park action +may be requested explicitly as a separate operation; disabling must never +implicitly park the arm. If a no-motion disable fails, the resulting state is +explicitly unresolved energized/unknown: it SHALL NOT be reported as disabled. +The system SHALL escalate to the operator and the approved e-stop procedure. ## Risks / Trade-offs - [Incorrect motor direction can move a joint opposite its planning command] - → Validate every motor under no-load direction commissioning before normal - operation; do not silently assume the configured sign is physically correct. -- [Gripper endpoint calibration actuates into physical end stops] - → Use the driver's bounded calibration procedure and validate aperture - travel without copying example CAN IDs or exposing an uncalibrated command. -- [An unavailable or invalid LFS gravity asset would produce limp zero-gain - behavior] → Validate the asset can be materialized and loaded by Pinocchio - before enabling gravity-compensation mode. + → Require an approved external vendor/bench-tool direction commissioning + result for all six joints before hardware enable. This is a precondition, + not an adapter-side motion routine: the driver enters gravity mode with + zero gains, so the adapter must not attempt to discover direction in control. +- [Gripper support is unavailable or calibration actuates into end stops] + → Block the gripper API until released upstream getter support exists; then + use only the driver's bounded calibration procedure. +- [An unavailable, invalid, or numerically unstable gravity asset could enable + unsafe zero-gain behavior] → Require model load and finite-`G(q)` preflight + on every enable/recovery path; use no-motion disable, never zero torque, on + failure. +- [No-motion disable can fail while the driver remains energized or unknown] + → Preserve an unresolved energized/unknown state, never claim disabled, and + escalate to the operator and approved e-stop procedure. - [URDF/Xacro sources disagree on limits or inertials] → Treat the active - gripper Xacro as the sole arm-limit authority and the expanded gripper URDF - as the gravity-model authority. + gripper Xacro as the fail-closed planning-limit authority and the fixed-finger + URDF as gravity-only authority. diff --git a/openspec/changes/add-openyam-damiao-adapter/proposal.md b/openspec/changes/add-openyam-damiao-adapter/proposal.md index f487100cf9..4d85c37658 100644 --- a/openspec/changes/add-openyam-damiao-adapter/proposal.md +++ b/openspec/changes/add-openyam-damiao-adapter/proposal.md @@ -2,23 +2,38 @@ OpenYAM currently uses a mock manipulator adapter, so DimOS cannot operate the physical arm. The existing Damiao runtime and OpenArm adapter establish a -tested integration pattern that can be reused while keeping OpenYAM's motor, -gripper, and kinematic-model requirements explicit. +reusable integration pattern, while OpenYAM needs explicit separation between +its gravity asset, active planning Xacro, hardware limits, and unavailable +gripper state. The physical integration also targets supported Linux hosts +including x86_64. ## What Changes - Add a hardware-backed OpenYAM manipulator adapter built on the shared Damiao runtime and generic arm adapter. -- Configure the six-joint arm as CAN IDs 1–6 (DM4340 shoulder group and - DM4310 distal group), while keeping the DM4310 CAN-ID-7 gripper separate - from the arm's six degrees of freedom. -- Expose the gripper through the existing manipulator API in aperture metres, - with adapter-local calibrated conversion to the driver's normalized opening. -- Activate the arm in gravity-compensation mode using zero position and - velocity gains plus feed-forward gravity torque from a stable, expanded - gripper-equipped URDF asset. -- Replace the OpenYAM mock adapter configuration with the hardware adapter and - register the physical hardware implementation. +- Configure the six-joint arm as CAN IDs 1–6 (DM4340 shoulder group and DM4310 + distal group), while keeping the DM4310 CAN-ID-7 gripper separate from the + arm's six degrees of freedom. +- Keep gripper operations unavailable until a released upstream + `can-motor-control` normalized calibrated-opening getter exists; do not + fabricate state. Then expose aperture metres through the existing API. +- Activate the arm in gravity-only compensation using zero position and + velocity gains plus feed-forward gravity torque from a stable, expanded, + fixed-finger six-DOF URDF, with loaded-model and finite-`G(q)` preflight on + every enable/recovery path. Gravity failure performs no-motion disable and + never sends zero torque. +- Keep that gravity URDF separate from the active Xacro, which is the + fail-closed planning and hardware-limit authority; reject duplicate, + nonfinite, missing, or invalid Xacro limit entries. +- Use physical encoder-zero home; the physical factory rejects mock-only + initial positions. Require approved external vendor/bench-tool direction + commissioning before enable because the driver enters gravity mode at zero + gains. Disable is no-motion by default, while park is explicit; a failed + disable remains unresolved energized/unknown, is never reported disabled, + and escalates to operator/e-stop procedure. +- Default-deny physical enable behind explicit `openyam_operator_approved` + configuration, exposed as `--openyam-operator-approved`; reject normal and + error-recovery enable before motor enable without it. ## Capabilities @@ -37,8 +52,7 @@ gripper, and kinematic-model requirements explicit. `dimos/hardware/manipulators/`. - Updates `dimos/robot/manipulators/openyam/config.py` to select the hardware implementation instead of the mock adapter. -- Adds a materialized OpenYAM gravity-model URDF asset to LFS-backed robot - resources. +- Adds a materialized fixed-finger, six-DOF gravity-only OpenYAM URDF asset to + LFS-backed robot resources; it does not supply planning limits. - Reuses the existing `can-motor-control` dependency and shared Damiao runtime; - it introduces no public API change beyond making the existing OpenYAM - manipulator and gripper operations functional on hardware. + it introduces no generic API change beyond the planned hardware integration. diff --git a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md index 68ffd798da..ae272f38ef 100644 --- a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md +++ b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md @@ -1,58 +1,120 @@ ## ADDED Requirements +### Requirement: Supported Linux architecture +The physical OpenYAM Damiao integration SHALL support Linux on x86_64 in +addition to any other explicitly supported Linux architecture. + +#### Scenario: Deploying on Linux x86_64 +- **WHEN** the physical OpenYAM integration is deployed on a Linux x86_64 host +- **THEN** the platform is supported by this capability + ### Requirement: Six-DOF OpenYAM Damiao arm control The system SHALL provide a hardware-backed OpenYAM manipulator adapter using the shared Damiao motor runtime. The adapter SHALL expose exactly six arm -degrees of freedom in `yam_joint1` through `yam_joint6` order, corresponding -to CAN IDs 1 through 6. It SHALL configure CAN IDs 1 through 3 as DM4340 and -IDs 4 through 6 as DM4310. The separate DM4310 gripper at CAN ID 7 SHALL NOT -be counted as an arm degree of freedom. +degrees of freedom in `yam_joint1` through `yam_joint6` order, corresponding to +CAN IDs 1 through 6. It SHALL configure CAN IDs 1 through 3 as DM4340 and IDs 4 +through 6 as DM4310. The separate DM4310 gripper at CAN ID 7 SHALL NOT be +counted as an arm degree of freedom. #### Scenario: Adapter reports OpenYAM arm topology - **WHEN** the OpenYAM hardware adapter is created - **THEN** it reports six arm degrees of freedom with the specified motor types and CAN ordering, while retaining CAN ID 7 as a separate gripper actuator -### Requirement: Gripper aperture interface -The system SHALL expose OpenYAM gripper position through the manipulator API -as an aperture in metres. It SHALL convert the supported aperture range -linearly to the driver's calibrated normalized opening, where `0` is closed -and `1` is open, without exposing normalized opening or motor angle through -the public API. +### Requirement: Default-deny operator approval for physical enable +Physical OpenYAM enable SHALL require the explicit operator configuration +`openyam_operator_approved=true`, exposed through the CLI as +`--openyam-operator-approved`. The default SHALL be false. The adapter SHALL +reject both normal and error-recovery enable before motor enable when approval +is absent or false. Approval SHALL NOT bypass gravity preflight, direction +commissioning, or limit validation. + +#### Scenario: Normal enable lacks operator approval +- **WHEN** normal physical enable is requested without + `openyam_operator_approved=true` +- **THEN** the adapter rejects enable before motor enable + +#### Scenario: Error recovery lacks operator approval +- **WHEN** error-recovery physical enable is requested without + `openyam_operator_approved=true` +- **THEN** the adapter rejects recovery enable before motor enable + +#### Scenario: Operator approval is supplied by CLI +- **WHEN** the operator explicitly supplies `--openyam-operator-approved` +- **THEN** the resolved configuration sets `openyam_operator_approved=true`, + while all other safety gates still apply -#### Scenario: Commanding a gripper aperture -- **WHEN** a caller writes a supported gripper aperture in metres -- **THEN** the adapter commands the corresponding calibrated normalized driver - opening for the separate CAN-ID-7 gripper +### Requirement: Gripper capability is upstream-gated +The system SHALL leave OpenYAM gripper position and command operations +unavailable until a released upstream `can-motor-control` API provides a +normalized, calibrated opening getter. It SHALL not fabricate gripper state, +infer it from motor angle, or invent a replacement getter. -#### Scenario: Reading a gripper aperture -- **WHEN** the separate gripper state is refreshed after calibration -- **THEN** the adapter returns its opening as an aperture in metres +#### Scenario: Gripper support is not released +- **WHEN** the upstream normalized calibrated-opening getter is unavailable +- **THEN** gripper position and command operations report unavailable and issue + no fabricated state or gripper command + +#### Scenario: Reading a gripper aperture after upstream support +- **WHEN** the released upstream getter is available and the gripper is + calibrated +- **THEN** the adapter may convert normalized opening to aperture metres, with + `0` closed and `1` open, without exposing normalized units publicly ### Requirement: Gravity-compensation activation -The system SHALL configure OpenYAM activation to use zero position and velocity -gains with feed-forward gravity torque computed from a valid, expanded, -gripper-equipped URDF gravity model. It SHALL NOT represent zero gains without -a valid gravity model as gravity compensation. +The system SHALL configure OpenYAM activation and recovery to use zero position +and velocity gains with feed-forward gravity torque computed from a valid, +expanded, fixed-finger six-DOF gravity-only URDF. Every enable or recovery path +SHALL load the model and preflight finite `G(q)` before sending any zero-gain +command. The active gripper Xacro SHALL remain the sole source of planning and +hardware command limits. It SHALL NOT represent zero gains without a valid +model and finite `G(q)` as gravity compensation. #### Scenario: Activating a configured OpenYAM arm -- **WHEN** the coordinator activates an OpenYAM adapter with a loadable gravity - model -- **THEN** the adapter enables the arm and sends commands with `Kp=0`, `Kd=0`, - and gravity feed-forward torque for the current arm configuration +- **WHEN** the coordinator activates an OpenYAM adapter +- **THEN** it first loads the fixed-finger model and verifies finite `G(q)` for + the current state, then enables the arm and sends `Kp=0`, `Kd=0`, and gravity + feed-forward torque + +#### Scenario: Gravity preflight fails on enable or recovery +- **WHEN** the model is missing, unloadable, or produces non-finite `G(q)` +- **THEN** the enable or recovery fails before zero-gain control, performs a + no-motion disable, and sends neither zero torque nor any other motion command + +#### Scenario: Disable versus park +- **WHEN** control is disabled +- **THEN** the adapter disables without issuing a motion command +- **WHEN** an operator explicitly requests park +- **THEN** parking is performed only as that separate, opt-in operation -#### Scenario: Gravity model is unavailable -- **WHEN** an OpenYAM hardware configuration lacks a loadable expanded gravity - model -- **THEN** activation fails before enabling zero-gain arm control +#### Scenario: No-motion disable fails +- **WHEN** a no-motion disable fails or its resulting actuator state cannot be + confirmed +- **THEN** the system enters an unresolved energized/unknown state, does not + report disabled, and escalates to the operator and approved e-stop procedure ### Requirement: OpenYAM physical hardware selection The system SHALL configure OpenYAM's physical manipulator hardware to use the -Damiao-backed adapter instead of the mock adapter. It SHALL preserve existing -encoder zeroes as home and SHALL use the active gripper Xacro as the authority -for arm command limits. +Damiao-backed adapter instead of the mock adapter. It SHALL preserve physical +encoder-zero home and SHALL parse the active gripper Xacro fail-closed as the +authority for arm command limits. The physical factory SHALL reject mock-only +initial-position configuration; initial positions SHALL be mock-only and +unavailable to physical construction. #### Scenario: Building the physical OpenYAM configuration - **WHEN** DimOS builds an OpenYAM blueprint for physical hardware -- **THEN** it instantiates the Damiao-backed adapter with six-joint limits from - the active gripper Xacro and does not add a homing or joint-offset procedure +- **THEN** it instantiates the Damiao-backed adapter only with six unique, + finite, valid limits parsed from the active gripper Xacro, uses encoder-zero + home, rejects mock initial positions, and does not add a homing or + joint-offset procedure + +### Requirement: External direction commissioning precondition +The system SHALL require approved external vendor or bench-tool direction +commissioning for all six arm motors before physical enable. The adapter SHALL +not perform direction discovery or commissioning by commanding the arm, because +the driver activates gravity mode with zero position and velocity gains. + +#### Scenario: Direction commissioning is absent +- **WHEN** physical enable is requested without an approved six-joint direction + commissioning result +- **THEN** enable is rejected before any arm command is sent diff --git a/openspec/changes/add-openyam-damiao-adapter/tasks.md b/openspec/changes/add-openyam-damiao-adapter/tasks.md index ee42233918..3b15612049 100644 --- a/openspec/changes/add-openyam-damiao-adapter/tasks.md +++ b/openspec/changes/add-openyam-damiao-adapter/tasks.md @@ -1,22 +1,31 @@ ## 1. Hardware resources and metadata -- [ ] 1.1 Add the materialized, gripper-equipped OpenYAM gravity-model URDF as - an LFS-backed resource and make it available to the hardware configuration. +- [ ] 1.0 Confirm supported Linux deployment includes x86_64. +- [ ] 1.1 Add the materialized, fixed-finger six-DOF gravity-only OpenYAM URDF + as an LFS-backed resource and make it available to the hardware configuration. - [ ] 1.2 Define OpenYAM arm and gripper Damiao metadata: IDs 1–3 as DM4340, - IDs 4–6 as DM4310, and gripper ID 7 as DM4310, with OpenYAM gains and limits. -- [ ] 1.3 Load the active gripper Xacro's six arm limits into the OpenYAM - hardware metadata without introducing homing or encoder-offset behavior. + IDs 4–6 as DM4310, and gripper ID 7 as DM4310, with OpenYAM gains. +- [ ] 1.3 Parse the active gripper Xacro's six arm limits fail-closed into the + hardware metadata, rejecting duplicate joints, missing/nonfinite values, and + invalid ranges; keep physical encoder-zero home, reject mock-only initial + positions in the physical factory, and add no homing or encoder offsets. +- [ ] 1.4 Add default-deny physical approval configuration + `openyam_operator_approved`, expose it as `--openyam-operator-approved`, and + reject normal and error-recovery enable before motor enable when false. ## 2. OpenYAM Damiao adapter - [ ] 2.1 Implement a six-DOF OpenYAM adapter that reuses `DamiaoArmAdapter` for arm state, MIT commands, and gravity feed-forward behavior. -- [ ] 2.2 Add an internal one-motor gripper group and implement gripper state - and commands without including it in the adapter's arm DOF. -- [ ] 2.3 Implement calibrated gripper endpoint handling and linear conversion - between metre aperture and normalized driver opening over the 0.096 m span. -- [ ] 2.4 Configure activation to require a loadable expanded gravity URDF and - send `Kp=Kd=0` with `G(q)` feed-forward torque. +- [ ] 2.2 Add an internal one-motor gripper group without including it in the + arm DOF, but keep gripper state and commands unavailable until released + upstream normalized calibrated-opening getter support exists. +- [ ] 2.3 After upstream support exists, implement its bounded calibration and + linear conversion between metre aperture and normalized opening over 0.096 m; + do not fabricate state before then. +- [ ] 2.4 Require loaded-model and finite-`G(q)` preflight on every enable and + recovery path before `Kp=Kd=0` with `G(q)` feed-forward; on failure perform + no-motion disable and send no zero torque, keeping optional park separate. ## 3. OpenYAM integration @@ -28,10 +37,16 @@ ## 4. Verification and commissioning support - [ ] 4.1 Add focused tests for six-DOF topology, motor metadata, separate - gripper behavior, and metre-to-opening conversion. -- [ ] 4.2 Add focused tests that gravity-compensation activation rejects a - missing or unloadable gravity model and uses zero gains with gravity torque. -- [ ] 4.3 Document the no-load motor direction commissioning procedure and - verify it against all six planning-joint directions before hardware motion. + gripper availability, and (after upstream support) aperture conversion. +- [ ] 4.2 Add focused tests that every enable/recovery path rejects a missing, + unloadable, or non-finite gravity result, uses zero gains only after + preflight, and performs no-motion disable without zero torque on failure. +- [ ] 4.3 Document the approved external vendor/bench-tool direction + commissioning precondition for all six joints; verify the adapter does not + discover direction under zero-gain gravity mode, disable never implicitly + parks, failed disable is unresolved energized/unknown rather than disabled + and escalates to operator/e-stop, and physical factories reject initial + positions; verify operator approval is required for normal and recovery + enable. - [ ] 4.4 Run the focused OpenYAM and Damiao test suite plus the blueprint registry generation test if registration changes generated blueprint output. diff --git a/pyproject.toml b/pyproject.toml index 8f95173e5d..c2fa3f2e81 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -274,6 +274,8 @@ manipulation = [ "xarm-python-sdk>=1.17.0", "a750-control; sys_platform == 'linux' and platform_machine == 'x86_64'", + "can-motor-control>=0.0.3; sys_platform == 'linux'", + # Mesh conversion (STL/DAE → OBJ for Drake collision geometry) "trimesh", "pycollada", diff --git a/uv.lock b/uv.lock index a21e78283a..a627fa7e9c 100644 --- a/uv.lock +++ b/uv.lock @@ -569,6 +569,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.3" +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/4f/3f/67a96cab4bc354d473ba8046fdfb8b49eb2c51c48b718aa4edbe1b04609d/can_motor_control-0.0.3.tar.gz", hash = "sha256:d56ce2ee322f634a52527bec9585026919eeaa8a3ab24569effbcc934110a2a1", size = 125413, upload-time = "2026-07-02T22:21:59.667Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/54/36/578b803a68c2286bad22bc28dd8882a0d63ad95c07293dee88bd4c68278c/can_motor_control-0.0.3-cp310-abi3-manylinux_2_34_x86_64.whl", hash = "sha256:6610d298a969060c48067be1c04aff4878a854a3f67e771d2352390a66c8fd23", size = 528026, upload-time = "2026-07-02T22:21:58.112Z" }, +] + [[package]] name = "cattrs" version = "25.3.0" @@ -1589,6 +1602,7 @@ agents = [ all = [ { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "aiortc" }, + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "coacd" }, { name = "cupy-cuda12x", marker = "platform_machine == 'x86_64'" }, @@ -1707,6 +1721,7 @@ 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" }, @@ -2010,6 +2025,7 @@ requires-dist = [ { name = "aiortc", marker = "extra == 'webrtc'", specifier = ">=1.14.0" }, { name = "annotation-protocol", specifier = ">=1.4.0" }, { name = "bleak", specifier = ">=3.0.2" }, + { name = "can-motor-control", marker = "sys_platform == 'linux' and extra == 'manipulation'", specifier = ">=0.0.3" }, { name = "chromadb", marker = "extra == 'perception'", specifier = ">=1.0.0" }, { name = "coacd", marker = "extra == 'scene'", specifier = ">=1.0.0" }, { name = "cryptography", specifier = ">=46.0.5" }, From 0c978bb1d5aa4ae7151c7a0abc56bb7727877c5b Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Tue, 21 Jul 2026 23:39:32 +0000 Subject: [PATCH 11/44] [autofix.ci] apply automated fixes --- .../manipulators/openyam_damiao/__init__.py | 6 +++-- .../manipulators/openyam_damiao/_registry.py | 4 +--- .../openyam_damiao/test_adapter.py | 23 ++++++++++++------- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/dimos/hardware/manipulators/openyam_damiao/__init__.py b/dimos/hardware/manipulators/openyam_damiao/__init__.py index 8f28372bf1..30d60fd798 100644 --- a/dimos/hardware/manipulators/openyam_damiao/__init__.py +++ b/dimos/hardware/manipulators/openyam_damiao/__init__.py @@ -15,6 +15,8 @@ def __getattr__(name: str) -> object: OpenYamDamiaoAdapter, ) - return {"OpenYAMDamiaoAdapter": OpenYAMDamiaoAdapter, - "OpenYamDamiaoAdapter": OpenYamDamiaoAdapter}[name] + return { + "OpenYAMDamiaoAdapter": OpenYAMDamiaoAdapter, + "OpenYamDamiaoAdapter": OpenYamDamiaoAdapter, + }[name] raise AttributeError(name) diff --git a/dimos/hardware/manipulators/openyam_damiao/_registry.py b/dimos/hardware/manipulators/openyam_damiao/_registry.py index 7c69825064..49d4b18393 100644 --- a/dimos/hardware/manipulators/openyam_damiao/_registry.py +++ b/dimos/hardware/manipulators/openyam_damiao/_registry.py @@ -4,7 +4,5 @@ # you may not use this file except in compliance with the License. ADAPTER_FACTORIES = { - "openyam_damiao": ( - "dimos.hardware.manipulators.openyam_damiao.adapter:OpenYamDamiaoAdapter" - ), + "openyam_damiao": ("dimos.hardware.manipulators.openyam_damiao.adapter:OpenYamDamiaoAdapter"), } diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py index 0c18652419..e76b039066 100644 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -59,7 +59,9 @@ def test_openyam_limits_are_loaded_from_active_model() -> None: limits = adapter.get_limits() assert limits.position_lower == pytest.approx([-3.92699, 0.0, 0.0, -1.65806, -1.5708, -2.35619]) - assert limits.position_upper == pytest.approx([1.5708, 3.66519, 4.01426, 1.65806, 1.5708, 1.8326]) + assert limits.position_upper == pytest.approx( + [1.5708, 3.66519, 4.01426, 1.65806, 1.5708, 1.8326] + ) assert limits.velocity_max == pytest.approx([3.0, 10.0, 3.0, 10.0, 3.0, 10.0]) @@ -110,11 +112,11 @@ def test_openyam_operator_gate_allows_approved_enable() -> None: def test_openyam_xacro_limits_reject_duplicate_joint_names(monkeypatch: pytest.MonkeyPatch) -> None: - joints = [ - JointDescription(f"yam_joint{i}", "revolute", -1.0, 1.0, 1.0) for i in range(1, 7) - ] + joints = [JointDescription(f"yam_joint{i}", "revolute", -1.0, 1.0, 1.0) for i in range(1, 7)] joints.append(joints[0]) - monkeypatch.setattr(adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints)) + monkeypatch.setattr( + adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints) + ) with pytest.raises(ValueError, match="duplicate"): adapter_module._active_arm_limits() @@ -129,12 +131,17 @@ def test_openyam_xacro_limits_reject_bad_values( ) -> None: joints = [ JointDescription( - f"yam_joint{i}", "revolute", lower if i == 1 else -1.0, - upper if i == 1 else 1.0, velocity if i == 1 else 1.0, + f"yam_joint{i}", + "revolute", + lower if i == 1 else -1.0, + upper if i == 1 else 1.0, + velocity if i == 1 else 1.0, ) for i in range(1, 7) ] - monkeypatch.setattr(adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints)) + monkeypatch.setattr( + adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints) + ) with pytest.raises(ValueError): adapter_module._active_arm_limits() From 00903f4790dea7ce965ea96d049521feeacb8053 Mon Sep 17 00:00:00 2001 From: cc Date: Tue, 21 Jul 2026 16:47:59 -0700 Subject: [PATCH 12/44] fix(manipulation): remove OpenYAM approval gate --- CONTEXT.md | 10 +------- dimos/core/global_config.py | 1 - .../manipulators/openyam_damiao/adapter.py | 14 ----------- .../openyam_damiao/test_adapter.py | 15 +----------- dimos/robot/manipulators/openyam/config.py | 1 - .../manipulators/openyam/test_openyam.py | 4 ---- docs/capabilities/manipulation/index.md | 2 -- .../manipulation/openyam_commissioning.md | 15 ------------ .../add-openyam-damiao-adapter/design.md | 12 ---------- .../add-openyam-damiao-adapter/proposal.md | 3 --- .../specs/openyam-damiao-control/spec.md | 23 ------------------- .../add-openyam-damiao-adapter/tasks.md | 7 ++---- 12 files changed, 4 insertions(+), 103 deletions(-) diff --git a/CONTEXT.md b/CONTEXT.md index 47fc8dc092..65718db27d 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -45,15 +45,7 @@ or motion command has been issued. **Arm activation**: The transition from read-only startup to active control after gravity-model loadability and finite-`G(q)` preflight. Every enable and recovery path repeats -this preflight and also requires explicit `openyam_operator_approved=true` -(`--openyam-operator-approved`). Without approval, normal and error-recovery -enable are rejected before motor enable. It does not imply gripper availability. - -**Operator approval**: -The explicit, default-deny physical-enable configuration -`openyam_operator_approved`, exposed through the -`--openyam-operator-approved` CLI option. It is an additional gate and does not -replace gravity preflight or other commissioning and validation gates. +this preflight before motor enable. It does not imply gripper availability. **Gravity-compensation mode**: The active arm state with zero position and velocity gains and feed-forward diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 959279d779..ec150c9bc9 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -51,7 +51,6 @@ class GlobalConfig(BaseSettings): xarm7_ip: str | None = None xarm6_ip: str | None = None can_port: str | None = None - openyam_operator_approved: bool = False device_path: str | None = None # device path for real robot (e.g. /dev/ttyUSB0) simulation: str = "" replay: bool = False diff --git a/dimos/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py index d22ed37be4..6efd6bb50a 100644 --- a/dimos/hardware/manipulators/openyam_damiao/adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/adapter.py @@ -116,14 +116,12 @@ def __init__( *, gravity_model_path: str | Path | None = None, gravity_comp: bool = True, - operator_approved: bool = False, **kwargs: Any, ) -> None: if not gravity_comp: raise ValueError("OpenYAM requires gravity compensation") if gravity_model_path is None or not Path(gravity_model_path).is_file(): raise ValueError("OpenYAM requires a valid gravity model path") - self._operator_approved = operator_approved lower, upper, velocity = _active_arm_limits() arm = _group_spec( bus_name=_BUS_NAME, @@ -152,18 +150,6 @@ def __init__( **kwargs, ) - def write_enable(self, enable: bool) -> bool: - """Gate every physical enable, including error-recovery enables.""" - if enable and not self._operator_approved: - return False - return super().write_enable(enable) - - def write_clear_errors(self) -> bool: - """Gate error recovery before it disables or re-enables the runtime.""" - if not self._operator_approved: - return False - return super().write_clear_errors() - def read_gripper_position(self) -> float | None: """Gripper feedback is disabled until the binding provides calibration.""" return None diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py index e76b039066..1493084bfd 100644 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -76,22 +76,9 @@ def test_openyam_requires_gravity_comp_and_model() -> None: ) -def test_openyam_operator_gate_rejects_normal_and_recovery_enable() -> None: - adapter = OpenYamDamiaoAdapter(gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True) - runtime = Mock() - adapter._runtime = runtime - - assert not adapter.activate() - assert not adapter.write_enable(True) - assert not adapter.write_clear_errors() - runtime.disable.assert_not_called() - runtime.enable.assert_not_called() - - -def test_openyam_operator_gate_allows_approved_enable() -> None: +def test_openyam_normal_enable_and_error_recovery() -> None: adapter = OpenYamDamiaoAdapter( gravity_model_path=GRAVITY_MODEL_PATH, - operator_approved=True, use_mock_bus=True, ) runtime = Mock() diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index 54d8f09ae3..fe82002cd0 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -81,7 +81,6 @@ def openyam_hardware( # planning/home positions into a live motor adapter. adapter_kwargs={ "gravity_model_path": OPENYAM_GRAVITY_MODEL_PATH, - "operator_approved": global_config.openyam_operator_approved, }, include_gripper=False, ) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 4f79814005..1e999fc410 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -74,7 +74,6 @@ def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: A assert hardware.adapter_type == "openyam_damiao" assert hardware.address == "can1" assert hardware.adapter_kwargs["gravity_model_path"] == OPENYAM_GRAVITY_MODEL_PATH - assert hardware.adapter_kwargs["operator_approved"] is False assert len(hardware.joints) == OPENYAM_DOF assert hardware.gripper_joints == [] assert "initial_positions" not in hardware.adapter_kwargs @@ -86,9 +85,6 @@ def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: A ) assert "initial_positions" not in direct.adapter_kwargs - monkeypatch.setattr(global_config, "openyam_operator_approved", True) - approved = openyam_hardware("arm") - assert approved.adapter_kwargs["operator_approved"] is True def test_openyam_simulation_hardware_remains_mock(monkeypatch: Any) -> None: diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index ad9b2d06c4..ea556eb15a 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -12,8 +12,6 @@ For the required vendor/approved-bench direction verification before OpenYAM planning or teleoperation, see the [OpenYAM direction commissioning guide](./openyam_commissioning.md). The DimOS driver does not issue commissioning position steps. -Physical runs are default-deny until the operator supplies -`--openyam-operator-approved` after completing the approved external checks. Recent addition: the A-750 keyboard teleop blueprint is now available via: diff --git a/docs/capabilities/manipulation/openyam_commissioning.md b/docs/capabilities/manipulation/openyam_commissioning.md index 4dafaf029d..2e09a386ca 100644 --- a/docs/capabilities/manipulation/openyam_commissioning.md +++ b/docs/capabilities/manipulation/openyam_commissioning.md @@ -76,21 +76,6 @@ released and integrated can gripper support be reconsidered. The direction commissioning procedure below applies to the six planning joints only, not the gripper. -## Operator approval gate - -Physical OpenYAM startup is default-deny. The operator must explicitly approve -the external direction-verification record and the safety preconditions before -enabling the driver: - -```bash -dimos --openyam-operator-approved run openyam-planner-coordinator -``` - -`--openyam-operator-approved` is a per-run acknowledgement, not a calibration -or a bypass of the gravity-model and state preflights. Without it, the adapter -will not enable motors, including an enable attempted during error recovery. -Mock/simulation hardware is unaffected by this physical approval gate. - ## External direction verification precondition Before starting this driver, have an approved vendor tool or controlled bench diff --git a/openspec/changes/add-openyam-damiao-adapter/design.md b/openspec/changes/add-openyam-damiao-adapter/design.md index 98407a7a08..cd61290fab 100644 --- a/openspec/changes/add-openyam-damiao-adapter/design.md +++ b/openspec/changes/add-openyam-damiao-adapter/design.md @@ -19,9 +19,6 @@ architectures SHALL include x86_64. - Require every enable and recovery path to load the gravity model and complete finite-`G(q)` preflight before zero-gain gravity operation; failures must disable with no motion command and must not send zero torque. -- Default-deny physical enable unless the explicit operator configuration - `openyam_operator_approved` is true, exposed through the CLI as - `--openyam-operator-approved`. - Preserve physical encoder-zero home; initial positions are mock-only. **Non-Goals:** @@ -79,15 +76,6 @@ evaluation, enabling, or recovery fails, the arm SHALL be disabled with no motion command; it SHALL not send zero torque as a failure response. Only after successful preflight may the command path send `Kp=Kd=0` and `tau=G(q)`. -### Require explicit operator approval for physical enable - -Physical OpenYAM enable SHALL default to denied. The only approval input is the -explicit operator configuration `openyam_operator_approved`, exposed as the -`--openyam-operator-approved` CLI option. Both normal enable and error-recovery -enable SHALL reject before motor enable when this value is absent or false. -Operator approval does not bypass gravity-model preflight, direction -commissioning, limit validation, or any other safety gate. - Disabling SHALL be a no-motion operation by default. An optional park action may be requested explicitly as a separate operation; disabling must never implicitly park the arm. If a no-motion disable fails, the resulting state is diff --git a/openspec/changes/add-openyam-damiao-adapter/proposal.md b/openspec/changes/add-openyam-damiao-adapter/proposal.md index 4d85c37658..32cae1ded1 100644 --- a/openspec/changes/add-openyam-damiao-adapter/proposal.md +++ b/openspec/changes/add-openyam-damiao-adapter/proposal.md @@ -31,9 +31,6 @@ including x86_64. gains. Disable is no-motion by default, while park is explicit; a failed disable remains unresolved energized/unknown, is never reported disabled, and escalates to operator/e-stop procedure. -- Default-deny physical enable behind explicit `openyam_operator_approved` - configuration, exposed as `--openyam-operator-approved`; reject normal and - error-recovery enable before motor enable without it. ## Capabilities diff --git a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md index ae272f38ef..32184bcdc0 100644 --- a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md +++ b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md @@ -21,29 +21,6 @@ counted as an arm degree of freedom. - **THEN** it reports six arm degrees of freedom with the specified motor types and CAN ordering, while retaining CAN ID 7 as a separate gripper actuator -### Requirement: Default-deny operator approval for physical enable -Physical OpenYAM enable SHALL require the explicit operator configuration -`openyam_operator_approved=true`, exposed through the CLI as -`--openyam-operator-approved`. The default SHALL be false. The adapter SHALL -reject both normal and error-recovery enable before motor enable when approval -is absent or false. Approval SHALL NOT bypass gravity preflight, direction -commissioning, or limit validation. - -#### Scenario: Normal enable lacks operator approval -- **WHEN** normal physical enable is requested without - `openyam_operator_approved=true` -- **THEN** the adapter rejects enable before motor enable - -#### Scenario: Error recovery lacks operator approval -- **WHEN** error-recovery physical enable is requested without - `openyam_operator_approved=true` -- **THEN** the adapter rejects recovery enable before motor enable - -#### Scenario: Operator approval is supplied by CLI -- **WHEN** the operator explicitly supplies `--openyam-operator-approved` -- **THEN** the resolved configuration sets `openyam_operator_approved=true`, - while all other safety gates still apply - ### Requirement: Gripper capability is upstream-gated The system SHALL leave OpenYAM gripper position and command operations unavailable until a released upstream `can-motor-control` API provides a diff --git a/openspec/changes/add-openyam-damiao-adapter/tasks.md b/openspec/changes/add-openyam-damiao-adapter/tasks.md index 3b15612049..c194b259ee 100644 --- a/openspec/changes/add-openyam-damiao-adapter/tasks.md +++ b/openspec/changes/add-openyam-damiao-adapter/tasks.md @@ -9,9 +9,6 @@ hardware metadata, rejecting duplicate joints, missing/nonfinite values, and invalid ranges; keep physical encoder-zero home, reject mock-only initial positions in the physical factory, and add no homing or encoder offsets. -- [ ] 1.4 Add default-deny physical approval configuration - `openyam_operator_approved`, expose it as `--openyam-operator-approved`, and - reject normal and error-recovery enable before motor enable when false. ## 2. OpenYAM Damiao adapter @@ -46,7 +43,7 @@ discover direction under zero-gain gravity mode, disable never implicitly parks, failed disable is unresolved energized/unknown rather than disabled and escalates to operator/e-stop, and physical factories reject initial - positions; verify operator approval is required for normal and recovery - enable. + positions; verify gravity preflight and external direction commissioning remain + required before normal and recovery enable. - [ ] 4.4 Run the focused OpenYAM and Damiao test suite plus the blueprint registry generation test if registration changes generated blueprint output. From 99cb862402720fbb49628c779b5eda14e821f9d1 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Tue, 21 Jul 2026 23:49:16 +0000 Subject: [PATCH 13/44] [autofix.ci] apply automated fixes --- dimos/robot/manipulators/openyam/test_openyam.py | 1 - 1 file changed, 1 deletion(-) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 1e999fc410..5acd80a3f2 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -86,7 +86,6 @@ def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: A assert "initial_positions" not in direct.adapter_kwargs - def test_openyam_simulation_hardware_remains_mock(monkeypatch: Any) -> None: monkeypatch.setattr(global_config, "simulation", "mujoco") From e26e78273661432894c4e26fcab1ab365f95328a Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 23 Jul 2026 11:32:37 -0700 Subject: [PATCH 14/44] chore: bump can lib version --- pyproject.toml | 4 ++-- uv.lock | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index c2fa3f2e81..320d073625 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -274,7 +274,7 @@ manipulation = [ "xarm-python-sdk>=1.17.0", "a750-control; sys_platform == 'linux' and platform_machine == 'x86_64'", - "can-motor-control>=0.0.3; sys_platform == 'linux'", + "can-motor-control>=0.0.4; sys_platform == 'linux'", # Mesh conversion (STL/DAE → OBJ for Drake collision geometry) "trimesh", @@ -473,7 +473,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 } +exclude-newer-package = { dimos-viewer = false, pyrealsense2-extended = false, dimos-lcm = false, lcm-dimos-fork = false, roboplan = false, can-motor-control = 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 a627fa7e9c..40c1cb0435 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 @@ -571,15 +572,15 @@ wheels = [ [[package]] name = "can-motor-control" -version = "0.0.3" +version = "0.0.4" 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/4f/3f/67a96cab4bc354d473ba8046fdfb8b49eb2c51c48b718aa4edbe1b04609d/can_motor_control-0.0.3.tar.gz", hash = "sha256:d56ce2ee322f634a52527bec9585026919eeaa8a3ab24569effbcc934110a2a1", size = 125413, upload-time = "2026-07-02T22:21:59.667Z" } +sdist = { url = "https://files.pythonhosted.org/packages/e1/f5/857df85f5d612be90ab852a74a604d865708aea6279d9277de13f2f56782/can_motor_control-0.0.4.tar.gz", hash = "sha256:615c16eab5e1fb010623765431e2fd24b743bfa9bea66e94b3f390db4b4f6ab1", size = 128133, upload-time = "2026-07-23T16:43:08.702Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/54/36/578b803a68c2286bad22bc28dd8882a0d63ad95c07293dee88bd4c68278c/can_motor_control-0.0.3-cp310-abi3-manylinux_2_34_x86_64.whl", hash = "sha256:6610d298a969060c48067be1c04aff4878a854a3f67e771d2352390a66c8fd23", size = 528026, upload-time = "2026-07-02T22:21:58.112Z" }, + { url = "https://files.pythonhosted.org/packages/6c/8e/dc4652bccc1b0e4b39c87c73cb189beddd5a3a91fb95d4c4431a17e7b339/can_motor_control-0.0.4-cp310-abi3-manylinux_2_34_x86_64.whl", hash = "sha256:ad63ead5a82bd9d71c68ed74b21c8b0b068ed35504eb472f5a93b5acf2277b50", size = 527595, upload-time = "2026-07-23T16:43:07.302Z" }, ] [[package]] @@ -2025,7 +2026,7 @@ requires-dist = [ { name = "aiortc", marker = "extra == 'webrtc'", specifier = ">=1.14.0" }, { name = "annotation-protocol", specifier = ">=1.4.0" }, { name = "bleak", specifier = ">=3.0.2" }, - { name = "can-motor-control", marker = "sys_platform == 'linux' and extra == 'manipulation'", specifier = ">=0.0.3" }, + { name = "can-motor-control", marker = "sys_platform == 'linux' and extra == 'manipulation'", specifier = ">=0.0.4" }, { name = "chromadb", marker = "extra == 'perception'", specifier = ">=1.0.0" }, { name = "coacd", marker = "extra == 'scene'", specifier = ">=1.0.0" }, { name = "cryptography", specifier = ">=46.0.5" }, From a6dca50e30aa02ca6bd19af1c52c25908c83bee0 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 24 Jul 2026 07:14:43 +0000 Subject: [PATCH 15/44] fix: enable compliant OpenYAM readback Select MIT mode and drain startup traffic before enabling Damiao motors. Send zero-torque OpenYAM frames for compliant encoder readback and expose Viser on all interfaces. --- dimos/hardware/damiao/runtime.py | 3 ++ dimos/hardware/damiao/test_adapters.py | 21 ++++++++++++++ .../manipulators/openyam_damiao/adapter.py | 20 +++++++++++++ .../openyam_damiao/test_adapter.py | 28 +++++++++++++++++++ .../visualization/viser/config.py | 2 +- 5 files changed, 73 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py index 15a8469e27..b62a8446dd 100644 --- a/dimos/hardware/damiao/runtime.py +++ b/dimos/hardware/damiao/runtime.py @@ -213,7 +213,10 @@ def enable(self) -> bool: if self._robot is None: return False try: + self._robot.set_mode("mit") + self._robot.tick(self._tick_deadline_us) self._robot.enable() + self._robot.tick(self._tick_deadline_us) except Exception: logger.exception("damiao runtime enable failed", adapter=self._adapter_type) # The binding may have enabled a subset of the robot before diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py index 91c1da6b43..6484776adf 100644 --- a/dimos/hardware/damiao/test_adapters.py +++ b/dimos/hardware/damiao/test_adapters.py @@ -363,8 +363,29 @@ def test_arm_adapter_preserves_enabled_state_when_safety_disable_fails(mocker) - assert adapter.read_enabled() is True + +def test_runtime_selects_mit_mode_before_enable(mocker) -> None: + runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) + robot = mocker.Mock() + runtime._robot = robot + + assert runtime.enable() is True + assert robot.method_calls[:4] == [ + mocker.call.set_mode("mit"), + mocker.call.tick(1_000), + mocker.call.enable(), + mocker.call.tick(1_000), + ] + + def test_runtime_preserves_enabled_state_when_partial_enable_rollback_fails() -> None: class _FailingRobot: + def set_mode(self, mode: str) -> None: + assert mode == "mit" + + def tick(self, deadline_us: int) -> None: + assert deadline_us == 1_000 + def enable(self) -> None: raise RuntimeError("partial enable") diff --git a/dimos/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py index 6efd6bb50a..2a774eda1c 100644 --- a/dimos/hardware/manipulators/openyam_damiao/adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/adapter.py @@ -150,6 +150,26 @@ def __init__( **kwargs, ) + def write_mit_commands( + self, + *, + q: list[float], + dq: list[float], + kp: list[float], + kd: list[float], + tau: list[float], + ) -> bool: + """Temporarily send zero-torque MIT frames for compliant position readback.""" + self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) + zeros = self._zero_vector() + return super().write_mit_commands( + q=zeros, + dq=zeros, + kp=zeros, + kd=zeros, + tau=zeros, + ) + def read_gripper_position(self) -> float | None: """Gripper feedback is disabled until the binding provides calibration.""" return None diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py index 1493084bfd..9d7fa69fef 100644 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -98,6 +98,34 @@ def test_openyam_normal_enable_and_error_recovery() -> None: runtime.enable.assert_called_once_with() +def test_openyam_temporarily_writes_zero_torque_mit_commands() -> None: + adapter = OpenYamDamiaoAdapter( + gravity_model_path=GRAVITY_MODEL_PATH, + use_mock_bus=True, + ) + runtime = Mock() + runtime.write_group_mit_commands.return_value = True + adapter._runtime = runtime + adapter._enabled = True + + assert adapter.write_mit_commands( + q=[1.0] * 6, + dq=[2.0] * 6, + kp=[3.0] * 6, + kd=[4.0] * 6, + tau=[5.0] * 6, + ) + + runtime.write_group_mit_commands.assert_called_once_with( + group_name="arm", + q=[0.0] * 6, + dq=[0.0] * 6, + kp=[0.0] * 6, + kd=[0.0] * 6, + tau=[0.0] * 6, + ) + + def test_openyam_xacro_limits_reject_duplicate_joint_names(monkeypatch: pytest.MonkeyPatch) -> None: joints = [JointDescription(f"yam_joint{i}", "revolute", -1.0, 1.0, 1.0) for i in range(1, 7)] joints.append(joints[0]) diff --git a/dimos/manipulation/visualization/viser/config.py b/dimos/manipulation/visualization/viser/config.py index 36ce7d06ea..d385e45233 100644 --- a/dimos/manipulation/visualization/viser/config.py +++ b/dimos/manipulation/visualization/viser/config.py @@ -26,7 +26,7 @@ class ViserVisualizationConfig(BaseModel): backend: Literal["viser"] = "viser" host: str = Field( - default="127.0.0.1", validation_alias=AliasChoices("host", "visualization_host") + default="0.0.0.0", validation_alias=AliasChoices("host", "visualization_host") ) port: int = Field(default=8095, validation_alias=AliasChoices("port", "visualization_port")) open_browser: bool = Field( From eb33dcb53344ffbc834c1abc3fe4b7b02d99f8d0 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 00:47:51 +0000 Subject: [PATCH 16/44] fix: make CAN motor activation reliable --- dimos/hardware/damiao/runtime.py | 53 ++++++++++++++- dimos/hardware/damiao/test_adapters.py | 23 +++++++ dimos/robot/cli/can.py | 90 ++++++++++++++++++++++++++ dimos/robot/cli/dimos.py | 2 + dimos/robot/cli/test_can.py | 54 ++++++++++++++++ 5 files changed, 219 insertions(+), 3 deletions(-) create mode 100644 dimos/robot/cli/can.py create mode 100644 dimos/robot/cli/test_can.py diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py index b62a8446dd..b19e2f8437 100644 --- a/dimos/hardware/damiao/runtime.py +++ b/dimos/hardware/damiao/runtime.py @@ -14,7 +14,7 @@ from __future__ import annotations -from collections.abc import Mapping, Sequence +from collections.abc import Callable, Mapping, Sequence from dataclasses import dataclass import importlib from pathlib import Path @@ -31,6 +31,31 @@ _DEFAULT_TICK_DEADLINE_US = 1_000 _DEFAULT_STATE_CACHE_TTL_S = 0.002 _DEFAULT_ADDRESS = "can0" +_ENOBUFS_RETRY_DELAYS_S = (0.001, 0.002, 0.003) +_MIN_RECOMMENDED_TX_QUEUE_LEN = 1_000 + + +def _is_enobufs(exc: BaseException) -> bool: + current: BaseException | None = exc + while current is not None: + if getattr(current, "errno", None) == 105: + return True + message = str(current).lower() + if "no buffer space available" in message or "os error 105" in message: + return True + current = current.__cause__ or current.__context__ + return False + + +def _retry_enobufs(operation: Callable[[], None]) -> None: + for delay_s in (*_ENOBUFS_RETRY_DELAYS_S, None): + try: + operation() + return + except Exception as exc: + if delay_s is None or not _is_enobufs(exc): + raise + time.sleep(delay_s) class DamiaoBindingUnavailableError(RuntimeError): @@ -146,6 +171,7 @@ def _build_robot(self) -> Any: codec = self._damiao.DamiaoCodec() for bus_name, bus_spec in self._robot_spec.buses.items(): address = str(bus_spec.address or _DEFAULT_ADDRESS) + self._warn_if_small_tx_queue(address) transport = ( self._can_motor_control.MockCanBus.new_fd(address) if self._use_mock_bus and bus_spec.fd @@ -167,6 +193,23 @@ def _build_robot(self) -> Any: builder = builder.add_arm(group_name, bus=group_spec.bus_name, motors=binding_specs) return builder.build() + def _warn_if_small_tx_queue(self, address: str) -> None: + if self._use_mock_bus: + return + queue_path = Path("/sys/class/net") / address / "tx_queue_len" + try: + queue_len = int(queue_path.read_text().strip()) + except (OSError, ValueError): + return + if queue_len < _MIN_RECOMMENDED_TX_QUEUE_LEN: + logger.warning( + "CAN transmit queue is too small for reliable motor activation", + interface=address, + txqueuelen=queue_len, + recommended=_MIN_RECOMMENDED_TX_QUEUE_LEN, + setup_command=f"dimos can setup {address}", + ) + def _resolve_motor_type(self, motor_type: object) -> object: if self._damiao is None: raise RuntimeError("Damiao binding module is not loaded") @@ -321,8 +364,12 @@ def write_group_mit_commands( f"command length does not match configured DOF for group {group_name!r}" ) try: - group.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) - self._robot.tick(self._tick_deadline_us) + + def send() -> None: + group.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) + self._robot.tick(self._tick_deadline_us) + + _retry_enobufs(send) except Exception: logger.exception("damiao runtime MIT command failed", group_name=group_name) return False diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py index 6484776adf..4fb1991da0 100644 --- a/dimos/hardware/damiao/test_adapters.py +++ b/dimos/hardware/damiao/test_adapters.py @@ -363,6 +363,29 @@ def test_arm_adapter_preserves_enabled_state_when_safety_disable_fails(mocker) - assert adapter.read_enabled() is True +def test_runtime_retries_mit_command_when_can_queue_is_temporarily_full(mocker) -> None: + runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) + robot = mocker.Mock() + group = mocker.Mock() + group.mit_control.side_effect = [ + RuntimeError("transport IO error: No buffer space available (os error 105)"), + None, + ] + runtime._robot = robot + runtime._groups = {"left": group} + runtime._enabled = True + sleep = mocker.patch("dimos.hardware.damiao.runtime.time.sleep") + + assert ( + runtime.write_group_mit_commands( + group_name="left", q=[0.0], dq=[0.0], kp=[0.0], kd=[0.0], tau=[0.0] + ) + is True + ) + assert group.mit_control.call_count == 2 + robot.tick.assert_called_once_with(1_000) + sleep.assert_called_once_with(0.001) + def test_runtime_selects_mit_mode_before_enable(mocker) -> None: runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) diff --git a/dimos/robot/cli/can.py b/dimos/robot/cli/can.py new file mode 100644 index 0000000000..2936ae61d2 --- /dev/null +++ b/dimos/robot/cli/can.py @@ -0,0 +1,90 @@ +# 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 + +"""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 + + +def _positive(value: int, name: str) -> None: + if value <= 0: + raise typer.BadParameter(f"{name} must be greater than zero") + + +@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, help="Nominal CAN bitrate in bits per second"), + txqueuelen: int = typer.Option(1_000, help="Kernel transmit queue length"), +) -> None: + """Configure, bring up, and verify a classic CAN interface.""" + _positive(bitrate, "bitrate") + _positive(txqueuelen, "txqueuelen") + _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/robot/cli/dimos.py b/dimos/robot/cli/dimos.py index 773d97d248..c87becc6e9 100644 --- a/dimos/robot/cli/dimos.py +++ b/dimos/robot/cli/dimos.py @@ -42,6 +42,7 @@ from dimos.mapping.utils.cli.rename import main as _map_rename_main from dimos.mapping.utils.cli.replay import main as _map_replay_main from dimos.mapping.utils.cli.replay_marker import main as _map_replay_marker_main +from dimos.robot.cli.can import app as can_app from dimos.robot.unitree.go2.cli.go2tool import app as go2tool_app from dimos.utils.logging_config import setup_logger from dimos.visualization.rerun.constants import RerunOpenOption @@ -152,6 +153,7 @@ def callback(**kwargs) -> None: # type: ignore[no-untyped-def] main.callback()(create_dynamic_callback()) # type: ignore[no-untyped-call] +main.add_typer(can_app, name="can") main.add_typer(go2tool_app, name="go2tool") diff --git a/dimos/robot/cli/test_can.py b/dimos/robot/cli/test_can.py new file mode 100644 index 0000000000..f27420da45 --- /dev/null +++ b/dimos/robot/cli/test_can.py @@ -0,0 +1,54 @@ +# 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 + +from subprocess import CompletedProcess + +from typer.testing import CliRunner + +from dimos.robot.cli.can import app + + +def test_setup_configures_and_verifies_can_interface(mocker) -> None: + mocker.patch("dimos.robot.cli.can.os.geteuid", return_value=1000) + run = mocker.patch( + "dimos.robot.cli.can.subprocess.run", + return_value=CompletedProcess([], 0, stdout="4: follower_l: UP qlen 1000\n", stderr=""), + ) + + result = CliRunner().invoke(app, ["setup", "follower_l"]) + + assert result.exit_code == 0 + assert "Running: sudo -- ip link set dev follower_l down" in result.stdout + assert "bitrate=1000000, txqueuelen=1000" in result.stdout + assert [call.args[0] for call in run.call_args_list] == [ + ["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_rejects_nonpositive_queue_length() -> None: + result = CliRunner().invoke(app, ["setup", "can0", "--txqueuelen", "0"]) + + assert result.exit_code == 2 + assert "txqueuelen must be greater than zero" in result.output From d1114734c59f2779cc6c6dca40a3fc62016d8877 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 06:02:31 +0000 Subject: [PATCH 17/44] feat(openyam): enable gravity-compensated teleop --- dimos/control/test_control.py | 23 ++++++ dimos/control/tick_loop.py | 6 +- .../manipulators/openyam_damiao/adapter.py | 39 +++++---- .../openyam_damiao/test_adapter.py | 82 +++++++++++++++---- dimos/robot/all_blueprints.py | 1 + .../manipulators/openyam/blueprints/teleop.py | 30 ++++++- dimos/robot/manipulators/openyam/config.py | 13 +-- .../manipulators/openyam/test_openyam.py | 41 ++++++++-- pyproject.toml | 1 + uv.lock | 2 + 10 files changed, 189 insertions(+), 49 deletions(-) diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 99834b090f..61ac235b03 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -970,6 +970,29 @@ def test_tick_loop_calls_compute(self, mock_adapter, wait_until): assert mock_task.compute.call_count > 0 + def test_rejected_hardware_write_is_reported(self, monkeypatch): + hardware = {"arm": MagicMock()} + hardware["arm"].write_command.return_value = False + log_error = MagicMock() + monkeypatch.setattr("dimos.control.tick_loop.logger.error", log_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)}) + + hardware["arm"].write_command.assert_called_once_with( + {"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/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py index 2a774eda1c..fe634a33cf 100644 --- a/dimos/hardware/manipulators/openyam_damiao/adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/adapter.py @@ -20,6 +20,9 @@ ) from dimos.robot.model_parser import parse_model from dimos.utils.data import LfsPath +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() OPENING_METRES = 0.096 _BUS_NAME = "openyam_can" @@ -118,10 +121,8 @@ def __init__( gravity_comp: bool = True, **kwargs: Any, ) -> None: - if not gravity_comp: - raise ValueError("OpenYAM requires gravity compensation") - if gravity_model_path is None or not Path(gravity_model_path).is_file(): - raise ValueError("OpenYAM requires a valid gravity model path") + if gravity_comp and (gravity_model_path is None or not Path(gravity_model_path).is_file()): + raise ValueError("OpenYAM gravity compensation requires a valid model path") lower, upper, velocity = _active_arm_limits() arm = _group_spec( bus_name=_BUS_NAME, @@ -129,8 +130,8 @@ def __init__( lower=lower, upper=upper, velocity=velocity, - kp=(0.0,) * 6, - kd=(0.0,) * 6, + kp=(80.0, 80.0, 80.0, 10.0, 10.0, 10.0), + kd=(5.0, 5.0, 5.0, 1.5, 1.5, 1.5), gravity_model_path=gravity_model_path, ) robot_spec = DamiaoRobotSpec( @@ -146,10 +147,19 @@ def __init__( robot_spec=robot_spec, group_name=_ARM_GROUP, gravity_model_path=gravity_model_path, - gravity_comp=True, + gravity_comp=gravity_comp, **kwargs, ) + self._write_armed_by_read = False + + def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float], list[float]]: + """Read feedback and arm exactly one subsequent motor write.""" + self._write_armed_by_read = False + state = super().refresh_state(force=force) + self._write_armed_by_read = True + return state + def write_mit_commands( self, *, @@ -159,16 +169,13 @@ def write_mit_commands( kd: list[float], tau: list[float], ) -> bool: - """Temporarily send zero-torque MIT frames for compliant position readback.""" + """Forward a command only after a successful feedback read.""" self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - zeros = self._zero_vector() - return super().write_mit_commands( - q=zeros, - dq=zeros, - kp=zeros, - kd=zeros, - tau=zeros, - ) + if not self._write_armed_by_read: + logger.error("OpenYAM rejected motor write without fresh position feedback") + return False + self._write_armed_by_read = False + return super().write_mit_commands(q=q, dq=dq, kp=kp, kd=kd, tau=tau) def read_gripper_position(self) -> float | None: """Gripper feedback is disabled until the binding provides calibration.""" diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py index 9d7fa69fef..52a4af41ca 100644 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -17,6 +17,7 @@ pytest.importorskip("dimos.hardware.damiao") +from dimos.hardware.damiao.runtime import DamiaoGroupState import dimos.hardware.manipulators.openyam_damiao.adapter as adapter_module from dimos.hardware.manipulators.openyam_damiao.adapter import ( ARM_MOTOR_SPECS, @@ -63,17 +64,42 @@ def test_openyam_limits_are_loaded_from_active_model() -> None: [1.5708, 3.66519, 4.01426, 1.65806, 1.5708, 1.8326] ) assert limits.velocity_max == pytest.approx([3.0, 10.0, 3.0, 10.0, 3.0, 10.0]) + assert adapter._kp == pytest.approx([80.0, 80.0, 80.0, 10.0, 10.0, 10.0]) + assert adapter._kd == pytest.approx([5.0, 5.0, 5.0, 1.5, 1.5, 1.5]) + assert adapter._gravity_comp -def test_openyam_requires_gravity_comp_and_model() -> None: - with pytest.raises(ValueError, match="gravity model"): - OpenYamDamiaoAdapter(use_mock_bus=True) +def test_openyam_allows_gravity_comp_to_be_disabled() -> None: + adapter = OpenYamDamiaoAdapter(gravity_comp=False, use_mock_bus=True) + + assert not adapter._gravity_comp + with pytest.raises(ValueError, match="gravity compensation"): - OpenYamDamiaoAdapter( - gravity_model_path=GRAVITY_MODEL_PATH, - gravity_comp=False, - use_mock_bus=True, - ) + OpenYamDamiaoAdapter(use_mock_bus=True) + + +def test_openyam_activation_holds_exact_feedback_position() -> None: + adapter = OpenYamDamiaoAdapter(gravity_comp=False, use_mock_bus=True) + runtime = Mock() + feedback = [-1.2, 0.1, 0.2, -0.3, 0.4, -0.5] + runtime.refresh_group_state.return_value = DamiaoGroupState( + q=feedback, dq=[0.0] * 6, tau=[0.0] * 6 + ) + runtime.enable.return_value = True + runtime.write_group_mit_commands.return_value = True + adapter._runtime = runtime + + assert adapter.activate() + + runtime.write_group_mit_commands.assert_called_once_with( + group_name="arm", + q=feedback, + dq=[0.0] * 6, + kp=[80.0, 80.0, 80.0, 10.0, 10.0, 10.0], + kd=[5.0, 5.0, 5.0, 1.5, 1.5, 1.5], + tau=[0.0] * 6, + ) + assert runtime.refresh_group_state.call_count >= 2 def test_openyam_normal_enable_and_error_recovery() -> None: @@ -98,16 +124,21 @@ def test_openyam_normal_enable_and_error_recovery() -> None: runtime.enable.assert_called_once_with() -def test_openyam_temporarily_writes_zero_torque_mit_commands() -> None: +def test_openyam_forwards_mit_commands() -> None: adapter = OpenYamDamiaoAdapter( gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True, ) runtime = Mock() + runtime.refresh_group_state.return_value = DamiaoGroupState( + q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6 + ) runtime.write_group_mit_commands.return_value = True adapter._runtime = runtime adapter._enabled = True + assert adapter.refresh_state(force=True)[0] == [0.25] * 6 + assert adapter.write_mit_commands( q=[1.0] * 6, dq=[2.0] * 6, @@ -118,13 +149,36 @@ def test_openyam_temporarily_writes_zero_torque_mit_commands() -> None: runtime.write_group_mit_commands.assert_called_once_with( group_name="arm", - q=[0.0] * 6, - dq=[0.0] * 6, - kp=[0.0] * 6, - kd=[0.0] * 6, - tau=[0.0] * 6, + q=[1.0] * 6, + dq=[2.0] * 6, + kp=[3.0] * 6, + kd=[4.0] * 6, + tau=[5.0] * 6, ) + assert not adapter.write_mit_commands( + q=[1.0] * 6, dq=[2.0] * 6, kp=[3.0] * 6, kd=[4.0] * 6, tau=[5.0] * 6 + ) + runtime.write_group_mit_commands.assert_called_once() + + +def test_openyam_failed_read_revokes_write_permission() -> None: + adapter = OpenYamDamiaoAdapter(gravity_comp=False, use_mock_bus=True) + runtime = Mock() + runtime.refresh_group_state.return_value = DamiaoGroupState( + q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6 + ) + adapter._runtime = runtime + adapter._enabled = True + + adapter.refresh_state(force=True) + runtime.refresh_group_state.side_effect = RuntimeError("feedback unavailable") + with pytest.raises(RuntimeError, match="feedback unavailable"): + adapter.refresh_state(force=True) + + assert not adapter.write_joint_positions([0.25] * 6) + runtime.write_group_mit_commands.assert_not_called() + def test_openyam_xacro_limits_reject_duplicate_joint_names(monkeypatch: pytest.MonkeyPatch) -> None: joints = [JointDescription(f"yam_joint{i}", "revolute", -1.0, 1.0, 1.0) for i in range(1, 7)] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index badf113633..93d9d8d84d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -72,6 +72,7 @@ "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-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", diff --git a/dimos/robot/manipulators/openyam/blueprints/teleop.py b/dimos/robot/manipulators/openyam/blueprints/teleop.py index 41156c6f01..a31255d5de 100644 --- a/dimos/robot/manipulators/openyam/blueprints/teleop.py +++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py @@ -19,10 +19,15 @@ from dimos.control.coordinator import ControlCoordinator 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, + eef_twist_task, + planner, + trajectory_task, +) from dimos.robot.manipulators.openyam.config import ( OPENYAM_DOF, - OPENYAM_MODEL_PATH, + OPENYAM_GRAVITY_MODEL_PATH, make_openyam_model_config, openyam_hardware, ) @@ -37,7 +42,7 @@ tasks=[ eef_twist_task( _openyam_keyboard_hw, - model_path=OPENYAM_MODEL_PATH, + model_path=OPENYAM_GRAVITY_MODEL_PATH, ee_joint_id=OPENYAM_DOF, ) ], @@ -47,3 +52,22 @@ visualization={"backend": "viser"}, ), ) + +_openyam_keyboard_planner_hw = openyam_hardware("arm") + +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( + _openyam_keyboard_planner_hw, + model_path=OPENYAM_GRAVITY_MODEL_PATH, + ee_joint_id=OPENYAM_DOF, + priority=10, + ), + trajectory_task(_openyam_keyboard_planner_hw, priority=20), + ], + ), +) diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index c7dead68b6..c02d181c67 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -31,7 +31,7 @@ OPENYAM_DOF = 6 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} @@ -82,6 +82,7 @@ def openyam_hardware( # planning/home positions into a live motor adapter. adapter_kwargs={ "gravity_model_path": OPENYAM_GRAVITY_MODEL_PATH, + "gravity_comp": True, }, include_gripper=False, ) @@ -94,19 +95,19 @@ def make_openyam_model_config( 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") + 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, @@ -116,7 +117,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 5acd80a3f2..84317fcaf6 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -25,6 +25,7 @@ ) from dimos.robot.manipulators.openyam.blueprints.teleop import ( keyboard_teleop_openyam, + keyboard_teleop_openyam_planner, ) from dimos.robot.manipulators.openyam.config import ( OPENYAM_DOF, @@ -34,6 +35,7 @@ make_openyam_model_config, openyam_hardware, ) +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule def _module_kwargs(blueprint: Blueprint, module_type: type) -> dict[str, Any]: @@ -47,12 +49,12 @@ def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]: def test_openyam_model_config_has_expected_links_and_mapping() -> 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_names == [f"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) + f"arm/joint{i}": f"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 config.base_link == "base" + assert config.end_effector_link == "gripper_tip" assert list(config.package_paths) == list(OPENYAM_PACKAGE_PATHS) assert config.gripper_hardware_id == "arm" @@ -74,6 +76,7 @@ def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: A assert hardware.adapter_type == "openyam_damiao" assert hardware.address == "can1" assert hardware.adapter_kwargs["gravity_model_path"] == OPENYAM_GRAVITY_MODEL_PATH + assert hardware.adapter_kwargs["gravity_comp"] is True assert len(hardware.joints) == OPENYAM_DOF assert hardware.gripper_joints == [] assert "initial_positions" not in hardware.adapter_kwargs @@ -113,12 +116,31 @@ def test_openyam_planner_blueprint_preserves_model_config() -> None: config = ManipulationModuleConfig(**kwargs).robots[0] 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.joint_names == [f"joint{i}" for i in range(1, OPENYAM_DOF + 1)] + assert config.end_effector_link == "gripper_tip" 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)] + tasks = _coordinator_kwargs(blueprint)["tasks"] + assert len(tasks) == 1 + trajectory = tasks[0] + assert trajectory.type == "trajectory" + assert trajectory.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] + assert trajectory.priority == 10 + assert all(atom.module is not KeyboardTeleopModule for atom in blueprint.blueprints) + + +def test_openyam_keyboard_planner_blueprint_combines_teleop_and_trajectory() -> None: + blueprint = keyboard_teleop_openyam_planner + tasks = _coordinator_kwargs(blueprint)["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 == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] + assert trajectory.priority == 20 + assert eef_twist.joint_names == trajectory.joint_names + assert eef_twist.params["ee_joint_id"] == OPENYAM_DOF + assert eef_twist.params["model_path"] == OPENYAM_GRAVITY_MODEL_PATH + assert eef_twist.priority == 10 + assert _module_kwargs(blueprint, KeyboardTeleopModule) == {} def test_openyam_coordinator_blueprint_uses_six_arm_joints() -> None: @@ -137,4 +159,5 @@ def test_openyam_teleop_blueprint_constructs_with_eef_twist() -> None: assert task.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] assert task.params["ee_joint_id"] == OPENYAM_DOF + assert task.params["model_path"] == OPENYAM_GRAVITY_MODEL_PATH assert _module_kwargs(blueprint, ManipulationModule)["visualization"] == {"backend": "viser"} diff --git a/pyproject.toml b/pyproject.toml index 84354f696e..663afb6853 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -286,6 +286,7 @@ manipulation = [ "pycollada", # Visualization (Optional) + "pygame>=2.6.1", "viser[urdf]>=1.0.29", "yourdfpy>=0.0.60", "xacro", diff --git a/uv.lock b/uv.lock index 516c019c85..67324ea49d 100644 --- a/uv.lock +++ b/uv.lock @@ -1742,6 +1742,7 @@ manipulation = [ { name = "pin-pink" }, { name = "piper-sdk" }, { name = "pycollada" }, + { name = "pygame" }, { name = "pyrealsense2-extended", marker = "sys_platform != 'darwin'" }, { name = "pyyaml" }, { name = "qpsolvers", extra = ["proxqp"] }, @@ -2121,6 +2122,7 @@ requires-dist = [ { name = "psutil", specifier = ">=7.0.0" }, { name = "pyarrow", marker = "extra == 'learning'" }, { name = "pycollada", marker = "extra == 'manipulation'" }, + { name = "pygame", marker = "extra == 'manipulation'", specifier = ">=2.6.1" }, { name = "pydantic" }, { name = "pydantic-settings", specifier = ">=2.11.0,<3" }, { name = "pygame", marker = "extra == 'sim'", specifier = ">=2.6.1" }, From 1ba65426ceb14e164cead1f09731d267cbf27ae6 Mon Sep 17 00:00:00 2001 From: cc <55869557+TomCC7@users.noreply.github.com> Date: Sat, 1 Aug 2026 02:32:42 -0400 Subject: [PATCH 18/44] Delete docs/capabilities/manipulation/openyam_commissioning.md --- .../manipulation/openyam_commissioning.md | 119 ------------------ 1 file changed, 119 deletions(-) delete mode 100644 docs/capabilities/manipulation/openyam_commissioning.md diff --git a/docs/capabilities/manipulation/openyam_commissioning.md b/docs/capabilities/manipulation/openyam_commissioning.md deleted file mode 100644 index 2e09a386ca..0000000000 --- a/docs/capabilities/manipulation/openyam_commissioning.md +++ /dev/null @@ -1,119 +0,0 @@ ---- -title: "OpenYAM Direction Commissioning" ---- - -## Purpose - -Before using normal OpenYAM planning or teleoperation on hardware, obtain an -approved vendor/bench-tool direction verification for every planning joint. -This driver cannot perform that verification: its OpenYAM gravity-compensation -path intentionally uses zero position gains and therefore cannot safely issue -position-step commissioning commands. Do not use this procedure to validate -the gripper. - -The six planning joints are mapped as follows: - -| Hardware joint | Planning joint | Expected positive mapping | -|---|---|---| -| `arm/joint1` | `yam_joint1` | A positive command increases `yam_joint1` position | -| `arm/joint2` | `yam_joint2` | A positive command increases `yam_joint2` position | -| `arm/joint3` | `yam_joint3` | A positive command increases `yam_joint3` position | -| `arm/joint4` | `yam_joint4` | A positive command increases `yam_joint4` position | -| `arm/joint5` | `yam_joint5` | A positive command increases `yam_joint5` position | -| `arm/joint6` | `yam_joint6` | A positive command increases `yam_joint6` position | - -The expected result is about the commanded joint only. The physical direction -depends on the joint's zero pose and installation; use the measured encoder -change, not a visual guess about clockwise or counter-clockwise motion, as the -pass criterion. - -## Lifecycle and safety behavior - -Stopping the driver or handling a fault does **not** automatically park the arm. -Neither event should be documented or relied on as a motion command. On stop or -fault, disable motion output and keep the workspace clear; the operator must -assess the hardware state and use the emergency or manual disable path as -appropriate. - -Parking is an explicit operator action that is permitted only while the system -is healthy and the operator has confirmed that a controlled park motion is safe. -Do not attempt to park from a fault handler, during an unhealthy state, or as an -implicit part of shutdown. A no-motion disable is the safe default when motion -must be prevented: it disables commanded motion without issuing a park move. - -Gravity-compensation mode also requires a preflight before activation. Verify -the supported OpenYAM inertial model, its required asset, joint state, limits, -and the mechanical support and workspace conditions. If any preflight check is -missing or invalid, do not enable gravity compensation; use the no-motion -disable path and resolve the issue first. - -## Safety prerequisites - -Complete all of the following before enabling the hardware: - -- Remove the payload, tool, and any object held by the gripper. -- Secure the base and support the arm so an unexpected motion cannot cause a - fall, collision, or pinch. -- Clear the workspace. Keep people, cables, and tools outside the motion - envelope, and keep an operator at the emergency stop. -- Confirm the emergency stop, motor power cut-off, and manual disable path - work before the first command. -- Confirm the correct CAN interface, motor IDs, joint limits, and zero/calibration - state. Stop if any state reading is missing, implausible, or stale. -- Use the lowest approved position gains and speed, and choose a small test - increment that stays well inside the joint limits. Never test at a limit. - -If the arm is not mechanically supported or any prerequisite is uncertain, do -not enable it. - -## Gripper status - -Physical gripper control and physical gripper position readback are **not -available** in this integration. Do not use, advertise, or validate a gripper -command or readback as a supported capability. Upstream must first release a -calibrated normalized-opening getter; only after that upstream dependency is -released and integrated can gripper support be reconsidered. The direction -commissioning procedure below applies to the six planning joints only, not the -gripper. - -## External direction verification precondition - -Before starting this driver, have an approved vendor tool or controlled bench -procedure verify `arm/joint1` through `arm/joint6` individually. That external -procedure must use its own documented safe low-speed command path; this driver -must not be used to issue the direction-test steps. Record the measured encoder -sign, joint identity, and any coupled motion. Stop and correct wiring, -calibration, or configuration if a sign is reversed, a joint is swapped, or -unexpected motion occurs. - -Only after the approved record is complete may this driver be connected for -gravity-compensated operation. On connection, verify state and limits without -requesting commissioning steps; disable output if any reading is stale or -implausible. - -## Record and approve the result - -Record one entry per joint in the commissioning log, including: - -- date, operator, hardware/firmware identity, and CAN interface; -- initial position and measured direction result from the approved external - procedure, including the tool used; -- commanded joint, observed `yam_jointN`, and whether any other joint moved; -- pass/fail status, faults or warnings, and the corrective action for failures. - -Normal hardware motion is **prohibited until all six directions pass** and the -record is reviewed by the responsible operator. A software test pass is not an -approval to skip this step. - -## Software coverage versus hardware validation - -The software tests can verify that OpenYAM exposes six joints, that -`arm/joint1` through `arm/joint6` map to `yam_joint1` through `yam_joint6`, and -that mock hardware accepts position updates. These tests do not energize a -motor and cannot detect reversed motor wiring, encoder polarity, swapped CAN -IDs, installation-specific motion, or unexpected mechanical coupling. - -Only the approved external procedure above verifies the actual direction -mapping on connected OpenYAM hardware. Treat the software coverage and the -on-hardware commissioning record as separate requirements; both must be -complete before normal planning, execution, or teleoperation. From 5e9d7798e1d84f73f214fc07db02e7ac96d355bc Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 06:59:00 +0000 Subject: [PATCH 19/44] spec: remove --- .../add-openyam-damiao-adapter/.openspec.yaml | 2 - .../add-openyam-damiao-adapter/design.md | 104 ------------------ .../add-openyam-damiao-adapter/proposal.md | 55 --------- .../specs/openyam-damiao-control/spec.md | 97 ---------------- .../add-openyam-damiao-adapter/tasks.md | 49 --------- 5 files changed, 307 deletions(-) delete mode 100644 openspec/changes/add-openyam-damiao-adapter/.openspec.yaml delete mode 100644 openspec/changes/add-openyam-damiao-adapter/design.md delete mode 100644 openspec/changes/add-openyam-damiao-adapter/proposal.md delete mode 100644 openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md delete mode 100644 openspec/changes/add-openyam-damiao-adapter/tasks.md diff --git a/openspec/changes/add-openyam-damiao-adapter/.openspec.yaml b/openspec/changes/add-openyam-damiao-adapter/.openspec.yaml deleted file mode 100644 index c0a8162549..0000000000 --- a/openspec/changes/add-openyam-damiao-adapter/.openspec.yaml +++ /dev/null @@ -1,2 +0,0 @@ -schema: spec-driven -created: 2026-07-21 diff --git a/openspec/changes/add-openyam-damiao-adapter/design.md b/openspec/changes/add-openyam-damiao-adapter/design.md deleted file mode 100644 index cd61290fab..0000000000 --- a/openspec/changes/add-openyam-damiao-adapter/design.md +++ /dev/null @@ -1,104 +0,0 @@ -## Context - -OpenYAM is one CAN-bus arm with six planning joints and a separate gripper: -CAN IDs 1–3 are DM4340, 4–6 are DM4310, and ID 7 is a DM4310 gripper. The -gravity model is a fixed-finger, six-DOF, gravity-only URDF. It is deliberately -separate from the active Xacro, which remains the planning and command-limit -authority; the runtime cannot expand Xacro. Supported Linux deployment -architectures SHALL include x86_64. - -## Goals / Non-Goals - -**Goals:** - -- Reuse the Damiao runtime, specs, and `DamiaoArmAdapter` pattern without - inheriting OpenArm-specific kinematics or hardware metadata. -- Present exactly six arm DOFs in `yam_joint1` through `yam_joint6` order. -- Keep the gripper unavailable until a released upstream `can-motor-control` - normalized, calibrated opening getter exists; do not fabricate gripper state. -- Require every enable and recovery path to load the gravity model and complete - finite-`G(q)` preflight before zero-gain gravity operation; failures must - disable with no motion command and must not send zero torque. -- Preserve physical encoder-zero home; initial positions are mock-only. - -**Non-Goals:** - -- Redesigning the generic manipulator API, Damiao runtime, or OpenYAM blueprint - structure. -- Using the whole-body Damiao adapter. -- Adding a homing routine, stored encoder offsets, or a seventh arm DOF. -- Generating Xacro at runtime or using the gravity URDF for planning limits. -- Providing fabricated gripper state or an invented upstream getter. - -## Decisions - -### Create a narrow OpenYAM adapter over `DamiaoArmAdapter` - -The adapter SHALL construct a six-motor arm `DamiaoJointGroupSpec` and retain -the inherited arm command/state behavior. It SHALL add a second internal -single-motor group for the gripper, without counting it in `get_dof()`. Gripper -position and command operations SHALL remain unavailable until released -upstream normalized calibrated-opening getter support exists. - -Using `DamiaoWholeBodyAdapter` was rejected because OpenYAM is one -manipulator, not a multi-limb robot. Copying the OpenArm adapter wholesale was -rejected because its topology, geometry, and calibration are OpenArm-specific. - -### Encode OpenYAM hardware metadata locally - -The adapter's arm group SHALL specify DM4340 motors at CAN IDs 1–3 and DM4310 -motors at IDs 4–6 in planning-joint order. The gripper group SHALL specify its -DM4310 at CAN ID 7. OpenYAM gains SHALL derive from its motor configuration. -Hardware limits SHALL be parsed fail-closed from the active gripper Xacro and -never from the gravity URDF. The parser SHALL reject duplicate joints, missing -or nonfinite values, and invalid ranges. Physical encoder zeroes remain the -home reference; the physical factory SHALL reject mock-only initial-position -configuration, with no homing or offset behavior. - -### Keep gripper support upstream-first - -When the upstream getter is released, the public gripper interface SHALL use -aperture metres and convert linearly to the driver's calibrated normalized -opening, where zero is closed and one is open. Until then, the adapter SHALL -report the capability as unavailable and SHALL neither command nor synthesize -its state. The nominal future aperture span is 0.096 m; endpoint calibration -must follow the upstream opening lifecycle. - -### Require a fixed-finger gravity-model asset for activation - -The hardware configuration SHALL provide an LFS-backed, expanded, fixed-finger -six-DOF URDF through `gravity_model_path`. It is gravity-only and is not a -source of planning joints or command limits. Before enabling or recovering the -arm, activation SHALL load the model, evaluate `G(q)` for the current six-joint -state, and reject non-finite results. Every such path SHALL complete this -preflight before sending any zero-gain command. If model loading, gravity -evaluation, enabling, or recovery fails, the arm SHALL be disabled with no -motion command; it SHALL not send zero torque as a failure response. Only -after successful preflight may the command path send `Kp=Kd=0` and `tau=G(q)`. - -Disabling SHALL be a no-motion operation by default. An optional park action -may be requested explicitly as a separate operation; disabling must never -implicitly park the arm. If a no-motion disable fails, the resulting state is -explicitly unresolved energized/unknown: it SHALL NOT be reported as disabled. -The system SHALL escalate to the operator and the approved e-stop procedure. - -## Risks / Trade-offs - -- [Incorrect motor direction can move a joint opposite its planning command] - → Require an approved external vendor/bench-tool direction commissioning - result for all six joints before hardware enable. This is a precondition, - not an adapter-side motion routine: the driver enters gravity mode with - zero gains, so the adapter must not attempt to discover direction in control. -- [Gripper support is unavailable or calibration actuates into end stops] - → Block the gripper API until released upstream getter support exists; then - use only the driver's bounded calibration procedure. -- [An unavailable, invalid, or numerically unstable gravity asset could enable - unsafe zero-gain behavior] → Require model load and finite-`G(q)` preflight - on every enable/recovery path; use no-motion disable, never zero torque, on - failure. -- [No-motion disable can fail while the driver remains energized or unknown] - → Preserve an unresolved energized/unknown state, never claim disabled, and - escalate to the operator and approved e-stop procedure. -- [URDF/Xacro sources disagree on limits or inertials] → Treat the active - gripper Xacro as the fail-closed planning-limit authority and the fixed-finger - URDF as gravity-only authority. diff --git a/openspec/changes/add-openyam-damiao-adapter/proposal.md b/openspec/changes/add-openyam-damiao-adapter/proposal.md deleted file mode 100644 index 32cae1ded1..0000000000 --- a/openspec/changes/add-openyam-damiao-adapter/proposal.md +++ /dev/null @@ -1,55 +0,0 @@ -## Why - -OpenYAM currently uses a mock manipulator adapter, so DimOS cannot operate the -physical arm. The existing Damiao runtime and OpenArm adapter establish a -reusable integration pattern, while OpenYAM needs explicit separation between -its gravity asset, active planning Xacro, hardware limits, and unavailable -gripper state. The physical integration also targets supported Linux hosts -including x86_64. - -## What Changes - -- Add a hardware-backed OpenYAM manipulator adapter built on the shared Damiao - runtime and generic arm adapter. -- Configure the six-joint arm as CAN IDs 1–6 (DM4340 shoulder group and DM4310 - distal group), while keeping the DM4310 CAN-ID-7 gripper separate from the - arm's six degrees of freedom. -- Keep gripper operations unavailable until a released upstream - `can-motor-control` normalized calibrated-opening getter exists; do not - fabricate state. Then expose aperture metres through the existing API. -- Activate the arm in gravity-only compensation using zero position and - velocity gains plus feed-forward gravity torque from a stable, expanded, - fixed-finger six-DOF URDF, with loaded-model and finite-`G(q)` preflight on - every enable/recovery path. Gravity failure performs no-motion disable and - never sends zero torque. -- Keep that gravity URDF separate from the active Xacro, which is the - fail-closed planning and hardware-limit authority; reject duplicate, - nonfinite, missing, or invalid Xacro limit entries. -- Use physical encoder-zero home; the physical factory rejects mock-only - initial positions. Require approved external vendor/bench-tool direction - commissioning before enable because the driver enters gravity mode at zero - gains. Disable is no-motion by default, while park is explicit; a failed - disable remains unresolved energized/unknown, is never reported disabled, - and escalates to operator/e-stop procedure. - -## Capabilities - -### New Capabilities - -- `openyam-damiao-control`: Hardware control of the six-joint OpenYAM arm and - its separate CAN-bus gripper through the Damiao motor runtime. - -### Modified Capabilities - -- None. - -## Impact - -- Adds an OpenYAM-specific adapter and physical-hardware registration under - `dimos/hardware/manipulators/`. -- Updates `dimos/robot/manipulators/openyam/config.py` to select the hardware - implementation instead of the mock adapter. -- Adds a materialized fixed-finger, six-DOF gravity-only OpenYAM URDF asset to - LFS-backed robot resources; it does not supply planning limits. -- Reuses the existing `can-motor-control` dependency and shared Damiao runtime; - it introduces no generic API change beyond the planned hardware integration. diff --git a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md b/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md deleted file mode 100644 index 32184bcdc0..0000000000 --- a/openspec/changes/add-openyam-damiao-adapter/specs/openyam-damiao-control/spec.md +++ /dev/null @@ -1,97 +0,0 @@ -## ADDED Requirements - -### Requirement: Supported Linux architecture -The physical OpenYAM Damiao integration SHALL support Linux on x86_64 in -addition to any other explicitly supported Linux architecture. - -#### Scenario: Deploying on Linux x86_64 -- **WHEN** the physical OpenYAM integration is deployed on a Linux x86_64 host -- **THEN** the platform is supported by this capability - -### Requirement: Six-DOF OpenYAM Damiao arm control -The system SHALL provide a hardware-backed OpenYAM manipulator adapter using -the shared Damiao motor runtime. The adapter SHALL expose exactly six arm -degrees of freedom in `yam_joint1` through `yam_joint6` order, corresponding to -CAN IDs 1 through 6. It SHALL configure CAN IDs 1 through 3 as DM4340 and IDs 4 -through 6 as DM4310. The separate DM4310 gripper at CAN ID 7 SHALL NOT be -counted as an arm degree of freedom. - -#### Scenario: Adapter reports OpenYAM arm topology -- **WHEN** the OpenYAM hardware adapter is created -- **THEN** it reports six arm degrees of freedom with the specified motor types - and CAN ordering, while retaining CAN ID 7 as a separate gripper actuator - -### Requirement: Gripper capability is upstream-gated -The system SHALL leave OpenYAM gripper position and command operations -unavailable until a released upstream `can-motor-control` API provides a -normalized, calibrated opening getter. It SHALL not fabricate gripper state, -infer it from motor angle, or invent a replacement getter. - -#### Scenario: Gripper support is not released -- **WHEN** the upstream normalized calibrated-opening getter is unavailable -- **THEN** gripper position and command operations report unavailable and issue - no fabricated state or gripper command - -#### Scenario: Reading a gripper aperture after upstream support -- **WHEN** the released upstream getter is available and the gripper is - calibrated -- **THEN** the adapter may convert normalized opening to aperture metres, with - `0` closed and `1` open, without exposing normalized units publicly - -### Requirement: Gravity-compensation activation -The system SHALL configure OpenYAM activation and recovery to use zero position -and velocity gains with feed-forward gravity torque computed from a valid, -expanded, fixed-finger six-DOF gravity-only URDF. Every enable or recovery path -SHALL load the model and preflight finite `G(q)` before sending any zero-gain -command. The active gripper Xacro SHALL remain the sole source of planning and -hardware command limits. It SHALL NOT represent zero gains without a valid -model and finite `G(q)` as gravity compensation. - -#### Scenario: Activating a configured OpenYAM arm -- **WHEN** the coordinator activates an OpenYAM adapter -- **THEN** it first loads the fixed-finger model and verifies finite `G(q)` for - the current state, then enables the arm and sends `Kp=0`, `Kd=0`, and gravity - feed-forward torque - -#### Scenario: Gravity preflight fails on enable or recovery -- **WHEN** the model is missing, unloadable, or produces non-finite `G(q)` -- **THEN** the enable or recovery fails before zero-gain control, performs a - no-motion disable, and sends neither zero torque nor any other motion command - -#### Scenario: Disable versus park -- **WHEN** control is disabled -- **THEN** the adapter disables without issuing a motion command -- **WHEN** an operator explicitly requests park -- **THEN** parking is performed only as that separate, opt-in operation - -#### Scenario: No-motion disable fails -- **WHEN** a no-motion disable fails or its resulting actuator state cannot be - confirmed -- **THEN** the system enters an unresolved energized/unknown state, does not - report disabled, and escalates to the operator and approved e-stop procedure - -### Requirement: OpenYAM physical hardware selection -The system SHALL configure OpenYAM's physical manipulator hardware to use the -Damiao-backed adapter instead of the mock adapter. It SHALL preserve physical -encoder-zero home and SHALL parse the active gripper Xacro fail-closed as the -authority for arm command limits. The physical factory SHALL reject mock-only -initial-position configuration; initial positions SHALL be mock-only and -unavailable to physical construction. - -#### Scenario: Building the physical OpenYAM configuration -- **WHEN** DimOS builds an OpenYAM blueprint for physical hardware -- **THEN** it instantiates the Damiao-backed adapter only with six unique, - finite, valid limits parsed from the active gripper Xacro, uses encoder-zero - home, rejects mock initial positions, and does not add a homing or - joint-offset procedure - -### Requirement: External direction commissioning precondition -The system SHALL require approved external vendor or bench-tool direction -commissioning for all six arm motors before physical enable. The adapter SHALL -not perform direction discovery or commissioning by commanding the arm, because -the driver activates gravity mode with zero position and velocity gains. - -#### Scenario: Direction commissioning is absent -- **WHEN** physical enable is requested without an approved six-joint direction - commissioning result -- **THEN** enable is rejected before any arm command is sent diff --git a/openspec/changes/add-openyam-damiao-adapter/tasks.md b/openspec/changes/add-openyam-damiao-adapter/tasks.md deleted file mode 100644 index c194b259ee..0000000000 --- a/openspec/changes/add-openyam-damiao-adapter/tasks.md +++ /dev/null @@ -1,49 +0,0 @@ -## 1. Hardware resources and metadata - -- [ ] 1.0 Confirm supported Linux deployment includes x86_64. -- [ ] 1.1 Add the materialized, fixed-finger six-DOF gravity-only OpenYAM URDF - as an LFS-backed resource and make it available to the hardware configuration. -- [ ] 1.2 Define OpenYAM arm and gripper Damiao metadata: IDs 1–3 as DM4340, - IDs 4–6 as DM4310, and gripper ID 7 as DM4310, with OpenYAM gains. -- [ ] 1.3 Parse the active gripper Xacro's six arm limits fail-closed into the - hardware metadata, rejecting duplicate joints, missing/nonfinite values, and - invalid ranges; keep physical encoder-zero home, reject mock-only initial - positions in the physical factory, and add no homing or encoder offsets. - -## 2. OpenYAM Damiao adapter - -- [ ] 2.1 Implement a six-DOF OpenYAM adapter that reuses `DamiaoArmAdapter` - for arm state, MIT commands, and gravity feed-forward behavior. -- [ ] 2.2 Add an internal one-motor gripper group without including it in the - arm DOF, but keep gripper state and commands unavailable until released - upstream normalized calibrated-opening getter support exists. -- [ ] 2.3 After upstream support exists, implement its bounded calibration and - linear conversion between metre aperture and normalized opening over 0.096 m; - do not fabricate state before then. -- [ ] 2.4 Require loaded-model and finite-`G(q)` preflight on every enable and - recovery path before `Kp=Kd=0` with `G(q)` feed-forward; on failure perform - no-motion disable and send no zero torque, keeping optional park separate. - -## 3. OpenYAM integration - -- [ ] 3.1 Register the OpenYAM hardware adapter and select it from the physical - OpenYAM configuration in place of the mock adapter. -- [ ] 3.2 Preserve simulation and mock configuration behavior where physical - Damiao hardware is not selected. - -## 4. Verification and commissioning support - -- [ ] 4.1 Add focused tests for six-DOF topology, motor metadata, separate - gripper availability, and (after upstream support) aperture conversion. -- [ ] 4.2 Add focused tests that every enable/recovery path rejects a missing, - unloadable, or non-finite gravity result, uses zero gains only after - preflight, and performs no-motion disable without zero torque on failure. -- [ ] 4.3 Document the approved external vendor/bench-tool direction - commissioning precondition for all six joints; verify the adapter does not - discover direction under zero-gain gravity mode, disable never implicitly - parks, failed disable is unresolved energized/unknown rather than disabled - and escalates to operator/e-stop, and physical factories reject initial - positions; verify gravity preflight and external direction commissioning remain - required before normal and recovery enable. -- [ ] 4.4 Run the focused OpenYAM and Damiao test suite plus the blueprint - registry generation test if registration changes generated blueprint output. From 65427f72a9303eece94696a0c392a225cc6a1ca9 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 00:09:43 -0700 Subject: [PATCH 20/44] update openyam description --- data/.lfs/yam_description.tar.gz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/data/.lfs/yam_description.tar.gz b/data/.lfs/yam_description.tar.gz index 603d61b1d7..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:deced1f7ef6ce3b72c32ab7c32f86ce6a1bf110dbc5d0efcabbe87b0f516bae0 -size 3274684 +oid sha256:eb8c04381e29ceb1340e818bc706ba866d706c37a254d5637babc7168a5de850 +size 5759450 From e87f26a0c069ad4f17a11b0bc7c4a737b12f1325 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 00:52:52 -0700 Subject: [PATCH 21/44] refactor(manipulation): address OpenYAM review feedback --- dimos/cli/can.py | 29 +- dimos/cli/dimos.py | 2 +- .../piper/cli.py => cli/piper.py} | 14 +- dimos/cli/test_can.py | 10 +- dimos/cli/test_dimos.py | 14 + dimos/cli/test_piper.py | 61 +++ dimos/hardware/damiao/__init__.py | 36 -- dimos/hardware/damiao/arm_adapter.py | 155 ++----- dimos/hardware/damiao/config.py | 225 ++++++++++ dimos/hardware/damiao/runtime.py | 384 ++++++------------ dimos/hardware/damiao/specs.py | 316 -------------- dimos/hardware/damiao/test_adapters.py | 294 ++++++-------- .../manipulators/openyam_damiao/__init__.py | 22 - .../manipulators/openyam_damiao/adapter.py | 129 +++--- .../openyam_damiao/test_adapter.py | 96 +++-- .../visualization/test_factory.py | 6 + .../visualization/viser/config.py | 2 +- dimos/robot/manipulators/openyam/config.py | 7 +- .../manipulators/openyam/test_openyam.py | 7 +- dimos/robot/manipulators/piper/test_cli.py | 68 ---- docs/capabilities/manipulation/index.md | 14 +- 21 files changed, 772 insertions(+), 1119 deletions(-) rename dimos/{robot/manipulators/piper/cli.py => cli/piper.py} (72%) create mode 100644 dimos/cli/test_piper.py delete mode 100644 dimos/hardware/damiao/__init__.py create mode 100644 dimos/hardware/damiao/config.py delete mode 100644 dimos/hardware/damiao/specs.py delete mode 100644 dimos/hardware/manipulators/openyam_damiao/__init__.py delete mode 100644 dimos/robot/manipulators/piper/test_cli.py diff --git a/dimos/cli/can.py b/dimos/cli/can.py index 2936ae61d2..bf75ec354a 100644 --- a/dimos/cli/can.py +++ b/dimos/cli/can.py @@ -1,10 +1,16 @@ -# Copyright 2025-2026 Dimensional Inc. +# 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.""" @@ -43,11 +49,6 @@ def _run_ip(*args: str, privileged: bool = False) -> subprocess.CompletedProcess raise typer.Exit(1) from exc -def _positive(value: int, name: str) -> None: - if value <= 0: - raise typer.BadParameter(f"{name} must be greater than zero") - - @app.command("status") def status(interface: str = typer.Argument(..., help="Linux CAN interface name")) -> None: """Show detailed CAN interface state and queue statistics.""" @@ -72,12 +73,20 @@ def up(interface: str = typer.Argument(..., help="Linux CAN interface name")) -> @app.command("setup") def setup( interface: str = typer.Argument(..., help="Linux CAN interface name"), - bitrate: int = typer.Option(1_000_000, help="Nominal CAN bitrate in bits per second"), - txqueuelen: int = typer.Option(1_000, help="Kernel transmit queue length"), + 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.""" - _positive(bitrate, "bitrate") - _positive(txqueuelen, "txqueuelen") + 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( diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index 43cb1e521b..d46240a96b 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -55,6 +55,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.piper import app as piper_app from dimos.cli.shell import shell from dimos.constants import CONFIG_DIR, LOG_DIR from dimos.core.daemon import daemonize, install_signal_handlers @@ -65,7 +66,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 diff --git a/dimos/robot/manipulators/piper/cli.py b/dimos/cli/piper.py similarity index 72% rename from dimos/robot/manipulators/piper/cli.py rename to dimos/cli/piper.py index 0e22fd5eea..55cbaa4b62 100644 --- a/dimos/robot/manipulators/piper/cli.py +++ b/dimos/cli/piper.py @@ -14,17 +14,17 @@ from __future__ import annotations -import subprocess - import typer +from dimos.cli.can import setup_interface + 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"), + bitrate: int = typer.Option(1_000_000, "--bitrate", min=1, help="CAN bitrate"), ) -> None: """Configure an existing Piper SocketCAN interface.""" if not typer.confirm( @@ -34,10 +34,4 @@ def can_activate( 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) + setup_interface(interface, bitrate=bitrate) diff --git a/dimos/cli/test_can.py b/dimos/cli/test_can.py index 855d92944a..c3e90a7f24 100644 --- a/dimos/cli/test_can.py +++ b/dimos/cli/test_can.py @@ -1,10 +1,16 @@ -# Copyright 2025-2026 Dimensional Inc. +# 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 subprocess import CompletedProcess @@ -51,4 +57,4 @@ def test_setup_rejects_nonpositive_queue_length() -> None: result = CliRunner().invoke(app, ["setup", "can0", "--txqueuelen", "0"]) assert result.exit_code == 2 - assert "txqueuelen must be greater than zero" in result.output + assert "x>=1" in result.output diff --git a/dimos/cli/test_dimos.py b/dimos/cli/test_dimos.py index 99a3d17ce3..f82f9fcdbf 100644 --- a/dimos/cli/test_dimos.py +++ b/dimos/cli/test_dimos.py @@ -32,6 +32,8 @@ import dimos.core.coordination.worker_manager_python as worker_manager_python from dimos.core.global_config import global_config from dimos.core.module import Module, ModuleConfig +from dimos.manipulation.manipulation_module import ManipulationModuleConfig +from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.robot import external_blueprints as external import dimos.utils.cache as cache_utils @@ -174,6 +176,18 @@ class TestModuleG(Module): assert kwargs["g"]["local_relay"] is True # the explicit flag wins its own key +def test_load_config_args_overrides_nested_viser_host(tmp_path: Path) -> None: + kwargs = load_config_args( + ManipulationModuleConfig, + ["visualization.backend=viser", "visualization.host=0.0.0.0"], + tmp_path / "config.json", + ) + + config = ManipulationModuleConfig(**kwargs) + assert isinstance(config.visualization, ViserVisualizationConfig) + assert config.visualization.host == "0.0.0.0" + + def test_run_composition_leaves_blueprint_alone_when_relay_disabled() -> None: class Config(ModuleConfig): pass diff --git a/dimos/cli/test_piper.py b/dimos/cli/test_piper.py new file mode 100644 index 0000000000..76b48fd582 --- /dev/null +++ b/dimos/cli/test_piper.py @@ -0,0 +1,61 @@ +# 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 typer.testing import CliRunner + +from dimos.cli.dimos import main +import dimos.cli.piper as piper + +runner = CliRunner() + + +def test_can_activate_confirms_before_generic_setup(mocker) -> None: + confirm = mocker.patch.object(piper.typer, "confirm", return_value=True) + setup_interface = mocker.patch.object(piper, "setup_interface") + + result = runner.invoke(main, ["piper", "can-activate", "can1", "--bitrate", "500000"]) + + assert result.exit_code == 0, result.output + confirm.assert_called_once() + setup_interface.assert_called_once_with("can1", bitrate=500000) + + +def test_can_activate_rejection_does_not_configure_interface(mocker) -> None: + mocker.patch.object(piper.typer, "confirm", return_value=False) + setup_interface = mocker.patch.object(piper, "setup_interface") + + result = runner.invoke(main, ["piper", "can-activate", "can0"]) + + assert result.exit_code == 1 + assert "Aborted." in result.output + setup_interface.assert_not_called() + + +def test_can_activate_uses_default_bitrate(mocker) -> None: + mocker.patch.object(piper.typer, "confirm", return_value=True) + setup_interface = mocker.patch.object(piper, "setup_interface") + + result = runner.invoke(main, ["piper", "can-activate", "can0"]) + + assert result.exit_code == 0, result.output + setup_interface.assert_called_once_with("can0", bitrate=1_000_000) + + +def test_can_activate_rejects_nonpositive_bitrate_before_confirmation(mocker) -> None: + confirm = mocker.patch.object(piper.typer, "confirm") + + result = runner.invoke(main, ["piper", "can-activate", "can0", "--bitrate", "0"]) + + assert result.exit_code == 2 + confirm.assert_not_called() diff --git a/dimos/hardware/damiao/__init__.py b/dimos/hardware/damiao/__init__.py deleted file mode 100644 index 221ac659e0..0000000000 --- a/dimos/hardware/damiao/__init__.py +++ /dev/null @@ -1,36 +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. - -"""Shared Damiao actuator/runtime adapters.""" - -from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter -from dimos.hardware.damiao.runtime import DamiaoBindingUnavailableError, DamiaoRobotRuntime -from dimos.hardware.damiao.specs import ( - DamiaoArmSpec, - DamiaoBusSpec, - DamiaoJointGroupSpec, - DamiaoMotorSpec, - DamiaoRobotSpec, -) - -__all__ = [ - "DamiaoArmAdapter", - "DamiaoArmSpec", - "DamiaoBindingUnavailableError", - "DamiaoBusSpec", - "DamiaoJointGroupSpec", - "DamiaoMotorSpec", - "DamiaoRobotRuntime", - "DamiaoRobotSpec", -] diff --git a/dimos/hardware/damiao/arm_adapter.py b/dimos/hardware/damiao/arm_adapter.py index cff4e3a3f2..67366b51be 100644 --- a/dimos/hardware/damiao/arm_adapter.py +++ b/dimos/hardware/damiao/arm_adapter.py @@ -14,19 +14,13 @@ from __future__ import annotations -from pathlib import Path from typing import Any import numpy as np +import pinocchio # type: ignore[import-not-found] -from dimos.hardware.damiao.runtime import ( - _DEFAULT_ADDRESS, - _DEFAULT_STATE_CACHE_TTL_S, - _DEFAULT_TICK_DEADLINE_US, - DamiaoBindingUnavailableError, - DamiaoRobotRuntime, -) -from dimos.hardware.damiao.specs import DamiaoArmSpec, DamiaoRobotSpec +from dimos.hardware.damiao.config import DamiaoArmConfig, DamiaoRuntimeConfig +from dimos.hardware.damiao.runtime import DamiaoArmRuntime from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo from dimos.utils.logging_config import setup_logger @@ -35,114 +29,59 @@ _CONTROL_MODE_INDEX = {mode: index for index, mode in enumerate(ControlMode)} -def _dynamic_attr(value: object, name: str) -> Any: - return getattr(value, name) - - class DamiaoArmAdapter: - """ManipulatorAdapter facade over one Damiao joint group.""" + """ManipulatorAdapter facade over one Damiao arm runtime.""" _adapter_type: str = "damiao" - _binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError - _supported_control_modes: tuple[ControlMode, ...] = ( - ControlMode.POSITION, - ControlMode.SERVO_POSITION, - ControlMode.TORQUE, - ) def __init__( self, *, - robot_spec: DamiaoRobotSpec, - group_name: str, + arm_config: DamiaoArmConfig, + runtime_config: DamiaoRuntimeConfig | None = None, dof: int | None = None, hardware_id: str = "arm", - kp: list[float] | None = None, - kd: list[float] | None = None, - gravity_comp: bool = True, - gravity_model_path: str | Path | None = None, - gravity_torque_limits: list[float] | tuple[float, ...] | None = None, - supported_control_modes: tuple[ControlMode, ...] | None = None, - use_mock_bus: bool = False, - config_path: str | Path | None = None, - tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, - state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, ) -> None: - robot_spec.validate() - if group_name not in robot_spec.groups: - raise ValueError(f"unknown Damiao group {group_name!r}") - group_spec = robot_spec.groups[group_name] - if dof is not None and dof != group_spec.dof: + runtime_config = runtime_config or DamiaoRuntimeConfig() + if dof is not None and dof != arm_config.dof: raise ValueError( - f"{type(self).__name__} only supports {group_spec.dof} DOF (got {dof})" + f"{type(self).__name__} only supports {arm_config.dof} DOF (got {dof})" ) - self._robot_spec = robot_spec - self._group_name = group_name - self._group_spec = group_spec + self._arm_config = arm_config + self._runtime_config = runtime_config self._hardware_id = hardware_id - self._dof = group_spec.dof - self._position_lower = list(group_spec.position_lower) - self._position_upper = list(group_spec.position_upper) - self._velocity_max = list(group_spec.velocity_max) - self._kp = list(kp) if kp is not None else list(group_spec.kp) - self._kd = list(kd) if kd is not None else list(group_spec.kd) + self._dof = arm_config.dof + self._position_lower = list(arm_config.position_lower) + self._position_upper = list(arm_config.position_upper) + self._velocity_max = list(arm_config.velocity_max) + self._kp = list(runtime_config.kp_override or arm_config.kp) + self._kd = list(runtime_config.kd_override or arm_config.kd) self._validate_length("kp", self._kp) self._validate_length("kd", self._kd) - self._gravity_comp = gravity_comp - resolved_gravity_model = ( - gravity_model_path if gravity_model_path is not None else group_spec.gravity_model_path - ) - self._gravity_model_path = str(resolved_gravity_model) if resolved_gravity_model else None - resolved_torque_limits = ( - gravity_torque_limits - if gravity_torque_limits is not None - else group_spec.gravity_torque_limits + self._gravity_comp = runtime_config.gravity_comp + self._gravity_model_path = ( + str(runtime_config.gravity_model_path) if runtime_config.gravity_model_path else None ) + resolved_torque_limits = arm_config.gravity_torque_limits self._gravity_torque_limits = ( list(resolved_torque_limits) if resolved_torque_limits else None ) if self._gravity_torque_limits is not None: self._validate_length("gravity_torque_limits", self._gravity_torque_limits) - self._supported_control_modes = ( - supported_control_modes or type(self)._supported_control_modes - ) + self._supported_control_modes = arm_config.supported_control_modes self._control_mode = ControlMode.POSITION self._last_positions: list[float] | None = None - self._pin_model: object | None = None - self._pin_data: object | None = None - self._use_mock_bus = use_mock_bus - self._config_path = config_path - self._tick_deadline_us = tick_deadline_us - self._state_cache_ttl_s = state_cache_ttl_s - self._runtime: DamiaoRobotRuntime | None = None + self._pin_model: Any | None = None + self._pin_data: Any | None = None + self._runtime: DamiaoArmRuntime | None = None self._connected = False self._enabled = False - @classmethod - def from_arm_spec( - cls, - *, - arm_spec: DamiaoArmSpec, - address: str | Path | None = _DEFAULT_ADDRESS, - **kwargs: Any, - ) -> DamiaoArmAdapter: - """Build a one-group adapter from a compatibility arm spec.""" - - robot_spec = DamiaoRobotSpec.from_arm_spec( - arm_spec, - address=str(address) if address is not None else _DEFAULT_ADDRESS, - ) - return cls(robot_spec=robot_spec, group_name=arm_spec.arm_name, **kwargs) - - def _create_runtime(self) -> DamiaoRobotRuntime: - return DamiaoRobotRuntime( - robot_spec=self._robot_spec, + def _create_runtime(self) -> DamiaoArmRuntime: + return DamiaoArmRuntime( + arm_config=self._arm_config, + runtime_config=self._runtime_config, adapter_type=self._adapter_type, - binding_error_type=self._binding_error_type, - use_mock_bus=self._use_mock_bus, - config_path=self._config_path, - tick_deadline_us=self._tick_deadline_us, - state_cache_ttl_s=self._state_cache_ttl_s, ) def _validate_length(self, name: str, values: list[float]) -> None: @@ -165,8 +104,6 @@ def connect(self) -> bool: self._load_gravity_model() self._connected = True self.refresh_state(force=True) - except self._binding_error_type: - raise except Exception: logger.exception( "damiao arm adapter connect failed", @@ -197,8 +134,8 @@ def deactivate(self) -> bool: def get_info(self) -> ManipulatorInfo: return ManipulatorInfo( - vendor=self._robot_spec.vendor, - model=self._robot_spec.model, + vendor=self._arm_config.vendor, + model=self._arm_config.model, dof=self._dof, firmware_version=None, serial_number=None, @@ -229,7 +166,7 @@ def read_enabled(self) -> bool: def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float], list[float]]: if self._runtime is None: raise RuntimeError(f"{type(self).__name__} is not connected") - state = self._runtime.refresh_group_state(self._group_name, force=force) + state = self._runtime.refresh_state(force=force) self._last_positions = list(state.q) return list(state.q), list(state.dq), list(state.tau) @@ -324,8 +261,7 @@ def write_mit_commands( if self._runtime is None or not self._enabled: return False self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - ok = self._runtime.write_group_mit_commands( - group_name=self._group_name, + ok = self._runtime.write_mit_commands( q=q, dq=dq, kp=kp, @@ -472,22 +408,19 @@ def _preflight_gravity(self) -> None: ) if self._pin_model is not None: - nq = getattr(self._pin_model, "nq", self._dof) - nv = getattr(self._pin_model, "nv", self._dof) + nq = self._pin_model.nq + nv = self._pin_model.nv if nq != self._dof or nv != self._dof: raise ValueError( f"gravity model dimensions ({nq}, {nv}) do not match adapter DOF {self._dof}" ) - names = getattr(self._pin_model, "names", None) - if names is None: - raise ValueError("gravity model does not expose joint order") - model_names = tuple(str(name) for name in names) + model_names = tuple(str(name) for name in self._pin_model.names) if model_names and model_names[0] == "universe": model_names = model_names[1:] - if model_names != self._group_spec.joint_names: + if model_names != self._arm_config.joint_names: raise ValueError( f"gravity model joint order {model_names!r} does not match " - f"configured order {self._group_spec.joint_names!r}" + f"configured order {self._arm_config.joint_names!r}" ) tau = self.compute_gravity_torques(q) if self._gravity_comp else self._zero_vector() @@ -529,18 +462,13 @@ def write_clear_errors(self) -> bool: def _load_gravity_model(self) -> None: if not self._gravity_comp or self._gravity_model_path is None or self._runtime is None: return - loaded = self._runtime.load_gravity_model(self._group_name, self._gravity_model_path) - if loaded is not None: - self._pin_model, self._pin_data = loaded + self._pin_model, self._pin_data = self._runtime.load_gravity_model(self._gravity_model_path) def compute_gravity_torques(self, q: list[float]) -> list[float]: self._validate_length("q", q) if self._pin_model is None or self._pin_data is None: raise RuntimeError("gravity compensation model is not loaded") - import pinocchio # type: ignore[import-not-found] - - compute_generalized_gravity = _dynamic_attr(pinocchio, "computeGeneralizedGravity") - tau = compute_generalized_gravity( + tau = pinocchio.computeGeneralizedGravity( self._pin_model, self._pin_data, np.array(q, dtype=np.float64) ) values = [float(tau[i]) for i in range(self._dof)] @@ -552,6 +480,3 @@ def compute_gravity_torques(self, q: list[float]) -> list[float]: float(np.clip(value, -limit, limit)) for value, limit in zip(values, self._gravity_torque_limits, strict=False) ] - - -__all__ = ["DamiaoArmAdapter"] diff --git a/dimos/hardware/damiao/config.py b/dimos/hardware/damiao/config.py new file mode 100644 index 0000000000..f7cbb579f6 --- /dev/null +++ b/dimos/hardware/damiao/config.py @@ -0,0 +1,225 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Sequence +import math +from pathlib import Path + +import attrs + +from dimos.hardware.manipulators.spec import ControlMode + +_NON_EMPTY_STRING = attrs.validators.and_( + attrs.validators.instance_of(str), + attrs.validators.min_len(1), +) +_NON_NEGATIVE_INT = attrs.validators.and_( + attrs.validators.instance_of(int), + attrs.validators.ge(0), +) +_POSITIVE_INT = attrs.validators.and_( + attrs.validators.instance_of(int), + attrs.validators.ge(1), +) + + +def _to_floats(values: Sequence[float]) -> tuple[float, ...]: + return tuple(float(value) for value in values) + + +def _to_optional_floats(values: Sequence[float] | None) -> tuple[float, ...] | None: + return None if values is None else _to_floats(values) + + +def _to_motors(values: Sequence[DamiaoMotorConfig]) -> tuple[DamiaoMotorConfig, ...]: + return tuple(values) + + +def _to_control_modes(values: Sequence[ControlMode]) -> tuple[ControlMode, ...]: + return tuple(values) + + +def _to_optional_path(value: str | Path | None) -> Path | None: + return None if value is None else Path(value) + + +def _finite_non_negative( + _instance: object, attribute: attrs.Attribute[float], value: float +) -> None: + if not math.isfinite(value) or value < 0.0: + raise ValueError(f"{attribute.name} must be finite and non-negative") + + +@attrs.frozen(slots=False) +class DamiaoMotorConfig: + """Physical identity for one Damiao motor in command-vector order.""" + + name: str = attrs.field(validator=_NON_EMPTY_STRING) + type: str | int = attrs.field(validator=attrs.validators.instance_of((str, int))) + send_id: int = attrs.field(validator=_NON_NEGATIVE_INT) + recv_id: int | None = attrs.field( + default=None, + validator=attrs.validators.optional(_NON_NEGATIVE_INT), + ) + + @property + def effective_recv_id(self) -> int: + """Return the explicit receive CAN ID, or Damiao's default response ID.""" + + return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) + + +@attrs.frozen(slots=False) +class DamiaoArmConfig: + """Immutable physical definition and capabilities for one Damiao arm.""" + + name: str = attrs.field(validator=_NON_EMPTY_STRING) + vendor: str = attrs.field(validator=_NON_EMPTY_STRING) + model: str = attrs.field(validator=_NON_EMPTY_STRING) + motors: tuple[DamiaoMotorConfig, ...] = attrs.field( + converter=_to_motors, + validator=attrs.validators.deep_iterable( + member_validator=attrs.validators.instance_of(DamiaoMotorConfig), + iterable_validator=attrs.validators.min_len(1), + ), + ) + position_lower: tuple[float, ...] = attrs.field(converter=_to_floats) + position_upper: tuple[float, ...] = attrs.field(converter=_to_floats) + velocity_max: tuple[float, ...] = attrs.field(converter=_to_floats) + kp: tuple[float, ...] = attrs.field(converter=_to_floats) + kd: tuple[float, ...] = attrs.field(converter=_to_floats) + gravity_torque_limits: tuple[float, ...] | None = attrs.field( + default=None, + converter=_to_optional_floats, + ) + fd: bool = attrs.field(default=False, validator=attrs.validators.instance_of(bool)) + supported_control_modes: tuple[ControlMode, ...] = attrs.field( + factory=lambda: ( + ControlMode.POSITION, + ControlMode.SERVO_POSITION, + ControlMode.TORQUE, + ), + converter=_to_control_modes, + validator=attrs.validators.deep_iterable( + member_validator=attrs.validators.instance_of(ControlMode), + iterable_validator=attrs.validators.min_len(1), + ), + ) + + @property + def dof(self) -> int: + """Return the number of joints described by this arm.""" + + return len(self.motors) + + @property + def joint_names(self) -> tuple[str, ...]: + """Return joint names in adapter and command-vector order.""" + + return tuple(motor.name for motor in self.motors) + + @motors.validator + def _validate_motor_identity( + self, + _attribute: attrs.Attribute[tuple[DamiaoMotorConfig, ...]], + motors: tuple[DamiaoMotorConfig, ...], + ) -> None: + identities: dict[str, Sequence[str | int]] = { + "joint names": [motor.name for motor in motors], + "send IDs": [motor.send_id for motor in motors], + "receive IDs": [motor.effective_recv_id for motor in motors], + } + for label, values in identities.items(): + if len(set(values)) != len(values): + raise ValueError(f"Damiao arm {self.name!r} has duplicate {label}: {values}") + + @position_lower.validator + def _validate_joint_vectors( + self, + _attribute: attrs.Attribute[tuple[float, ...]], + _value: tuple[float, ...], + ) -> None: + vectors = { + "position_lower": self.position_lower, + "position_upper": self.position_upper, + "velocity_max": self.velocity_max, + "kp": self.kp, + "kd": self.kd, + } + if self.gravity_torque_limits is not None: + vectors["gravity_torque_limits"] = self.gravity_torque_limits + for label, values in vectors.items(): + if len(values) != self.dof: + raise ValueError(f"{label} length {len(values)} does not match arm DOF {self.dof}") + if not all(math.isfinite(value) for value in values): + raise ValueError(f"{label} values must be finite") + if any( + lower > upper + for lower, upper in zip(self.position_lower, self.position_upper, strict=True) + ): + raise ValueError("position lower limits must not exceed upper limits") + if any(value <= 0.0 for value in self.velocity_max): + raise ValueError("velocity limits must be greater than zero") + if any(value < 0.0 for value in (*self.kp, *self.kd)): + raise ValueError("default gains must be non-negative") + if self.gravity_torque_limits is not None and any( + value < 0.0 for value in self.gravity_torque_limits + ): + raise ValueError("gravity torque limits must be non-negative") + + @supported_control_modes.validator + def _validate_control_modes( + self, + _attribute: attrs.Attribute[tuple[ControlMode, ...]], + modes: tuple[ControlMode, ...], + ) -> None: + if len(set(modes)) != len(modes): + raise ValueError("supported control modes must be unique") + + +@attrs.frozen(slots=False) +class DamiaoRuntimeConfig: + """Deployment-specific settings and optional overrides for a Damiao arm.""" + + address: str = attrs.field(default="can0", converter=str, validator=_NON_EMPTY_STRING) + gravity_comp: bool = attrs.field(default=True, validator=attrs.validators.instance_of(bool)) + gravity_model_path: Path | None = attrs.field(default=None, converter=_to_optional_path) + kp_override: tuple[float, ...] | None = attrs.field( + default=None, + converter=_to_optional_floats, + ) + kd_override: tuple[float, ...] | None = attrs.field( + default=None, + converter=_to_optional_floats, + ) + use_mock_bus: bool = attrs.field(default=False, validator=attrs.validators.instance_of(bool)) + config_path: Path | None = attrs.field(default=None, converter=_to_optional_path) + tick_deadline_us: int = attrs.field(default=1_000, validator=_POSITIVE_INT) + state_cache_ttl_s: float = attrs.field( + default=0.002, + converter=float, + validator=_finite_non_negative, + ) + + @kp_override.validator + @kd_override.validator + def _validate_gain_override( + self, + attribute: attrs.Attribute[tuple[float, ...] | None], + values: tuple[float, ...] | None, + ) -> None: + if values is not None and any(not math.isfinite(value) or value < 0.0 for value in values): + raise ValueError(f"{attribute.name} values must be finite and non-negative") diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py index b19e2f8437..e66718f263 100644 --- a/dimos/hardware/damiao/runtime.py +++ b/dimos/hardware/damiao/runtime.py @@ -14,31 +14,51 @@ from __future__ import annotations -from collections.abc import Callable, Mapping, Sequence +from collections.abc import Callable, Sequence from dataclasses import dataclass -import importlib +import errno from pathlib import Path import time -from typing import Any, cast +from typing import Any +import can_motor_control +from can_motor_control import damiao import numpy as np +import pinocchio # type: ignore[import-not-found] -from dimos.hardware.damiao.specs import DamiaoJointGroupSpec, DamiaoRobotSpec +from dimos.hardware.damiao.config import DamiaoArmConfig, DamiaoRuntimeConfig from dimos.utils.logging_config import setup_logger logger = setup_logger() -_DEFAULT_TICK_DEADLINE_US = 1_000 -_DEFAULT_STATE_CACHE_TTL_S = 0.002 -_DEFAULT_ADDRESS = "can0" +_ARM_NAME = "arm" +_BUS_NAME = "can" _ENOBUFS_RETRY_DELAYS_S = (0.001, 0.002, 0.003) _MIN_RECOMMENDED_TX_QUEUE_LEN = 1_000 +_MOTOR_TYPES_BY_NAME = { + "DM3507": damiao.MotorType.DM3507, + "DM4310": damiao.MotorType.DM4310, + "DM4310_48V": damiao.MotorType.DM4310_48V, + "DM4340": damiao.MotorType.DM4340, + "DM4340_48V": damiao.MotorType.DM4340_48V, + "DM6006": damiao.MotorType.DM6006, + "DM8006": damiao.MotorType.DM8006, + "DM8009": damiao.MotorType.DM8009, + "DM10010L": damiao.MotorType.DM10010L, + "DM10010": damiao.MotorType.DM10010, + "DMH3510": damiao.MotorType.DMH3510, + "DMH6215": damiao.MotorType.DMH6215, + "DMG6220": damiao.MotorType.DMG6220, +} +_MOTOR_TYPES_BY_VALUE = { + int(motor_type): motor_type for motor_type in _MOTOR_TYPES_BY_NAME.values() +} def _is_enobufs(exc: BaseException) -> bool: current: BaseException | None = exc while current is not None: - if getattr(current, "errno", None) == 105: + if isinstance(current, OSError) and current.errno == errno.ENOBUFS: return True message = str(current).lower() if "no buffer space available" in message or "os error 105" in message: @@ -58,104 +78,54 @@ def _retry_enobufs(operation: Callable[[], None]) -> None: time.sleep(delay_s) -class DamiaoBindingUnavailableError(RuntimeError): - """Raised when the optional can_motor_control binding is unavailable.""" - - @dataclass(frozen=True) class DamiaoGroupState: - """State vectors for one Damiao joint group.""" + """State vectors for one Damiao arm.""" q: list[float] dq: list[float] tau: list[float] -def _load_can_motor_control( - *, - adapter_type: str, - error_type: type[RuntimeError] = DamiaoBindingUnavailableError, -) -> tuple[Any, Any]: - """Lazily load the optional Rust-backed binding and Damiao codec module.""" - - try: - can_motor_control = importlib.import_module("can_motor_control") - damiao = importlib.import_module("can_motor_control.damiao") - except ImportError as exc: - raise error_type( - f"The selected '{adapter_type}' adapter requires the Rust-backed " - "can-motor-control Python binding in the active environment. On " - "supported Linux systems, install dimos[manipulation] before " - f"selecting adapter_type='{adapter_type}'." - ) from exc - return can_motor_control, damiao - - -def _dynamic_attr(value: object, name: str) -> Any: - return getattr(value, name) - - -class DamiaoRobotRuntime: - """Binding-backed runtime for one Damiao-based robot spec.""" +class DamiaoArmRuntime: + """Binding-backed runtime for one Damiao arm.""" def __init__( self, *, - robot_spec: DamiaoRobotSpec, + arm_config: DamiaoArmConfig, + runtime_config: DamiaoRuntimeConfig, adapter_type: str = "damiao", - binding_error_type: type[RuntimeError] = DamiaoBindingUnavailableError, - use_mock_bus: bool = False, - config_path: str | Path | None = None, - tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US, - state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S, ) -> None: - robot_spec.validate() - self._robot_spec = robot_spec + self._arm_config = arm_config + self._runtime_config = runtime_config self._adapter_type = adapter_type - self._binding_error_type = binding_error_type - self._use_mock_bus = use_mock_bus - self._config_path = str(config_path) if config_path is not None else None - self._tick_deadline_us = tick_deadline_us - self._state_cache_ttl_s = state_cache_ttl_s self._robot: Any | None = None - self._groups: dict[str, Any] = {} - self._state_cache: dict[str, DamiaoGroupState] = {} - self._state_cache_time: dict[str, float] = {} - self._can_motor_control: Any | None = None - self._damiao: Any | None = None + self._arm: Any | None = None + self._state_cache: DamiaoGroupState | None = None + self._state_cache_time = 0.0 self._connected = False self._enabled = False @property - def robot_spec(self) -> DamiaoRobotSpec: - return self._robot_spec + def arm_config(self) -> DamiaoArmConfig: + return self._arm_config def connect(self) -> bool: - """Connect the binding robot and cache group handles.""" + """Connect the binding robot and cache its arm handle.""" try: - self._can_motor_control, self._damiao = _load_can_motor_control( - adapter_type=self._adapter_type, - error_type=self._binding_error_type, - ) robot = self._build_robot() robot.connect() - groups: dict[str, Any] = {} - for group_name, group_spec in self._robot_spec.groups.items(): - group = robot[group_name] - if len(group) != group_spec.dof: - raise RuntimeError( - f"can_motor_control group {group_name!r} has {len(group)} joints, " - f"expected {group_spec.dof}" - ) - groups[group_name] = group + arm = robot[_ARM_NAME] + if len(arm) != self._arm_config.dof: + raise RuntimeError( + f"can_motor_control arm has {len(arm)} joints, expected {self._arm_config.dof}" + ) self._robot = robot - self._groups = groups + self._arm = arm self._connected = True - for group_name in self._robot_spec.groups: - self.refresh_group_state(group_name, force=True) - except self._binding_error_type: - raise + self.refresh_state(force=True) except Exception: logger.exception("damiao runtime connect failed", adapter=self._adapter_type) self.disconnect() @@ -163,38 +133,38 @@ def connect(self) -> bool: return True def _build_robot(self) -> Any: - if self._can_motor_control is None or self._damiao is None: - raise RuntimeError("can_motor_control binding is not loaded") - if self._config_path is not None: - return self._can_motor_control.Robot.from_config(self._config_path) - builder = self._can_motor_control.Robot.builder() - codec = self._damiao.DamiaoCodec() - for bus_name, bus_spec in self._robot_spec.buses.items(): - address = str(bus_spec.address or _DEFAULT_ADDRESS) - self._warn_if_small_tx_queue(address) + if self._runtime_config.config_path is not None: + return can_motor_control.Robot.from_config(str(self._runtime_config.config_path)) + + address = self._runtime_config.address + self._warn_if_small_tx_queue(address) + transport: can_motor_control.MockCanBus | can_motor_control.SocketCanBus + if self._runtime_config.use_mock_bus: transport = ( - self._can_motor_control.MockCanBus.new_fd(address) - if self._use_mock_bus and bus_spec.fd - else self._can_motor_control.MockCanBus(address) - if self._use_mock_bus - else self._can_motor_control.SocketCanBus(address, fd=bus_spec.fd) + can_motor_control.MockCanBus.new_fd(address) + if self._arm_config.fd + else can_motor_control.MockCanBus(address) ) - builder = builder.add_bus(bus_name, transport, codec) - for group_name, group_spec in self._robot_spec.groups.items(): - binding_specs = [ - self._can_motor_control.MotorSpec( - motor.name, - cast("int", self._resolve_motor_type(motor.type)), - motor.send_id, - motor.effective_recv_id, - ) - for motor in group_spec.motors - ] - builder = builder.add_arm(group_name, bus=group_spec.bus_name, motors=binding_specs) - return builder.build() + else: + transport = can_motor_control.SocketCanBus(address, fd=self._arm_config.fd) + motors = [ + can_motor_control.MotorSpec( + motor.name, + int(self._resolve_motor_type(motor.type)), + motor.send_id, + motor.effective_recv_id, + ) + for motor in self._arm_config.motors + ] + return ( + can_motor_control.Robot.builder() + .add_bus(_BUS_NAME, transport, damiao.DamiaoCodec()) + .add_arm(_ARM_NAME, bus=_BUS_NAME, motors=motors) + .build() + ) def _warn_if_small_tx_queue(self, address: str) -> None: - if self._use_mock_bus: + if self._runtime_config.use_mock_bus: return queue_path = Path("/sys/class/net") / address / "tx_queue_len" try: @@ -210,27 +180,14 @@ def _warn_if_small_tx_queue(self, address: str) -> None: setup_command=f"dimos can setup {address}", ) - def _resolve_motor_type(self, motor_type: object) -> object: - if self._damiao is None: - raise RuntimeError("Damiao binding module is not loaded") - if isinstance(motor_type, str): - try: - return getattr(self._damiao.MotorType, motor_type) - except AttributeError as exc: - raise ValueError(f"Unknown Damiao motor type {motor_type!r}") from exc - if not isinstance(motor_type, int): - return motor_type - for name in dir(self._damiao.MotorType): - if name.startswith("_"): - continue - candidate = getattr(self._damiao.MotorType, name) - try: - candidate_value = int(candidate) - except (TypeError, ValueError): - continue - if candidate_value == motor_type: - return candidate - raise ValueError(f"Unknown Damiao motor type value {motor_type!r}") + @staticmethod + def _resolve_motor_type(motor_type: str | int) -> damiao.MotorType: + try: + if isinstance(motor_type, str): + return _MOTOR_TYPES_BY_NAME[motor_type] + return _MOTOR_TYPES_BY_VALUE[motor_type] + except KeyError as exc: + raise ValueError(f"Unknown Damiao motor type {motor_type!r}") from exc def disconnect(self) -> None: """Disable and drop the underlying binding robot.""" @@ -245,9 +202,9 @@ def disconnect(self) -> None: self._enabled = False if disabled else True self._connected = False self._robot = None - self._groups = {} - self._state_cache = {} - self._state_cache_time = {} + self._arm = None + self._state_cache = None + self._state_cache_time = 0.0 def is_connected(self) -> bool: return self._connected @@ -257,13 +214,11 @@ def enable(self) -> bool: return False try: self._robot.set_mode("mit") - self._robot.tick(self._tick_deadline_us) + self._robot.tick(self._runtime_config.tick_deadline_us) self._robot.enable() - self._robot.tick(self._tick_deadline_us) + self._robot.tick(self._runtime_config.tick_deadline_us) except Exception: logger.exception("damiao runtime enable failed", adapter=self._adapter_type) - # The binding may have enabled a subset of the robot before - # reporting an error. Never leave that partial state live. try: disabled = self._robot.disable() except Exception: @@ -292,158 +247,67 @@ def disable(self) -> bool: def is_enabled(self) -> bool: return self._enabled - def group_spec(self, group_name: str) -> DamiaoJointGroupSpec: - try: - return self._robot_spec.groups[group_name] - except KeyError as exc: - raise ValueError(f"unknown Damiao group {group_name!r}") from exc - - def refresh_group_state(self, group_name: str, *, force: bool = False) -> DamiaoGroupState: - group_spec = self.group_spec(group_name) - group = self._groups.get(group_name) - if self._robot is None or group is None: - raise RuntimeError("DamiaoRobotRuntime is not connected") + def refresh_state(self, *, force: bool = False) -> DamiaoGroupState: + if self._robot is None or self._arm is None: + raise RuntimeError("DamiaoArmRuntime is not connected") now = time.monotonic() - cached = self._state_cache.get(group_name) - cached_at = self._state_cache_time.get(group_name, 0.0) - if not force and cached is not None and now - cached_at <= self._state_cache_ttl_s: - return cached - group.refresh() - self._robot.tick(self._tick_deadline_us) + if ( + not force + and self._state_cache is not None + and now - self._state_cache_time <= self._runtime_config.state_cache_ttl_s + ): + return self._state_cache + self._arm.refresh() + self._robot.tick(self._runtime_config.tick_deadline_us) state = DamiaoGroupState( - q=group.positions().astype(np.float64).tolist(), - dq=group.velocities().astype(np.float64).tolist(), - tau=group.torques().astype(np.float64).tolist(), + q=self._arm.positions().astype(np.float64).tolist(), + dq=self._arm.velocities().astype(np.float64).tolist(), + tau=self._arm.torques().astype(np.float64).tolist(), ) - if any(len(values) != group_spec.dof for values in (state.q, state.dq, state.tau)): - raise RuntimeError( - f"state length does not match configured DOF for group {group_name!r}" - ) + if any(len(values) != self._arm_config.dof for values in (state.q, state.dq, state.tau)): + raise RuntimeError("state length does not match configured arm DOF") if any( not np.isfinite(values).all() for values in (np.asarray(state.q), np.asarray(state.dq), np.asarray(state.tau)) ): - raise RuntimeError(f"state contains non-finite values for group {group_name!r}") - self._state_cache[group_name] = state - self._state_cache_time[group_name] = time.monotonic() + raise RuntimeError("state contains non-finite values") + self._state_cache = state + self._state_cache_time = time.monotonic() return state - def has_group_states(self, group_names: Sequence[str]) -> bool: - """Return true only when every requested group has a fresh complete state.""" - - try: - for group_name in group_names: - self.refresh_group_state(group_name, force=False) - except Exception: - return False - return True - - def read_group_states(self, group_names: Sequence[str]) -> list[DamiaoGroupState]: - """Read state for groups in the requested order.""" - - return [self.refresh_group_state(group_name, force=False) for group_name in group_names] - - def write_group_mit_commands( + def write_mit_commands( self, *, - group_name: str, q: Sequence[float], dq: Sequence[float], kp: Sequence[float], kd: Sequence[float], tau: Sequence[float], ) -> bool: - """Write one MIT command frame to a group.""" + """Write one MIT command frame to the arm.""" - group_spec = self.group_spec(group_name) - group = self._groups.get(group_name) - if self._robot is None or group is None or not self._enabled: + if self._robot is None or self._arm is None or not self._enabled: return False - if any(len(values) != group_spec.dof for values in (q, dq, kp, kd, tau)): - raise ValueError( - f"command length does not match configured DOF for group {group_name!r}" - ) + if any(len(values) != self._arm_config.dof for values in (q, dq, kp, kd, tau)): + raise ValueError("command length does not match configured arm DOF") try: def send() -> None: - group.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) - self._robot.tick(self._tick_deadline_us) + assert self._arm is not None + assert self._robot is not None + self._arm.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) + self._robot.tick(self._runtime_config.tick_deadline_us) _retry_enobufs(send) except Exception: - logger.exception("damiao runtime MIT command failed", group_name=group_name) - return False - self._state_cache.pop(group_name, None) - self._state_cache_time.pop(group_name, None) - return True - - def write_groups_mit_commands( - self, - commands: Mapping[ - str, - tuple[ - Sequence[float], Sequence[float], Sequence[float], Sequence[float], Sequence[float] - ], - ], - ) -> bool: - """Stage MIT commands for multiple groups and tick once. - - The binding's group ``mit_control`` call stages commands; ``robot.tick`` - sends them. Validate all groups and command lengths before staging so a - bad frame is rejected without sending a partial whole-body command. - """ - - if self._robot is None or not self._enabled: - return False - for group_name, values in commands.items(): - group_spec = self.group_spec(group_name) - group = self._groups.get(group_name) - if group is None: - return False - q, dq, kp, kd, tau = values - if any(len(vector) != group_spec.dof for vector in (q, dq, kp, kd, tau)): - raise ValueError( - f"command length does not match configured DOF for group {group_name!r}" - ) - try: - for group_name, values in commands.items(): - q, dq, kp, kd, tau = values - self._groups[group_name].mit_control( - np.column_stack([kp, kd, q, dq, tau]).astype(np.float64) - ) - self._robot.tick(self._tick_deadline_us) - except Exception: - logger.exception("damiao runtime batched MIT command failed") + logger.exception("damiao runtime MIT command failed") return False - for group_name in commands: - self._state_cache.pop(group_name, None) - self._state_cache_time.pop(group_name, None) + self._state_cache = None + self._state_cache_time = 0.0 return True - def load_gravity_model( - self, - group_name: str, - model_path: str | Path | None = None, - ) -> tuple[object, object] | None: - """Load a Pinocchio gravity model for a configured group, if present.""" + def load_gravity_model(self, model_path: str | Path) -> tuple[object, object]: + """Load a Pinocchio gravity model for the arm.""" - resolved_model_path = ( - model_path if model_path is not None else self.group_spec(group_name).gravity_model_path - ) - if resolved_model_path is None: - return None - import pinocchio # type: ignore[import-not-found] - - build_model_from_urdf = _dynamic_attr(pinocchio, "buildModelFromUrdf") - model = build_model_from_urdf(str(resolved_model_path)) - return model, _dynamic_attr(model, "createData")() - - -__all__ = [ - "_DEFAULT_ADDRESS", - "_DEFAULT_STATE_CACHE_TTL_S", - "_DEFAULT_TICK_DEADLINE_US", - "DamiaoBindingUnavailableError", - "DamiaoGroupState", - "DamiaoRobotRuntime", -] + model = pinocchio.buildModelFromUrdf(str(model_path)) + return model, model.createData() diff --git a/dimos/hardware/damiao/specs.py b/dimos/hardware/damiao/specs.py deleted file mode 100644 index f67d6a0ecf..0000000000 --- a/dimos/hardware/damiao/specs.py +++ /dev/null @@ -1,316 +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. - -from __future__ import annotations - -from collections.abc import Mapping, Sequence -from dataclasses import dataclass -from pathlib import Path - - -@dataclass(frozen=True) -class DamiaoMotorSpec: - """Typed metadata for one Damiao motor in adapter joint order.""" - - name: str - type: object - send_id: int - recv_id: int | None = None - - @property - def effective_recv_id(self) -> int: - """Return the explicit receive CAN ID, or Damiao's default response ID.""" - - return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) - - -@dataclass(frozen=True) -class DamiaoBusSpec: - """Named communication channel for Damiao motors.""" - - address: str | Path = "can0" - fd: bool = False - - -@dataclass(frozen=True) -class DamiaoJointGroupSpec: - """Ordered Damiao joints forming a controllable physical group.""" - - bus_name: str - motors: tuple[DamiaoMotorSpec, ...] - position_lower: tuple[float, ...] - position_upper: tuple[float, ...] - velocity_max: tuple[float, ...] - kp: tuple[float, ...] - kd: tuple[float, ...] - gravity_model_path: str | Path | None = None - gravity_torque_limits: tuple[float, ...] | None = None - supports_velocity: bool = False - - @property - def dof(self) -> int: - """Return the number of joints described by this group spec.""" - - return len(self.motors) - - @property - def joint_names(self) -> tuple[str, ...]: - """Return joint names in command-vector order.""" - - return tuple(motor.name for motor in self.motors) - - def validate(self, *, group_name: str, bus_names: set[str] | None = None) -> None: - """Validate per-group metadata and optional bus reference.""" - - if not self.motors: - raise ValueError(f"DamiaoJointGroupSpec {group_name!r} requires at least one motor") - if bus_names is not None and self.bus_name not in bus_names: - raise ValueError(f"group {group_name!r} references unknown bus {self.bus_name!r}") - send_ids = [motor.send_id for motor in self.motors] - if len(set(send_ids)) != len(send_ids): - raise ValueError(f"duplicate send_id in group {group_name!r}: {send_ids}") - recv_ids = [motor.effective_recv_id for motor in self.motors] - if len(set(recv_ids)) != len(recv_ids): - raise ValueError(f"duplicate recv_id in group {group_name!r}: {recv_ids}") - joint_names = [motor.name for motor in self.motors] - if len(set(joint_names)) != len(joint_names): - raise ValueError(f"duplicate joint name in group {group_name!r}: {joint_names}") - for name, values in { - "position_lower": self.position_lower, - "position_upper": self.position_upper, - "velocity_max": self.velocity_max, - "kp": self.kp, - "kd": self.kd, - }.items(): - if len(values) != self.dof: - raise ValueError( - f"{name} length {len(values)} does not match dof {self.dof} " - f"for group {group_name!r}" - ) - for index, (lower, upper) in enumerate( - zip(self.position_lower, self.position_upper, strict=True), - ): - if lower > upper: - raise ValueError( - f"position_lower[{index}] > position_upper[{index}] for group {group_name!r}" - ) - if self.gravity_torque_limits is not None and len(self.gravity_torque_limits) != self.dof: - raise ValueError( - f"gravity_torque_limits length does not match dof for group {group_name!r}" - ) - - -@dataclass(frozen=True) -class DamiaoRobotSpec: - """Python-native Damiao robot config with named buses and joint groups.""" - - name: str - vendor: str - model: str - buses: Mapping[str, DamiaoBusSpec] - groups: Mapping[str, DamiaoJointGroupSpec] - requires_binding: bool = False - - @property - def joint_names(self) -> tuple[str, ...]: - """Return all group joint names in mapping iteration order.""" - - return tuple(joint for group in self.groups.values() for joint in group.joint_names) - - def group_joint_names(self, group_names: Sequence[str]) -> tuple[str, ...]: - """Return concatenated joint names for the requested groups.""" - - return tuple( - joint for group_name in group_names for joint in self.groups[group_name].joint_names - ) - - def validate(self) -> None: - """Validate bus/group references and global joint-name uniqueness.""" - - if not self.buses: - raise ValueError("DamiaoRobotSpec requires at least one bus") - if not self.groups: - raise ValueError("DamiaoRobotSpec requires at least one joint group") - bus_names = set(self.buses) - all_joint_names: list[str] = [] - ids_by_bus: dict[str, set[int]] = {bus_name: set() for bus_name in bus_names} - for group_name, group in self.groups.items(): - group.validate(group_name=group_name, bus_names=bus_names) - all_joint_names.extend(group.joint_names) - bus_ids = ids_by_bus[group.bus_name] - for motor in group.motors: - if motor.send_id in bus_ids: - raise ValueError(f"duplicate send_id {motor.send_id} on bus {group.bus_name!r}") - bus_ids.add(motor.send_id) - if len(set(all_joint_names)) != len(all_joint_names): - raise ValueError(f"duplicate joint names across DamiaoRobotSpec: {all_joint_names}") - - @classmethod - def from_arm_spec( - cls, - arm_spec: DamiaoArmSpec, - *, - address: str | Path = "can0", - ) -> DamiaoRobotSpec: - """Build a one-group robot spec from a compatibility arm spec.""" - - return cls( - name=arm_spec.name, - vendor=arm_spec.vendor, - model=arm_spec.model, - buses={arm_spec.bus_name: DamiaoBusSpec(address=address, fd=arm_spec.fd)}, - groups={ - arm_spec.arm_name: DamiaoJointGroupSpec( - bus_name=arm_spec.bus_name, - motors=arm_spec.motors, - position_lower=arm_spec.position_lower, - position_upper=arm_spec.position_upper, - velocity_max=arm_spec.velocity_max, - kp=arm_spec.kp, - kd=arm_spec.kd, - gravity_model_path=arm_spec.gravity_model_path, - gravity_torque_limits=arm_spec.gravity_torque_limits, - supports_velocity=arm_spec.supports_velocity, - ) - }, - requires_binding=arm_spec.requires_binding, - ) - - -@dataclass(frozen=True) -class DamiaoArmSpec: - """Compatibility metadata for a single Damiao arm/group adapter.""" - - name: str - vendor: str - model: str - motors: tuple[DamiaoMotorSpec, ...] - position_lower: tuple[float, ...] - position_upper: tuple[float, ...] - velocity_max: tuple[float, ...] - kp: tuple[float, ...] - kd: tuple[float, ...] - gravity_model_path: str | Path | None = None - gravity_torque_limits: tuple[float, ...] | None = None - requires_binding: bool = False - bus_name: str = "can" - arm_name: str = "arm" - fd: bool = False - supports_velocity: bool = False - - @property - def dof(self) -> int: - """Return the number of joints described by this arm spec.""" - - return len(self.motors) - - @property - def joint_names(self) -> tuple[str, ...]: - """Return joint names in adapter and command-vector order.""" - - return tuple(motor.name for motor in self.motors) - - @classmethod - def from_values( - cls, - *, - name: str, - vendor: str, - model: str, - motors: Sequence[Mapping[str, object] | DamiaoMotorSpec], - position_lower: list[float] | tuple[float, ...], - position_upper: list[float] | tuple[float, ...], - velocity_max: list[float] | tuple[float, ...], - kp: list[float] | tuple[float, ...], - kd: list[float] | tuple[float, ...], - gravity_model_path: str | Path | None = None, - gravity_torque_limits: list[float] | tuple[float, ...] | None = None, - requires_binding: bool = False, - bus_name: str = "can", - arm_name: str = "arm", - fd: bool = False, - supports_velocity: bool = False, - ) -> DamiaoArmSpec: - """Build a typed arm spec from list/tuple metadata values.""" - - return cls( - name=name, - vendor=vendor, - model=model, - motors=coerce_motor_specs(motors, len(motors)), - position_lower=tuple(float(value) for value in position_lower), - position_upper=tuple(float(value) for value in position_upper), - velocity_max=tuple(float(value) for value in velocity_max), - kp=tuple(float(value) for value in kp), - kd=tuple(float(value) for value in kd), - gravity_model_path=gravity_model_path, - gravity_torque_limits=( - tuple(float(value) for value in gravity_torque_limits) - if gravity_torque_limits is not None - else None - ), - requires_binding=requires_binding, - bus_name=bus_name, - arm_name=arm_name, - fd=fd, - supports_velocity=supports_velocity, - ) - - def validate(self) -> None: - """Validate CAN ID uniqueness and per-joint metadata lengths.""" - - DamiaoRobotSpec.from_arm_spec(self).validate() - - -def coerce_motor_specs( - motor_specs: Sequence[Mapping[str, object] | DamiaoMotorSpec], - dof: int, -) -> tuple[DamiaoMotorSpec, ...]: - """Normalize mapping or dataclass motor metadata into typed motor specs.""" - - specs: list[DamiaoMotorSpec] = [] - for spec in motor_specs: - if isinstance(spec, DamiaoMotorSpec): - specs.append(spec) - else: - name = spec.get("name") - send_id = spec.get("send_id") - recv_id = spec.get("recv_id") - if not isinstance(name, str): - raise TypeError("motor spec name must be a string") - if not isinstance(send_id, int): - raise TypeError("motor spec send_id must be an integer") - if recv_id is not None and not isinstance(recv_id, int): - raise TypeError("motor spec recv_id must be an integer") - specs.append( - DamiaoMotorSpec( - name=name, - type=spec.get("type"), - send_id=send_id, - recv_id=recv_id, - ) - ) - if len(specs) != dof: - raise ValueError(f"motor_specs length {len(specs)} does not match dof {dof}") - return tuple(specs) - - -__all__ = [ - "DamiaoArmSpec", - "DamiaoBusSpec", - "DamiaoJointGroupSpec", - "DamiaoMotorSpec", - "DamiaoRobotSpec", - "coerce_motor_specs", -] diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py index 4fb1991da0..c28731f1bb 100644 --- a/dimos/hardware/damiao/test_adapters.py +++ b/dimos/hardware/damiao/test_adapters.py @@ -16,37 +16,34 @@ from types import SimpleNamespace +import attrs import pytest from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter -from dimos.hardware.damiao.runtime import DamiaoGroupState, DamiaoRobotRuntime -from dimos.hardware.damiao.specs import ( - DamiaoArmSpec, - DamiaoBusSpec, - DamiaoJointGroupSpec, - DamiaoMotorSpec, - DamiaoRobotSpec, +from dimos.hardware.damiao.config import ( + DamiaoArmConfig, + DamiaoMotorConfig, + DamiaoRuntimeConfig, ) +from dimos.hardware.damiao.runtime import DamiaoArmRuntime, DamiaoGroupState from dimos.hardware.manipulators.spec import ControlMode class _FakeRuntime: - def __init__(self, *, fresh: bool = True, write_ok: bool = True) -> None: - self.fresh = fresh + def __init__(self, *, write_ok: bool = True) -> None: self.write_ok = write_ok self.connected = False self.enabled = False self.disconnect_calls = 0 - self.batched_calls = 0 self.writes: list[ - tuple[str, list[float], list[float], list[float], list[float], list[float]] + tuple[list[float], list[float], list[float], list[float], list[float]] ] = [] - self.loaded_gravity_models: list[tuple[str, str | None]] = [] - self.states = { - "left": DamiaoGroupState(q=[0.1], dq=[0.2], tau=[0.3]), - "right": DamiaoGroupState(q=[-0.1], dq=[-0.2], tau=[-0.3]), - "arm": DamiaoGroupState(q=[0.4, -0.4], dq=[0.5, -0.5], tau=[0.6, -0.6]), - } + self.loaded_gravity_models: list[str] = [] + self.state = DamiaoGroupState( + q=[0.4, -0.4], + dq=[0.5, -0.5], + tau=[0.6, -0.6], + ) def connect(self) -> bool: self.connected = True @@ -68,22 +65,13 @@ def disable(self) -> bool: def is_enabled(self) -> bool: return self.enabled - def refresh_group_state(self, group_name: str, *, force: bool = False) -> DamiaoGroupState: + def refresh_state(self, *, force: bool = False) -> DamiaoGroupState: del force - return self.states[group_name] - - def has_group_states(self, group_names: tuple[str, ...]) -> bool: - return self.fresh and all(group_name in self.states for group_name in group_names) - - def read_group_states(self, group_names: tuple[str, ...]) -> list[DamiaoGroupState]: - if not self.has_group_states(group_names): - raise RuntimeError("stale state") - return [self.states[group_name] for group_name in group_names] + return self.state - def write_group_mit_commands( + def write_mit_commands( self, *, - group_name: str, q: list[float], dq: list[float], kp: list[float], @@ -92,34 +80,22 @@ def write_group_mit_commands( ) -> bool: if not self.write_ok: return False - self.writes.append((group_name, list(q), list(dq), list(kp), list(kd), list(tau))) - return True - - def write_groups_mit_commands( - self, - commands: dict[str, tuple[list[float], list[float], list[float], list[float], list[float]]], - ) -> bool: - self.batched_calls += 1 - if not self.write_ok: - return False - for group_name, values in commands.items(): - q, dq, kp, kd, tau = values - self.writes.append((group_name, list(q), list(dq), list(kp), list(kd), list(tau))) + self.writes.append((list(q), list(dq), list(kp), list(kd), list(tau))) return True - def load_gravity_model(self, group_name: str, model_path: str | None = None) -> None: - self.loaded_gravity_models.append((group_name, model_path)) - return None + def load_gravity_model(self, model_path: str) -> tuple[object, object]: + self.loaded_gravity_models.append(model_path) + return SimpleNamespace(nq=2, nv=2, names=["universe", "j1", "j2"]), object() -def _arm_spec() -> DamiaoArmSpec: - return DamiaoArmSpec( +def _arm_config(**changes: object) -> DamiaoArmConfig: + config = DamiaoArmConfig( name="test_damiao", vendor="Damiao", model="TestArm", motors=( - DamiaoMotorSpec("j1", "DM4310", 0x01, 0x11), - DamiaoMotorSpec("j2", "DM4310", 0x02, 0x12), + DamiaoMotorConfig("j1", "DM4310", 0x01, 0x11), + DamiaoMotorConfig("j2", "DM4310", 0x02, 0x12), ), position_lower=(-1.0, -2.0), position_upper=(1.0, 2.0), @@ -128,97 +104,53 @@ def _arm_spec() -> DamiaoArmSpec: kd=(0.1, 0.2), gravity_torque_limits=(7.0, 8.0), ) + return attrs.evolve(config, **changes) -def _whole_body_spec() -> DamiaoRobotSpec: - return DamiaoRobotSpec( - name="test_body", - vendor="Damiao", - model="TestBody", - buses={ - "left_can": DamiaoBusSpec(address="can1", fd=True), - "right_can": DamiaoBusSpec(address="can0", fd=True), - }, - groups={ - "left": DamiaoJointGroupSpec( - bus_name="left_can", - motors=(DamiaoMotorSpec("left_joint", "DM4310", 0x01, 0x11),), - position_lower=(-1.0,), - position_upper=(1.0,), - velocity_max=(3.0,), - kp=(5.0,), - kd=(0.1,), - ), - "right": DamiaoJointGroupSpec( - bus_name="right_can", - motors=(DamiaoMotorSpec("right_joint", "DM4310", 0x01, 0x11),), - position_lower=(-2.0,), - position_upper=(2.0,), - velocity_max=(4.0,), - kp=(6.0,), - kd=(0.2,), - ), - }, - ) +def test_arm_config_normalizes_sequences_and_is_frozen() -> None: + config = _arm_config(position_lower=[-1, -2]) + assert config.position_lower == (-1.0, -2.0) + assert config.joint_names == ("j1", "j2") + with pytest.raises(attrs.exceptions.FrozenInstanceError): + config.position_lower = (0.0, 0.0) -def test_robot_spec_rejects_unknown_group_bus() -> None: - spec = DamiaoRobotSpec( - name="bad", - vendor="Damiao", - model="Bad", - buses={"can": DamiaoBusSpec()}, - groups={ - "arm": DamiaoJointGroupSpec( - bus_name="missing", - motors=(DamiaoMotorSpec("j1", "DM4310", 0x01, 0x11),), - position_lower=(-1.0,), - position_upper=(1.0,), - velocity_max=(1.0,), - kp=(1.0,), - kd=(0.1,), + +def test_arm_config_rejects_duplicate_motor_identity_at_construction() -> None: + with pytest.raises(ValueError, match="duplicate send IDs"): + _arm_config( + motors=( + DamiaoMotorConfig("j1", "DM4310", 0x01, 0x11), + DamiaoMotorConfig("j2", "DM4310", 0x01, 0x12), ) - }, - ) + ) - with pytest.raises(ValueError, match="unknown bus"): - spec.validate() +@pytest.mark.parametrize( + ("changes", "message"), + [ + ({"kp": (1.0,)}, "kp length"), + ({"position_lower": (2.0, -2.0)}, "lower limits"), + ({"velocity_max": (0.0, 1.0)}, "velocity limits"), + ], +) +def test_arm_config_rejects_invalid_joint_vectors_at_construction( + changes: dict[str, object], message: str +) -> None: + with pytest.raises(ValueError, match=message): + _arm_config(**changes) -def test_robot_spec_rejects_duplicate_send_ids_on_shared_bus() -> None: - spec = DamiaoRobotSpec( - name="bad_ids", - vendor="Damiao", - model="BadIds", - buses={"can": DamiaoBusSpec()}, - groups={ - "left": DamiaoJointGroupSpec( - bus_name="can", - motors=(DamiaoMotorSpec("left_joint", "DM4310", 0x01, 0x11),), - position_lower=(-1.0,), - position_upper=(1.0,), - velocity_max=(1.0,), - kp=(1.0,), - kd=(0.1,), - ), - "right": DamiaoJointGroupSpec( - bus_name="can", - motors=(DamiaoMotorSpec("right_joint", "DM4310", 0x01, 0x12),), - position_lower=(-1.0,), - position_upper=(1.0,), - velocity_max=(1.0,), - kp=(1.0,), - kd=(0.1,), - ), - }, - ) - with pytest.raises(ValueError, match="duplicate send_id 1 on bus 'can'"): - spec.validate() +def test_runtime_config_rejects_invalid_typed_overrides() -> None: + with pytest.raises(ValueError, match="kp_override"): + DamiaoRuntimeConfig(kp_override=[1.0, float("nan")]) def test_arm_adapter_reports_limits_and_modes() -> None: - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_comp=False) + adapter = DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) assert adapter.get_dof() == 2 assert adapter.get_limits().position_lower == [-1.0, -2.0] @@ -226,16 +158,39 @@ def test_arm_adapter_reports_limits_and_modes() -> None: assert adapter.set_control_mode(ControlMode.VELOCITY) is False +def test_arm_adapter_resolves_runtime_gain_overrides() -> None: + adapter = DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig( + gravity_comp=False, + kp_override=[9.0, 8.0], + kd_override=[0.9, 0.8], + ), + ) + + assert adapter._kp == [9.0, 8.0] + assert adapter._kd == [0.9, 0.8] + + +def test_arm_adapter_rejects_override_with_wrong_dof() -> None: + with pytest.raises(ValueError, match="kp length"): + DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(gravity_comp=False, kp_override=[1.0]), + ) + + def test_arm_adapter_uses_fake_runtime_for_startup_hold(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_comp=False) + adapter = DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) assert adapter.connect() is True assert adapter.write_enable(True) is True - assert runtime.writes[-1] == ( - "arm", [0.4, -0.4], [0.0, 0.0], [5.0, 6.0], @@ -244,23 +199,26 @@ def test_arm_adapter_uses_fake_runtime_for_startup_hold(mocker) -> None: ) -def test_arm_adapter_passes_gravity_model_override_to_runtime(mocker) -> None: +def test_arm_adapter_passes_gravity_model_to_runtime(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec( - arm_spec=_arm_spec(), - gravity_model_path="override.urdf", + adapter = DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(gravity_model_path="override.urdf"), ) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) assert adapter.connect() is True - - assert runtime.loaded_gravity_models == [("arm", "override.urdf")] + assert runtime.loaded_gravity_models == ["override.urdf"] def test_arm_adapter_rejects_nonfinite_positions_before_enable(mocker) -> None: runtime = _FakeRuntime() - runtime.states["arm"] = DamiaoGroupState(q=[float("nan"), 0.0], dq=[0.0, 0.0], tau=[0.0, 0.0]) - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + runtime.state = DamiaoGroupState( + q=[float("nan"), 0.0], + dq=[0.0, 0.0], + tau=[0.0, 0.0], + ) + adapter = DamiaoArmAdapter(arm_config=_arm_config()) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) assert adapter.connect() is True @@ -271,7 +229,7 @@ def test_arm_adapter_rejects_nonfinite_positions_before_enable(mocker) -> None: def test_arm_adapter_gravity_compensation_rejects_missing_model_before_enable(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + adapter = DamiaoArmAdapter(arm_config=_arm_config()) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) assert adapter.connect() is True @@ -282,7 +240,7 @@ def test_arm_adapter_gravity_compensation_rejects_missing_model_before_enable(mo def test_arm_adapter_error_recovery_runs_gravity_preflight(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + adapter = DamiaoArmAdapter(arm_config=_arm_config()) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) preflight = mocker.patch.object(adapter, "_preflight_gravity") mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) @@ -295,7 +253,7 @@ def test_arm_adapter_error_recovery_runs_gravity_preflight(mocker) -> None: def test_arm_adapter_disables_without_zero_torque_on_gravity_state_failure(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + adapter = DamiaoArmAdapter(arm_config=_arm_config()) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) mocker.patch.object(adapter, "_load_gravity_model") mocker.patch.object(adapter, "_preflight_gravity") @@ -304,7 +262,7 @@ def test_arm_adapter_disables_without_zero_torque_on_gravity_state_failure(mocke assert adapter.connect() is True assert adapter.write_enable(True) is True writes_before_failure = list(runtime.writes) - runtime.refresh_group_state = mocker.Mock(side_effect=RuntimeError("state read failed")) + mocker.patch.object(runtime, "refresh_state", side_effect=RuntimeError("state read failed")) assert adapter.write_joint_positions([0.2, -0.2]) is False assert runtime.enabled is False @@ -314,12 +272,16 @@ def test_arm_adapter_disables_without_zero_torque_on_gravity_state_failure(mocke def test_arm_adapter_rejects_incompatible_gravity_model_before_enable(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_model_path="arm.urdf") + adapter = DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(gravity_model_path="arm.urdf"), + ) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - adapter._pin_model = SimpleNamespace(nq=2, nv=2, names=["universe", "j2", "j1"]) mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) assert adapter.connect() is True + adapter._pin_model = SimpleNamespace(nq=2, nv=2, names=["universe", "j2", "j1"]) + adapter._pin_data = object() assert adapter.write_enable(True) is False assert runtime.enabled is False assert runtime.writes == [] @@ -327,7 +289,7 @@ def test_arm_adapter_rejects_incompatible_gravity_model_before_enable(mocker) -> def test_arm_adapter_rolls_back_when_hold_command_fails(mocker) -> None: runtime = _FakeRuntime(write_ok=False) - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + adapter = DamiaoArmAdapter(arm_config=_arm_config()) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) assert adapter.connect() is True @@ -338,8 +300,11 @@ def test_arm_adapter_rolls_back_when_hold_command_fails(mocker) -> None: def test_arm_adapter_preserves_enabled_state_when_rollback_disable_fails(mocker) -> None: runtime = _FakeRuntime(write_ok=False) - runtime.disable = mocker.Mock(return_value=False) - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec(), gravity_comp=False) + mocker.patch.object(runtime, "disable", return_value=False) + adapter = DamiaoArmAdapter( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) assert adapter.connect() is True @@ -349,46 +314,52 @@ def test_arm_adapter_preserves_enabled_state_when_rollback_disable_fails(mocker) def test_arm_adapter_preserves_enabled_state_when_safety_disable_fails(mocker) -> None: runtime = _FakeRuntime() - adapter = DamiaoArmAdapter.from_arm_spec(arm_spec=_arm_spec()) + adapter = DamiaoArmAdapter(arm_config=_arm_config()) mocker.patch.object(adapter, "_create_runtime", return_value=runtime) mocker.patch.object(adapter, "_preflight_gravity") mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) assert adapter.connect() is True assert adapter.write_enable(True) is True - runtime.disable = mocker.Mock(return_value=False) - runtime.refresh_group_state = mocker.Mock(side_effect=RuntimeError("state read failed")) + mocker.patch.object(runtime, "disable", return_value=False) + mocker.patch.object(runtime, "refresh_state", side_effect=RuntimeError("state read failed")) assert adapter.write_joint_positions([0.2, -0.2]) is False assert adapter.read_enabled() is True def test_runtime_retries_mit_command_when_can_queue_is_temporarily_full(mocker) -> None: - runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(), + ) robot = mocker.Mock() - group = mocker.Mock() - group.mit_control.side_effect = [ + arm = mocker.Mock() + arm.mit_control.side_effect = [ RuntimeError("transport IO error: No buffer space available (os error 105)"), None, ] runtime._robot = robot - runtime._groups = {"left": group} + runtime._arm = arm runtime._enabled = True sleep = mocker.patch("dimos.hardware.damiao.runtime.time.sleep") assert ( - runtime.write_group_mit_commands( - group_name="left", q=[0.0], dq=[0.0], kp=[0.0], kd=[0.0], tau=[0.0] + runtime.write_mit_commands( + q=[0.0] * 2, dq=[0.0] * 2, kp=[0.0] * 2, kd=[0.0] * 2, tau=[0.0] * 2 ) is True ) - assert group.mit_control.call_count == 2 + assert arm.mit_control.call_count == 2 robot.tick.assert_called_once_with(1_000) sleep.assert_called_once_with(0.001) def test_runtime_selects_mit_mode_before_enable(mocker) -> None: - runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(), + ) robot = mocker.Mock() runtime._robot = robot @@ -415,7 +386,10 @@ def enable(self) -> None: def disable(self) -> bool: return False - runtime = DamiaoRobotRuntime(robot_spec=_whole_body_spec()) + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(), + ) runtime._robot = _FailingRobot() assert runtime.enable() is False diff --git a/dimos/hardware/manipulators/openyam_damiao/__init__.py b/dimos/hardware/manipulators/openyam_damiao/__init__.py deleted file mode 100644 index 30d60fd798..0000000000 --- a/dimos/hardware/manipulators/openyam_damiao/__init__.py +++ /dev/null @@ -1,22 +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. - -"""Damiao hardware adapter for the six-axis OpenYAM arm.""" - -__all__ = ["OpenYAMDamiaoAdapter", "OpenYamDamiaoAdapter"] - - -def __getattr__(name: str) -> object: - if name in __all__: - from dimos.hardware.manipulators.openyam_damiao.adapter import ( - OpenYAMDamiaoAdapter, - OpenYamDamiaoAdapter, - ) - - return { - "OpenYAMDamiaoAdapter": OpenYAMDamiaoAdapter, - "OpenYamDamiaoAdapter": OpenYamDamiaoAdapter, - }[name] - raise AttributeError(name) diff --git a/dimos/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py index fe634a33cf..31737af15f 100644 --- a/dimos/hardware/manipulators/openyam_damiao/adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/adapter.py @@ -2,6 +2,15 @@ # # 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's six-axis Damiao adapter.""" @@ -9,14 +18,14 @@ import math from pathlib import Path -from typing import Any, cast - -from dimos.hardware.damiao import ( - DamiaoArmAdapter, - DamiaoBusSpec, - DamiaoJointGroupSpec, - DamiaoMotorSpec, - DamiaoRobotSpec, + +import attrs + +from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter +from dimos.hardware.damiao.config import ( + DamiaoArmConfig, + DamiaoMotorConfig, + DamiaoRuntimeConfig, ) from dimos.robot.model_parser import parse_model from dimos.utils.data import LfsPath @@ -25,20 +34,18 @@ logger = setup_logger() OPENING_METRES = 0.096 -_BUS_NAME = "openyam_can" -_ARM_GROUP = "arm" _OPENYAM_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper.urdf.xacro" _OPENYAM_PACKAGE_PATHS = {"yam_description": Path(LfsPath("yam_description"))} -ARM_MOTOR_SPECS = tuple( - DamiaoMotorSpec( +ARM_MOTOR_CONFIGS = tuple( + DamiaoMotorConfig( name=f"yam_joint{index}", type="DM4340" if index <= 3 else "DM4310", send_id=index, ) for index in range(1, 7) ) -GRIPPER_MOTOR_SPECS = (DamiaoMotorSpec("yam_gripper", "DM4310", 7),) +GRIPPER_MOTOR_CONFIGS = (DamiaoMotorConfig("yam_gripper", "DM4310", 7),) def aperture_to_opening(aperture: float) -> float: @@ -55,29 +62,6 @@ def opening_to_aperture(opening: float) -> float: return opening * OPENING_METRES -def _group_spec( - *, - bus_name: str, - motors: tuple[DamiaoMotorSpec, ...], - lower: tuple[float, ...], - upper: tuple[float, ...], - velocity: tuple[float, ...], - kp: tuple[float, ...], - kd: tuple[float, ...], - gravity_model_path: str | Path | None = None, -) -> DamiaoJointGroupSpec: - return DamiaoJointGroupSpec( - bus_name=bus_name, - motors=motors, - position_lower=lower, - position_upper=upper, - velocity_max=velocity, - kp=kp, - kd=kd, - gravity_model_path=gravity_model_path, - ) - - def _active_arm_limits() -> tuple[tuple[float, ...], tuple[float, ...], tuple[float, ...]]: """Read arm limits from the active planning Xacro, failing closed.""" model = parse_model(_OPENYAM_MODEL_PATH, package_paths=_OPENYAM_PACKAGE_PATHS) @@ -88,25 +72,41 @@ def _active_arm_limits() -> tuple[tuple[float, ...], tuple[float, ...], tuple[fl resolved = [joint for joint in joints if joint is not None] if len(resolved) != 6: raise ValueError("active OpenYAM Xacro does not define all six arm joints") + limits: list[tuple[float, float, float]] = [] for joint in resolved: lower = joint.lower_limit upper = joint.upper_limit velocity = joint.velocity_limit if lower is None or upper is None or velocity is None: raise ValueError("active OpenYAM Xacro has incomplete or nonfinite arm limits") - lower = cast("float", lower) - upper = cast("float", upper) - velocity = cast("float", velocity) if not all(math.isfinite(value) for value in (lower, upper, velocity)): raise ValueError("active OpenYAM Xacro has incomplete or nonfinite arm limits") if lower > upper: raise ValueError(f"active OpenYAM Xacro has inverted limits for {joint.name}") if velocity <= 0: raise ValueError(f"active OpenYAM Xacro has nonpositive velocity for {joint.name}") + limits.append((lower, upper, velocity)) return ( - tuple(cast("float", joint.lower_limit) for joint in resolved), - tuple(cast("float", joint.upper_limit) for joint in resolved), - tuple(cast("float", joint.velocity_limit) for joint in resolved), + tuple(lower for lower, _, _ in limits), + tuple(upper for _, upper, _ in limits), + tuple(velocity for _, _, velocity in limits), + ) + + +def make_openyam_damiao_arm_config() -> DamiaoArmConfig: + """Build the canonical OpenYAM arm profile from the active planning model.""" + + lower, upper, velocity = _active_arm_limits() + return DamiaoArmConfig( + name="openyam", + vendor="Damiao", + model="OpenYAM", + motors=ARM_MOTOR_CONFIGS, + position_lower=lower, + position_upper=upper, + velocity_max=velocity, + kp=(80.0, 80.0, 80.0, 10.0, 10.0, 10.0), + kd=(5.0, 5.0, 5.0, 1.5, 1.5, 1.5), ) @@ -115,40 +115,25 @@ class OpenYamDamiaoAdapter(DamiaoArmAdapter): def __init__( self, - address: str = "can0", + address: str | Path | None = None, *, - gravity_model_path: str | Path | None = None, - gravity_comp: bool = True, - **kwargs: Any, + runtime_config: DamiaoRuntimeConfig | None = None, + dof: int | None = None, + hardware_id: str = "arm", ) -> None: - if gravity_comp and (gravity_model_path is None or not Path(gravity_model_path).is_file()): + runtime_config = runtime_config or DamiaoRuntimeConfig() + if address is not None: + runtime_config = attrs.evolve(runtime_config, address=str(address)) + if runtime_config.gravity_comp and ( + runtime_config.gravity_model_path is None + or not runtime_config.gravity_model_path.is_file() + ): raise ValueError("OpenYAM gravity compensation requires a valid model path") - lower, upper, velocity = _active_arm_limits() - arm = _group_spec( - bus_name=_BUS_NAME, - motors=ARM_MOTOR_SPECS, - lower=lower, - upper=upper, - velocity=velocity, - kp=(80.0, 80.0, 80.0, 10.0, 10.0, 10.0), - kd=(5.0, 5.0, 5.0, 1.5, 1.5, 1.5), - gravity_model_path=gravity_model_path, - ) - robot_spec = DamiaoRobotSpec( - name="openyam", - vendor="Damiao", - model="OpenYAM", - buses={_BUS_NAME: DamiaoBusSpec(address=address)}, - # The upstream binding has no calibrated normalized gripper - # readback API. Do not expose a guessed/raw or last-command state. - groups={_ARM_GROUP: arm}, - ) super().__init__( - robot_spec=robot_spec, - group_name=_ARM_GROUP, - gravity_model_path=gravity_model_path, - gravity_comp=gravity_comp, - **kwargs, + arm_config=make_openyam_damiao_arm_config(), + runtime_config=runtime_config, + dof=dof, + hardware_id=hardware_id, ) self._write_armed_by_read = False diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py index 52a4af41ca..b3adc803e4 100644 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -2,6 +2,15 @@ # # 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. """Focused OpenYAM adapter tests. @@ -15,13 +24,14 @@ import pytest -pytest.importorskip("dimos.hardware.damiao") +pytest.importorskip("can_motor_control") +from dimos.hardware.damiao.config import DamiaoRuntimeConfig from dimos.hardware.damiao.runtime import DamiaoGroupState import dimos.hardware.manipulators.openyam_damiao.adapter as adapter_module from dimos.hardware.manipulators.openyam_damiao.adapter import ( - ARM_MOTOR_SPECS, - GRIPPER_MOTOR_SPECS, + ARM_MOTOR_CONFIGS, + GRIPPER_MOTOR_CONFIGS, OPENING_METRES, OpenYamDamiaoAdapter, aperture_to_opening, @@ -41,22 +51,32 @@ def test_gripper_aperture_conversion_is_linear() -> None: def test_openyam_motor_topology() -> None: - assert [motor.name for motor in ARM_MOTOR_SPECS] == [f"yam_joint{i}" for i in range(1, 7)] - assert [motor.send_id for motor in ARM_MOTOR_SPECS] == list(range(1, 7)) - assert [motor.type for motor in ARM_MOTOR_SPECS] == ["DM4340"] * 3 + ["DM4310"] * 3 - assert GRIPPER_MOTOR_SPECS[0].send_id == 7 - assert GRIPPER_MOTOR_SPECS[0].type == "DM4310" + assert [motor.name for motor in ARM_MOTOR_CONFIGS] == [f"yam_joint{i}" for i in range(1, 7)] + assert [motor.send_id for motor in ARM_MOTOR_CONFIGS] == list(range(1, 7)) + assert [motor.type for motor in ARM_MOTOR_CONFIGS] == ["DM4340"] * 3 + ["DM4310"] * 3 + assert GRIPPER_MOTOR_CONFIGS[0].send_id == 7 + assert GRIPPER_MOTOR_CONFIGS[0].type == "DM4310" def test_openyam_physical_gripper_is_disabled_without_calibrated_readback() -> None: - adapter = OpenYamDamiaoAdapter(gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True) + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig( + gravity_model_path=GRAVITY_MODEL_PATH, + use_mock_bus=True, + ) + ) assert adapter.read_gripper_position() is None assert not adapter.write_gripper_position(0.01) def test_openyam_limits_are_loaded_from_active_model() -> None: - adapter = OpenYamDamiaoAdapter(gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True) + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig( + gravity_model_path=GRAVITY_MODEL_PATH, + use_mock_bus=True, + ) + ) limits = adapter.get_limits() assert limits.position_lower == pytest.approx([-3.92699, 0.0, 0.0, -1.65806, -1.5708, -2.35619]) @@ -70,42 +90,45 @@ def test_openyam_limits_are_loaded_from_active_model() -> None: def test_openyam_allows_gravity_comp_to_be_disabled() -> None: - adapter = OpenYamDamiaoAdapter(gravity_comp=False, use_mock_bus=True) + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig(gravity_comp=False, use_mock_bus=True) + ) assert not adapter._gravity_comp with pytest.raises(ValueError, match="gravity compensation"): - OpenYamDamiaoAdapter(use_mock_bus=True) + OpenYamDamiaoAdapter(runtime_config=DamiaoRuntimeConfig(use_mock_bus=True)) def test_openyam_activation_holds_exact_feedback_position() -> None: - adapter = OpenYamDamiaoAdapter(gravity_comp=False, use_mock_bus=True) + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig(gravity_comp=False, use_mock_bus=True) + ) runtime = Mock() feedback = [-1.2, 0.1, 0.2, -0.3, 0.4, -0.5] - runtime.refresh_group_state.return_value = DamiaoGroupState( - q=feedback, dq=[0.0] * 6, tau=[0.0] * 6 - ) + runtime.refresh_state.return_value = DamiaoGroupState(q=feedback, dq=[0.0] * 6, tau=[0.0] * 6) runtime.enable.return_value = True - runtime.write_group_mit_commands.return_value = True + runtime.write_mit_commands.return_value = True adapter._runtime = runtime assert adapter.activate() - runtime.write_group_mit_commands.assert_called_once_with( - group_name="arm", + runtime.write_mit_commands.assert_called_once_with( q=feedback, dq=[0.0] * 6, kp=[80.0, 80.0, 80.0, 10.0, 10.0, 10.0], kd=[5.0, 5.0, 5.0, 1.5, 1.5, 1.5], tau=[0.0] * 6, ) - assert runtime.refresh_group_state.call_count >= 2 + assert runtime.refresh_state.call_count >= 2 def test_openyam_normal_enable_and_error_recovery() -> None: adapter = OpenYamDamiaoAdapter( - gravity_model_path=GRAVITY_MODEL_PATH, - use_mock_bus=True, + runtime_config=DamiaoRuntimeConfig( + gravity_model_path=GRAVITY_MODEL_PATH, + use_mock_bus=True, + ), ) runtime = Mock() runtime.enable.return_value = True @@ -126,14 +149,14 @@ def test_openyam_normal_enable_and_error_recovery() -> None: def test_openyam_forwards_mit_commands() -> None: adapter = OpenYamDamiaoAdapter( - gravity_model_path=GRAVITY_MODEL_PATH, - use_mock_bus=True, + runtime_config=DamiaoRuntimeConfig( + gravity_model_path=GRAVITY_MODEL_PATH, + use_mock_bus=True, + ), ) runtime = Mock() - runtime.refresh_group_state.return_value = DamiaoGroupState( - q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6 - ) - runtime.write_group_mit_commands.return_value = True + runtime.refresh_state.return_value = DamiaoGroupState(q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6) + runtime.write_mit_commands.return_value = True adapter._runtime = runtime adapter._enabled = True @@ -147,8 +170,7 @@ def test_openyam_forwards_mit_commands() -> None: tau=[5.0] * 6, ) - runtime.write_group_mit_commands.assert_called_once_with( - group_name="arm", + runtime.write_mit_commands.assert_called_once_with( q=[1.0] * 6, dq=[2.0] * 6, kp=[3.0] * 6, @@ -159,25 +181,25 @@ def test_openyam_forwards_mit_commands() -> None: assert not adapter.write_mit_commands( q=[1.0] * 6, dq=[2.0] * 6, kp=[3.0] * 6, kd=[4.0] * 6, tau=[5.0] * 6 ) - runtime.write_group_mit_commands.assert_called_once() + runtime.write_mit_commands.assert_called_once() def test_openyam_failed_read_revokes_write_permission() -> None: - adapter = OpenYamDamiaoAdapter(gravity_comp=False, use_mock_bus=True) - runtime = Mock() - runtime.refresh_group_state.return_value = DamiaoGroupState( - q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6 + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig(gravity_comp=False, use_mock_bus=True) ) + runtime = Mock() + runtime.refresh_state.return_value = DamiaoGroupState(q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6) adapter._runtime = runtime adapter._enabled = True adapter.refresh_state(force=True) - runtime.refresh_group_state.side_effect = RuntimeError("feedback unavailable") + runtime.refresh_state.side_effect = RuntimeError("feedback unavailable") with pytest.raises(RuntimeError, match="feedback unavailable"): adapter.refresh_state(force=True) assert not adapter.write_joint_positions([0.25] * 6) - runtime.write_group_mit_commands.assert_not_called() + runtime.write_mit_commands.assert_not_called() def test_openyam_xacro_limits_reject_duplicate_joint_names(monkeypatch: pytest.MonkeyPatch) -> None: diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index 9f952f0deb..9f08539e26 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -268,6 +268,12 @@ def test_config_validates_viser_visualization() -> None: assert config.visualization.panel_enabled is False +def test_viser_config_defaults_to_loopback_host() -> None: + config = ViserVisualizationConfig() + + assert config.host == "127.0.0.1" + + def test_config_meshcat_requires_world_visualization() -> None: config = ManipulationModuleConfig.model_validate({"visualization": {"backend": "meshcat"}}) diff --git a/dimos/manipulation/visualization/viser/config.py b/dimos/manipulation/visualization/viser/config.py index d385e45233..36ce7d06ea 100644 --- a/dimos/manipulation/visualization/viser/config.py +++ b/dimos/manipulation/visualization/viser/config.py @@ -26,7 +26,7 @@ class ViserVisualizationConfig(BaseModel): backend: Literal["viser"] = "viser" host: str = Field( - default="0.0.0.0", validation_alias=AliasChoices("host", "visualization_host") + default="127.0.0.1", validation_alias=AliasChoices("host", "visualization_host") ) port: int = Field(default=8095, validation_alias=AliasChoices("port", "visualization_port")) open_browser: bool = Field( diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index c02d181c67..42c915d91c 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -20,6 +20,7 @@ from dimos.control.components import HardwareComponent, HardwareType, make_joints from dimos.core.global_config import global_config +from dimos.hardware.damiao.config import DamiaoRuntimeConfig from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import ( @@ -81,8 +82,10 @@ def openyam_hardware( # Physical encoder zeros are established by the driver; never pass # planning/home positions into a live motor adapter. adapter_kwargs={ - "gravity_model_path": OPENYAM_GRAVITY_MODEL_PATH, - "gravity_comp": True, + "runtime_config": DamiaoRuntimeConfig( + gravity_model_path=OPENYAM_GRAVITY_MODEL_PATH, + gravity_comp=True, + ), }, include_gripper=False, ) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 84317fcaf6..baf133d40f 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -17,6 +17,7 @@ from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import Blueprint from dimos.core.global_config import global_config +from dimos.hardware.damiao.config import DamiaoRuntimeConfig from dimos.hardware.manipulators.mock.adapter import MockAdapter from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig from dimos.robot.manipulators.openyam.blueprints.basic import ( @@ -75,8 +76,10 @@ def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: A assert hardware.adapter_type == "openyam_damiao" assert hardware.address == "can1" - assert hardware.adapter_kwargs["gravity_model_path"] == OPENYAM_GRAVITY_MODEL_PATH - assert hardware.adapter_kwargs["gravity_comp"] is True + runtime_config = hardware.adapter_kwargs["runtime_config"] + assert isinstance(runtime_config, DamiaoRuntimeConfig) + assert runtime_config.gravity_model_path == OPENYAM_GRAVITY_MODEL_PATH + assert runtime_config.gravity_comp is True assert len(hardware.joints) == OPENYAM_DOF assert hardware.gripper_joints == [] assert "initial_positions" not in hardware.adapter_kwargs 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/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 3c19546f26..33830780ef 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -7,11 +7,6 @@ the default world and native path planner. ## Quick Start -For the required vendor/approved-bench direction verification before OpenYAM -planning or teleoperation, see the -[OpenYAM direction commissioning guide](./openyam_commissioning.md). The -DimOS driver does not issue commissioning position steps. - Recent addition: the A-750 keyboard teleop blueprint is now available via: ```bash @@ -198,6 +193,15 @@ uv run dimos run xarm7-planner-coordinator \ -o manipulationmodule.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 From 47a1cb9b1be48671b550a65880552e8ca5800f07 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 20:35:12 +0000 Subject: [PATCH 22/44] fix: dm api --- dimos/hardware/damiao/runtime.py | 2 +- dimos/hardware/damiao/test_adapters.py | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py index e66718f263..882a94b976 100644 --- a/dimos/hardware/damiao/runtime.py +++ b/dimos/hardware/damiao/runtime.py @@ -150,7 +150,7 @@ def _build_robot(self) -> Any: motors = [ can_motor_control.MotorSpec( motor.name, - int(self._resolve_motor_type(motor.type)), + self._resolve_motor_type(motor.type), motor.send_id, motor.effective_recv_id, ) diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py index c28731f1bb..6b94503c07 100644 --- a/dimos/hardware/damiao/test_adapters.py +++ b/dimos/hardware/damiao/test_adapters.py @@ -146,6 +146,17 @@ def test_runtime_config_rejects_invalid_typed_overrides() -> None: DamiaoRuntimeConfig(kp_override=[1.0, float("nan")]) +def test_runtime_builds_robot_with_binding_motor_types() -> None: + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(use_mock_bus=True), + ) + + robot = runtime._build_robot() + + assert len(robot["arm"]) == 2 + + def test_arm_adapter_reports_limits_and_modes() -> None: adapter = DamiaoArmAdapter( arm_config=_arm_config(), From 8a89a388b1ec224bb9d2fb4f1be6b09478e2e1d7 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 15:28:21 -0700 Subject: [PATCH 23/44] feat: add normalized OpenYAM gripper support --- dimos/control/coordinator.py | 31 +++++++ dimos/control/hardware_interface.py | 24 +++-- dimos/control/test_control.py | 51 +++++++++++ dimos/hardware/damiao/config.py | 47 ++++++++++ dimos/hardware/damiao/runtime.py | 89 +++++++++++++++++-- dimos/hardware/damiao/test_adapters.py | 88 ++++++++++++++++++ .../manipulators/openyam_damiao/adapter.py | 39 ++++---- .../openyam_damiao/test_adapter.py | 40 ++++----- dimos/manipulation/manipulation_module.py | 16 +++- dimos/manipulation/test_manipulation_unit.py | 15 ++++ .../manipulators/openyam/blueprints/teleop.py | 17 +++- dimos/robot/manipulators/openyam/config.py | 3 +- .../manipulators/openyam/test_openyam.py | 18 +++- docs/capabilities/manipulation/index.md | 11 +++ 14 files changed, 416 insertions(+), 73 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index eeed8dead0..51b4237acf 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -915,6 +915,37 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool: return False return hw.adapter.write_gripper_position(position) + @rpc + def open_gripper(self, hardware_id: str) -> bool: + """Open a gripper to its configured adapter-native endpoint.""" + + return self._set_gripper_endpoint(hardware_id, open_position=True) + + @rpc + def close_gripper(self, hardware_id: str) -> bool: + """Close a gripper to its configured adapter-native endpoint.""" + + return self._set_gripper_endpoint(hardware_id, open_position=False) + + def _set_gripper_endpoint(self, hardware_id: str, *, open_position: bool) -> bool: + with self._hardware_lock: + hw = self._hardware.get(hardware_id) + if hw is None: + logger.warning(f"Hardware '{hardware_id}' not found for gripper command") + return False + if isinstance(hw, ConnectedTwistBase): + logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support") + return False + configured = ( + hw.component.gripper_open_position + if open_position + else hw.component.gripper_closed_position + ) + fallback = 0.85 if open_position else 0.0 + return hw.adapter.write_gripper_position( + configured if configured is not None else fallback + ) + @rpc def get_gripper_position(self, hardware_id: str) -> float | None: """Get gripper position from a specific hardware device. diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 3a7c74f430..178db772bb 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -125,14 +125,13 @@ def read_state(self) -> dict[JointName, JointState]: # Append gripper joint(s) via adapter gripper method if self._gripper_joints: gripper_pos = self._adapter.read_gripper_position() - for gj in self._gripper_joints: - result[gj] = JointState( - position=self._physical_to_normalized(gripper_pos) - if gripper_pos is not None - else 0.0, - velocity=0.0, - effort=0.0, - ) + if gripper_pos is not None: + for gj in self._gripper_joints: + result[gj] = JointState( + position=self._physical_to_normalized(gripper_pos), + velocity=0.0, + effort=0.0, + ) return result @@ -213,12 +212,9 @@ def _initialize_last_commanded(self) -> None: # Initialize gripper joint(s) from adapter if self._gripper_joints: gripper_pos = self._adapter.read_gripper_position() - for gj in self._gripper_joints: - self._last_commanded[gj] = ( - self._physical_to_normalized(gripper_pos) - if gripper_pos is not None - else 0.0 - ) + if gripper_pos is not None: + for gj in self._gripper_joints: + self._last_commanded[gj] = self._physical_to_normalized(gripper_pos) self._initialized = True return diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 61ac235b03..b8dcd1f80e 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -206,6 +206,22 @@ def test_normalized_gripper_commands_are_mapped_at_hardware_boundary(self, mock_ ((0.07,), {}), ] + def test_missing_gripper_feedback_is_not_fabricated_or_commanded(self, mock_adapter): + mock_adapter.read_gripper_position.return_value = None + component = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + gripper_joints=["arm/gripper"], + gripper_open_position=1.0, + gripper_closed_position=0.0, + ) + hardware = ConnectedHardware(mock_adapter, component) + + assert "arm/gripper" not in hardware.read_state() + assert hardware.write_command({"arm/joint1": 0.1}, ControlMode.POSITION) + mock_adapter.write_gripper_position.assert_not_called() + def test_joint_names_prefixed(self, connected_hardware): names = connected_hardware.joint_names assert names == [ @@ -253,6 +269,41 @@ def make(**kwargs: Any) -> ControlCoordinator: class TestControlCoordinatorLifecycle: + @pytest.mark.parametrize( + ("open_endpoint", "closed_endpoint", "expected"), + [ + (1.0, 0.0, [1.0, 0.0]), + (0.07, 0.0, [0.07, 0.0]), + (0.85, 0.0, [0.85, 0.0]), + (None, None, [0.85, 0.0]), + ], + ) + def test_gripper_open_close_use_configured_endpoints( + self, + make_coordinator, + mock_adapter, + open_endpoint: float | None, + closed_endpoint: float | None, + expected: list[float], + ) -> None: + component = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + gripper_joints=["arm/gripper"], + gripper_open_position=open_endpoint, + gripper_closed_position=closed_endpoint, + ) + coordinator = make_coordinator() + coordinator._hardware = {"arm": ConnectedHardware(mock_adapter, component)} + mock_adapter.write_gripper_position.return_value = True + + assert coordinator.open_gripper("arm") is True + assert coordinator.close_gripper("arm") is True + assert [ + call.args[0] for call in mock_adapter.write_gripper_position.call_args_list + ] == expected + def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator): coordinator = make_coordinator() matching_task = RecordingTask("eef") diff --git a/dimos/hardware/damiao/config.py b/dimos/hardware/damiao/config.py index f7cbb579f6..e70494944e 100644 --- a/dimos/hardware/damiao/config.py +++ b/dimos/hardware/damiao/config.py @@ -17,6 +17,7 @@ from collections.abc import Sequence import math from pathlib import Path +from typing import Literal import attrs @@ -63,6 +64,11 @@ def _finite_non_negative( raise ValueError(f"{attribute.name} must be finite and non-negative") +def _opening_current(_instance: object, attribute: attrs.Attribute[float], value: float) -> None: + if not math.isfinite(value) or not 0.0 < value <= 1.0: + raise ValueError(f"{attribute.name} must be finite and in (0, 1]") + + @attrs.frozen(slots=False) class DamiaoMotorConfig: """Physical identity for one Damiao motor in command-vector order.""" @@ -82,6 +88,23 @@ def effective_recv_id(self) -> int: return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) +@attrs.frozen(slots=False) +class DamiaoGripperConfig: + """Physical definition for one normalized Damiao gripper group.""" + + motor: DamiaoMotorConfig = attrs.field( + validator=attrs.validators.instance_of(DamiaoMotorConfig) + ) + opening_direction: Literal["increasing_position", "decreasing_position"] = attrs.field( + validator=attrs.validators.in_(("increasing_position", "decreasing_position")) + ) + default_current: float = attrs.field( + default=0.15, + converter=float, + validator=_opening_current, + ) + + @attrs.frozen(slots=False) class DamiaoArmConfig: """Immutable physical definition and capabilities for one Damiao arm.""" @@ -105,6 +128,10 @@ class DamiaoArmConfig: default=None, converter=_to_optional_floats, ) + gripper: DamiaoGripperConfig | None = attrs.field( + default=None, + validator=attrs.validators.optional(attrs.validators.instance_of(DamiaoGripperConfig)), + ) fd: bool = attrs.field(default=False, validator=attrs.validators.instance_of(bool)) supported_control_modes: tuple[ControlMode, ...] = attrs.field( factory=lambda: ( @@ -189,6 +216,26 @@ def _validate_control_modes( if len(set(modes)) != len(modes): raise ValueError("supported control modes must be unique") + @gripper.validator + def _validate_gripper_identity( + self, + _attribute: attrs.Attribute[DamiaoGripperConfig | None], + gripper: DamiaoGripperConfig | None, + ) -> None: + if gripper is None: + return + motor = gripper.motor + if motor.name in self.joint_names: + raise ValueError( + f"Damiao arm {self.name!r} gripper duplicates motor name {motor.name!r}" + ) + if motor.send_id in {arm_motor.send_id for arm_motor in self.motors}: + raise ValueError(f"Damiao arm {self.name!r} gripper duplicates send ID {motor.send_id}") + if motor.effective_recv_id in {arm_motor.effective_recv_id for arm_motor in self.motors}: + raise ValueError( + f"Damiao arm {self.name!r} gripper duplicates receive ID {motor.effective_recv_id}" + ) + @attrs.frozen(slots=False) class DamiaoRuntimeConfig: diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py index 882a94b976..472deabfe1 100644 --- a/dimos/hardware/damiao/runtime.py +++ b/dimos/hardware/damiao/runtime.py @@ -17,6 +17,7 @@ from collections.abc import Callable, Sequence from dataclasses import dataclass import errno +import math from pathlib import Path import time from typing import Any @@ -33,6 +34,7 @@ _ARM_NAME = "arm" _BUS_NAME = "can" +_GRIPPER_NAME = "gripper" _ENOBUFS_RETRY_DELAYS_S = (0.001, 0.002, 0.003) _MIN_RECOMMENDED_TX_QUEUE_LEN = 1_000 _MOTOR_TYPES_BY_NAME = { @@ -102,6 +104,7 @@ def __init__( self._adapter_type = adapter_type self._robot: Any | None = None self._arm: Any | None = None + self._gripper: Any | None = None self._state_cache: DamiaoGroupState | None = None self._state_cache_time = 0.0 self._connected = False @@ -124,6 +127,7 @@ def connect(self) -> bool: ) self._robot = robot self._arm = arm + self._gripper = robot[_GRIPPER_NAME] if self._arm_config.gripper is not None else None self._connected = True self.refresh_state(force=True) except Exception: @@ -156,12 +160,27 @@ def _build_robot(self) -> Any: ) for motor in self._arm_config.motors ] - return ( + builder = ( can_motor_control.Robot.builder() .add_bus(_BUS_NAME, transport, damiao.DamiaoCodec()) .add_arm(_ARM_NAME, bus=_BUS_NAME, motors=motors) - .build() ) + if self._arm_config.gripper is not None: + gripper = self._arm_config.gripper + motor = gripper.motor + builder = builder.add_gripper( + _GRIPPER_NAME, + bus=_BUS_NAME, + motor=can_motor_control.MotorSpec( + motor.name, + self._resolve_motor_type(motor.type), + motor.send_id, + motor.effective_recv_id, + ), + opening_direction=gripper.opening_direction, + default_current=gripper.default_current, + ) + return builder.build() def _warn_if_small_tx_queue(self, address: str) -> None: if self._runtime_config.use_mock_bus: @@ -203,6 +222,7 @@ def disconnect(self) -> None: self._connected = False self._robot = None self._arm = None + self._gripper = None self._state_cache = None self._state_cache_time = 0.0 @@ -217,18 +237,22 @@ def enable(self) -> bool: self._robot.tick(self._runtime_config.tick_deadline_us) self._robot.enable() self._robot.tick(self._runtime_config.tick_deadline_us) + if self._gripper is not None: + self._validated_gripper_opening() except Exception: logger.exception("damiao runtime enable failed", adapter=self._adapter_type) try: disabled = self._robot.disable() except Exception: logger.warning("damiao runtime rollback disable failed", exc_info=True) - disabled = False - if disabled is not True: logger.error("damiao runtime partial enable could not disable hardware") self._enabled = True - else: - self._enabled = False + return False + if disabled is False: + logger.error("damiao runtime partial enable could not disable hardware") + self._enabled = True + return False + self._enabled = False return False self._enabled = True return True @@ -247,6 +271,59 @@ def disable(self) -> bool: def is_enabled(self) -> bool: return self._enabled + def _validated_gripper_opening(self) -> float: + if self._gripper is None: + raise RuntimeError("DamiaoArmRuntime has no configured gripper") + try: + opening = float(self._gripper.opening) + except AttributeError as exc: + raise RuntimeError( + "can_motor_control Gripper.opening is required for calibrated readback" + ) from exc + if not math.isfinite(opening) or not 0.0 <= opening <= 1.0: + raise RuntimeError( + f"gripper opening feedback must be finite and in [0, 1], got {opening}" + ) + return opening + + def read_gripper_opening(self) -> float | None: + """Read the calibrated normalized gripper opening.""" + + if self._robot is None or self._gripper is None or not self._enabled: + return None + try: + self._gripper.refresh() + self._robot.tick(self._runtime_config.tick_deadline_us) + return self._validated_gripper_opening() + except Exception: + logger.exception("damiao runtime gripper read failed", adapter=self._adapter_type) + return None + + def write_gripper_opening(self, opening: float) -> bool: + """Command a calibrated normalized gripper opening.""" + + if ( + self._robot is None + or self._gripper is None + or not self._enabled + or not math.isfinite(opening) + or not 0.0 <= opening <= 1.0 + ): + return False + try: + + def send() -> None: + assert self._gripper is not None + assert self._robot is not None + self._gripper.set_opening(opening) + self._robot.tick(self._runtime_config.tick_deadline_us) + + _retry_enobufs(send) + except Exception: + logger.exception("damiao runtime gripper command failed", adapter=self._adapter_type) + return False + return True + def refresh_state(self, *, force: bool = False) -> DamiaoGroupState: if self._robot is None or self._arm is None: raise RuntimeError("DamiaoArmRuntime is not connected") diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py index 6b94503c07..0c5411e2b4 100644 --- a/dimos/hardware/damiao/test_adapters.py +++ b/dimos/hardware/damiao/test_adapters.py @@ -22,6 +22,7 @@ from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter from dimos.hardware.damiao.config import ( DamiaoArmConfig, + DamiaoGripperConfig, DamiaoMotorConfig, DamiaoRuntimeConfig, ) @@ -126,6 +127,26 @@ def test_arm_config_rejects_duplicate_motor_identity_at_construction() -> None: ) +def test_arm_config_rejects_gripper_can_id_collision() -> None: + with pytest.raises(ValueError, match="gripper duplicates send ID"): + _arm_config( + gripper=DamiaoGripperConfig( + motor=DamiaoMotorConfig("gripper", "DM4310", 0x02, 0x18), + opening_direction="decreasing_position", + ) + ) + + +@pytest.mark.parametrize("current", [0.0, 1.1, float("nan")]) +def test_gripper_config_rejects_invalid_default_current(current: float) -> None: + with pytest.raises(ValueError, match="default_current"): + DamiaoGripperConfig( + motor=DamiaoMotorConfig("gripper", "DM4310", 0x08, 0x18), + opening_direction="decreasing_position", + default_current=current, + ) + + @pytest.mark.parametrize( ("changes", "message"), [ @@ -157,6 +178,25 @@ def test_runtime_builds_robot_with_binding_motor_types() -> None: assert len(robot["arm"]) == 2 +def test_runtime_builds_separate_normalized_gripper_group() -> None: + gripper = DamiaoGripperConfig( + motor=DamiaoMotorConfig("gripper", "DM4310", 0x08, 0x18), + opening_direction="decreasing_position", + default_current=0.15, + ) + runtime = DamiaoArmRuntime( + arm_config=_arm_config(gripper=gripper), + runtime_config=DamiaoRuntimeConfig(use_mock_bus=True), + ) + + robot = runtime._build_robot() + + assert robot.group_names() == ["arm", "gripper"] + assert len(robot["arm"]) == 2 + assert robot["gripper"].motor.send_id == 0x08 + assert robot["gripper"].motor.recv_id == 0x18 + + def test_arm_adapter_reports_limits_and_modes() -> None: adapter = DamiaoArmAdapter( arm_config=_arm_config(), @@ -383,6 +423,54 @@ def test_runtime_selects_mit_mode_before_enable(mocker) -> None: ] +def test_runtime_enable_requires_normalized_gripper_readback_and_rolls_back(mocker) -> None: + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(), + ) + robot = mocker.Mock() + gripper = SimpleNamespace() + runtime._robot = robot + runtime._gripper = gripper + + assert runtime.enable() is False + robot.disable.assert_called_once_with() + assert runtime.is_enabled() is False + + +def test_runtime_reads_and_writes_normalized_gripper_opening(mocker) -> None: + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(), + ) + robot = mocker.Mock() + gripper = mocker.Mock() + gripper.opening = 0.4 + runtime._robot = robot + runtime._gripper = gripper + runtime._enabled = True + + assert runtime.read_gripper_opening() == 0.4 + assert runtime.write_gripper_opening(0.75) is True + gripper.refresh.assert_called_once_with() + gripper.set_opening.assert_called_once_with(0.75) + assert robot.tick.call_args_list == [mocker.call(1_000), mocker.call(1_000)] + + +@pytest.mark.parametrize("opening", [-0.01, 1.01, float("nan"), float("inf")]) +def test_runtime_rejects_invalid_normalized_gripper_commands(mocker, opening: float) -> None: + runtime = DamiaoArmRuntime( + arm_config=_arm_config(), + runtime_config=DamiaoRuntimeConfig(), + ) + runtime._robot = mocker.Mock() + runtime._gripper = mocker.Mock() + runtime._enabled = True + + assert runtime.write_gripper_opening(opening) is False + runtime._gripper.set_opening.assert_not_called() + + def test_runtime_preserves_enabled_state_when_partial_enable_rollback_fails() -> None: class _FailingRobot: def set_mode(self, mode: str) -> None: diff --git a/dimos/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py index 31737af15f..d0f2c758e7 100644 --- a/dimos/hardware/manipulators/openyam_damiao/adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/adapter.py @@ -24,6 +24,7 @@ from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter from dimos.hardware.damiao.config import ( DamiaoArmConfig, + DamiaoGripperConfig, DamiaoMotorConfig, DamiaoRuntimeConfig, ) @@ -33,7 +34,6 @@ logger = setup_logger() -OPENING_METRES = 0.096 _OPENYAM_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper.urdf.xacro" _OPENYAM_PACKAGE_PATHS = {"yam_description": Path(LfsPath("yam_description"))} @@ -45,21 +45,12 @@ ) for index in range(1, 7) ) -GRIPPER_MOTOR_CONFIGS = (DamiaoMotorConfig("yam_gripper", "DM4310", 7),) - - -def aperture_to_opening(aperture: float) -> float: - """Convert a metre aperture to the driver's normalized opening.""" - if not 0.0 <= aperture <= OPENING_METRES: - raise ValueError(f"gripper aperture must be in [0, {OPENING_METRES}] m") - return aperture / OPENING_METRES - - -def opening_to_aperture(opening: float) -> float: - """Convert a calibrated normalized opening to a metre aperture.""" - if not 0.0 <= opening <= 1.0: - raise ValueError("gripper opening must be in [0, 1]") - return opening * OPENING_METRES +GRIPPER_MOTOR_CONFIG = DamiaoMotorConfig("yam_gripper", "DM4310", 0x08, 0x18) +OPENYAM_GRIPPER_CONFIG = DamiaoGripperConfig( + motor=GRIPPER_MOTOR_CONFIG, + opening_direction="decreasing_position", + default_current=0.15, +) def _active_arm_limits() -> tuple[tuple[float, ...], tuple[float, ...], tuple[float, ...]]: @@ -107,11 +98,12 @@ def make_openyam_damiao_arm_config() -> DamiaoArmConfig: velocity_max=velocity, kp=(80.0, 80.0, 80.0, 10.0, 10.0, 10.0), kd=(5.0, 5.0, 5.0, 1.5, 1.5, 1.5), + gripper=OPENYAM_GRIPPER_CONFIG, ) class OpenYamDamiaoAdapter(DamiaoArmAdapter): - """Six-DOF OpenYAM arm; physical gripper IO is fail-closed.""" + """Six-DOF OpenYAM arm with calibrated normalized gripper IO.""" def __init__( self, @@ -163,13 +155,16 @@ def write_mit_commands( return super().write_mit_commands(q=q, dq=dq, kp=kp, kd=kd, tau=tau) def read_gripper_position(self) -> float | None: - """Gripper feedback is disabled until the binding provides calibration.""" - return None + """Read normalized gripper opening, where zero is closed and one is open.""" + if self._runtime is None: + return None + return self._runtime.read_gripper_opening() def write_gripper_position(self, position: float) -> bool: - """Reject physical gripper commands without calibrated feedback.""" - del position - return False + """Command normalized gripper opening, where zero is closed and one is open.""" + if self._runtime is None: + return False + return self._runtime.write_gripper_opening(position) OpenYAMDamiaoAdapter = OpenYamDamiaoAdapter diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py index b3adc803e4..8f4fda8235 100644 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py @@ -31,11 +31,10 @@ import dimos.hardware.manipulators.openyam_damiao.adapter as adapter_module from dimos.hardware.manipulators.openyam_damiao.adapter import ( ARM_MOTOR_CONFIGS, - GRIPPER_MOTOR_CONFIGS, - OPENING_METRES, + GRIPPER_MOTOR_CONFIG, + OPENYAM_GRIPPER_CONFIG, OpenYamDamiaoAdapter, - aperture_to_opening, - opening_to_aperture, + make_openyam_damiao_arm_config, ) from dimos.robot.model_parser import JointDescription, ModelDescription from dimos.utils.data import LfsPath @@ -43,31 +42,34 @@ GRAVITY_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper_gravity.urdf" -def test_gripper_aperture_conversion_is_linear() -> None: - assert aperture_to_opening(0.0) == 0.0 - assert aperture_to_opening(OPENING_METRES / 2) == pytest.approx(0.5) - assert aperture_to_opening(OPENING_METRES) == 1.0 - assert opening_to_aperture(0.5) == pytest.approx(OPENING_METRES / 2) - - def test_openyam_motor_topology() -> None: assert [motor.name for motor in ARM_MOTOR_CONFIGS] == [f"yam_joint{i}" for i in range(1, 7)] assert [motor.send_id for motor in ARM_MOTOR_CONFIGS] == list(range(1, 7)) assert [motor.type for motor in ARM_MOTOR_CONFIGS] == ["DM4340"] * 3 + ["DM4310"] * 3 - assert GRIPPER_MOTOR_CONFIGS[0].send_id == 7 - assert GRIPPER_MOTOR_CONFIGS[0].type == "DM4310" + assert GRIPPER_MOTOR_CONFIG.send_id == 0x08 + assert GRIPPER_MOTOR_CONFIG.effective_recv_id == 0x18 + assert GRIPPER_MOTOR_CONFIG.type == "DM4310" + assert OPENYAM_GRIPPER_CONFIG.opening_direction == "decreasing_position" + assert OPENYAM_GRIPPER_CONFIG.default_current == 0.15 + assert make_openyam_damiao_arm_config().gripper is OPENYAM_GRIPPER_CONFIG -def test_openyam_physical_gripper_is_disabled_without_calibrated_readback() -> None: +def test_openyam_gripper_delegates_normalized_io() -> None: adapter = OpenYamDamiaoAdapter( runtime_config=DamiaoRuntimeConfig( gravity_model_path=GRAVITY_MODEL_PATH, use_mock_bus=True, ) ) + runtime = Mock() + runtime.read_gripper_opening.return_value = 0.4 + runtime.write_gripper_opening.return_value = True + adapter._runtime = runtime - assert adapter.read_gripper_position() is None - assert not adapter.write_gripper_position(0.01) + assert adapter.read_gripper_position() == 0.4 + assert adapter.write_gripper_position(0.75) + runtime.read_gripper_opening.assert_called_once_with() + runtime.write_gripper_opening.assert_called_once_with(0.75) def test_openyam_limits_are_loaded_from_active_model() -> None: @@ -236,9 +238,3 @@ def test_openyam_xacro_limits_reject_bad_values( with pytest.raises(ValueError): adapter_module._active_arm_limits() - - -@pytest.mark.parametrize("value", [-1e-6, OPENING_METRES + 1e-6]) -def test_gripper_aperture_rejects_out_of_range(value: float) -> None: - with pytest.raises(ValueError): - aperture_to_opening(value) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 137d8af72c..ea63e70464 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -1649,6 +1649,18 @@ def _set_gripper_position(self, position: float, robot_name: RobotName | None = return False return self._control_coordinator.set_gripper_position(hw_id, position) + def _set_gripper_endpoint( + self, *, open_position: bool, robot_name: RobotName | None = None + ) -> bool: + """Internal: command the configured open or closed endpoint.""" + + hw_id = self._get_gripper_hardware_id(robot_name) + if hw_id is None: + return False + if open_position: + return self._control_coordinator.open_gripper(hw_id) + return self._control_coordinator.close_gripper(hw_id) + @rpc def get_gripper(self, robot_name: RobotName | None = None) -> float | None: """Get gripper position in meters. @@ -1683,7 +1695,7 @@ def open_gripper(self, robot_name: str | None = None) -> SkillResult[Manipulatio Args: robot_name: Robot to control (only needed for multi-arm setups). """ - if self._set_gripper_position(0.85, robot_name): + if self._set_gripper_endpoint(open_position=True, robot_name=robot_name): return SkillResult.ok("Gripper opened") return SkillResult.fail("GRIPPER_FAILED", "Failed to open gripper") @@ -1694,7 +1706,7 @@ def close_gripper(self, robot_name: str | None = None) -> SkillResult[Manipulati Args: robot_name: Robot to control (only needed for multi-arm setups). """ - if self._set_gripper_position(0.0, robot_name): + if self._set_gripper_endpoint(open_position=False, robot_name=robot_name): return SkillResult.ok("Gripper closed") return SkillResult.fail("GRIPPER_FAILED", "Failed to close gripper") diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 2bcc8a9afc..23a0d2083b 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -65,6 +65,21 @@ def _control_coordinator( return coordinator +def test_gripper_endpoint_skills_delegate_to_coordinator( + module_factory, mocker: MockerFixture +) -> None: + module = module_factory() + coordinator = module._control_coordinator + coordinator.open_gripper.return_value = True + coordinator.close_gripper.return_value = True + mocker.patch.object(module, "_get_gripper_hardware_id", return_value="arm") + + assert module.open_gripper().success is True + assert module.close_gripper().success is True + coordinator.open_gripper.assert_called_once_with("arm") + coordinator.close_gripper.assert_called_once_with("arm") + + @pytest.fixture def robot_config(): """Create a robot config for testing.""" diff --git a/dimos/robot/manipulators/openyam/blueprints/teleop.py b/dimos/robot/manipulators/openyam/blueprints/teleop.py index a31255d5de..f33b8eb1e0 100644 --- a/dimos/robot/manipulators/openyam/blueprints/teleop.py +++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py @@ -16,7 +16,7 @@ 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 ( @@ -35,6 +35,17 @@ _openyam_keyboard_hw = openyam_hardware("arm") + +def _gripper_task() -> TaskConfig: + return TaskConfig( + name="servo_gripper", + type="servo", + joint_names=["arm/gripper"], + priority=20, + params={"timeout": 0.0, "default_positions": [0.0]}, + ) + + keyboard_teleop_openyam = autoconnect( KeyboardTeleopModule.blueprint(), ControlCoordinator.blueprint( @@ -44,7 +55,8 @@ _openyam_keyboard_hw, model_path=OPENYAM_GRAVITY_MODEL_PATH, ee_joint_id=OPENYAM_DOF, - ) + ), + _gripper_task(), ], ), ManipulationModule.blueprint( @@ -67,6 +79,7 @@ ee_joint_id=OPENYAM_DOF, priority=10, ), + _gripper_task(), trajectory_task(_openyam_keyboard_planner_hw, priority=20), ], ), diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index 42c915d91c..45ae4f6a58 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -61,6 +61,8 @@ def make_openyam_hardware( address=address, auto_enable=auto_enable, gripper_joints=[f"{hw_id}/gripper"] if include_gripper else [], + gripper_open_position=1.0 if include_gripper else None, + gripper_closed_position=0.0 if include_gripper else None, adapter_kwargs=kwargs, ) @@ -87,7 +89,6 @@ def openyam_hardware( gravity_comp=True, ), }, - include_gripper=False, ) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index baf133d40f..a553442d50 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -66,6 +66,8 @@ def test_openyam_mock_hardware_has_gripper() -> None: 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"] + assert hardware.gripper_open_position == 1.0 + assert hardware.gripper_closed_position == 0.0 def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: Any) -> None: @@ -81,7 +83,9 @@ def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: A assert runtime_config.gravity_model_path == OPENYAM_GRAVITY_MODEL_PATH assert runtime_config.gravity_comp is True assert len(hardware.joints) == OPENYAM_DOF - assert hardware.gripper_joints == [] + assert hardware.gripper_joints == ["arm/gripper"] + assert hardware.gripper_open_position == 1.0 + assert hardware.gripper_closed_position == 0.0 assert "initial_positions" not in hardware.adapter_kwargs direct = make_openyam_hardware( @@ -136,6 +140,7 @@ def test_openyam_keyboard_planner_blueprint_combines_teleop_and_trajectory() -> tasks = _coordinator_kwargs(blueprint)["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") + gripper = next(task for task in tasks if task.name == "servo_gripper") assert trajectory.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] assert trajectory.priority == 20 @@ -143,6 +148,9 @@ def test_openyam_keyboard_planner_blueprint_combines_teleop_and_trajectory() -> assert eef_twist.params["ee_joint_id"] == OPENYAM_DOF assert eef_twist.params["model_path"] == OPENYAM_GRAVITY_MODEL_PATH assert eef_twist.priority == 10 + assert gripper.type == "servo" + assert gripper.joint_names == ["arm/gripper"] + assert gripper.params == {"timeout": 0.0, "default_positions": [0.0]} assert _module_kwargs(blueprint, KeyboardTeleopModule) == {} @@ -156,11 +164,13 @@ def test_openyam_coordinator_blueprint_uses_six_arm_joints() -> None: def test_openyam_teleop_blueprint_constructs_with_eef_twist() -> None: blueprint = keyboard_teleop_openyam - task = next( - task for task in _coordinator_kwargs(blueprint)["tasks"] if task.type == "eef_twist" - ) + tasks = _coordinator_kwargs(blueprint)["tasks"] + task = next(task for task in tasks if task.type == "eef_twist") + gripper = next(task for task in tasks if task.name == "servo_gripper") assert task.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] assert task.params["ee_joint_id"] == OPENYAM_DOF assert task.params["model_path"] == OPENYAM_GRAVITY_MODEL_PATH + assert gripper.joint_names == ["arm/gripper"] + assert gripper.params["default_positions"] == [0.0] assert _module_kwargs(blueprint, ManipulationModule)["visualization"] == {"backend": "viser"} diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 33830780ef..34f3b2264f 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -20,10 +20,21 @@ 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-piper # Piper 6-DOF +dimos run keyboard-teleop-openyam # OpenYAM 6-DOF + normalized gripper dimos run keyboard-teleop-xarm6 # XArm6 6-DOF dimos run keyboard-teleop-xarm7 # XArm7 7-DOF ``` +OpenYAM's physical blueprint includes its DM4310 gripper by default. Gripper +commands and feedback are normalized: `0.0` is fully closed and `1.0` is fully +open. Enabling the hardware automatically calibrates both mechanical endpoints, +so clear the jaws and workspace before startup. The hardware profile uses CAN +IDs `0x08`/`0x18`, decreasing motor position as the opening direction, and +`0.15` per-unit calibration current. Calibration failure prevents the combined +arm and gripper runtime from enabling. The installed `can-motor-control` build +must expose calibrated `Gripper.opening` feedback; DimOS fails activation rather +than inferring feedback from raw motor angle. + Open the Meshcat URL printed in the terminal (default `http://localhost:7000`) to see the robot. Keyboard controls: From a6b14beeadf87f92e04166a9d9ef0c4edd09b325 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 23:50:50 +0000 Subject: [PATCH 24/44] fix: open gripper api --- pyproject.toml | 2 +- uv.lock | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 663afb6853..4d31f51fba 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -279,7 +279,7 @@ manipulation = [ "xarm-python-sdk>=1.17.0", "a750-control; sys_platform == 'linux' and platform_machine == 'x86_64'", - "can-motor-control>=0.0.4; sys_platform == 'linux'", + "can-motor-control>=0.0.5; sys_platform == 'linux'", # Mesh conversion (STL/DAE → OBJ for Drake collision geometry) "trimesh", diff --git a/uv.lock b/uv.lock index 67324ea49d..a724ffcf5d 100644 --- a/uv.lock +++ b/uv.lock @@ -599,15 +599,15 @@ wheels = [ [[package]] name = "can-motor-control" -version = "0.0.4" +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/e1/f5/857df85f5d612be90ab852a74a604d865708aea6279d9277de13f2f56782/can_motor_control-0.0.4.tar.gz", hash = "sha256:615c16eab5e1fb010623765431e2fd24b743bfa9bea66e94b3f390db4b4f6ab1", size = 128133, upload-time = "2026-07-23T16:43:08.702Z" } +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/6c/8e/dc4652bccc1b0e4b39c87c73cb189beddd5a3a91fb95d4c4431a17e7b339/can_motor_control-0.0.4-cp310-abi3-manylinux_2_34_x86_64.whl", hash = "sha256:ad63ead5a82bd9d71c68ed74b21c8b0b068ed35504eb472f5a93b5acf2277b50", size = 527595, upload-time = "2026-07-23T16:43:07.302Z" }, + { 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]] @@ -2051,7 +2051,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.4" }, + { 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" }, From 15f59194dae510fd78fcd6914ac718f64bf77def Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 21:14:46 -0700 Subject: [PATCH 25/44] refactor: model OpenYAM as whole-body hardware --- CONTEXT.md | 33 ++ dimos/cli/dimos.py | 2 - dimos/cli/piper.py | 37 -- dimos/cli/test_piper.py | 61 --- dimos/control/coordinator.py | 31 -- dimos/control/hardware_interface.py | 24 +- dimos/control/test_control.py | 51 -- dimos/hardware/damiao/arm_adapter.py | 482 ----------------- dimos/hardware/damiao/config.py | 272 ---------- dimos/hardware/damiao/runtime.py | 390 -------------- dimos/hardware/damiao/test_adapters.py | 495 ------------------ .../manipulators/openyam_damiao/_registry.py | 8 - .../manipulators/openyam_damiao/adapter.py | 170 ------ .../openyam_damiao/test_adapter.py | 240 --------- dimos/hardware/test_adapter_registries.py | 14 +- dimos/hardware/whole_body/damiao/adapter.py | 371 +++++++++++++ dimos/hardware/whole_body/damiao/config.py | 47 ++ .../whole_body/damiao/test_adapter.py | 280 ++++++++++ dimos/hardware/whole_body/mock/_registry.py | 17 + dimos/hardware/whole_body/mock/adapter.py | 76 +++ .../hardware/whole_body/mock/test_adapter.py | 42 ++ .../whole_body/openyam_damiao/_registry.py | 17 + .../whole_body/openyam_damiao/adapter.py | 71 +++ .../whole_body/openyam_damiao/test_adapter.py | 46 ++ dimos/hardware/whole_body/spec.py | 18 +- dimos/manipulation/manipulation_module.py | 16 +- dimos/manipulation/test_manipulation_unit.py | 15 - .../manipulators/openyam/blueprints/basic.py | 25 +- .../manipulators/openyam/blueprints/teleop.py | 54 +- dimos/robot/manipulators/openyam/config.py | 82 +-- .../manipulators/openyam/test_openyam.py | 154 +++--- ...t-can-motor-control-own-damiao-topology.md | 5 + ...an-motor-control-as-whole-body-hardware.md | 21 + docs/capabilities/manipulation/index.md | 17 +- .../manipulation/piper_integration.md | 10 +- pyproject.toml | 2 + uv.lock | 10 +- 37 files changed, 1231 insertions(+), 2475 deletions(-) create mode 100644 CONTEXT.md delete mode 100644 dimos/cli/piper.py delete mode 100644 dimos/cli/test_piper.py delete mode 100644 dimos/hardware/damiao/arm_adapter.py delete mode 100644 dimos/hardware/damiao/config.py delete mode 100644 dimos/hardware/damiao/runtime.py delete mode 100644 dimos/hardware/damiao/test_adapters.py delete mode 100644 dimos/hardware/manipulators/openyam_damiao/_registry.py delete mode 100644 dimos/hardware/manipulators/openyam_damiao/adapter.py delete mode 100644 dimos/hardware/manipulators/openyam_damiao/test_adapter.py create mode 100644 dimos/hardware/whole_body/damiao/adapter.py create mode 100644 dimos/hardware/whole_body/damiao/config.py create mode 100644 dimos/hardware/whole_body/damiao/test_adapter.py create mode 100644 dimos/hardware/whole_body/mock/_registry.py create mode 100644 dimos/hardware/whole_body/mock/adapter.py create mode 100644 dimos/hardware/whole_body/mock/test_adapter.py create mode 100644 dimos/hardware/whole_body/openyam_damiao/_registry.py create mode 100644 dimos/hardware/whole_body/openyam_damiao/adapter.py create mode 100644 dimos/hardware/whole_body/openyam_damiao/test_adapter.py create mode 100644 docs/adr/0001-let-can-motor-control-own-damiao-topology.md create mode 100644 docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..6fa1476b98 --- /dev/null +++ b/CONTEXT.md @@ -0,0 +1,33 @@ +# DimOS Robotics + +Language for robot hardware capabilities and their representation across DimOS. + +## Language + +**Gripper opening**: +A calibrated normalized gripper aperture where `0.0` is fully closed and `1.0` is fully open, used consistently for commands and feedback. +_Avoid_: Gripper position, gripper angle, gripper distance + +**Hardware topology**: +The inherent arrangement and identity of a robot's buses, actuator groups, and capabilities. It defines what kind of robot something is and does not vary between runs. +_Avoid_: Runtime configuration, deployment configuration + +**Runtime configuration**: +Deployment and control-policy values that may vary between runs without changing the robot's hardware topology. +_Avoid_: Hardware topology, robot definition + +**Whole-body hardware**: +A physical robot controlled as one ordered set of named joints, with tasks selecting arm, gripper, or other subsets by joint name. +_Avoid_: Manipulator collection, adapter bundle + +**Residual torque**: +Task-requested joint torque added above the gravity compensation computed from the robot model. Zero residual torque requests gravity support without additional task effort. +_Avoid_: Raw motor torque, total torque + +**Hardware identifier**: +The name of the physical owner of connection, lifecycle, state reads, and command writes. +_Avoid_: Arm name, joint prefix + +**Joint namespace**: +The logical prefix that groups joints for planning and task ownership independently of which physical hardware owns them. +_Avoid_: Hardware identifier diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index d46240a96b..46724fb2e2 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -55,7 +55,6 @@ 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.piper import app as piper_app from dimos.cli.shell import shell from dimos.constants import CONFIG_DIR, LOG_DIR from dimos.core.daemon import daemonize, install_signal_handlers @@ -179,7 +178,6 @@ def callback(**kwargs) -> None: # type: ignore[no-untyped-def] main.callback()(create_dynamic_callback()) # type: ignore[no-untyped-call] main.add_typer(can_app, name="can") 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/piper.py b/dimos/cli/piper.py deleted file mode 100644 index 55cbaa4b62..0000000000 --- a/dimos/cli/piper.py +++ /dev/null @@ -1,37 +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 typer - -from dimos.cli.can import setup_interface - -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", min=1, 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) - - setup_interface(interface, bitrate=bitrate) diff --git a/dimos/cli/test_piper.py b/dimos/cli/test_piper.py deleted file mode 100644 index 76b48fd582..0000000000 --- a/dimos/cli/test_piper.py +++ /dev/null @@ -1,61 +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 typer.testing import CliRunner - -from dimos.cli.dimos import main -import dimos.cli.piper as piper - -runner = CliRunner() - - -def test_can_activate_confirms_before_generic_setup(mocker) -> None: - confirm = mocker.patch.object(piper.typer, "confirm", return_value=True) - setup_interface = mocker.patch.object(piper, "setup_interface") - - result = runner.invoke(main, ["piper", "can-activate", "can1", "--bitrate", "500000"]) - - assert result.exit_code == 0, result.output - confirm.assert_called_once() - setup_interface.assert_called_once_with("can1", bitrate=500000) - - -def test_can_activate_rejection_does_not_configure_interface(mocker) -> None: - mocker.patch.object(piper.typer, "confirm", return_value=False) - setup_interface = mocker.patch.object(piper, "setup_interface") - - result = runner.invoke(main, ["piper", "can-activate", "can0"]) - - assert result.exit_code == 1 - assert "Aborted." in result.output - setup_interface.assert_not_called() - - -def test_can_activate_uses_default_bitrate(mocker) -> None: - mocker.patch.object(piper.typer, "confirm", return_value=True) - setup_interface = mocker.patch.object(piper, "setup_interface") - - result = runner.invoke(main, ["piper", "can-activate", "can0"]) - - assert result.exit_code == 0, result.output - setup_interface.assert_called_once_with("can0", bitrate=1_000_000) - - -def test_can_activate_rejects_nonpositive_bitrate_before_confirmation(mocker) -> None: - confirm = mocker.patch.object(piper.typer, "confirm") - - result = runner.invoke(main, ["piper", "can-activate", "can0", "--bitrate", "0"]) - - assert result.exit_code == 2 - confirm.assert_not_called() diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 51b4237acf..eeed8dead0 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -915,37 +915,6 @@ def set_gripper_position(self, hardware_id: str, position: float) -> bool: return False return hw.adapter.write_gripper_position(position) - @rpc - def open_gripper(self, hardware_id: str) -> bool: - """Open a gripper to its configured adapter-native endpoint.""" - - return self._set_gripper_endpoint(hardware_id, open_position=True) - - @rpc - def close_gripper(self, hardware_id: str) -> bool: - """Close a gripper to its configured adapter-native endpoint.""" - - return self._set_gripper_endpoint(hardware_id, open_position=False) - - def _set_gripper_endpoint(self, hardware_id: str, *, open_position: bool) -> bool: - with self._hardware_lock: - hw = self._hardware.get(hardware_id) - if hw is None: - logger.warning(f"Hardware '{hardware_id}' not found for gripper command") - return False - if isinstance(hw, ConnectedTwistBase): - logger.warning(f"Hardware '{hardware_id}' is a twist base, no gripper support") - return False - configured = ( - hw.component.gripper_open_position - if open_position - else hw.component.gripper_closed_position - ) - fallback = 0.85 if open_position else 0.0 - return hw.adapter.write_gripper_position( - configured if configured is not None else fallback - ) - @rpc def get_gripper_position(self, hardware_id: str) -> float | None: """Get gripper position from a specific hardware device. diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index 178db772bb..3a7c74f430 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -125,13 +125,14 @@ def read_state(self) -> dict[JointName, JointState]: # Append gripper joint(s) via adapter gripper method if self._gripper_joints: gripper_pos = self._adapter.read_gripper_position() - if gripper_pos is not None: - for gj in self._gripper_joints: - result[gj] = JointState( - position=self._physical_to_normalized(gripper_pos), - velocity=0.0, - effort=0.0, - ) + for gj in self._gripper_joints: + result[gj] = JointState( + position=self._physical_to_normalized(gripper_pos) + if gripper_pos is not None + else 0.0, + velocity=0.0, + effort=0.0, + ) return result @@ -212,9 +213,12 @@ def _initialize_last_commanded(self) -> None: # Initialize gripper joint(s) from adapter if self._gripper_joints: gripper_pos = self._adapter.read_gripper_position() - if gripper_pos is not None: - for gj in self._gripper_joints: - self._last_commanded[gj] = self._physical_to_normalized(gripper_pos) + for gj in self._gripper_joints: + self._last_commanded[gj] = ( + self._physical_to_normalized(gripper_pos) + if gripper_pos is not None + else 0.0 + ) self._initialized = True return diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index b8dcd1f80e..61ac235b03 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -206,22 +206,6 @@ def test_normalized_gripper_commands_are_mapped_at_hardware_boundary(self, mock_ ((0.07,), {}), ] - def test_missing_gripper_feedback_is_not_fabricated_or_commanded(self, mock_adapter): - mock_adapter.read_gripper_position.return_value = None - component = HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - gripper_joints=["arm/gripper"], - gripper_open_position=1.0, - gripper_closed_position=0.0, - ) - hardware = ConnectedHardware(mock_adapter, component) - - assert "arm/gripper" not in hardware.read_state() - assert hardware.write_command({"arm/joint1": 0.1}, ControlMode.POSITION) - mock_adapter.write_gripper_position.assert_not_called() - def test_joint_names_prefixed(self, connected_hardware): names = connected_hardware.joint_names assert names == [ @@ -269,41 +253,6 @@ def make(**kwargs: Any) -> ControlCoordinator: class TestControlCoordinatorLifecycle: - @pytest.mark.parametrize( - ("open_endpoint", "closed_endpoint", "expected"), - [ - (1.0, 0.0, [1.0, 0.0]), - (0.07, 0.0, [0.07, 0.0]), - (0.85, 0.0, [0.85, 0.0]), - (None, None, [0.85, 0.0]), - ], - ) - def test_gripper_open_close_use_configured_endpoints( - self, - make_coordinator, - mock_adapter, - open_endpoint: float | None, - closed_endpoint: float | None, - expected: list[float], - ) -> None: - component = HardwareComponent( - hardware_id="arm", - hardware_type=HardwareType.MANIPULATOR, - joints=make_joints("arm", 6), - gripper_joints=["arm/gripper"], - gripper_open_position=open_endpoint, - gripper_closed_position=closed_endpoint, - ) - coordinator = make_coordinator() - coordinator._hardware = {"arm": ConnectedHardware(mock_adapter, component)} - mock_adapter.write_gripper_position.return_value = True - - assert coordinator.open_gripper("arm") is True - assert coordinator.close_gripper("arm") is True - assert [ - call.args[0] for call in mock_adapter.write_gripper_position.call_args_list - ] == expected - def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator): coordinator = make_coordinator() matching_task = RecordingTask("eef") diff --git a/dimos/hardware/damiao/arm_adapter.py b/dimos/hardware/damiao/arm_adapter.py deleted file mode 100644 index 67366b51be..0000000000 --- a/dimos/hardware/damiao/arm_adapter.py +++ /dev/null @@ -1,482 +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. - -from __future__ import annotations - -from typing import Any - -import numpy as np -import pinocchio # type: ignore[import-not-found] - -from dimos.hardware.damiao.config import DamiaoArmConfig, DamiaoRuntimeConfig -from dimos.hardware.damiao.runtime import DamiaoArmRuntime -from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -_CONTROL_MODE_INDEX = {mode: index for index, mode in enumerate(ControlMode)} - - -class DamiaoArmAdapter: - """ManipulatorAdapter facade over one Damiao arm runtime.""" - - _adapter_type: str = "damiao" - - def __init__( - self, - *, - arm_config: DamiaoArmConfig, - runtime_config: DamiaoRuntimeConfig | None = None, - dof: int | None = None, - hardware_id: str = "arm", - ) -> None: - runtime_config = runtime_config or DamiaoRuntimeConfig() - if dof is not None and dof != arm_config.dof: - raise ValueError( - f"{type(self).__name__} only supports {arm_config.dof} DOF (got {dof})" - ) - self._arm_config = arm_config - self._runtime_config = runtime_config - self._hardware_id = hardware_id - self._dof = arm_config.dof - self._position_lower = list(arm_config.position_lower) - self._position_upper = list(arm_config.position_upper) - self._velocity_max = list(arm_config.velocity_max) - self._kp = list(runtime_config.kp_override or arm_config.kp) - self._kd = list(runtime_config.kd_override or arm_config.kd) - self._validate_length("kp", self._kp) - self._validate_length("kd", self._kd) - self._gravity_comp = runtime_config.gravity_comp - self._gravity_model_path = ( - str(runtime_config.gravity_model_path) if runtime_config.gravity_model_path else None - ) - resolved_torque_limits = arm_config.gravity_torque_limits - self._gravity_torque_limits = ( - list(resolved_torque_limits) if resolved_torque_limits else None - ) - if self._gravity_torque_limits is not None: - self._validate_length("gravity_torque_limits", self._gravity_torque_limits) - self._supported_control_modes = arm_config.supported_control_modes - self._control_mode = ControlMode.POSITION - self._last_positions: list[float] | None = None - self._pin_model: Any | None = None - self._pin_data: Any | None = None - self._runtime: DamiaoArmRuntime | None = None - self._connected = False - self._enabled = False - - def _create_runtime(self) -> DamiaoArmRuntime: - return DamiaoArmRuntime( - arm_config=self._arm_config, - runtime_config=self._runtime_config, - adapter_type=self._adapter_type, - ) - - def _validate_length(self, name: str, values: list[float]) -> None: - if len(values) != self._dof: - raise ValueError(f"{name} length {len(values)} does not match dof {self._dof}") - - def _validate_command_lengths(self, **commands: list[float]) -> None: - for name, values in commands.items(): - self._validate_length(name, values) - - def _zero_vector(self) -> list[float]: - return [0.0] * self._dof - - def connect(self) -> bool: - try: - runtime = self._create_runtime() - if not runtime.connect(): - return False - self._runtime = runtime - self._load_gravity_model() - self._connected = True - self.refresh_state(force=True) - except Exception: - logger.exception( - "damiao arm adapter connect failed", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - ) - self.disconnect() - return False - return True - - def disconnect(self) -> None: - if self._runtime is not None: - self._runtime.disconnect() - self._enabled = self._runtime.is_enabled() - self._runtime = None - self._connected = False - - def is_connected(self) -> bool: - return self._connected - - def activate(self) -> bool: - return self.write_enable(True) - - def deactivate(self) -> bool: - stopped = self.write_stop() - disabled = self.write_enable(False) - return stopped and disabled - - def get_info(self) -> ManipulatorInfo: - return ManipulatorInfo( - vendor=self._arm_config.vendor, - model=self._arm_config.model, - dof=self._dof, - firmware_version=None, - serial_number=None, - ) - - def get_dof(self) -> int: - return self._dof - - def get_limits(self) -> JointLimits: - return JointLimits( - position_lower=list(self._position_lower), - position_upper=list(self._position_upper), - velocity_max=list(self._velocity_max), - ) - - def set_control_mode(self, mode: ControlMode) -> bool: - if mode not in self._supported_control_modes: - return False - self._control_mode = mode - return True - - def get_control_mode(self) -> ControlMode: - return self._control_mode - - def read_enabled(self) -> bool: - return self._enabled - - def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float], list[float]]: - if self._runtime is None: - raise RuntimeError(f"{type(self).__name__} is not connected") - state = self._runtime.refresh_state(force=force) - self._last_positions = list(state.q) - return list(state.q), list(state.dq), list(state.tau) - - def read_joint_positions(self) -> list[float]: - return list(self.refresh_state()[0]) - - def read_joint_velocities(self) -> list[float]: - return list(self.refresh_state()[1]) - - def read_joint_efforts(self) -> list[float]: - return list(self.refresh_state()[2]) - - def read_state(self) -> dict[str, int]: - return {"state": 1 if self._enabled else 0, "mode": _CONTROL_MODE_INDEX[self._control_mode]} - - def read_error(self) -> tuple[int, str]: - return 0, "" - - def read_cartesian_position(self) -> dict[str, float] | None: - return None - - def write_cartesian_position(self, pose: dict[str, float], velocity: float = 1.0) -> bool: - return False - - def read_gripper_position(self) -> float | None: - return None - - def write_gripper_position(self, position: float) -> bool: - return False - - def read_force_torque(self) -> list[float] | None: - return None - - def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: - if self._runtime is None or not self._enabled or len(positions) != self._dof: - return False - velocity = max(0.0, min(1.0, velocity)) - if self._gravity_comp: - try: - tau = self.compute_gravity_torques(self.read_joint_positions()) - except Exception: - logger.warning( - "damiao arm adapter gravity command safety failure", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - exc_info=True, - ) - return self._disable_after_safety_failure() - else: - tau = self._zero_vector() - return self.write_mit_commands( - q=list(positions), - dq=self._zero_vector(), - kp=[kp * velocity for kp in self._kp], - kd=list(self._kd), - tau=tau, - ) - - def write_joint_velocities(self, velocities: list[float]) -> bool: - return False - - def write_joint_torques(self, efforts: list[float]) -> bool: - if self._runtime is None or not self._enabled or len(efforts) != self._dof: - return False - try: - q = ( - self._last_positions - if self._last_positions is not None - else self.read_joint_positions() - ) - except Exception: - if self._gravity_comp: - return self._disable_after_safety_failure() - raise - return self.write_mit_commands( - q=q, - dq=self._zero_vector(), - kp=self._zero_vector(), - kd=self._zero_vector(), - tau=efforts, - ) - - def write_mit_commands( - self, - *, - q: list[float], - dq: list[float], - kp: list[float], - kd: list[float], - tau: list[float], - ) -> bool: - if self._runtime is None or not self._enabled: - return False - self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - ok = self._runtime.write_mit_commands( - q=q, - dq=dq, - kp=kp, - kd=kd, - tau=tau, - ) - if ok: - self._last_positions = list(q) - self._control_mode = ( - ControlMode.TORQUE if all(k == 0.0 for k in kp) else ControlMode.POSITION - ) - return ok - - def write_stop(self) -> bool: - if self._runtime is None: - return False - if self._gravity_comp and self._enabled: - try: - q_now = self.read_joint_positions() - except Exception: - logger.warning( - "damiao arm adapter gravity stop safety failure", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - exc_info=True, - ) - return self._disable_after_safety_failure() - try: - tau = self.compute_gravity_torques(q_now) - except Exception: - logger.warning( - "damiao arm adapter gravity stop safety failure", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - exc_info=True, - ) - return self._disable_after_safety_failure() - return self.write_mit_commands( - q=q_now, - dq=self._zero_vector(), - kp=list(self._kp), - kd=list(self._kd), - tau=tau, - ) - disabled = self._runtime.disable() - if disabled: - self._enabled = False - return disabled - - def write_enable(self, enable: bool) -> bool: - if self._runtime is None: - return False - if not enable: - ok = self._runtime.disable() - if ok: - self._enabled = False - else: - self._enabled = True - return ok - - # Do every model/state check while the motors are still disabled. - # This is deliberately kept in the generic adapter rather than in a - # robot-specific implementation: a bad URDF must not result in a - # live actuator state. - try: - self._preflight_gravity() - except Exception: - logger.exception( - "damiao arm adapter rejected enable preflight", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - ) - return False - - ok = self._runtime.enable() - if not ok: - return False - self._enabled = enable - try: - positions = self.read_joint_positions() - if not self.write_joint_positions(positions): - self._rollback_enable() - return False - except Exception: - logger.exception( - "damiao arm adapter enable hold failed", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - ) - self._rollback_enable() - return False - return True - - def _rollback_enable(self) -> None: - """Disable after a failure occurring after runtime enable.""" - - if self._runtime is not None: - try: - disabled = self._runtime.disable() - except Exception: - logger.warning("damiao arm adapter enable rollback failed", exc_info=True) - disabled = False - if disabled: - self._enabled = False - return - logger.error("damiao arm adapter enable rollback could not disable hardware") - self._enabled = True - - def _disable_after_safety_failure(self) -> bool: - """Disable without sending a fallback (possibly zero-torque) command.""" - - if self._runtime is not None: - try: - disabled = self._runtime.disable() - except Exception: - logger.exception( - "damiao arm adapter safety disable failed", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - ) - disabled = False - if not disabled: - logger.error("damiao arm adapter safety disable could not disable hardware") - self._enabled = True - return False - self._enabled = False - return False - - def _preflight_gravity(self) -> None: - """Validate state and gravity output before enabling any motor. - - Pinocchio models expose their joint order and generalized dimensions; - checking both here prevents silently applying a valid-looking torque - vector to the wrong joints. - """ - - if self._gravity_comp and (self._pin_model is None or self._pin_data is None): - raise ValueError("gravity compensation requires a loaded gravity model") - - q, _, _ = self.refresh_state(force=True) - if len(q) != self._dof or not np.isfinite(np.asarray(q, dtype=np.float64)).all(): - raise ValueError( - "gravity preflight requires finite positions in configured joint order" - ) - - if self._pin_model is not None: - nq = self._pin_model.nq - nv = self._pin_model.nv - if nq != self._dof or nv != self._dof: - raise ValueError( - f"gravity model dimensions ({nq}, {nv}) do not match adapter DOF {self._dof}" - ) - model_names = tuple(str(name) for name in self._pin_model.names) - if model_names and model_names[0] == "universe": - model_names = model_names[1:] - if model_names != self._arm_config.joint_names: - raise ValueError( - f"gravity model joint order {model_names!r} does not match " - f"configured order {self._arm_config.joint_names!r}" - ) - - tau = self.compute_gravity_torques(q) if self._gravity_comp else self._zero_vector() - if len(tau) != self._dof or not np.isfinite(np.asarray(tau, dtype=np.float64)).all(): - raise ValueError("gravity preflight requires finite torque values matching adapter DOF") - if self._gravity_torque_limits is not None and any( - not np.isfinite(limit) or limit < 0.0 for limit in self._gravity_torque_limits - ): - raise ValueError("gravity torque limits must be finite and non-negative") - - def write_clear_errors(self) -> bool: - if self._runtime is None: - return False - if not self._runtime.disable(): - self._enabled = True - return False - self._enabled = False - try: - self._preflight_gravity() - except Exception: - logger.exception( - "damiao arm adapter rejected error-recovery enable preflight", - adapter=type(self).__name__, - hardware_id=self._hardware_id, - ) - return False - if not self._runtime.enable(): - return False - self._enabled = True - try: - ok = self.write_joint_positions(self.read_joint_positions()) - except Exception: - self._rollback_enable() - return False - if not ok: - self._rollback_enable() - return ok - - def _load_gravity_model(self) -> None: - if not self._gravity_comp or self._gravity_model_path is None or self._runtime is None: - return - self._pin_model, self._pin_data = self._runtime.load_gravity_model(self._gravity_model_path) - - def compute_gravity_torques(self, q: list[float]) -> list[float]: - self._validate_length("q", q) - if self._pin_model is None or self._pin_data is None: - raise RuntimeError("gravity compensation model is not loaded") - tau = pinocchio.computeGeneralizedGravity( - self._pin_model, self._pin_data, np.array(q, dtype=np.float64) - ) - values = [float(tau[i]) for i in range(self._dof)] - if not np.isfinite(np.asarray(values, dtype=np.float64)).all(): - raise RuntimeError("gravity computation returned non-finite torque values") - if self._gravity_torque_limits is None: - return values - return [ - float(np.clip(value, -limit, limit)) - for value, limit in zip(values, self._gravity_torque_limits, strict=False) - ] diff --git a/dimos/hardware/damiao/config.py b/dimos/hardware/damiao/config.py deleted file mode 100644 index e70494944e..0000000000 --- a/dimos/hardware/damiao/config.py +++ /dev/null @@ -1,272 +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. - -from __future__ import annotations - -from collections.abc import Sequence -import math -from pathlib import Path -from typing import Literal - -import attrs - -from dimos.hardware.manipulators.spec import ControlMode - -_NON_EMPTY_STRING = attrs.validators.and_( - attrs.validators.instance_of(str), - attrs.validators.min_len(1), -) -_NON_NEGATIVE_INT = attrs.validators.and_( - attrs.validators.instance_of(int), - attrs.validators.ge(0), -) -_POSITIVE_INT = attrs.validators.and_( - attrs.validators.instance_of(int), - attrs.validators.ge(1), -) - - -def _to_floats(values: Sequence[float]) -> tuple[float, ...]: - return tuple(float(value) for value in values) - - -def _to_optional_floats(values: Sequence[float] | None) -> tuple[float, ...] | None: - return None if values is None else _to_floats(values) - - -def _to_motors(values: Sequence[DamiaoMotorConfig]) -> tuple[DamiaoMotorConfig, ...]: - return tuple(values) - - -def _to_control_modes(values: Sequence[ControlMode]) -> tuple[ControlMode, ...]: - return tuple(values) - - -def _to_optional_path(value: str | Path | None) -> Path | None: - return None if value is None else Path(value) - - -def _finite_non_negative( - _instance: object, attribute: attrs.Attribute[float], value: float -) -> None: - if not math.isfinite(value) or value < 0.0: - raise ValueError(f"{attribute.name} must be finite and non-negative") - - -def _opening_current(_instance: object, attribute: attrs.Attribute[float], value: float) -> None: - if not math.isfinite(value) or not 0.0 < value <= 1.0: - raise ValueError(f"{attribute.name} must be finite and in (0, 1]") - - -@attrs.frozen(slots=False) -class DamiaoMotorConfig: - """Physical identity for one Damiao motor in command-vector order.""" - - name: str = attrs.field(validator=_NON_EMPTY_STRING) - type: str | int = attrs.field(validator=attrs.validators.instance_of((str, int))) - send_id: int = attrs.field(validator=_NON_NEGATIVE_INT) - recv_id: int | None = attrs.field( - default=None, - validator=attrs.validators.optional(_NON_NEGATIVE_INT), - ) - - @property - def effective_recv_id(self) -> int: - """Return the explicit receive CAN ID, or Damiao's default response ID.""" - - return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) - - -@attrs.frozen(slots=False) -class DamiaoGripperConfig: - """Physical definition for one normalized Damiao gripper group.""" - - motor: DamiaoMotorConfig = attrs.field( - validator=attrs.validators.instance_of(DamiaoMotorConfig) - ) - opening_direction: Literal["increasing_position", "decreasing_position"] = attrs.field( - validator=attrs.validators.in_(("increasing_position", "decreasing_position")) - ) - default_current: float = attrs.field( - default=0.15, - converter=float, - validator=_opening_current, - ) - - -@attrs.frozen(slots=False) -class DamiaoArmConfig: - """Immutable physical definition and capabilities for one Damiao arm.""" - - name: str = attrs.field(validator=_NON_EMPTY_STRING) - vendor: str = attrs.field(validator=_NON_EMPTY_STRING) - model: str = attrs.field(validator=_NON_EMPTY_STRING) - motors: tuple[DamiaoMotorConfig, ...] = attrs.field( - converter=_to_motors, - validator=attrs.validators.deep_iterable( - member_validator=attrs.validators.instance_of(DamiaoMotorConfig), - iterable_validator=attrs.validators.min_len(1), - ), - ) - position_lower: tuple[float, ...] = attrs.field(converter=_to_floats) - position_upper: tuple[float, ...] = attrs.field(converter=_to_floats) - velocity_max: tuple[float, ...] = attrs.field(converter=_to_floats) - kp: tuple[float, ...] = attrs.field(converter=_to_floats) - kd: tuple[float, ...] = attrs.field(converter=_to_floats) - gravity_torque_limits: tuple[float, ...] | None = attrs.field( - default=None, - converter=_to_optional_floats, - ) - gripper: DamiaoGripperConfig | None = attrs.field( - default=None, - validator=attrs.validators.optional(attrs.validators.instance_of(DamiaoGripperConfig)), - ) - fd: bool = attrs.field(default=False, validator=attrs.validators.instance_of(bool)) - supported_control_modes: tuple[ControlMode, ...] = attrs.field( - factory=lambda: ( - ControlMode.POSITION, - ControlMode.SERVO_POSITION, - ControlMode.TORQUE, - ), - converter=_to_control_modes, - validator=attrs.validators.deep_iterable( - member_validator=attrs.validators.instance_of(ControlMode), - iterable_validator=attrs.validators.min_len(1), - ), - ) - - @property - def dof(self) -> int: - """Return the number of joints described by this arm.""" - - return len(self.motors) - - @property - def joint_names(self) -> tuple[str, ...]: - """Return joint names in adapter and command-vector order.""" - - return tuple(motor.name for motor in self.motors) - - @motors.validator - def _validate_motor_identity( - self, - _attribute: attrs.Attribute[tuple[DamiaoMotorConfig, ...]], - motors: tuple[DamiaoMotorConfig, ...], - ) -> None: - identities: dict[str, Sequence[str | int]] = { - "joint names": [motor.name for motor in motors], - "send IDs": [motor.send_id for motor in motors], - "receive IDs": [motor.effective_recv_id for motor in motors], - } - for label, values in identities.items(): - if len(set(values)) != len(values): - raise ValueError(f"Damiao arm {self.name!r} has duplicate {label}: {values}") - - @position_lower.validator - def _validate_joint_vectors( - self, - _attribute: attrs.Attribute[tuple[float, ...]], - _value: tuple[float, ...], - ) -> None: - vectors = { - "position_lower": self.position_lower, - "position_upper": self.position_upper, - "velocity_max": self.velocity_max, - "kp": self.kp, - "kd": self.kd, - } - if self.gravity_torque_limits is not None: - vectors["gravity_torque_limits"] = self.gravity_torque_limits - for label, values in vectors.items(): - if len(values) != self.dof: - raise ValueError(f"{label} length {len(values)} does not match arm DOF {self.dof}") - if not all(math.isfinite(value) for value in values): - raise ValueError(f"{label} values must be finite") - if any( - lower > upper - for lower, upper in zip(self.position_lower, self.position_upper, strict=True) - ): - raise ValueError("position lower limits must not exceed upper limits") - if any(value <= 0.0 for value in self.velocity_max): - raise ValueError("velocity limits must be greater than zero") - if any(value < 0.0 for value in (*self.kp, *self.kd)): - raise ValueError("default gains must be non-negative") - if self.gravity_torque_limits is not None and any( - value < 0.0 for value in self.gravity_torque_limits - ): - raise ValueError("gravity torque limits must be non-negative") - - @supported_control_modes.validator - def _validate_control_modes( - self, - _attribute: attrs.Attribute[tuple[ControlMode, ...]], - modes: tuple[ControlMode, ...], - ) -> None: - if len(set(modes)) != len(modes): - raise ValueError("supported control modes must be unique") - - @gripper.validator - def _validate_gripper_identity( - self, - _attribute: attrs.Attribute[DamiaoGripperConfig | None], - gripper: DamiaoGripperConfig | None, - ) -> None: - if gripper is None: - return - motor = gripper.motor - if motor.name in self.joint_names: - raise ValueError( - f"Damiao arm {self.name!r} gripper duplicates motor name {motor.name!r}" - ) - if motor.send_id in {arm_motor.send_id for arm_motor in self.motors}: - raise ValueError(f"Damiao arm {self.name!r} gripper duplicates send ID {motor.send_id}") - if motor.effective_recv_id in {arm_motor.effective_recv_id for arm_motor in self.motors}: - raise ValueError( - f"Damiao arm {self.name!r} gripper duplicates receive ID {motor.effective_recv_id}" - ) - - -@attrs.frozen(slots=False) -class DamiaoRuntimeConfig: - """Deployment-specific settings and optional overrides for a Damiao arm.""" - - address: str = attrs.field(default="can0", converter=str, validator=_NON_EMPTY_STRING) - gravity_comp: bool = attrs.field(default=True, validator=attrs.validators.instance_of(bool)) - gravity_model_path: Path | None = attrs.field(default=None, converter=_to_optional_path) - kp_override: tuple[float, ...] | None = attrs.field( - default=None, - converter=_to_optional_floats, - ) - kd_override: tuple[float, ...] | None = attrs.field( - default=None, - converter=_to_optional_floats, - ) - use_mock_bus: bool = attrs.field(default=False, validator=attrs.validators.instance_of(bool)) - config_path: Path | None = attrs.field(default=None, converter=_to_optional_path) - tick_deadline_us: int = attrs.field(default=1_000, validator=_POSITIVE_INT) - state_cache_ttl_s: float = attrs.field( - default=0.002, - converter=float, - validator=_finite_non_negative, - ) - - @kp_override.validator - @kd_override.validator - def _validate_gain_override( - self, - attribute: attrs.Attribute[tuple[float, ...] | None], - values: tuple[float, ...] | None, - ) -> None: - if values is not None and any(not math.isfinite(value) or value < 0.0 for value in values): - raise ValueError(f"{attribute.name} values must be finite and non-negative") diff --git a/dimos/hardware/damiao/runtime.py b/dimos/hardware/damiao/runtime.py deleted file mode 100644 index 472deabfe1..0000000000 --- a/dimos/hardware/damiao/runtime.py +++ /dev/null @@ -1,390 +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. - -from __future__ import annotations - -from collections.abc import Callable, Sequence -from dataclasses import dataclass -import errno -import math -from pathlib import Path -import time -from typing import Any - -import can_motor_control -from can_motor_control import damiao -import numpy as np -import pinocchio # type: ignore[import-not-found] - -from dimos.hardware.damiao.config import DamiaoArmConfig, DamiaoRuntimeConfig -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -_ARM_NAME = "arm" -_BUS_NAME = "can" -_GRIPPER_NAME = "gripper" -_ENOBUFS_RETRY_DELAYS_S = (0.001, 0.002, 0.003) -_MIN_RECOMMENDED_TX_QUEUE_LEN = 1_000 -_MOTOR_TYPES_BY_NAME = { - "DM3507": damiao.MotorType.DM3507, - "DM4310": damiao.MotorType.DM4310, - "DM4310_48V": damiao.MotorType.DM4310_48V, - "DM4340": damiao.MotorType.DM4340, - "DM4340_48V": damiao.MotorType.DM4340_48V, - "DM6006": damiao.MotorType.DM6006, - "DM8006": damiao.MotorType.DM8006, - "DM8009": damiao.MotorType.DM8009, - "DM10010L": damiao.MotorType.DM10010L, - "DM10010": damiao.MotorType.DM10010, - "DMH3510": damiao.MotorType.DMH3510, - "DMH6215": damiao.MotorType.DMH6215, - "DMG6220": damiao.MotorType.DMG6220, -} -_MOTOR_TYPES_BY_VALUE = { - int(motor_type): motor_type for motor_type in _MOTOR_TYPES_BY_NAME.values() -} - - -def _is_enobufs(exc: BaseException) -> bool: - current: BaseException | None = exc - while current is not None: - if isinstance(current, OSError) and current.errno == errno.ENOBUFS: - return True - message = str(current).lower() - if "no buffer space available" in message or "os error 105" in message: - return True - current = current.__cause__ or current.__context__ - return False - - -def _retry_enobufs(operation: Callable[[], None]) -> None: - for delay_s in (*_ENOBUFS_RETRY_DELAYS_S, None): - try: - operation() - return - except Exception as exc: - if delay_s is None or not _is_enobufs(exc): - raise - time.sleep(delay_s) - - -@dataclass(frozen=True) -class DamiaoGroupState: - """State vectors for one Damiao arm.""" - - q: list[float] - dq: list[float] - tau: list[float] - - -class DamiaoArmRuntime: - """Binding-backed runtime for one Damiao arm.""" - - def __init__( - self, - *, - arm_config: DamiaoArmConfig, - runtime_config: DamiaoRuntimeConfig, - adapter_type: str = "damiao", - ) -> None: - self._arm_config = arm_config - self._runtime_config = runtime_config - self._adapter_type = adapter_type - self._robot: Any | None = None - self._arm: Any | None = None - self._gripper: Any | None = None - self._state_cache: DamiaoGroupState | None = None - self._state_cache_time = 0.0 - self._connected = False - self._enabled = False - - @property - def arm_config(self) -> DamiaoArmConfig: - return self._arm_config - - def connect(self) -> bool: - """Connect the binding robot and cache its arm handle.""" - - try: - robot = self._build_robot() - robot.connect() - arm = robot[_ARM_NAME] - if len(arm) != self._arm_config.dof: - raise RuntimeError( - f"can_motor_control arm has {len(arm)} joints, expected {self._arm_config.dof}" - ) - self._robot = robot - self._arm = arm - self._gripper = robot[_GRIPPER_NAME] if self._arm_config.gripper is not None else None - self._connected = True - self.refresh_state(force=True) - except Exception: - logger.exception("damiao runtime connect failed", adapter=self._adapter_type) - self.disconnect() - return False - return True - - def _build_robot(self) -> Any: - if self._runtime_config.config_path is not None: - return can_motor_control.Robot.from_config(str(self._runtime_config.config_path)) - - address = self._runtime_config.address - self._warn_if_small_tx_queue(address) - transport: can_motor_control.MockCanBus | can_motor_control.SocketCanBus - if self._runtime_config.use_mock_bus: - transport = ( - can_motor_control.MockCanBus.new_fd(address) - if self._arm_config.fd - else can_motor_control.MockCanBus(address) - ) - else: - transport = can_motor_control.SocketCanBus(address, fd=self._arm_config.fd) - motors = [ - can_motor_control.MotorSpec( - motor.name, - self._resolve_motor_type(motor.type), - motor.send_id, - motor.effective_recv_id, - ) - for motor in self._arm_config.motors - ] - builder = ( - can_motor_control.Robot.builder() - .add_bus(_BUS_NAME, transport, damiao.DamiaoCodec()) - .add_arm(_ARM_NAME, bus=_BUS_NAME, motors=motors) - ) - if self._arm_config.gripper is not None: - gripper = self._arm_config.gripper - motor = gripper.motor - builder = builder.add_gripper( - _GRIPPER_NAME, - bus=_BUS_NAME, - motor=can_motor_control.MotorSpec( - motor.name, - self._resolve_motor_type(motor.type), - motor.send_id, - motor.effective_recv_id, - ), - opening_direction=gripper.opening_direction, - default_current=gripper.default_current, - ) - return builder.build() - - def _warn_if_small_tx_queue(self, address: str) -> None: - if self._runtime_config.use_mock_bus: - return - queue_path = Path("/sys/class/net") / address / "tx_queue_len" - try: - queue_len = int(queue_path.read_text().strip()) - except (OSError, ValueError): - return - if queue_len < _MIN_RECOMMENDED_TX_QUEUE_LEN: - logger.warning( - "CAN transmit queue is too small for reliable motor activation", - interface=address, - txqueuelen=queue_len, - recommended=_MIN_RECOMMENDED_TX_QUEUE_LEN, - setup_command=f"dimos can setup {address}", - ) - - @staticmethod - def _resolve_motor_type(motor_type: str | int) -> damiao.MotorType: - try: - if isinstance(motor_type, str): - return _MOTOR_TYPES_BY_NAME[motor_type] - return _MOTOR_TYPES_BY_VALUE[motor_type] - except KeyError as exc: - raise ValueError(f"Unknown Damiao motor type {motor_type!r}") from exc - - def disconnect(self) -> None: - """Disable and drop the underlying binding robot.""" - - disabled = True - if self._robot is not None: - try: - self._robot.disable() - except Exception: - logger.warning("damiao runtime disable on disconnect failed", exc_info=True) - disabled = False - self._enabled = False if disabled else True - self._connected = False - self._robot = None - self._arm = None - self._gripper = None - self._state_cache = None - self._state_cache_time = 0.0 - - def is_connected(self) -> bool: - return self._connected - - def enable(self) -> bool: - if self._robot is None: - return False - try: - self._robot.set_mode("mit") - self._robot.tick(self._runtime_config.tick_deadline_us) - self._robot.enable() - self._robot.tick(self._runtime_config.tick_deadline_us) - if self._gripper is not None: - self._validated_gripper_opening() - except Exception: - logger.exception("damiao runtime enable failed", adapter=self._adapter_type) - try: - disabled = self._robot.disable() - except Exception: - logger.warning("damiao runtime rollback disable failed", exc_info=True) - logger.error("damiao runtime partial enable could not disable hardware") - self._enabled = True - return False - if disabled is False: - logger.error("damiao runtime partial enable could not disable hardware") - self._enabled = True - return False - self._enabled = False - return False - self._enabled = True - return True - - def disable(self) -> bool: - if self._robot is None: - return False - try: - self._robot.disable() - except Exception: - logger.exception("damiao runtime disable failed", adapter=self._adapter_type) - return False - self._enabled = False - return True - - def is_enabled(self) -> bool: - return self._enabled - - def _validated_gripper_opening(self) -> float: - if self._gripper is None: - raise RuntimeError("DamiaoArmRuntime has no configured gripper") - try: - opening = float(self._gripper.opening) - except AttributeError as exc: - raise RuntimeError( - "can_motor_control Gripper.opening is required for calibrated readback" - ) from exc - if not math.isfinite(opening) or not 0.0 <= opening <= 1.0: - raise RuntimeError( - f"gripper opening feedback must be finite and in [0, 1], got {opening}" - ) - return opening - - def read_gripper_opening(self) -> float | None: - """Read the calibrated normalized gripper opening.""" - - if self._robot is None or self._gripper is None or not self._enabled: - return None - try: - self._gripper.refresh() - self._robot.tick(self._runtime_config.tick_deadline_us) - return self._validated_gripper_opening() - except Exception: - logger.exception("damiao runtime gripper read failed", adapter=self._adapter_type) - return None - - def write_gripper_opening(self, opening: float) -> bool: - """Command a calibrated normalized gripper opening.""" - - if ( - self._robot is None - or self._gripper is None - or not self._enabled - or not math.isfinite(opening) - or not 0.0 <= opening <= 1.0 - ): - return False - try: - - def send() -> None: - assert self._gripper is not None - assert self._robot is not None - self._gripper.set_opening(opening) - self._robot.tick(self._runtime_config.tick_deadline_us) - - _retry_enobufs(send) - except Exception: - logger.exception("damiao runtime gripper command failed", adapter=self._adapter_type) - return False - return True - - def refresh_state(self, *, force: bool = False) -> DamiaoGroupState: - if self._robot is None or self._arm is None: - raise RuntimeError("DamiaoArmRuntime is not connected") - now = time.monotonic() - if ( - not force - and self._state_cache is not None - and now - self._state_cache_time <= self._runtime_config.state_cache_ttl_s - ): - return self._state_cache - self._arm.refresh() - self._robot.tick(self._runtime_config.tick_deadline_us) - state = DamiaoGroupState( - q=self._arm.positions().astype(np.float64).tolist(), - dq=self._arm.velocities().astype(np.float64).tolist(), - tau=self._arm.torques().astype(np.float64).tolist(), - ) - if any(len(values) != self._arm_config.dof for values in (state.q, state.dq, state.tau)): - raise RuntimeError("state length does not match configured arm DOF") - if any( - not np.isfinite(values).all() - for values in (np.asarray(state.q), np.asarray(state.dq), np.asarray(state.tau)) - ): - raise RuntimeError("state contains non-finite values") - self._state_cache = state - self._state_cache_time = time.monotonic() - return state - - def write_mit_commands( - self, - *, - q: Sequence[float], - dq: Sequence[float], - kp: Sequence[float], - kd: Sequence[float], - tau: Sequence[float], - ) -> bool: - """Write one MIT command frame to the arm.""" - - if self._robot is None or self._arm is None or not self._enabled: - return False - if any(len(values) != self._arm_config.dof for values in (q, dq, kp, kd, tau)): - raise ValueError("command length does not match configured arm DOF") - try: - - def send() -> None: - assert self._arm is not None - assert self._robot is not None - self._arm.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64)) - self._robot.tick(self._runtime_config.tick_deadline_us) - - _retry_enobufs(send) - except Exception: - logger.exception("damiao runtime MIT command failed") - return False - self._state_cache = None - self._state_cache_time = 0.0 - return True - - def load_gravity_model(self, model_path: str | Path) -> tuple[object, object]: - """Load a Pinocchio gravity model for the arm.""" - - model = pinocchio.buildModelFromUrdf(str(model_path)) - return model, model.createData() diff --git a/dimos/hardware/damiao/test_adapters.py b/dimos/hardware/damiao/test_adapters.py deleted file mode 100644 index 0c5411e2b4..0000000000 --- a/dimos/hardware/damiao/test_adapters.py +++ /dev/null @@ -1,495 +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. - -from __future__ import annotations - -from types import SimpleNamespace - -import attrs -import pytest - -from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter -from dimos.hardware.damiao.config import ( - DamiaoArmConfig, - DamiaoGripperConfig, - DamiaoMotorConfig, - DamiaoRuntimeConfig, -) -from dimos.hardware.damiao.runtime import DamiaoArmRuntime, DamiaoGroupState -from dimos.hardware.manipulators.spec import ControlMode - - -class _FakeRuntime: - def __init__(self, *, write_ok: bool = True) -> None: - self.write_ok = write_ok - self.connected = False - self.enabled = False - self.disconnect_calls = 0 - self.writes: list[ - tuple[list[float], list[float], list[float], list[float], list[float]] - ] = [] - self.loaded_gravity_models: list[str] = [] - self.state = DamiaoGroupState( - q=[0.4, -0.4], - dq=[0.5, -0.5], - tau=[0.6, -0.6], - ) - - def connect(self) -> bool: - self.connected = True - return True - - def disconnect(self) -> None: - self.disconnect_calls += 1 - self.connected = False - self.enabled = False - - def enable(self) -> bool: - self.enabled = True - return True - - def disable(self) -> bool: - self.enabled = False - return True - - def is_enabled(self) -> bool: - return self.enabled - - def refresh_state(self, *, force: bool = False) -> DamiaoGroupState: - del force - return self.state - - def write_mit_commands( - self, - *, - q: list[float], - dq: list[float], - kp: list[float], - kd: list[float], - tau: list[float], - ) -> bool: - if not self.write_ok: - return False - self.writes.append((list(q), list(dq), list(kp), list(kd), list(tau))) - return True - - def load_gravity_model(self, model_path: str) -> tuple[object, object]: - self.loaded_gravity_models.append(model_path) - return SimpleNamespace(nq=2, nv=2, names=["universe", "j1", "j2"]), object() - - -def _arm_config(**changes: object) -> DamiaoArmConfig: - config = DamiaoArmConfig( - name="test_damiao", - vendor="Damiao", - model="TestArm", - motors=( - DamiaoMotorConfig("j1", "DM4310", 0x01, 0x11), - DamiaoMotorConfig("j2", "DM4310", 0x02, 0x12), - ), - position_lower=(-1.0, -2.0), - position_upper=(1.0, 2.0), - velocity_max=(3.0, 4.0), - kp=(5.0, 6.0), - kd=(0.1, 0.2), - gravity_torque_limits=(7.0, 8.0), - ) - return attrs.evolve(config, **changes) - - -def test_arm_config_normalizes_sequences_and_is_frozen() -> None: - config = _arm_config(position_lower=[-1, -2]) - - assert config.position_lower == (-1.0, -2.0) - assert config.joint_names == ("j1", "j2") - with pytest.raises(attrs.exceptions.FrozenInstanceError): - config.position_lower = (0.0, 0.0) - - -def test_arm_config_rejects_duplicate_motor_identity_at_construction() -> None: - with pytest.raises(ValueError, match="duplicate send IDs"): - _arm_config( - motors=( - DamiaoMotorConfig("j1", "DM4310", 0x01, 0x11), - DamiaoMotorConfig("j2", "DM4310", 0x01, 0x12), - ) - ) - - -def test_arm_config_rejects_gripper_can_id_collision() -> None: - with pytest.raises(ValueError, match="gripper duplicates send ID"): - _arm_config( - gripper=DamiaoGripperConfig( - motor=DamiaoMotorConfig("gripper", "DM4310", 0x02, 0x18), - opening_direction="decreasing_position", - ) - ) - - -@pytest.mark.parametrize("current", [0.0, 1.1, float("nan")]) -def test_gripper_config_rejects_invalid_default_current(current: float) -> None: - with pytest.raises(ValueError, match="default_current"): - DamiaoGripperConfig( - motor=DamiaoMotorConfig("gripper", "DM4310", 0x08, 0x18), - opening_direction="decreasing_position", - default_current=current, - ) - - -@pytest.mark.parametrize( - ("changes", "message"), - [ - ({"kp": (1.0,)}, "kp length"), - ({"position_lower": (2.0, -2.0)}, "lower limits"), - ({"velocity_max": (0.0, 1.0)}, "velocity limits"), - ], -) -def test_arm_config_rejects_invalid_joint_vectors_at_construction( - changes: dict[str, object], message: str -) -> None: - with pytest.raises(ValueError, match=message): - _arm_config(**changes) - - -def test_runtime_config_rejects_invalid_typed_overrides() -> None: - with pytest.raises(ValueError, match="kp_override"): - DamiaoRuntimeConfig(kp_override=[1.0, float("nan")]) - - -def test_runtime_builds_robot_with_binding_motor_types() -> None: - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(use_mock_bus=True), - ) - - robot = runtime._build_robot() - - assert len(robot["arm"]) == 2 - - -def test_runtime_builds_separate_normalized_gripper_group() -> None: - gripper = DamiaoGripperConfig( - motor=DamiaoMotorConfig("gripper", "DM4310", 0x08, 0x18), - opening_direction="decreasing_position", - default_current=0.15, - ) - runtime = DamiaoArmRuntime( - arm_config=_arm_config(gripper=gripper), - runtime_config=DamiaoRuntimeConfig(use_mock_bus=True), - ) - - robot = runtime._build_robot() - - assert robot.group_names() == ["arm", "gripper"] - assert len(robot["arm"]) == 2 - assert robot["gripper"].motor.send_id == 0x08 - assert robot["gripper"].motor.recv_id == 0x18 - - -def test_arm_adapter_reports_limits_and_modes() -> None: - adapter = DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(gravity_comp=False), - ) - - assert adapter.get_dof() == 2 - assert adapter.get_limits().position_lower == [-1.0, -2.0] - assert adapter.set_control_mode(ControlMode.TORQUE) is True - assert adapter.set_control_mode(ControlMode.VELOCITY) is False - - -def test_arm_adapter_resolves_runtime_gain_overrides() -> None: - adapter = DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig( - gravity_comp=False, - kp_override=[9.0, 8.0], - kd_override=[0.9, 0.8], - ), - ) - - assert adapter._kp == [9.0, 8.0] - assert adapter._kd == [0.9, 0.8] - - -def test_arm_adapter_rejects_override_with_wrong_dof() -> None: - with pytest.raises(ValueError, match="kp length"): - DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(gravity_comp=False, kp_override=[1.0]), - ) - - -def test_arm_adapter_uses_fake_runtime_for_startup_hold(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(gravity_comp=False), - ) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - - assert adapter.connect() is True - assert adapter.write_enable(True) is True - assert runtime.writes[-1] == ( - [0.4, -0.4], - [0.0, 0.0], - [5.0, 6.0], - [0.1, 0.2], - [0.0, 0.0], - ) - - -def test_arm_adapter_passes_gravity_model_to_runtime(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(gravity_model_path="override.urdf"), - ) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - - assert adapter.connect() is True - assert runtime.loaded_gravity_models == ["override.urdf"] - - -def test_arm_adapter_rejects_nonfinite_positions_before_enable(mocker) -> None: - runtime = _FakeRuntime() - runtime.state = DamiaoGroupState( - q=[float("nan"), 0.0], - dq=[0.0, 0.0], - tau=[0.0, 0.0], - ) - adapter = DamiaoArmAdapter(arm_config=_arm_config()) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - - assert adapter.connect() is True - assert adapter.write_enable(True) is False - assert runtime.enabled is False - assert runtime.writes == [] - - -def test_arm_adapter_gravity_compensation_rejects_missing_model_before_enable(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter(arm_config=_arm_config()) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - - assert adapter.connect() is True - assert adapter.write_enable(True) is False - assert runtime.enabled is False - assert runtime.writes == [] - - -def test_arm_adapter_error_recovery_runs_gravity_preflight(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter(arm_config=_arm_config()) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - preflight = mocker.patch.object(adapter, "_preflight_gravity") - mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) - - assert adapter.connect() is True - assert adapter.write_clear_errors() is True - preflight.assert_called_once_with() - assert runtime.enabled is True - - -def test_arm_adapter_disables_without_zero_torque_on_gravity_state_failure(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter(arm_config=_arm_config()) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - mocker.patch.object(adapter, "_load_gravity_model") - mocker.patch.object(adapter, "_preflight_gravity") - mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) - - assert adapter.connect() is True - assert adapter.write_enable(True) is True - writes_before_failure = list(runtime.writes) - mocker.patch.object(runtime, "refresh_state", side_effect=RuntimeError("state read failed")) - - assert adapter.write_joint_positions([0.2, -0.2]) is False - assert runtime.enabled is False - assert adapter.read_enabled() is False - assert runtime.writes == writes_before_failure - - -def test_arm_adapter_rejects_incompatible_gravity_model_before_enable(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(gravity_model_path="arm.urdf"), - ) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) - - assert adapter.connect() is True - adapter._pin_model = SimpleNamespace(nq=2, nv=2, names=["universe", "j2", "j1"]) - adapter._pin_data = object() - assert adapter.write_enable(True) is False - assert runtime.enabled is False - assert runtime.writes == [] - - -def test_arm_adapter_rolls_back_when_hold_command_fails(mocker) -> None: - runtime = _FakeRuntime(write_ok=False) - adapter = DamiaoArmAdapter(arm_config=_arm_config()) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - - assert adapter.connect() is True - assert adapter.write_enable(True) is False - assert runtime.enabled is False - assert adapter.read_enabled() is False - - -def test_arm_adapter_preserves_enabled_state_when_rollback_disable_fails(mocker) -> None: - runtime = _FakeRuntime(write_ok=False) - mocker.patch.object(runtime, "disable", return_value=False) - adapter = DamiaoArmAdapter( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(gravity_comp=False), - ) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - - assert adapter.connect() is True - assert adapter.write_enable(True) is False - assert adapter.read_enabled() is True - - -def test_arm_adapter_preserves_enabled_state_when_safety_disable_fails(mocker) -> None: - runtime = _FakeRuntime() - adapter = DamiaoArmAdapter(arm_config=_arm_config()) - mocker.patch.object(adapter, "_create_runtime", return_value=runtime) - mocker.patch.object(adapter, "_preflight_gravity") - mocker.patch.object(adapter, "compute_gravity_torques", return_value=[0.0, 0.0]) - - assert adapter.connect() is True - assert adapter.write_enable(True) is True - mocker.patch.object(runtime, "disable", return_value=False) - mocker.patch.object(runtime, "refresh_state", side_effect=RuntimeError("state read failed")) - - assert adapter.write_joint_positions([0.2, -0.2]) is False - assert adapter.read_enabled() is True - - -def test_runtime_retries_mit_command_when_can_queue_is_temporarily_full(mocker) -> None: - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(), - ) - robot = mocker.Mock() - arm = mocker.Mock() - arm.mit_control.side_effect = [ - RuntimeError("transport IO error: No buffer space available (os error 105)"), - None, - ] - runtime._robot = robot - runtime._arm = arm - runtime._enabled = True - sleep = mocker.patch("dimos.hardware.damiao.runtime.time.sleep") - - assert ( - runtime.write_mit_commands( - q=[0.0] * 2, dq=[0.0] * 2, kp=[0.0] * 2, kd=[0.0] * 2, tau=[0.0] * 2 - ) - is True - ) - assert arm.mit_control.call_count == 2 - robot.tick.assert_called_once_with(1_000) - sleep.assert_called_once_with(0.001) - - -def test_runtime_selects_mit_mode_before_enable(mocker) -> None: - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(), - ) - robot = mocker.Mock() - runtime._robot = robot - - assert runtime.enable() is True - assert robot.method_calls[:4] == [ - mocker.call.set_mode("mit"), - mocker.call.tick(1_000), - mocker.call.enable(), - mocker.call.tick(1_000), - ] - - -def test_runtime_enable_requires_normalized_gripper_readback_and_rolls_back(mocker) -> None: - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(), - ) - robot = mocker.Mock() - gripper = SimpleNamespace() - runtime._robot = robot - runtime._gripper = gripper - - assert runtime.enable() is False - robot.disable.assert_called_once_with() - assert runtime.is_enabled() is False - - -def test_runtime_reads_and_writes_normalized_gripper_opening(mocker) -> None: - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(), - ) - robot = mocker.Mock() - gripper = mocker.Mock() - gripper.opening = 0.4 - runtime._robot = robot - runtime._gripper = gripper - runtime._enabled = True - - assert runtime.read_gripper_opening() == 0.4 - assert runtime.write_gripper_opening(0.75) is True - gripper.refresh.assert_called_once_with() - gripper.set_opening.assert_called_once_with(0.75) - assert robot.tick.call_args_list == [mocker.call(1_000), mocker.call(1_000)] - - -@pytest.mark.parametrize("opening", [-0.01, 1.01, float("nan"), float("inf")]) -def test_runtime_rejects_invalid_normalized_gripper_commands(mocker, opening: float) -> None: - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(), - ) - runtime._robot = mocker.Mock() - runtime._gripper = mocker.Mock() - runtime._enabled = True - - assert runtime.write_gripper_opening(opening) is False - runtime._gripper.set_opening.assert_not_called() - - -def test_runtime_preserves_enabled_state_when_partial_enable_rollback_fails() -> None: - class _FailingRobot: - def set_mode(self, mode: str) -> None: - assert mode == "mit" - - def tick(self, deadline_us: int) -> None: - assert deadline_us == 1_000 - - def enable(self) -> None: - raise RuntimeError("partial enable") - - def disable(self) -> bool: - return False - - runtime = DamiaoArmRuntime( - arm_config=_arm_config(), - runtime_config=DamiaoRuntimeConfig(), - ) - runtime._robot = _FailingRobot() - - assert runtime.enable() is False - assert runtime.is_enabled() is True diff --git a/dimos/hardware/manipulators/openyam_damiao/_registry.py b/dimos/hardware/manipulators/openyam_damiao/_registry.py deleted file mode 100644 index 49d4b18393..0000000000 --- a/dimos/hardware/manipulators/openyam_damiao/_registry.py +++ /dev/null @@ -1,8 +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. - -ADAPTER_FACTORIES = { - "openyam_damiao": ("dimos.hardware.manipulators.openyam_damiao.adapter:OpenYamDamiaoAdapter"), -} diff --git a/dimos/hardware/manipulators/openyam_damiao/adapter.py b/dimos/hardware/manipulators/openyam_damiao/adapter.py deleted file mode 100644 index d0f2c758e7..0000000000 --- a/dimos/hardware/manipulators/openyam_damiao/adapter.py +++ /dev/null @@ -1,170 +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. - -"""OpenYAM's six-axis Damiao adapter.""" - -from __future__ import annotations - -import math -from pathlib import Path - -import attrs - -from dimos.hardware.damiao.arm_adapter import DamiaoArmAdapter -from dimos.hardware.damiao.config import ( - DamiaoArmConfig, - DamiaoGripperConfig, - DamiaoMotorConfig, - DamiaoRuntimeConfig, -) -from dimos.robot.model_parser import parse_model -from dimos.utils.data import LfsPath -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -_OPENYAM_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper.urdf.xacro" -_OPENYAM_PACKAGE_PATHS = {"yam_description": Path(LfsPath("yam_description"))} - -ARM_MOTOR_CONFIGS = tuple( - DamiaoMotorConfig( - name=f"yam_joint{index}", - type="DM4340" if index <= 3 else "DM4310", - send_id=index, - ) - for index in range(1, 7) -) -GRIPPER_MOTOR_CONFIG = DamiaoMotorConfig("yam_gripper", "DM4310", 0x08, 0x18) -OPENYAM_GRIPPER_CONFIG = DamiaoGripperConfig( - motor=GRIPPER_MOTOR_CONFIG, - opening_direction="decreasing_position", - default_current=0.15, -) - - -def _active_arm_limits() -> tuple[tuple[float, ...], tuple[float, ...], tuple[float, ...]]: - """Read arm limits from the active planning Xacro, failing closed.""" - model = parse_model(_OPENYAM_MODEL_PATH, package_paths=_OPENYAM_PACKAGE_PATHS) - names = [joint.name for joint in model.joints] - if len(names) != len(set(names)): - raise ValueError("active OpenYAM Xacro contains duplicate joint names") - joints = [model.get_joint(f"yam_joint{index}") for index in range(1, 7)] - resolved = [joint for joint in joints if joint is not None] - if len(resolved) != 6: - raise ValueError("active OpenYAM Xacro does not define all six arm joints") - limits: list[tuple[float, float, float]] = [] - for joint in resolved: - lower = joint.lower_limit - upper = joint.upper_limit - velocity = joint.velocity_limit - if lower is None or upper is None or velocity is None: - raise ValueError("active OpenYAM Xacro has incomplete or nonfinite arm limits") - if not all(math.isfinite(value) for value in (lower, upper, velocity)): - raise ValueError("active OpenYAM Xacro has incomplete or nonfinite arm limits") - if lower > upper: - raise ValueError(f"active OpenYAM Xacro has inverted limits for {joint.name}") - if velocity <= 0: - raise ValueError(f"active OpenYAM Xacro has nonpositive velocity for {joint.name}") - limits.append((lower, upper, velocity)) - return ( - tuple(lower for lower, _, _ in limits), - tuple(upper for _, upper, _ in limits), - tuple(velocity for _, _, velocity in limits), - ) - - -def make_openyam_damiao_arm_config() -> DamiaoArmConfig: - """Build the canonical OpenYAM arm profile from the active planning model.""" - - lower, upper, velocity = _active_arm_limits() - return DamiaoArmConfig( - name="openyam", - vendor="Damiao", - model="OpenYAM", - motors=ARM_MOTOR_CONFIGS, - position_lower=lower, - position_upper=upper, - velocity_max=velocity, - kp=(80.0, 80.0, 80.0, 10.0, 10.0, 10.0), - kd=(5.0, 5.0, 5.0, 1.5, 1.5, 1.5), - gripper=OPENYAM_GRIPPER_CONFIG, - ) - - -class OpenYamDamiaoAdapter(DamiaoArmAdapter): - """Six-DOF OpenYAM arm with calibrated normalized gripper IO.""" - - def __init__( - self, - address: str | Path | None = None, - *, - runtime_config: DamiaoRuntimeConfig | None = None, - dof: int | None = None, - hardware_id: str = "arm", - ) -> None: - runtime_config = runtime_config or DamiaoRuntimeConfig() - if address is not None: - runtime_config = attrs.evolve(runtime_config, address=str(address)) - if runtime_config.gravity_comp and ( - runtime_config.gravity_model_path is None - or not runtime_config.gravity_model_path.is_file() - ): - raise ValueError("OpenYAM gravity compensation requires a valid model path") - super().__init__( - arm_config=make_openyam_damiao_arm_config(), - runtime_config=runtime_config, - dof=dof, - hardware_id=hardware_id, - ) - - self._write_armed_by_read = False - - def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float], list[float]]: - """Read feedback and arm exactly one subsequent motor write.""" - self._write_armed_by_read = False - state = super().refresh_state(force=force) - self._write_armed_by_read = True - return state - - def write_mit_commands( - self, - *, - q: list[float], - dq: list[float], - kp: list[float], - kd: list[float], - tau: list[float], - ) -> bool: - """Forward a command only after a successful feedback read.""" - self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - if not self._write_armed_by_read: - logger.error("OpenYAM rejected motor write without fresh position feedback") - return False - self._write_armed_by_read = False - return super().write_mit_commands(q=q, dq=dq, kp=kp, kd=kd, tau=tau) - - def read_gripper_position(self) -> float | None: - """Read normalized gripper opening, where zero is closed and one is open.""" - if self._runtime is None: - return None - return self._runtime.read_gripper_opening() - - def write_gripper_position(self, position: float) -> bool: - """Command normalized gripper opening, where zero is closed and one is open.""" - if self._runtime is None: - return False - return self._runtime.write_gripper_opening(position) - - -OpenYAMDamiaoAdapter = OpenYamDamiaoAdapter diff --git a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py b/dimos/hardware/manipulators/openyam_damiao/test_adapter.py deleted file mode 100644 index 8f4fda8235..0000000000 --- a/dimos/hardware/manipulators/openyam_damiao/test_adapter.py +++ /dev/null @@ -1,240 +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. - -"""Focused OpenYAM adapter tests. - -The shared Damiao runtime is an optional hardware dependency in this checkout; -the tests become active when that runtime is installed (as they are in the -hardware test environment). -""" - -from pathlib import Path -from unittest.mock import Mock - -import pytest - -pytest.importorskip("can_motor_control") - -from dimos.hardware.damiao.config import DamiaoRuntimeConfig -from dimos.hardware.damiao.runtime import DamiaoGroupState -import dimos.hardware.manipulators.openyam_damiao.adapter as adapter_module -from dimos.hardware.manipulators.openyam_damiao.adapter import ( - ARM_MOTOR_CONFIGS, - GRIPPER_MOTOR_CONFIG, - OPENYAM_GRIPPER_CONFIG, - OpenYamDamiaoAdapter, - make_openyam_damiao_arm_config, -) -from dimos.robot.model_parser import JointDescription, ModelDescription -from dimos.utils.data import LfsPath - -GRAVITY_MODEL_PATH = Path(LfsPath("yam_description")) / "urdf/yam_gripper_gravity.urdf" - - -def test_openyam_motor_topology() -> None: - assert [motor.name for motor in ARM_MOTOR_CONFIGS] == [f"yam_joint{i}" for i in range(1, 7)] - assert [motor.send_id for motor in ARM_MOTOR_CONFIGS] == list(range(1, 7)) - assert [motor.type for motor in ARM_MOTOR_CONFIGS] == ["DM4340"] * 3 + ["DM4310"] * 3 - assert GRIPPER_MOTOR_CONFIG.send_id == 0x08 - assert GRIPPER_MOTOR_CONFIG.effective_recv_id == 0x18 - assert GRIPPER_MOTOR_CONFIG.type == "DM4310" - assert OPENYAM_GRIPPER_CONFIG.opening_direction == "decreasing_position" - assert OPENYAM_GRIPPER_CONFIG.default_current == 0.15 - assert make_openyam_damiao_arm_config().gripper is OPENYAM_GRIPPER_CONFIG - - -def test_openyam_gripper_delegates_normalized_io() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig( - gravity_model_path=GRAVITY_MODEL_PATH, - use_mock_bus=True, - ) - ) - runtime = Mock() - runtime.read_gripper_opening.return_value = 0.4 - runtime.write_gripper_opening.return_value = True - adapter._runtime = runtime - - assert adapter.read_gripper_position() == 0.4 - assert adapter.write_gripper_position(0.75) - runtime.read_gripper_opening.assert_called_once_with() - runtime.write_gripper_opening.assert_called_once_with(0.75) - - -def test_openyam_limits_are_loaded_from_active_model() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig( - gravity_model_path=GRAVITY_MODEL_PATH, - use_mock_bus=True, - ) - ) - - limits = adapter.get_limits() - assert limits.position_lower == pytest.approx([-3.92699, 0.0, 0.0, -1.65806, -1.5708, -2.35619]) - assert limits.position_upper == pytest.approx( - [1.5708, 3.66519, 4.01426, 1.65806, 1.5708, 1.8326] - ) - assert limits.velocity_max == pytest.approx([3.0, 10.0, 3.0, 10.0, 3.0, 10.0]) - assert adapter._kp == pytest.approx([80.0, 80.0, 80.0, 10.0, 10.0, 10.0]) - assert adapter._kd == pytest.approx([5.0, 5.0, 5.0, 1.5, 1.5, 1.5]) - assert adapter._gravity_comp - - -def test_openyam_allows_gravity_comp_to_be_disabled() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig(gravity_comp=False, use_mock_bus=True) - ) - - assert not adapter._gravity_comp - - with pytest.raises(ValueError, match="gravity compensation"): - OpenYamDamiaoAdapter(runtime_config=DamiaoRuntimeConfig(use_mock_bus=True)) - - -def test_openyam_activation_holds_exact_feedback_position() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig(gravity_comp=False, use_mock_bus=True) - ) - runtime = Mock() - feedback = [-1.2, 0.1, 0.2, -0.3, 0.4, -0.5] - runtime.refresh_state.return_value = DamiaoGroupState(q=feedback, dq=[0.0] * 6, tau=[0.0] * 6) - runtime.enable.return_value = True - runtime.write_mit_commands.return_value = True - adapter._runtime = runtime - - assert adapter.activate() - - runtime.write_mit_commands.assert_called_once_with( - q=feedback, - dq=[0.0] * 6, - kp=[80.0, 80.0, 80.0, 10.0, 10.0, 10.0], - kd=[5.0, 5.0, 5.0, 1.5, 1.5, 1.5], - tau=[0.0] * 6, - ) - assert runtime.refresh_state.call_count >= 2 - - -def test_openyam_normal_enable_and_error_recovery() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig( - gravity_model_path=GRAVITY_MODEL_PATH, - use_mock_bus=True, - ), - ) - runtime = Mock() - runtime.enable.return_value = True - adapter._runtime = runtime - adapter._preflight_gravity = Mock() - adapter.read_joint_positions = Mock(return_value=[0.0] * 6) - adapter.write_joint_positions = Mock(return_value=True) - - assert adapter.activate() - runtime.enable.assert_called_once_with() - - runtime.reset_mock() - runtime.disable.return_value = True - assert adapter.write_clear_errors() - runtime.disable.assert_called_once_with() - runtime.enable.assert_called_once_with() - - -def test_openyam_forwards_mit_commands() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig( - gravity_model_path=GRAVITY_MODEL_PATH, - use_mock_bus=True, - ), - ) - runtime = Mock() - runtime.refresh_state.return_value = DamiaoGroupState(q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6) - runtime.write_mit_commands.return_value = True - adapter._runtime = runtime - adapter._enabled = True - - assert adapter.refresh_state(force=True)[0] == [0.25] * 6 - - assert adapter.write_mit_commands( - q=[1.0] * 6, - dq=[2.0] * 6, - kp=[3.0] * 6, - kd=[4.0] * 6, - tau=[5.0] * 6, - ) - - runtime.write_mit_commands.assert_called_once_with( - q=[1.0] * 6, - dq=[2.0] * 6, - kp=[3.0] * 6, - kd=[4.0] * 6, - tau=[5.0] * 6, - ) - - assert not adapter.write_mit_commands( - q=[1.0] * 6, dq=[2.0] * 6, kp=[3.0] * 6, kd=[4.0] * 6, tau=[5.0] * 6 - ) - runtime.write_mit_commands.assert_called_once() - - -def test_openyam_failed_read_revokes_write_permission() -> None: - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig(gravity_comp=False, use_mock_bus=True) - ) - runtime = Mock() - runtime.refresh_state.return_value = DamiaoGroupState(q=[0.25] * 6, dq=[0.0] * 6, tau=[0.0] * 6) - adapter._runtime = runtime - adapter._enabled = True - - adapter.refresh_state(force=True) - runtime.refresh_state.side_effect = RuntimeError("feedback unavailable") - with pytest.raises(RuntimeError, match="feedback unavailable"): - adapter.refresh_state(force=True) - - assert not adapter.write_joint_positions([0.25] * 6) - runtime.write_mit_commands.assert_not_called() - - -def test_openyam_xacro_limits_reject_duplicate_joint_names(monkeypatch: pytest.MonkeyPatch) -> None: - joints = [JointDescription(f"yam_joint{i}", "revolute", -1.0, 1.0, 1.0) for i in range(1, 7)] - joints.append(joints[0]) - monkeypatch.setattr( - adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints) - ) - - with pytest.raises(ValueError, match="duplicate"): - adapter_module._active_arm_limits() - - -@pytest.mark.parametrize( - ("lower", "upper", "velocity"), - [(2.0, 1.0, 1.0), (0.0, 1.0, 0.0), (0.0, 1.0, float("nan"))], -) -def test_openyam_xacro_limits_reject_bad_values( - monkeypatch: pytest.MonkeyPatch, lower: float, upper: float, velocity: float -) -> None: - joints = [ - JointDescription( - f"yam_joint{i}", - "revolute", - lower if i == 1 else -1.0, - upper if i == 1 else 1.0, - velocity if i == 1 else 1.0, - ) - for i in range(1, 7) - ] - monkeypatch.setattr( - adapter_module, "parse_model", lambda *args, **kwargs: ModelDescription(joints=joints) - ) - - with pytest.raises(ValueError): - adapter_module._active_arm_limits() diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 09647a3e1f..9b00544624 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -42,7 +42,10 @@ } # 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. @@ -51,7 +54,6 @@ "a750", "mock", "openarm", - "openyam_damiao", "piper", "sim_mujoco", "xarm", @@ -63,7 +65,13 @@ "transport_ros", "unitree_go2", }, - "whole_body": {"sim_mujoco_g1", "transport_lcm", "transport_ros"}, + "whole_body": { + "mock_whole_body", + "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..7139b4815c --- /dev/null +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -0,0 +1,371 @@ +# 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_model_path: Path | None = None + 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: + 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 + + @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") + 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: + 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 + if self.gravity_model_path is None or not self.gravity_model_path.is_file(): + raise ValueError("gravity compensation requires an existing URDF") + self._pin_model = pinocchio.buildModelFromUrdf(str(self.gravity_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..93c184f747 --- /dev/null +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -0,0 +1,280 @@ +# 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 typing import cast + +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._positions = np.asarray(positions, dtype=np.float64) + self.modes: list[str] = [] + self.commands: list[np.ndarray] = [] + + def positions(self) -> np.ndarray: + return self._positions + + def velocities(self) -> np.ndarray: + return np.zeros_like(self._positions) + + def torques(self) -> np.ndarray: + return np.zeros_like(self._positions) + + def set_mode(self, mode: str) -> None: + self.modes.append(mode) + + def mit_control(self, commands: np.ndarray) -> None: + self.commands.append(commands) + + +class FakeGripper: + def __init__(self, opening: float) -> None: + self.opening = opening + self.commands: list[float] = [] + + def set_opening(self, opening: float) -> None: + self.commands.append(opening) + + +class FakeRobot: + def __init__(self, groups: dict[str, FakeArm | FakeGripper]) -> None: + self.groups = groups + self.connected = False + self.enable_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: + 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 + + def refresh(self) -> None: + self.refresh_count += 1 + + def tick(self, _deadline: int) -> None: + self.tick_count += 1 + + def is_connected(self) -> bool: + return self.connected + + +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) + + +@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 dual_adapter(dual_robot: FakeRobot, mocker: MockerFixture) -> 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], + ) + adapter = DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + dof=6, + ) + assert adapter.connect() + assert adapter.activate() + return adapter + + +def test_dual_arm_state_includes_both_normalized_grippers( + dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + ticks_before = dual_robot.tick_count + + assert dual_adapter.joint_names == ( + "left_arm/joint1", + "left_arm/joint2", + "right_arm/joint1", + "right_arm/joint2", + "left_arm/gripper", + "right_arm/gripper", + ) + assert 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), + ] + assert dual_robot.tick_count == ticks_before + + +def test_combined_command_ticks_once_and_splits_groups( + dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + ticks_before = dual_robot.tick_count + + assert dual_adapter.write_motor_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 dual_robot.tick_count == ticks_before + 1 + left_arm = cast("FakeArm", dual_robot["left_arm"]) + right_arm = cast("FakeArm", dual_robot["right_arm"]) + assert left_arm.commands[-1].tolist() == [ + [10.0, 0.0, 1.0, 16000.0, 0.0], + [11.0, 0.0, 1.1, 16000.0, 0.0], + ] + assert right_arm.commands[-1].tolist() == [ + [20.0, 0.0, 2.0, 16000.0, 0.0], + [21.0, 0.0, 2.1, 16000.0, 0.0], + ] + assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [0.25] + assert cast("FakeGripper", dual_robot["right_gripper"]).commands == [0.75] + + +def test_gripper_command_rejects_out_of_range( + dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + commands = [MotorCommand(q=0.0)] * 4 + [MotorCommand(q=-0.1), MotorCommand(q=0.5)] + + assert not dual_adapter.write_motor_commands(commands) + assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [] + + +def test_gravity_is_added_to_commanded_residual_torque( + dual_adapter: DualAdapter, + dual_robot: FakeRobot, + mocker: MockerFixture, +) -> None: + mocker.patch.object( + dual_adapter, + "_gravity_torques", + return_value=np.asarray([1.0, 2.0, 3.0, 4.0]), + ) + + commands = [MotorCommand(q=0.0, tau=0.5)] * 4 + [MotorCommand(q=0.5)] * 2 + assert dual_adapter.write_motor_commands(commands) + + 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_activation_failure_disables_robot( + dual_robot: FakeRobot, + mocker: MockerFixture, +) -> None: + 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], + ) + adapter = DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) + assert adapter.connect() + dual_robot.enable_error = RuntimeError("calibration failed") + + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_gripper_state_becomes_available_only_after_calibration( + dual_robot: FakeRobot, + mocker: MockerFixture, +) -> None: + 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], + ) + adapter = DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) + + assert not adapter.has_motor_states() + assert adapter.connect() + assert not adapter.has_motor_states() + assert adapter.activate() + assert adapter.has_motor_states() + + +def test_runtime_config_rejects_unknown_bus_override(dual_robot: FakeRobot) -> None: + with pytest.raises(ValueError, match="unknown CAN bus"): + DualAdapter( + dual_robot, + runtime_config=DamiaoRuntimeConfig(bus_addresses={"missing": "can9"}), + ) diff --git a/dimos/hardware/whole_body/mock/_registry.py b/dimos/hardware/whole_body/mock/_registry.py new file mode 100644 index 0000000000..fe6e27362b --- /dev/null +++ b/dimos/hardware/whole_body/mock/_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 = { + "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..27005d9ab9 --- /dev/null +++ b/dimos/hardware/whole_body/mock/adapter.py @@ -0,0 +1,76 @@ +# 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 pathlib import Path + +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState + + +class MockWholeBodyAdapter: + """Stateful ordered whole-body IO without robot-specific behavior.""" + + def __init__( + self, + address: str | Path | None = None, + *, + dof: int, + initial_positions: list[float] | None = None, + hardware_id: str = "whole_body", + domain_id: int = 0, + ) -> None: + del address + del hardware_id + del domain_id + 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..d5d87ebf6c --- /dev/null +++ b/dimos/hardware/whole_body/mock/test_adapter.py @@ -0,0 +1,42 @@ +# 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 dimos.hardware.whole_body.mock.adapter import MockWholeBodyAdapter +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState + + +def test_mock_whole_body_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_mock_whole_body_rejects_wrong_command_count() -> 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()] 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..657cd59ed6 --- /dev/null +++ b/dimos/hardware/whole_body/openyam_damiao/adapter.py @@ -0,0 +1,71 @@ +# 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.""" + + arm_joints = { + "arm": tuple(f"arm/joint{index}" for index in range(1, 7)), + } + gripper_joints = {"gripper": "arm/gripper"} + bus_defaults = {"can": "can0"} + gravity_model_path = Path(LfsPath("yam_description")) / "urdf/yam_gripper_gravity.urdf" + gravity_joint_names = tuple(f"yam_joint{index}" for index in range(1, 7)) + + def _build_robot(self) -> can_motor_control.Robot: + arm_motors = [ + can_motor_control.MotorSpec( + f"yam_joint{index}", + damiao.MotorType.DM4340 if index <= 3 else damiao.MotorType.DM4310, + index, + index | 0x10, + ) + for index in range(1, 7) + ] + gripper_motor = can_motor_control.MotorSpec( + "yam_gripper", + damiao.MotorType.DM4310, + 0x08, + 0x18, + ) + return ( + can_motor_control.Robot.builder() + .add_bus( + "can", + can_motor_control.SocketCanBus(self.bus_address("can")), + damiao.DamiaoCodec(), + ) + .add_arm("arm", bus="can", motors=arm_motors) + .add_gripper( + "gripper", + bus="can", + 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..64c1169128 --- /dev/null +++ b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py @@ -0,0 +1,46 @@ +# 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 can_motor_control +from pytest_mock import MockerFixture + +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.openyam_damiao.adapter import OpenYamDamiaoAdapter + + +def test_openyam_builds_upstream_arm_and_gripper(mocker: MockerFixture) -> None: + mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig( + bus_addresses={"can": "test_can"}, + gravity_comp=False, + ) + ) + + robot = adapter._build_robot() + + assert robot.bus_names() == ["can"] + assert robot.group_names() == ["arm", "gripper"] + assert isinstance(robot["arm"], can_motor_control.Arm) + assert len(robot["arm"]) == 6 + assert isinstance(robot["gripper"], can_motor_control.Gripper) + assert adapter.joint_names == ( + "arm/joint1", + "arm/joint2", + "arm/joint3", + "arm/joint4", + "arm/joint5", + "arm/joint6", + "arm/gripper", + ) 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/manipulation_module.py b/dimos/manipulation/manipulation_module.py index ea63e70464..137d8af72c 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -1649,18 +1649,6 @@ def _set_gripper_position(self, position: float, robot_name: RobotName | None = return False return self._control_coordinator.set_gripper_position(hw_id, position) - def _set_gripper_endpoint( - self, *, open_position: bool, robot_name: RobotName | None = None - ) -> bool: - """Internal: command the configured open or closed endpoint.""" - - hw_id = self._get_gripper_hardware_id(robot_name) - if hw_id is None: - return False - if open_position: - return self._control_coordinator.open_gripper(hw_id) - return self._control_coordinator.close_gripper(hw_id) - @rpc def get_gripper(self, robot_name: RobotName | None = None) -> float | None: """Get gripper position in meters. @@ -1695,7 +1683,7 @@ def open_gripper(self, robot_name: str | None = None) -> SkillResult[Manipulatio Args: robot_name: Robot to control (only needed for multi-arm setups). """ - if self._set_gripper_endpoint(open_position=True, robot_name=robot_name): + if self._set_gripper_position(0.85, robot_name): return SkillResult.ok("Gripper opened") return SkillResult.fail("GRIPPER_FAILED", "Failed to open gripper") @@ -1706,7 +1694,7 @@ def close_gripper(self, robot_name: str | None = None) -> SkillResult[Manipulati Args: robot_name: Robot to control (only needed for multi-arm setups). """ - if self._set_gripper_endpoint(open_position=False, robot_name=robot_name): + if self._set_gripper_position(0.0, robot_name): return SkillResult.ok("Gripper closed") return SkillResult.fail("GRIPPER_FAILED", "Failed to close gripper") diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 23a0d2083b..2bcc8a9afc 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -65,21 +65,6 @@ def _control_coordinator( return coordinator -def test_gripper_endpoint_skills_delegate_to_coordinator( - module_factory, mocker: MockerFixture -) -> None: - module = module_factory() - coordinator = module._control_coordinator - coordinator.open_gripper.return_value = True - coordinator.close_gripper.return_value = True - mocker.patch.object(module, "_get_gripper_hardware_id", return_value="arm") - - assert module.open_gripper().success is True - assert module.close_gripper().success is True - coordinator.open_gripper.assert_called_once_with("arm") - coordinator.close_gripper.assert_called_once_with("arm") - - @pytest.fixture def robot_config(): """Create a robot config for testing.""" diff --git a/dimos/robot/manipulators/openyam/blueprints/basic.py b/dimos/robot/manipulators/openyam/blueprints/basic.py index b24e19f256..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 ( + OPENYAM_ARM_JOINTS, make_openyam_model_config, openyam_hardware, ) -_openyam_planner_hw = 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 = 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 f33b8eb1e0..1ef8937228 100644 --- a/dimos/robot/manipulators/openyam/blueprints/teleop.py +++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py @@ -21,28 +21,55 @@ from dimos.manipulation.manipulation_module import ManipulationModule from dimos.robot.manipulators.common.blueprints import ( coordinator, - eef_twist_task, planner, - trajectory_task, +) +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_GRAVITY_MODEL_PATH, + OPENYAM_GRIPPER_JOINT, make_openyam_model_config, openyam_hardware, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_openyam_keyboard_hw = 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=["arm/gripper"], + joint_names=[OPENYAM_GRIPPER_JOINT], priority=20, - params={"timeout": 0.0, "default_positions": [0.0]}, + params={"timeout": 0.0}, ) @@ -51,11 +78,7 @@ def _gripper_task() -> TaskConfig: ControlCoordinator.blueprint( hardware=[_openyam_keyboard_hw], tasks=[ - eef_twist_task( - _openyam_keyboard_hw, - model_path=OPENYAM_GRAVITY_MODEL_PATH, - ee_joint_id=OPENYAM_DOF, - ), + _eef_twist_task(), _gripper_task(), ], ), @@ -65,7 +88,7 @@ def _gripper_task() -> TaskConfig: ), ) -_openyam_keyboard_planner_hw = openyam_hardware("arm") +_openyam_keyboard_planner_hw = openyam_hardware() keyboard_teleop_openyam_planner = autoconnect( KeyboardTeleopModule.blueprint(), @@ -73,14 +96,9 @@ def _gripper_task() -> TaskConfig: coordinator( hardware=[_openyam_keyboard_planner_hw], tasks=[ - eef_twist_task( - _openyam_keyboard_planner_hw, - model_path=OPENYAM_GRAVITY_MODEL_PATH, - ee_joint_id=OPENYAM_DOF, - priority=10, - ), + _eef_twist_task(priority=10), _gripper_task(), - trajectory_task(_openyam_keyboard_planner_hw, priority=20), + _trajectory_task(priority=20), ], ), ) diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index 45ae4f6a58..bef82ab30d 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -12,15 +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.damiao.config import DamiaoRuntimeConfig +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 ( @@ -31,64 +32,36 @@ 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 / "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", - *, - adapter_type: str = "mock", - address: str | None = None, - auto_enable: bool = True, - home_joints: list[float] | None = None, - adapter_kwargs: dict[str, object] | None = None, - include_gripper: bool = True, -) -> HardwareComponent: - """Create OpenYAM hardware with six arm joints and one gripper channel.""" - kwargs: dict[str, object] = {} - if adapter_type == "mock" and home_joints is not None: - kwargs["initial_positions"] = home_joints - if adapter_kwargs: - kwargs.update(adapter_kwargs) +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 not global_config.simulation: + adapter_kwargs["runtime_config"] = DamiaoRuntimeConfig( + bus_addresses={"can": 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), + hardware_id=OPENYAM_HARDWARE_ID, + hardware_type=HardwareType.WHOLE_BODY, + joints=list(OPENYAM_JOINTS), adapter_type=adapter_type, - address=address, - auto_enable=auto_enable, - gripper_joints=[f"{hw_id}/gripper"] if include_gripper else [], - gripper_open_position=1.0 if include_gripper else None, - gripper_closed_position=0.0 if include_gripper else None, - adapter_kwargs=kwargs, - ) - - -def openyam_hardware( - hw_id: str = "arm", - *, - home_joints: list[float] | None = None, -) -> HardwareComponent: - """Select mock hardware in simulation and the OpenYAM Damiao adapter on hardware.""" - if global_config.simulation: - return make_openyam_hardware(hw_id, home_joints=home_joints) - if not Path(OPENYAM_GRAVITY_MODEL_PATH).is_file(): - raise ValueError(f"OpenYAM gravity model is missing: {OPENYAM_GRAVITY_MODEL_PATH}") - return make_openyam_hardware( - hw_id, - adapter_type="openyam_damiao", - address=global_config.can_port or "can0", - # Physical encoder zeros are established by the driver; never pass - # planning/home positions into a live motor adapter. - adapter_kwargs={ - "runtime_config": DamiaoRuntimeConfig( - gravity_model_path=OPENYAM_GRAVITY_MODEL_PATH, - gravity_comp=True, - ), - }, + 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), + ), ) @@ -98,7 +71,7 @@ def make_openyam_model_config( joint_prefix: str | None = None, home_joints: list[float] | None = None, ) -> RobotModelConfig: - """Build a planning config for the gripper-equipped OpenYAM.""" + """Build the six-arm-joint planning model for OpenYAM.""" local_joint_names = joint_names(OPENYAM_DOF) return RobotModelConfig( name=name, @@ -123,6 +96,5 @@ def make_openyam_model_config( joint_prefix=joint_prefix, 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 a553442d50..25133aead1 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -14,11 +14,11 @@ from typing import Any +from dimos.control.components import HardwareType from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import Blueprint from dimos.core.global_config import global_config -from dimos.hardware.damiao.config import DamiaoRuntimeConfig -from dimos.hardware.manipulators.mock.adapter import MockAdapter +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig from dimos.robot.manipulators.openyam.blueprints.basic import ( coordinator_openyam, @@ -29,10 +29,13 @@ keyboard_teleop_openyam_planner, ) from dimos.robot.manipulators.openyam.config import ( + OPENYAM_ARM_JOINTS, OPENYAM_DOF, OPENYAM_GRAVITY_MODEL_PATH, + OPENYAM_GRIPPER_JOINT, + OPENYAM_HARDWARE_ID, + OPENYAM_JOINTS, OPENYAM_PACKAGE_PATHS, - make_openyam_hardware, make_openyam_model_config, openyam_hardware, ) @@ -47,7 +50,7 @@ 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_openyam_model_config_maps_only_six_arm_joints() -> None: config = make_openyam_model_config(name="arm") assert config.joint_names == [f"joint{i}" for i in range(1, OPENYAM_DOF + 1)] @@ -57,120 +60,87 @@ def test_openyam_model_config_has_expected_links_and_mapping() -> None: assert config.base_link == "base" assert config.end_effector_link == "gripper_tip" assert list(config.package_paths) == list(OPENYAM_PACKAGE_PATHS) - assert config.gripper_hardware_id == "arm" + assert config.gripper_hardware_id is None -def test_openyam_mock_hardware_has_gripper() -> None: - hardware = make_openyam_hardware("arm") - - 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"] - assert hardware.gripper_open_position == 1.0 - assert hardware.gripper_closed_position == 0.0 - - -def test_openyam_physical_hardware_uses_registered_damiao_adapter(monkeypatch: Any) -> None: +def test_openyam_physical_hardware_is_one_whole_body(monkeypatch: Any) -> None: monkeypatch.setattr(global_config, "simulation", "") monkeypatch.setattr(global_config, "can_port", "can1") - hardware = openyam_hardware("arm") + hardware = openyam_hardware() + assert hardware.hardware_id == OPENYAM_HARDWARE_ID + assert hardware.hardware_type is HardwareType.WHOLE_BODY assert hardware.adapter_type == "openyam_damiao" - assert hardware.address == "can1" - runtime_config = hardware.adapter_kwargs["runtime_config"] - assert isinstance(runtime_config, DamiaoRuntimeConfig) - assert runtime_config.gravity_model_path == OPENYAM_GRAVITY_MODEL_PATH - assert runtime_config.gravity_comp is True - assert len(hardware.joints) == OPENYAM_DOF - assert hardware.gripper_joints == ["arm/gripper"] - assert hardware.gripper_open_position == 1.0 - assert hardware.gripper_closed_position == 0.0 - assert "initial_positions" not in hardware.adapter_kwargs - - direct = make_openyam_hardware( - "arm", - adapter_type="openyam_damiao", - home_joints=[0.1] * OPENYAM_DOF, - ) - assert "initial_positions" not in direct.adapter_kwargs - - -def test_openyam_simulation_hardware_remains_mock(monkeypatch: Any) -> None: + assert hardware.joints == OPENYAM_JOINTS + assert hardware.gripper_joints == [] + assert hardware.wb_config is not None + assert hardware.wb_config.kp == (80.0, 80.0, 80.0, 10.0, 10.0, 10.0, 0.0) + assert hardware.wb_config.kd == (5.0, 5.0, 5.0, 1.5, 1.5, 1.5, 0.0) + runtime = hardware.adapter_kwargs["runtime_config"] + assert isinstance(runtime, DamiaoRuntimeConfig) + assert runtime.bus_addresses == {"can": "can1"} + assert runtime.gravity_comp is True + + +def test_openyam_simulation_uses_generic_whole_body_mock(monkeypatch: Any) -> None: monkeypatch.setattr(global_config, "simulation", "mujoco") - hardware = openyam_hardware("arm") - - assert hardware.adapter_type == "mock" - assert hardware.address is None + hardware = openyam_hardware() + assert hardware.adapter_type == "mock_whole_body" + assert hardware.adapter_kwargs == {} + assert hardware.joints == OPENYAM_JOINTS -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_planner_blueprint_preserves_model_config() -> None: +def test_openyam_planner_blueprint_keeps_gripper_out_of_trajectory() -> None: blueprint = openyam_planner_coordinator kwargs = _module_kwargs(blueprint, ManipulationModule) config = ManipulationModuleConfig(**kwargs).robots[0] + hardware = _coordinator_kwargs(blueprint)["hardware"][0] + trajectory = _coordinator_kwargs(blueprint)["tasks"][0] assert config.name == "arm" assert config.joint_names == [f"joint{i}" for i in range(1, OPENYAM_DOF + 1)] - assert config.end_effector_link == "gripper_tip" - assert config.gripper_hardware_id == "arm" - tasks = _coordinator_kwargs(blueprint)["tasks"] - assert len(tasks) == 1 - trajectory = tasks[0] + assert hardware.joints == OPENYAM_JOINTS assert trajectory.type == "trajectory" - assert trajectory.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] - assert trajectory.priority == 10 + assert trajectory.joint_names == OPENYAM_ARM_JOINTS + assert OPENYAM_GRIPPER_JOINT not in trajectory.joint_names assert all(atom.module is not KeyboardTeleopModule for atom in blueprint.blueprints) -def test_openyam_keyboard_planner_blueprint_combines_teleop_and_trajectory() -> None: - blueprint = keyboard_teleop_openyam_planner - tasks = _coordinator_kwargs(blueprint)["tasks"] +def test_openyam_keyboard_planner_has_independent_idle_gripper_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") gripper = next(task for task in tasks if task.name == "servo_gripper") - assert trajectory.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] + assert trajectory.joint_names == OPENYAM_ARM_JOINTS assert trajectory.priority == 20 - assert eef_twist.joint_names == trajectory.joint_names - assert eef_twist.params["ee_joint_id"] == OPENYAM_DOF - assert eef_twist.params["model_path"] == OPENYAM_GRAVITY_MODEL_PATH - assert eef_twist.priority == 10 - assert gripper.type == "servo" - assert gripper.joint_names == ["arm/gripper"] - assert gripper.params == {"timeout": 0.0, "default_positions": [0.0]} - assert _module_kwargs(blueprint, KeyboardTeleopModule) == {} - - -def test_openyam_coordinator_blueprint_uses_six_arm_joints() -> None: - blueprint = coordinator_openyam - 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 - - -def test_openyam_teleop_blueprint_constructs_with_eef_twist() -> None: - blueprint = keyboard_teleop_openyam - tasks = _coordinator_kwargs(blueprint)["tasks"] - task = next(task for task in tasks if task.type == "eef_twist") + assert eef_twist.joint_names == OPENYAM_ARM_JOINTS + assert eef_twist.params == { + "model_path": OPENYAM_GRAVITY_MODEL_PATH, + "ee_joint_id": OPENYAM_DOF, + } + assert gripper.joint_names == [OPENYAM_GRIPPER_JOINT] + assert gripper.params == {"timeout": 0.0} + + +def test_openyam_coordinator_registers_all_joints_but_arm_task_claims_six() -> None: + kwargs = _coordinator_kwargs(coordinator_openyam) + + assert kwargs["hardware"][0].joints == OPENYAM_JOINTS + assert kwargs["tasks"][0].joint_names == OPENYAM_ARM_JOINTS + + +def test_openyam_teleop_uses_separate_arm_and_gripper_tasks() -> None: + tasks = _coordinator_kwargs(keyboard_teleop_openyam)["tasks"] + eef_twist = next(task for task in tasks if task.type == "eef_twist") gripper = next(task for task in tasks if task.name == "servo_gripper") - assert task.joint_names == [f"arm/joint{i}" for i in range(1, OPENYAM_DOF + 1)] - assert task.params["ee_joint_id"] == OPENYAM_DOF - assert task.params["model_path"] == OPENYAM_GRAVITY_MODEL_PATH - assert gripper.joint_names == ["arm/gripper"] - assert gripper.params["default_positions"] == [0.0] - assert _module_kwargs(blueprint, ManipulationModule)["visualization"] == {"backend": "viser"} + assert eef_twist.joint_names == OPENYAM_ARM_JOINTS + assert gripper.joint_names == [OPENYAM_GRIPPER_JOINT] + assert "default_positions" not in gripper.params + assert _module_kwargs(keyboard_teleop_openyam, ManipulationModule)["visualization"] == { + "backend": "viser" + } diff --git a/docs/adr/0001-let-can-motor-control-own-damiao-topology.md b/docs/adr/0001-let-can-motor-control-own-damiao-topology.md new file mode 100644 index 0000000000..72a4345d33 --- /dev/null +++ b/docs/adr/0001-let-can-motor-control-own-damiao-topology.md @@ -0,0 +1,5 @@ +# Let can-motor-control and adapter subclasses own Damiao topology + +Each Damiao adapter subclass constructs its inherent hardware topology directly with upstream `can_motor_control` types instead of mirroring buses, motors, arm groups, gripper groups, or topology validation in DimOS configuration types. Module configuration contains only deployment and control-policy values that may vary at runtime, including address overrides keyed by the subclass's inherent bus names. Topology construction remains an implementation detail of the subclass, avoiding divergent validation while preserving upstream multi-arm and multi-gripper composition. + +The shared Damiao runtime configuration is limited to named bus-address overrides, the gravity-compensation switch, and the upstream tick deadline. Gains use the existing whole-body configuration; mock transports belong to test subclasses rather than production configuration. diff --git a/docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md b/docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md new file mode 100644 index 0000000000..7d0cde30cf --- /dev/null +++ b/docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md @@ -0,0 +1,21 @@ +# Model can-motor-control robots as whole-body hardware + +DimOS integrates each upstream `can_motor_control.Robot` through `WholeBodyAdapter` as one ordered set of named joints rather than wrapping each arm in `ManipulatorAdapter`. Arm groups contribute angular joints and gripper groups contribute calibrated normalized opening joints; tasks select any arm or gripper subset by name, while the adapter queues all group commands and advances the upstream robot once per control cycle. This matches the existing G1 integration shape and lets the same adapter naturally cover single-arm and dual-arm topologies without inventing richer manipulator behavior that upstream does not provide. + +Whole-body position coordinates are joint-specific: arm positions are radians, while calibrated gripper openings use `[0.0, 1.0]`. Until calibrated opening velocity and physical jaw effort exist, gripper velocity and effort feedback are zero and gripper commands use only the position target; other command fields are ignored for that joint. + +Damiao grippers use the same arbitrated joint-command path as other whole-body joints. The Damiao integration does not add or implement manipulator-specific gripper RPCs; existing xArm and Piper gripper methods remain compatibility paths for those adapters. + +The generic Damiao whole-body adapter provides URDF-based gravity compensation for angular joints. It treats commanded torque as residual torque and sends `tau_command + g(q)` while passing position, velocity, and gains through unchanged; normalized gripper joints are excluded. The gravity-model path is inherent to the robot subclass, a runtime switch enables or disables compensation, and model mismatch or non-finite gravity output prevents activation. + +The generic Damiao adapter and robot-specific subclasses live in the whole-body hardware family and register with its adapter registry. They are not manipulator adapters and do not register with the manipulator hardware family. + +The adapter delegates topology validation, CAN routing, transport errors, lifecycle ordering, and gripper calibration to upstream. DimOS performs gravity-model preflight before activation, converts errors at the whole-body interface, and advances the complete upstream robot once per control cycle; it does not add local transport retries, kernel-interface probes, or per-arm caches. + +Robot subclasses use inherent class properties mapping upstream arm-group names to ordered DimOS arm joints and upstream gripper-group names to normalized DimOS gripper joints. These mappings are the only robot-specific integration metadata; they do not re-describe upstream physical topology. + +Simulation and blueprint tests use a generic in-memory whole-body adapter rather than the manipulator mock. It stores ordered joint state and accepts whole-body command vectors without adding robot-specific behavior. + +The hardware identifier names the physical upstream robot, while logical arm and gripper prefixes remain joint namespaces used by planners and tasks. A single OpenYAM therefore uses hardware identifier `openyam` with `arm/...` joints; a dual-arm robot can use one physical identifier with independent `left_arm/...` and `right_arm/...` joint subsets. + +Gripper servo tasks start without a default target. Upstream calibration establishes real opening feedback, and normal control sends no gripper target until an explicit command arrives; calibration motion to both endpoints remains an activation-time safety consideration. diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 34f3b2264f..1b6b887978 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -20,20 +20,17 @@ 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-piper # Piper 6-DOF -dimos run keyboard-teleop-openyam # OpenYAM 6-DOF + normalized gripper +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's physical blueprint includes its DM4310 gripper by default. Gripper -commands and feedback are normalized: `0.0` is fully closed and `1.0` is fully -open. Enabling the hardware automatically calibrates both mechanical endpoints, -so clear the jaws and workspace before startup. The hardware profile uses CAN -IDs `0x08`/`0x18`, decreasing motor position as the opening direction, and -`0.15` per-unit calibration current. Calibration failure prevents the combined -arm and gripper runtime from enabling. The installed `can-motor-control` build -must expose calibrated `Gripper.opening` feedback; DimOS fails activation rather -than inferring feedback from raw motor angle. +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. Open the Meshcat URL printed in the terminal (default `http://localhost:7000`) to see the robot. diff --git a/docs/capabilities/manipulation/piper_integration.md b/docs/capabilities/manipulation/piper_integration.md index 4df8656ba3..3e7a9158f8 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 can setup can0 ``` For a non-default bitrate, pass `--bitrate` explicitly: ```bash -dimos piper can-activate can0 --bitrate 500000 +dimos 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 can status can0 ``` ## Run a Piper blueprint diff --git a/pyproject.toml b/pyproject.toml index 4d31f51fba..6afc81468e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -397,6 +397,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 = [ diff --git a/uv.lock b/uv.lock index a724ffcf5d..9af2d20466 100644 --- a/uv.lock +++ b/uv.lock @@ -1885,6 +1885,7 @@ browser-tests = [ ] lint = [ { name = "aiortc" }, + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "dimos", extra = ["visualization", "web", "webrtc"] }, { name = "einops" }, @@ -1924,6 +1925,7 @@ lint = [ { name = "xacro" }, ] project-deps = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "dimos", extra = ["visualization", "web", "webrtc"] }, { name = "einops" }, @@ -1946,6 +1948,7 @@ project-deps = [ { name = "xacro" }, ] tests = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "coacd" }, { name = "coverage" }, @@ -1993,6 +1996,7 @@ tests = [ { name = "xacro" }, ] tests-self-hosted = [ + { name = "can-motor-control", marker = "sys_platform == 'linux'" }, { name = "chromadb" }, { name = "coacd" }, { name = "coverage" }, @@ -2122,9 +2126,9 @@ requires-dist = [ { name = "psutil", specifier = ">=7.0.0" }, { name = "pyarrow", marker = "extra == 'learning'" }, { name = "pycollada", marker = "extra == 'manipulation'" }, - { name = "pygame", marker = "extra == 'manipulation'", specifier = ">=2.6.1" }, { 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'" }, @@ -2178,6 +2182,7 @@ 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" }, @@ -2216,6 +2221,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" }, @@ -2238,6 +2244,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" }, @@ -2286,6 +2293,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" }, From b8bb4f3f3e17dbbef9a6d1e3ea8304a4cb5513d5 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 21:21:03 -0700 Subject: [PATCH 26/44] spec: remove --- CONTEXT.md | 33 ------------------- ...t-can-motor-control-own-damiao-topology.md | 5 --- ...an-motor-control-as-whole-body-hardware.md | 21 ------------ 3 files changed, 59 deletions(-) delete mode 100644 CONTEXT.md delete mode 100644 docs/adr/0001-let-can-motor-control-own-damiao-topology.md delete mode 100644 docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md diff --git a/CONTEXT.md b/CONTEXT.md deleted file mode 100644 index 6fa1476b98..0000000000 --- a/CONTEXT.md +++ /dev/null @@ -1,33 +0,0 @@ -# DimOS Robotics - -Language for robot hardware capabilities and their representation across DimOS. - -## Language - -**Gripper opening**: -A calibrated normalized gripper aperture where `0.0` is fully closed and `1.0` is fully open, used consistently for commands and feedback. -_Avoid_: Gripper position, gripper angle, gripper distance - -**Hardware topology**: -The inherent arrangement and identity of a robot's buses, actuator groups, and capabilities. It defines what kind of robot something is and does not vary between runs. -_Avoid_: Runtime configuration, deployment configuration - -**Runtime configuration**: -Deployment and control-policy values that may vary between runs without changing the robot's hardware topology. -_Avoid_: Hardware topology, robot definition - -**Whole-body hardware**: -A physical robot controlled as one ordered set of named joints, with tasks selecting arm, gripper, or other subsets by joint name. -_Avoid_: Manipulator collection, adapter bundle - -**Residual torque**: -Task-requested joint torque added above the gravity compensation computed from the robot model. Zero residual torque requests gravity support without additional task effort. -_Avoid_: Raw motor torque, total torque - -**Hardware identifier**: -The name of the physical owner of connection, lifecycle, state reads, and command writes. -_Avoid_: Arm name, joint prefix - -**Joint namespace**: -The logical prefix that groups joints for planning and task ownership independently of which physical hardware owns them. -_Avoid_: Hardware identifier diff --git a/docs/adr/0001-let-can-motor-control-own-damiao-topology.md b/docs/adr/0001-let-can-motor-control-own-damiao-topology.md deleted file mode 100644 index 72a4345d33..0000000000 --- a/docs/adr/0001-let-can-motor-control-own-damiao-topology.md +++ /dev/null @@ -1,5 +0,0 @@ -# Let can-motor-control and adapter subclasses own Damiao topology - -Each Damiao adapter subclass constructs its inherent hardware topology directly with upstream `can_motor_control` types instead of mirroring buses, motors, arm groups, gripper groups, or topology validation in DimOS configuration types. Module configuration contains only deployment and control-policy values that may vary at runtime, including address overrides keyed by the subclass's inherent bus names. Topology construction remains an implementation detail of the subclass, avoiding divergent validation while preserving upstream multi-arm and multi-gripper composition. - -The shared Damiao runtime configuration is limited to named bus-address overrides, the gravity-compensation switch, and the upstream tick deadline. Gains use the existing whole-body configuration; mock transports belong to test subclasses rather than production configuration. diff --git a/docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md b/docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md deleted file mode 100644 index 7d0cde30cf..0000000000 --- a/docs/adr/0002-model-can-motor-control-as-whole-body-hardware.md +++ /dev/null @@ -1,21 +0,0 @@ -# Model can-motor-control robots as whole-body hardware - -DimOS integrates each upstream `can_motor_control.Robot` through `WholeBodyAdapter` as one ordered set of named joints rather than wrapping each arm in `ManipulatorAdapter`. Arm groups contribute angular joints and gripper groups contribute calibrated normalized opening joints; tasks select any arm or gripper subset by name, while the adapter queues all group commands and advances the upstream robot once per control cycle. This matches the existing G1 integration shape and lets the same adapter naturally cover single-arm and dual-arm topologies without inventing richer manipulator behavior that upstream does not provide. - -Whole-body position coordinates are joint-specific: arm positions are radians, while calibrated gripper openings use `[0.0, 1.0]`. Until calibrated opening velocity and physical jaw effort exist, gripper velocity and effort feedback are zero and gripper commands use only the position target; other command fields are ignored for that joint. - -Damiao grippers use the same arbitrated joint-command path as other whole-body joints. The Damiao integration does not add or implement manipulator-specific gripper RPCs; existing xArm and Piper gripper methods remain compatibility paths for those adapters. - -The generic Damiao whole-body adapter provides URDF-based gravity compensation for angular joints. It treats commanded torque as residual torque and sends `tau_command + g(q)` while passing position, velocity, and gains through unchanged; normalized gripper joints are excluded. The gravity-model path is inherent to the robot subclass, a runtime switch enables or disables compensation, and model mismatch or non-finite gravity output prevents activation. - -The generic Damiao adapter and robot-specific subclasses live in the whole-body hardware family and register with its adapter registry. They are not manipulator adapters and do not register with the manipulator hardware family. - -The adapter delegates topology validation, CAN routing, transport errors, lifecycle ordering, and gripper calibration to upstream. DimOS performs gravity-model preflight before activation, converts errors at the whole-body interface, and advances the complete upstream robot once per control cycle; it does not add local transport retries, kernel-interface probes, or per-arm caches. - -Robot subclasses use inherent class properties mapping upstream arm-group names to ordered DimOS arm joints and upstream gripper-group names to normalized DimOS gripper joints. These mappings are the only robot-specific integration metadata; they do not re-describe upstream physical topology. - -Simulation and blueprint tests use a generic in-memory whole-body adapter rather than the manipulator mock. It stores ordered joint state and accepts whole-body command vectors without adding robot-specific behavior. - -The hardware identifier names the physical upstream robot, while logical arm and gripper prefixes remain joint namespaces used by planners and tasks. A single OpenYAM therefore uses hardware identifier `openyam` with `arm/...` joints; a dual-arm robot can use one physical identifier with independent `left_arm/...` and `right_arm/...` joint subsets. - -Gripper servo tasks start without a default target. Upstream calibration establishes real opening feedback, and normal control sends no gripper target until an explicit command arrives; calibration motion to both endpoints remains an activation-time safety consideration. From 7c14d8cb796e5a5577bdc392f5426be7adde1929 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 1 Aug 2026 22:01:07 -0700 Subject: [PATCH 27/44] fix: clarify OpenYAM adapter topology --- dimos/hardware/whole_body/damiao/adapter.py | 18 +++++++++++++++--- .../hardware/whole_body/damiao/test_adapter.py | 7 +++++++ dimos/hardware/whole_body/mock/adapter.py | 9 +-------- dimos/hardware/whole_body/mock/test_adapter.py | 8 +++++++- .../whole_body/openyam_damiao/adapter.py | 17 +++++++++++------ .../whole_body/openyam_damiao/test_adapter.py | 15 +++++++++++++-- dimos/robot/manipulators/openyam/config.py | 2 +- .../robot/manipulators/openyam/test_openyam.py | 2 +- 8 files changed, 56 insertions(+), 22 deletions(-) diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index 7139b4815c..6cbe6d4eb2 100644 --- a/dimos/hardware/whole_body/damiao/adapter.py +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -40,7 +40,6 @@ class DamiaoWholeBodyAdapter(ABC): arm_joints: dict[str, tuple[str, ...]] = {} gripper_joints: dict[str, str] = {} bus_defaults: dict[str, str] = {} - gravity_model_path: Path | None = None gravity_joint_names: tuple[str, ...] = () def __init__( @@ -52,6 +51,13 @@ def __init__( 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") @@ -94,6 +100,11 @@ def bus_address(self, name: str) -> str: 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.""" @@ -316,9 +327,10 @@ def _refresh(self) -> None: def _load_gravity_model(self) -> None: if not self._runtime_config.gravity_comp: return - if self.gravity_model_path is None or not self.gravity_model_path.is_file(): + 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(self.gravity_model_path)) + self._pin_model = pinocchio.buildModelFromUrdf(str(model_path)) self._pin_data = self._pin_model.createData() def _preflight_gravity(self) -> None: diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index 93c184f747..a3633aace3 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -278,3 +278,10 @@ def test_runtime_config_rejects_unknown_bus_override(dual_robot: FakeRobot) -> N dual_robot, runtime_config=DamiaoRuntimeConfig(bus_addresses={"missing": "can9"}), ) + + +def test_scalar_address_directs_multi_bus_users_to_named_overrides( + dual_robot: FakeRobot, +) -> None: + with pytest.raises(ValueError, match="runtime_config.bus_addresses"): + DualAdapter(dual_robot, address="can0") diff --git a/dimos/hardware/whole_body/mock/adapter.py b/dimos/hardware/whole_body/mock/adapter.py index 27005d9ab9..0111a1f2d3 100644 --- a/dimos/hardware/whole_body/mock/adapter.py +++ b/dimos/hardware/whole_body/mock/adapter.py @@ -16,8 +16,6 @@ from __future__ import annotations -from pathlib import Path - from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState @@ -26,16 +24,11 @@ class MockWholeBodyAdapter: def __init__( self, - address: str | Path | None = None, *, dof: int, initial_positions: list[float] | None = None, - hardware_id: str = "whole_body", - domain_id: int = 0, + **_: object, ) -> None: - del address - del hardware_id - del domain_id positions = initial_positions or [0.0] * dof if len(positions) != dof: raise ValueError(f"expected {dof} initial positions, got {len(positions)}") diff --git a/dimos/hardware/whole_body/mock/test_adapter.py b/dimos/hardware/whole_body/mock/test_adapter.py index d5d87ebf6c..e69616e5e9 100644 --- a/dimos/hardware/whole_body/mock/test_adapter.py +++ b/dimos/hardware/whole_body/mock/test_adapter.py @@ -17,7 +17,13 @@ def test_mock_whole_body_applies_ordered_commands() -> None: - adapter = MockWholeBodyAdapter(dof=2, initial_positions=[0.1, 0.2]) + adapter = MockWholeBodyAdapter( + dof=2, + initial_positions=[0.1, 0.2], + address=None, + hardware_id="test_robot", + domain_id=0, + ) assert adapter.connect() assert adapter.write_motor_commands( diff --git a/dimos/hardware/whole_body/openyam_damiao/adapter.py b/dimos/hardware/whole_body/openyam_damiao/adapter.py index 657cd59ed6..c352b6dcaa 100644 --- a/dimos/hardware/whole_body/openyam_damiao/adapter.py +++ b/dimos/hardware/whole_body/openyam_damiao/adapter.py @@ -28,14 +28,19 @@ 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 = {"can": "can0"} - gravity_model_path = Path(LfsPath("yam_description")) / "urdf/yam_gripper_gravity.urdf" + 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( @@ -55,14 +60,14 @@ def _build_robot(self) -> can_motor_control.Robot: return ( can_motor_control.Robot.builder() .add_bus( - "can", - can_motor_control.SocketCanBus(self.bus_address("can")), + self.bus_name, + can_motor_control.SocketCanBus(self.bus_address(self.bus_name)), damiao.DamiaoCodec(), ) - .add_arm("arm", bus="can", motors=arm_motors) + .add_arm("arm", bus=self.bus_name, motors=arm_motors) .add_gripper( "gripper", - bus="can", + bus=self.bus_name, motor=gripper_motor, opening_direction="decreasing_position", default_current=0.15, diff --git a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py index 64c1169128..6834e61558 100644 --- a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py +++ b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py @@ -12,25 +12,36 @@ # See the License for the specific language governing permissions and # limitations under the License. +import runpy + import can_motor_control 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 +def test_import_does_not_resolve_gravity_model_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_builds_upstream_arm_and_gripper(mocker: MockerFixture) -> None: mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) adapter = OpenYamDamiaoAdapter( runtime_config=DamiaoRuntimeConfig( - bus_addresses={"can": "test_can"}, + bus_addresses={"openyam": "test_can"}, gravity_comp=False, ) ) robot = adapter._build_robot() - assert robot.bus_names() == ["can"] + assert robot.bus_names() == ["openyam"] assert robot.group_names() == ["arm", "gripper"] assert isinstance(robot["arm"], can_motor_control.Arm) assert len(robot["arm"]) == 6 diff --git a/dimos/robot/manipulators/openyam/config.py b/dimos/robot/manipulators/openyam/config.py index bef82ab30d..f303e43586 100644 --- a/dimos/robot/manipulators/openyam/config.py +++ b/dimos/robot/manipulators/openyam/config.py @@ -48,7 +48,7 @@ def openyam_hardware() -> HardwareComponent: adapter_kwargs: dict[str, object] = {} if not global_config.simulation: adapter_kwargs["runtime_config"] = DamiaoRuntimeConfig( - bus_addresses={"can": global_config.can_port or "can0"}, + bus_addresses={"openyam": global_config.can_port or "can0"}, gravity_comp=True, ) return HardwareComponent( diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 25133aead1..1645dca9da 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -79,7 +79,7 @@ def test_openyam_physical_hardware_is_one_whole_body(monkeypatch: Any) -> None: assert hardware.wb_config.kd == (5.0, 5.0, 5.0, 1.5, 1.5, 1.5, 0.0) runtime = hardware.adapter_kwargs["runtime_config"] assert isinstance(runtime, DamiaoRuntimeConfig) - assert runtime.bus_addresses == {"can": "can1"} + assert runtime.bus_addresses == {"openyam": "can1"} assert runtime.gravity_comp is True From 05d014eeb3ca24c7dd2d862121ac77ebfde369e5 Mon Sep 17 00:00:00 2001 From: cc Date: Sun, 2 Aug 2026 22:49:42 -0700 Subject: [PATCH 28/44] test: strengthen OpenYAM driver coverage --- dimos/cli/test_can.py | 201 +++++- dimos/control/test_control.py | 5 +- .../blueprint_config/test_parser.py | 2 +- .../whole_body/damiao/test_adapter.py | 627 +++++++++++++++--- .../hardware/whole_body/mock/test_adapter.py | 54 +- .../whole_body/openyam_damiao/test_adapter.py | 68 +- .../visualization/test_factory.py | 6 - .../manipulators/openyam/test_openyam.py | 84 ++- 8 files changed, 909 insertions(+), 138 deletions(-) diff --git a/dimos/cli/test_can.py b/dimos/cli/test_can.py index c3e90a7f24..a0c603d0dd 100644 --- a/dimos/cli/test_can.py +++ b/dimos/cli/test_can.py @@ -12,49 +12,202 @@ # See the License for the specific language governing permissions and # limitations under the License. -from subprocess import CompletedProcess +import subprocess +import pytest +from pytest_mock import MockerFixture from typer.testing import CliRunner from dimos.cli.can import app -def test_setup_configures_and_verifies_can_interface(mocker) -> None: +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=CompletedProcess([], 0, stdout="4: follower_l: UP qlen 1000\n", stderr=""), + return_value=subprocess.CompletedProcess( + [], + 0, + stdout="4: follower_l: UP qlen 1000\n", + stderr="", + ), ) result = CliRunner().invoke(app, ["setup", "follower_l"]) - assert result.exit_code == 0 + 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 [call.args[0] for call in run.call_args_list] == [ - ["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"], + assert run.call_args_list == [ + mocker.call( + ["ip", "link", "show", "dev", "follower_l"], + check=True, + capture_output=True, + text=True, + ), + mocker.call( + ["sudo", "--", "ip", "link", "set", "dev", "follower_l", "down"], + check=True, + capture_output=False, + text=True, + ), + mocker.call( + [ + "sudo", + "--", + "ip", + "link", + "set", + "dev", + "follower_l", + "type", + "can", + "bitrate", + "1000000", + ], + check=True, + capture_output=False, + text=True, + ), + mocker.call( + [ + "sudo", + "--", + "ip", + "link", + "set", + "dev", + "follower_l", + "txqueuelen", + "1000", + ], + check=True, + capture_output=False, + text=True, + ), + mocker.call( + ["sudo", "--", "ip", "link", "set", "dev", "follower_l", "up"], + check=True, + capture_output=False, + text=True, + ), + mocker.call( + ["ip", "-details", "-statistics", "link", "show", "dev", "follower_l"], + check=True, + capture_output=True, + text=True, + ), ] -def test_setup_rejects_nonpositive_queue_length() -> None: +def test_setup_nonpositive_queue_length_returns_usage_error() -> None: result = CliRunner().invoke(app, ["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 = CliRunner().invoke(app, ["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 = CliRunner().invoke(app, ["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 = CliRunner().invoke(app, ["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 = CliRunner().invoke(app, ["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 = CliRunner().invoke(app, ["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 = CliRunner().invoke(app, ["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 61ac235b03..6068c2524b 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -970,11 +970,10 @@ def test_tick_loop_calls_compute(self, mock_adapter, wait_until): assert mock_task.compute.call_count > 0 - def test_rejected_hardware_write_is_reported(self, monkeypatch): + def test_write_all_hardware_rejected_command_logs_error(self, mocker): hardware = {"arm": MagicMock()} hardware["arm"].write_command.return_value = False - log_error = MagicMock() - monkeypatch.setattr("dimos.control.tick_loop.logger.error", log_error) + log_error = mocker.patch("dimos.control.tick_loop.logger.error") tick_loop = TickLoop( tick_rate=100.0, hardware=hardware, diff --git a/dimos/core/coordination/blueprint_config/test_parser.py b/dimos/core/coordination/blueprint_config/test_parser.py index feec20b511..040b8b5a24 100644 --- a/dimos/core/coordination/blueprint_config/test_parser.py +++ b/dimos/core/coordination/blueprint_config/test_parser.py @@ -413,7 +413,7 @@ class UnionModule(Module): } -def test_parse_overrides_nested_viser_host() -> None: +def test_parse_nested_viser_host_returns_overridden_config() -> None: parsed = BlueprintConfigParser(ManipulationModule.blueprint()).parse( [ "--visualization.backend", diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index a3633aace3..0c3df67918 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -14,6 +14,8 @@ from __future__ import annotations +from collections.abc import Callable, Iterator +from pathlib import Path from typing import cast import can_motor_control @@ -28,32 +30,43 @@ class FakeArm: def __init__(self, positions: list[float]) -> None: - self._positions = np.asarray(positions, dtype=np.float64) + 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._positions + return self.position_values def velocities(self) -> np.ndarray: - return np.zeros_like(self._positions) + return self.velocity_values def torques(self) -> np.ndarray: - return np.zeros_like(self._positions) + 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) @@ -61,7 +74,11 @@ 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 @@ -71,6 +88,8 @@ 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: @@ -80,12 +99,18 @@ def enable(self) -> None: 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 @@ -110,6 +135,35 @@ 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( @@ -123,7 +177,7 @@ def dual_robot() -> FakeRobot: @pytest.fixture -def dual_adapter(dual_robot: FakeRobot, mocker: MockerFixture) -> DualAdapter: +def adapter_factory(mocker: MockerFixture) -> Callable[..., DualAdapter]: mocker.patch.object( DualAdapter, "_require_arm", @@ -134,23 +188,247 @@ def dual_adapter(dual_robot: FakeRobot, mocker: MockerFixture) -> 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 active_dual_adapter( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> Iterator[DualAdapter]: + adapter = adapter_factory(dual_robot, dof=6) + assert adapter.connect() + assert adapter.activate() + yield adapter + 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), - dof=6, ) + + 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( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) assert adapter.connect() assert adapter.activate() - return adapter + adapter.disconnect() + + assert dual_robot.disable_count == 1 + assert not adapter.is_connected() + assert not adapter.has_motor_states() -def test_dual_arm_state_includes_both_normalized_grippers( - dual_adapter: DualAdapter, + +def test_disconnect_disable_failure_still_clears_local_state( dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], ) -> None: - ticks_before = dual_robot.tick_count + 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( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + assert adapter.connect() + assert adapter.activate() + + assert adapter.deactivate() + assert dual_robot.disable_count == 1 + assert not 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 - assert dual_adapter.joint_names == ( + +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_has_motor_states_disconnected_adapter_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + + assert not adapter.has_motor_states() + + +def test_has_motor_states_uncalibrated_grippers_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], +) -> None: + adapter = adapter_factory(dual_robot) + assert adapter.connect() + + assert not adapter.has_motor_states() + + +def test_has_motor_states_activated_adapter_returns_true( + active_dual_adapter: DualAdapter, +) -> None: + assert active_dual_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", @@ -158,7 +436,12 @@ def test_dual_arm_state_includes_both_normalized_grippers( "left_arm/gripper", "right_arm/gripper", ) - assert dual_adapter.read_motor_states() == [ + + +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), @@ -166,122 +449,318 @@ def test_dual_arm_state_includes_both_normalized_grippers( MotorState(q=0.5), MotorState(q=0.6), ] - assert dual_robot.tick_count == ticks_before -def test_combined_command_ticks_once_and_splits_groups( - dual_adapter: DualAdapter, +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 - assert dual_adapter.write_motor_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), - ] - ) + active_dual_adapter.read_motor_states() - assert dual_robot.tick_count == ticks_before + 1 - left_arm = cast("FakeArm", dual_robot["left_arm"]) - right_arm = cast("FakeArm", dual_robot["right_arm"]) - assert left_arm.commands[-1].tolist() == [ + 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].tolist() == [ [10.0, 0.0, 1.0, 16000.0, 0.0], [11.0, 0.0, 1.1, 16000.0, 0.0], ] - assert right_arm.commands[-1].tolist() == [ + assert cast("FakeArm", dual_robot["right_arm"]).commands[-1].tolist() == [ [20.0, 0.0, 2.0, 16000.0, 0.0], [21.0, 0.0, 2.1, 16000.0, 0.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_gripper_command_rejects_out_of_range( - dual_adapter: DualAdapter, +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 dual_adapter.write_motor_commands(commands) + assert not active_dual_adapter.write_motor_commands(commands) + assert cast("FakeArm", dual_robot["left_arm"]).commands == [] + assert cast("FakeArm", dual_robot["right_arm"]).commands == [] assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [] + assert cast("FakeGripper", dual_robot["right_gripper"]).commands == [] -def test_gravity_is_added_to_commanded_residual_torque( - dual_adapter: DualAdapter, +def test_write_motor_commands_nonfinite_arm_value_rejects_without_writes( + active_dual_adapter: DualAdapter, dual_robot: FakeRobot, - mocker: MockerFixture, ) -> None: - mocker.patch.object( - dual_adapter, - "_gravity_torques", - return_value=np.asarray([1.0, 2.0, 3.0, 4.0]), + 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 cast("FakeArm", dual_robot["left_arm"]).commands == [] + assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [] + + +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), ) - commands = [MotorCommand(q=0.0, tau=0.5)] * 4 + [MotorCommand(q=0.5)] * 2 - assert dual_adapter.write_motor_commands(commands) + assert not adapter.connect() + assert dual_robot.disable_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_connect_existing_gravity_model_loads_model( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + mocker: MockerFixture, + tmp_path: Path, +) -> None: + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + model = FakePinModel() + build_model = mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + return_value=model, + ) + adapter = GravityDualAdapter( + dual_robot, + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + ) + + assert adapter.connect() + build_model.assert_called_once_with(str(model_path)) -def test_activation_failure_disables_robot( +def test_activate_gravity_model_dimension_mismatch_returns_false( dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], mocker: MockerFixture, + tmp_path: Path, ) -> None: - 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], + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + return_value=FakePinModel(nq=3), ) - adapter = DualAdapter( + adapter = GravityDualAdapter( dual_robot, - runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), ) assert adapter.connect() - dual_robot.enable_error = RuntimeError("calibration failed") assert not adapter.activate() assert dual_robot.disable_count == 1 -def test_gripper_state_becomes_available_only_after_calibration( +def test_activate_gravity_joint_order_mismatch_returns_false( dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], mocker: MockerFixture, + tmp_path: Path, ) -> None: - 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], + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + return_value=FakePinModel(names=("universe", "right1", "left2", "left1", "right2")), ) - adapter = DualAdapter( + adapter = GravityDualAdapter( dual_robot, - runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), ) + assert adapter.connect() - assert not adapter.has_motor_states() + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_activate_nonfinite_arm_positions_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + mocker: MockerFixture, + tmp_path: Path, +) -> None: + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + return_value=FakePinModel(), + ) + adapter = GravityDualAdapter( + dual_robot, + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + ) assert adapter.connect() - assert not adapter.has_motor_states() - assert adapter.activate() - assert adapter.has_motor_states() + cast("FakeArm", dual_robot["left_arm"]).position_values[0] = np.nan + assert not adapter.activate() + assert dual_robot.disable_count == 1 -def test_runtime_config_rejects_unknown_bus_override(dual_robot: FakeRobot) -> None: - with pytest.raises(ValueError, match="unknown CAN bus"): - DualAdapter( - dual_robot, - runtime_config=DamiaoRuntimeConfig(bus_addresses={"missing": "can9"}), - ) +def test_activate_nonfinite_gravity_output_returns_false( + dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + mocker: MockerFixture, + tmp_path: Path, +) -> None: + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + return_value=FakePinModel(), + ) + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.computeGeneralizedGravity", + return_value=np.asarray([1.0, 2.0, np.nan, 4.0]), + ) + adapter = GravityDualAdapter( + dual_robot, + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + ) + assert adapter.connect() -def test_scalar_address_directs_multi_bus_users_to_named_overrides( + assert not adapter.activate() + assert dual_robot.disable_count == 1 + + +def test_write_motor_commands_gravity_enabled_adds_computed_torque( dual_robot: FakeRobot, + adapter_factory: Callable[..., DualAdapter], + mocker: MockerFixture, + tmp_path: Path, ) -> None: - with pytest.raises(ValueError, match="runtime_config.bus_addresses"): - DualAdapter(dual_robot, address="can0") + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + mocker.patch( + "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", + return_value=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]), + ) + adapter = GravityDualAdapter( + dual_robot, + model_path, + runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + ) + 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] diff --git a/dimos/hardware/whole_body/mock/test_adapter.py b/dimos/hardware/whole_body/mock/test_adapter.py index e69616e5e9..b0715e1713 100644 --- a/dimos/hardware/whole_body/mock/test_adapter.py +++ b/dimos/hardware/whole_body/mock/test_adapter.py @@ -12,11 +12,13 @@ # 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_mock_whole_body_applies_ordered_commands() -> None: +def test_write_motor_commands_connected_adapter_applies_ordered_commands() -> None: adapter = MockWholeBodyAdapter( dof=2, initial_positions=[0.1, 0.2], @@ -40,9 +42,57 @@ def test_mock_whole_body_applies_ordered_commands() -> None: assert adapter.read_imu() == IMUState() -def test_mock_whole_body_rejects_wrong_command_count() -> None: +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_disconnect_connected_adapter_clears_connection_and_state_availability() -> None: + adapter = MockWholeBodyAdapter(dof=1) + assert adapter.connect() + + adapter.disconnect() + + assert not adapter.is_connected() + assert not adapter.has_motor_states() + + +def test_activate_disconnected_adapter_returns_false() -> None: + adapter = MockWholeBodyAdapter(dof=1) + + assert not adapter.activate() + + +def test_deactivate_disconnected_adapter_returns_false() -> None: + adapter = MockWholeBodyAdapter(dof=1) + + assert not adapter.deactivate() + + +def test_activate_connected_adapter_returns_true() -> None: + adapter = MockWholeBodyAdapter(dof=1) + assert adapter.connect() + + assert adapter.activate() + + +def test_deactivate_connected_adapter_returns_true() -> None: + adapter = MockWholeBodyAdapter(dof=1) + assert adapter.connect() + + assert adapter.deactivate() diff --git a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py index 6834e61558..012997c5ce 100644 --- a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py +++ b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py @@ -22,7 +22,7 @@ from dimos.hardware.whole_body.openyam_damiao.adapter import OpenYamDamiaoAdapter -def test_import_does_not_resolve_gravity_model_lfs(mocker: MockerFixture) -> None: +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__) @@ -30,7 +30,9 @@ def test_import_does_not_resolve_gravity_model_lfs(mocker: MockerFixture) -> Non get_data.assert_not_called() -def test_openyam_builds_upstream_arm_and_gripper(mocker: MockerFixture) -> None: +def test_build_robot_openyam_topology_builds_expected_arm_and_gripper( + mocker: MockerFixture, +) -> None: mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) adapter = OpenYamDamiaoAdapter( runtime_config=DamiaoRuntimeConfig( @@ -46,6 +48,52 @@ def test_openyam_builds_upstream_arm_and_gripper(mocker: MockerFixture) -> None: assert isinstance(robot["arm"], can_motor_control.Arm) assert len(robot["arm"]) == 6 assert isinstance(robot["gripper"], can_motor_control.Gripper) + motor_addresses = [ + ( + robot["arm"]["yam_joint1"].name, + robot["arm"]["yam_joint1"].send_id, + robot["arm"]["yam_joint1"].recv_id, + ), + ( + robot["arm"]["yam_joint2"].name, + robot["arm"]["yam_joint2"].send_id, + robot["arm"]["yam_joint2"].recv_id, + ), + ( + robot["arm"]["yam_joint3"].name, + robot["arm"]["yam_joint3"].send_id, + robot["arm"]["yam_joint3"].recv_id, + ), + ( + robot["arm"]["yam_joint4"].name, + robot["arm"]["yam_joint4"].send_id, + robot["arm"]["yam_joint4"].recv_id, + ), + ( + robot["arm"]["yam_joint5"].name, + robot["arm"]["yam_joint5"].send_id, + robot["arm"]["yam_joint5"].recv_id, + ), + ( + robot["arm"]["yam_joint6"].name, + robot["arm"]["yam_joint6"].send_id, + robot["arm"]["yam_joint6"].recv_id, + ), + ] + assert motor_addresses == [ + ("yam_joint1", 1, 17), + ("yam_joint2", 2, 18), + ("yam_joint3", 3, 19), + ("yam_joint4", 4, 20), + ("yam_joint5", 5, 21), + ("yam_joint6", 6, 22), + ] + gripper_motor = robot["gripper"].motor + assert (gripper_motor.name, gripper_motor.send_id, gripper_motor.recv_id) == ( + "yam_gripper", + 8, + 24, + ) assert adapter.joint_names == ( "arm/joint1", "arm/joint2", @@ -55,3 +103,19 @@ def test_openyam_builds_upstream_arm_and_gripper(mocker: MockerFixture) -> None: "arm/joint6", "arm/gripper", ) + + +def test_connect_mock_can_bus_validates_real_upstream_groups( + mocker: MockerFixture, +) -> None: + mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) + adapter = OpenYamDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) + + assert adapter.connect() + try: + assert adapter.is_connected() + assert not adapter.has_motor_states() + finally: + adapter.disconnect() diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index 9f08539e26..9f952f0deb 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -268,12 +268,6 @@ def test_config_validates_viser_visualization() -> None: assert config.visualization.panel_enabled is False -def test_viser_config_defaults_to_loopback_host() -> None: - config = ViserVisualizationConfig() - - assert config.host == "127.0.0.1" - - def test_config_meshcat_requires_world_visualization() -> None: config = ManipulationModuleConfig.model_validate({"visualization": {"backend": "meshcat"}}) diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index 1645dca9da..cdde1e073b 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -14,6 +14,8 @@ 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 @@ -39,7 +41,6 @@ make_openyam_model_config, openyam_hardware, ) -from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule def _module_kwargs(blueprint: Blueprint, module_type: type) -> dict[str, Any]: @@ -50,12 +51,17 @@ def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]: return _module_kwargs(blueprint, ControlCoordinator) -def test_openyam_model_config_maps_only_six_arm_joints() -> None: +def test_make_openyam_model_config_default_name_maps_only_six_arm_joints() -> None: config = make_openyam_model_config(name="arm") - assert config.joint_names == [f"joint{i}" for i in range(1, OPENYAM_DOF + 1)] + assert config.joint_names == ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] assert config.joint_name_mapping == { - f"arm/joint{i}": f"joint{i}" for i in range(1, OPENYAM_DOF + 1) + "arm/joint1": "joint1", + "arm/joint2": "joint2", + "arm/joint3": "joint3", + "arm/joint4": "joint4", + "arm/joint5": "joint5", + "arm/joint6": "joint6", } assert config.base_link == "base" assert config.end_effector_link == "gripper_tip" @@ -63,7 +69,9 @@ def test_openyam_model_config_maps_only_six_arm_joints() -> None: assert config.gripper_hardware_id is None -def test_openyam_physical_hardware_is_one_whole_body(monkeypatch: Any) -> None: +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") @@ -83,7 +91,9 @@ def test_openyam_physical_hardware_is_one_whole_body(monkeypatch: Any) -> None: assert runtime.gravity_comp is True -def test_openyam_simulation_uses_generic_whole_body_mock(monkeypatch: Any) -> None: +def test_openyam_hardware_simulation_mode_returns_generic_whole_body_mock( + monkeypatch: pytest.MonkeyPatch, +) -> None: monkeypatch.setattr(global_config, "simulation", "mujoco") hardware = openyam_hardware() @@ -93,54 +103,76 @@ def test_openyam_simulation_uses_generic_whole_body_mock(monkeypatch: Any) -> No assert hardware.joints == OPENYAM_JOINTS -def test_openyam_planner_blueprint_keeps_gripper_out_of_trajectory() -> None: - blueprint = openyam_planner_coordinator - kwargs = _module_kwargs(blueprint, ManipulationModule) - config = ManipulationModuleConfig(**kwargs).robots[0] - hardware = _coordinator_kwargs(blueprint)["hardware"][0] - trajectory = _coordinator_kwargs(blueprint)["tasks"][0] +def test_openyam_planner_coordinator_trajectory_claims_only_arm_joints() -> None: + hardware = _coordinator_kwargs(openyam_planner_coordinator)["hardware"][0] + trajectory = _coordinator_kwargs(openyam_planner_coordinator)["tasks"][0] - assert config.name == "arm" - assert config.joint_names == [f"joint{i}" for i in range(1, OPENYAM_DOF + 1)] assert hardware.joints == OPENYAM_JOINTS assert trajectory.type == "trajectory" assert trajectory.joint_names == OPENYAM_ARM_JOINTS assert OPENYAM_GRIPPER_JOINT not in trajectory.joint_names - assert all(atom.module is not KeyboardTeleopModule for atom in blueprint.blueprints) -def test_openyam_keyboard_planner_has_independent_idle_gripper_task() -> None: +def test_openyam_planner_coordinator_model_uses_arm_planning_group() -> None: + kwargs = _module_kwargs(openyam_planner_coordinator, ManipulationModule) + config = ManipulationModuleConfig(**kwargs).robots[0] + + assert config.name == "arm" + assert config.joint_names == ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] + + +def test_keyboard_teleop_openyam_planner_eef_task_controls_only_arm() -> 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") - gripper = next(task for task in tasks if task.name == "servo_gripper") - assert trajectory.joint_names == OPENYAM_ARM_JOINTS - assert trajectory.priority == 20 assert eef_twist.joint_names == OPENYAM_ARM_JOINTS assert eef_twist.params == { "model_path": OPENYAM_GRAVITY_MODEL_PATH, "ee_joint_id": OPENYAM_DOF, } + + +def test_keyboard_teleop_openyam_planner_gripper_task_is_independent_and_idle() -> None: + tasks = _coordinator_kwargs(keyboard_teleop_openyam_planner)["tasks"] + gripper = next(task for task in tasks if task.name == "servo_gripper") + assert gripper.joint_names == [OPENYAM_GRIPPER_JOINT] assert gripper.params == {"timeout": 0.0} -def test_openyam_coordinator_registers_all_joints_but_arm_task_claims_six() -> None: +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_coordinator_openyam_arm_task_claims_six_of_seven_registered_joints() -> None: kwargs = _coordinator_kwargs(coordinator_openyam) assert kwargs["hardware"][0].joints == OPENYAM_JOINTS assert kwargs["tasks"][0].joint_names == OPENYAM_ARM_JOINTS -def test_openyam_teleop_uses_separate_arm_and_gripper_tasks() -> None: +def test_keyboard_teleop_openyam_eef_task_controls_only_arm() -> None: tasks = _coordinator_kwargs(keyboard_teleop_openyam)["tasks"] eef_twist = next(task for task in tasks if task.type == "eef_twist") - gripper = next(task for task in tasks if task.name == "servo_gripper") assert eef_twist.joint_names == OPENYAM_ARM_JOINTS + + +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 - assert _module_kwargs(keyboard_teleop_openyam, ManipulationModule)["visualization"] == { - "backend": "viser" - } + + +def test_keyboard_teleop_openyam_visualization_uses_viser_backend() -> None: + visualization = _module_kwargs(keyboard_teleop_openyam, ManipulationModule)["visualization"] + + assert visualization == {"backend": "viser"} From 52091b3a11283efd2076f1db016958da058c6c69 Mon Sep 17 00:00:00 2001 From: cc Date: Sun, 2 Aug 2026 23:29:22 -0700 Subject: [PATCH 29/44] test: refocus OpenYAM driver coverage --- dimos/cli/test_can.py | 93 +++---- dimos/control/test_control.py | 3 - .../whole_body/damiao/test_adapter.py | 235 ++++++++---------- .../hardware/whole_body/mock/test_adapter.py | 41 +-- .../whole_body/openyam_damiao/adapter.py | 13 +- .../whole_body/openyam_damiao/test_adapter.py | 102 ++------ .../manipulators/openyam/test_openyam.py | 106 ++------ 7 files changed, 190 insertions(+), 403 deletions(-) diff --git a/dimos/cli/test_can.py b/dimos/cli/test_can.py index a0c603d0dd..2b235351bc 100644 --- a/dimos/cli/test_can.py +++ b/dimos/cli/test_can.py @@ -13,6 +13,7 @@ # limitations under the License. import subprocess +from unittest.mock import Mock import pytest from pytest_mock import MockerFixture @@ -21,6 +22,10 @@ from dimos.cli.can import app +def _subprocess_argv(run: Mock) -> list[list[str]]: + return [call.args[0] for call in run.call_args_list] + + def test_setup_valid_options_configures_and_verifies_can_interface( mocker: MockerFixture, ) -> None: @@ -40,65 +45,35 @@ def test_setup_valid_options_configures_and_verifies_can_interface( 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 run.call_args_list == [ - mocker.call( - ["ip", "link", "show", "dev", "follower_l"], - check=True, - capture_output=True, - text=True, - ), - mocker.call( - ["sudo", "--", "ip", "link", "set", "dev", "follower_l", "down"], - check=True, - capture_output=False, - text=True, - ), - mocker.call( - [ - "sudo", - "--", - "ip", - "link", - "set", - "dev", - "follower_l", - "type", - "can", - "bitrate", - "1000000", - ], - check=True, - capture_output=False, - text=True, - ), - mocker.call( - [ - "sudo", - "--", - "ip", - "link", - "set", - "dev", - "follower_l", - "txqueuelen", - "1000", - ], - check=True, - capture_output=False, - text=True, - ), - mocker.call( - ["sudo", "--", "ip", "link", "set", "dev", "follower_l", "up"], - check=True, - capture_output=False, - text=True, - ), - mocker.call( - ["ip", "-details", "-statistics", "link", "show", "dev", "follower_l"], - check=True, - capture_output=True, - text=True, - ), + 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"], ] diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 6068c2524b..e08e46f91c 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -985,9 +985,6 @@ def test_write_all_hardware_rejected_command_logs_error(self, mocker): tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.25}, ControlMode.SERVO_POSITION)}) - hardware["arm"].write_command.assert_called_once_with( - {"arm/joint1": 0.25}, ControlMode.SERVO_POSITION - ) log_error.assert_called_once_with( "Hardware arm rejected SERVO_POSITION command from control task" ) diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index 0c3df67918..cf3373edcd 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -17,6 +17,7 @@ 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 @@ -115,6 +116,15 @@ def tick(self, _deadline: int) -> None: 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 = { @@ -197,17 +207,55 @@ def create(robot: FakeRobot, **kwargs: object) -> DualAdapter: @pytest.fixture -def active_dual_adapter( +def connected_dual_adapter( dual_robot: FakeRobot, adapter_factory: Callable[..., DualAdapter], ) -> Iterator[DualAdapter]: adapter = adapter_factory(dual_robot, dof=6) assert adapter.connect() - assert adapter.activate() 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: @@ -303,18 +351,18 @@ def test_connect_invalid_upstream_group_rolls_back_robot( def test_disconnect_connected_robot_disables_and_clears_state( + connected_dual_adapter: DualAdapter, dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], ) -> None: - adapter = adapter_factory(dual_robot) - assert adapter.connect() - assert adapter.activate() + assert not connected_dual_adapter.has_motor_states() + assert connected_dual_adapter.activate() + assert connected_dual_adapter.has_motor_states() - adapter.disconnect() + connected_dual_adapter.disconnect() assert dual_robot.disable_count == 1 - assert not adapter.is_connected() - assert not adapter.has_motor_states() + assert not connected_dual_adapter.is_connected() + assert not connected_dual_adapter.has_motor_states() def test_disconnect_disable_failure_still_clears_local_state( @@ -355,16 +403,13 @@ def test_activate_enable_failure_disables_robot( def test_deactivate_connected_adapter_disables_robot( + active_dual_adapter: DualAdapter, dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], ) -> None: - adapter = adapter_factory(dual_robot) - assert adapter.connect() - assert adapter.activate() - - assert adapter.deactivate() + assert active_dual_adapter.has_motor_states() + assert active_dual_adapter.deactivate() assert dual_robot.disable_count == 1 - assert not adapter.has_motor_states() + assert not active_dual_adapter.has_motor_states() def test_deactivate_disconnected_adapter_returns_false( @@ -390,31 +435,6 @@ def test_deactivate_disable_failure_returns_false( assert adapter.has_motor_states() -def test_has_motor_states_disconnected_adapter_returns_false( - dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], -) -> None: - adapter = adapter_factory(dual_robot) - - assert not adapter.has_motor_states() - - -def test_has_motor_states_uncalibrated_grippers_returns_false( - dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], -) -> None: - adapter = adapter_factory(dual_robot) - assert adapter.connect() - - assert not adapter.has_motor_states() - - -def test_has_motor_states_activated_adapter_returns_true( - active_dual_adapter: DualAdapter, -) -> None: - assert active_dual_adapter.has_motor_states() - - def test_read_motor_states_disconnected_adapter_raises_runtime_error( dual_robot: FakeRobot, adapter_factory: Callable[..., DualAdapter], @@ -532,13 +552,25 @@ def test_write_motor_commands_multiple_arms_routes_ordered_values( assert active_dual_adapter.write_motor_commands(commands) - assert cast("FakeArm", dual_robot["left_arm"]).commands[-1].tolist() == [ - [10.0, 0.0, 1.0, 16000.0, 0.0], - [11.0, 0.0, 1.1, 16000.0, 0.0], - ] - assert cast("FakeArm", dual_robot["right_arm"]).commands[-1].tolist() == [ - [20.0, 0.0, 2.0, 16000.0, 0.0], - [21.0, 0.0, 2.1, 16000.0, 0.0], + 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, ] @@ -572,10 +604,7 @@ def test_write_motor_commands_out_of_range_gripper_rejects_without_writes( commands = [MotorCommand(q=0.0)] * 4 + [MotorCommand(q=-0.1), MotorCommand(q=0.5)] assert not active_dual_adapter.write_motor_commands(commands) - assert cast("FakeArm", dual_robot["left_arm"]).commands == [] - assert cast("FakeArm", dual_robot["right_arm"]).commands == [] - assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [] - assert cast("FakeGripper", dual_robot["right_gripper"]).commands == [] + assert dual_robot.command_count() == 0 def test_write_motor_commands_nonfinite_arm_value_rejects_without_writes( @@ -585,8 +614,7 @@ def test_write_motor_commands_nonfinite_arm_value_rejects_without_writes( 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 cast("FakeArm", dual_robot["left_arm"]).commands == [] - assert cast("FakeGripper", dual_robot["left_gripper"]).commands == [] + assert dual_robot.command_count() == 0 def test_write_motor_commands_upstream_tick_failure_returns_false( @@ -614,45 +642,20 @@ def test_connect_missing_gravity_model_rolls_back_robot( def test_connect_existing_gravity_model_loads_model( - dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], - mocker: MockerFixture, - tmp_path: Path, + gravity_adapter_factory: Callable[..., GravityDualAdapter], + pin_model_builder: Mock, ) -> None: - model_path = tmp_path / "robot.urdf" - model_path.write_text("") - model = FakePinModel() - build_model = mocker.patch( - "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", - return_value=model, - ) - adapter = GravityDualAdapter( - dual_robot, - model_path, - runtime_config=DamiaoRuntimeConfig(gravity_comp=True), - ) + adapter = gravity_adapter_factory(model=FakePinModel()) assert adapter.connect() - build_model.assert_called_once_with(str(model_path)) + pin_model_builder.assert_called_once() def test_activate_gravity_model_dimension_mismatch_returns_false( dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], - mocker: MockerFixture, - tmp_path: Path, + gravity_adapter_factory: Callable[..., GravityDualAdapter], ) -> None: - model_path = tmp_path / "robot.urdf" - model_path.write_text("") - mocker.patch( - "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", - return_value=FakePinModel(nq=3), - ) - adapter = GravityDualAdapter( - dual_robot, - model_path, - runtime_config=DamiaoRuntimeConfig(gravity_comp=True), - ) + adapter = gravity_adapter_factory(model=FakePinModel(nq=3)) assert adapter.connect() assert not adapter.activate() @@ -661,20 +664,10 @@ def test_activate_gravity_model_dimension_mismatch_returns_false( def test_activate_gravity_joint_order_mismatch_returns_false( dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], - mocker: MockerFixture, - tmp_path: Path, + gravity_adapter_factory: Callable[..., GravityDualAdapter], ) -> None: - model_path = tmp_path / "robot.urdf" - model_path.write_text("") - mocker.patch( - "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", - return_value=FakePinModel(names=("universe", "right1", "left2", "left1", "right2")), - ) - adapter = GravityDualAdapter( - dual_robot, - model_path, - runtime_config=DamiaoRuntimeConfig(gravity_comp=True), + adapter = gravity_adapter_factory( + model=FakePinModel(names=("universe", "right1", "left2", "left1", "right2")), ) assert adapter.connect() @@ -684,21 +677,9 @@ def test_activate_gravity_joint_order_mismatch_returns_false( def test_activate_nonfinite_arm_positions_returns_false( dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], - mocker: MockerFixture, - tmp_path: Path, + gravity_adapter_factory: Callable[..., GravityDualAdapter], ) -> None: - model_path = tmp_path / "robot.urdf" - model_path.write_text("") - mocker.patch( - "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", - return_value=FakePinModel(), - ) - adapter = GravityDualAdapter( - dual_robot, - model_path, - runtime_config=DamiaoRuntimeConfig(gravity_comp=True), - ) + adapter = gravity_adapter_factory(model=FakePinModel()) assert adapter.connect() cast("FakeArm", dual_robot["left_arm"]).position_values[0] = np.nan @@ -708,25 +689,14 @@ def test_activate_nonfinite_arm_positions_returns_false( def test_activate_nonfinite_gravity_output_returns_false( dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], + gravity_adapter_factory: Callable[..., GravityDualAdapter], mocker: MockerFixture, - tmp_path: Path, ) -> None: - model_path = tmp_path / "robot.urdf" - model_path.write_text("") - mocker.patch( - "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", - return_value=FakePinModel(), - ) + 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]), ) - adapter = GravityDualAdapter( - dual_robot, - model_path, - runtime_config=DamiaoRuntimeConfig(gravity_comp=True), - ) assert adapter.connect() assert not adapter.activate() @@ -735,25 +705,14 @@ def test_activate_nonfinite_gravity_output_returns_false( def test_write_motor_commands_gravity_enabled_adds_computed_torque( dual_robot: FakeRobot, - adapter_factory: Callable[..., DualAdapter], + gravity_adapter_factory: Callable[..., GravityDualAdapter], mocker: MockerFixture, - tmp_path: Path, ) -> None: - model_path = tmp_path / "robot.urdf" - model_path.write_text("") - mocker.patch( - "dimos.hardware.whole_body.damiao.adapter.pinocchio.buildModelFromUrdf", - return_value=FakePinModel(), - ) + 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]), ) - adapter = GravityDualAdapter( - dual_robot, - model_path, - runtime_config=DamiaoRuntimeConfig(gravity_comp=True), - ) assert adapter.connect() assert adapter.activate() compute_gravity.reset_mock() diff --git a/dimos/hardware/whole_body/mock/test_adapter.py b/dimos/hardware/whole_body/mock/test_adapter.py index b0715e1713..0e93d84e0e 100644 --- a/dimos/hardware/whole_body/mock/test_adapter.py +++ b/dimos/hardware/whole_body/mock/test_adapter.py @@ -19,13 +19,7 @@ def test_write_motor_commands_connected_adapter_applies_ordered_commands() -> None: - adapter = MockWholeBodyAdapter( - dof=2, - initial_positions=[0.1, 0.2], - address=None, - hardware_id="test_robot", - domain_id=0, - ) + adapter = MockWholeBodyAdapter(dof=2, initial_positions=[0.1, 0.2]) assert adapter.connect() assert adapter.write_motor_commands( @@ -62,37 +56,18 @@ def test_write_motor_commands_disconnected_adapter_rejects_command() -> None: assert adapter.read_motor_states() == [MotorState()] -def test_disconnect_connected_adapter_clears_connection_and_state_availability() -> None: +def test_connection_lifecycle_controls_availability_and_activation() -> None: adapter = MockWholeBodyAdapter(dof=1) - assert adapter.connect() - - adapter.disconnect() - - assert not adapter.is_connected() - assert not adapter.has_motor_states() - - -def test_activate_disconnected_adapter_returns_false() -> None: - adapter = MockWholeBodyAdapter(dof=1) - assert not adapter.activate() - - -def test_deactivate_disconnected_adapter_returns_false() -> None: - adapter = MockWholeBodyAdapter(dof=1) - assert not adapter.deactivate() - -def test_activate_connected_adapter_returns_true() -> None: - adapter = MockWholeBodyAdapter(dof=1) assert adapter.connect() - + assert adapter.is_connected() + assert adapter.has_motor_states() assert adapter.activate() + assert adapter.deactivate() + adapter.disconnect() -def test_deactivate_connected_adapter_returns_true() -> None: - adapter = MockWholeBodyAdapter(dof=1) - assert adapter.connect() - - assert adapter.deactivate() + assert not adapter.is_connected() + assert not adapter.has_motor_states() diff --git a/dimos/hardware/whole_body/openyam_damiao/adapter.py b/dimos/hardware/whole_body/openyam_damiao/adapter.py index c352b6dcaa..9493f506d1 100644 --- a/dimos/hardware/whole_body/openyam_damiao/adapter.py +++ b/dimos/hardware/whole_body/openyam_damiao/adapter.py @@ -43,13 +43,12 @@ def gravity_model_path(self) -> Path: def _build_robot(self) -> can_motor_control.Robot: arm_motors = [ - can_motor_control.MotorSpec( - f"yam_joint{index}", - damiao.MotorType.DM4340 if index <= 3 else damiao.MotorType.DM4310, - index, - index | 0x10, - ) - for index in range(1, 7) + 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", diff --git a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py index 012997c5ce..386fe27af6 100644 --- a/dimos/hardware/whole_body/openyam_damiao/test_adapter.py +++ b/dimos/hardware/whole_body/openyam_damiao/test_adapter.py @@ -12,14 +12,27 @@ # 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: @@ -30,92 +43,13 @@ def test_import_lazy_gravity_model_does_not_resolve_lfs(mocker: MockerFixture) - get_data.assert_not_called() -def test_build_robot_openyam_topology_builds_expected_arm_and_gripper( - mocker: MockerFixture, +def test_openyam_topology_connects_arm_and_gripper( + openyam_adapter: OpenYamDamiaoAdapter, ) -> None: - mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig( - bus_addresses={"openyam": "test_can"}, - gravity_comp=False, - ) - ) - - robot = adapter._build_robot() + robot = openyam_adapter._build_robot() - assert robot.bus_names() == ["openyam"] assert robot.group_names() == ["arm", "gripper"] assert isinstance(robot["arm"], can_motor_control.Arm) - assert len(robot["arm"]) == 6 + assert len(robot["arm"]) == OPENYAM_DOF assert isinstance(robot["gripper"], can_motor_control.Gripper) - motor_addresses = [ - ( - robot["arm"]["yam_joint1"].name, - robot["arm"]["yam_joint1"].send_id, - robot["arm"]["yam_joint1"].recv_id, - ), - ( - robot["arm"]["yam_joint2"].name, - robot["arm"]["yam_joint2"].send_id, - robot["arm"]["yam_joint2"].recv_id, - ), - ( - robot["arm"]["yam_joint3"].name, - robot["arm"]["yam_joint3"].send_id, - robot["arm"]["yam_joint3"].recv_id, - ), - ( - robot["arm"]["yam_joint4"].name, - robot["arm"]["yam_joint4"].send_id, - robot["arm"]["yam_joint4"].recv_id, - ), - ( - robot["arm"]["yam_joint5"].name, - robot["arm"]["yam_joint5"].send_id, - robot["arm"]["yam_joint5"].recv_id, - ), - ( - robot["arm"]["yam_joint6"].name, - robot["arm"]["yam_joint6"].send_id, - robot["arm"]["yam_joint6"].recv_id, - ), - ] - assert motor_addresses == [ - ("yam_joint1", 1, 17), - ("yam_joint2", 2, 18), - ("yam_joint3", 3, 19), - ("yam_joint4", 4, 20), - ("yam_joint5", 5, 21), - ("yam_joint6", 6, 22), - ] - gripper_motor = robot["gripper"].motor - assert (gripper_motor.name, gripper_motor.send_id, gripper_motor.recv_id) == ( - "yam_gripper", - 8, - 24, - ) - assert adapter.joint_names == ( - "arm/joint1", - "arm/joint2", - "arm/joint3", - "arm/joint4", - "arm/joint5", - "arm/joint6", - "arm/gripper", - ) - - -def test_connect_mock_can_bus_validates_real_upstream_groups( - mocker: MockerFixture, -) -> None: - mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) - adapter = OpenYamDamiaoAdapter( - runtime_config=DamiaoRuntimeConfig(gravity_comp=False), - ) - - assert adapter.connect() - try: - assert adapter.is_connected() - assert not adapter.has_motor_states() - finally: - adapter.disconnect() + assert openyam_adapter.connect() diff --git a/dimos/robot/manipulators/openyam/test_openyam.py b/dimos/robot/manipulators/openyam/test_openyam.py index cdde1e073b..cbb67bef4b 100644 --- a/dimos/robot/manipulators/openyam/test_openyam.py +++ b/dimos/robot/manipulators/openyam/test_openyam.py @@ -20,8 +20,6 @@ from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import Blueprint from dimos.core.global_config import global_config -from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig -from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig from dimos.robot.manipulators.openyam.blueprints.basic import ( coordinator_openyam, openyam_planner_coordinator, @@ -33,11 +31,9 @@ from dimos.robot.manipulators.openyam.config import ( OPENYAM_ARM_JOINTS, OPENYAM_DOF, - OPENYAM_GRAVITY_MODEL_PATH, OPENYAM_GRIPPER_JOINT, OPENYAM_HARDWARE_ID, OPENYAM_JOINTS, - OPENYAM_PACKAGE_PATHS, make_openyam_model_config, openyam_hardware, ) @@ -51,22 +47,14 @@ def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]: return _module_kwargs(blueprint, ControlCoordinator) -def test_make_openyam_model_config_default_name_maps_only_six_arm_joints() -> None: +def test_make_openyam_model_config_maps_only_arm_joints() -> None: config = make_openyam_model_config(name="arm") - assert config.joint_names == ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] - assert config.joint_name_mapping == { - "arm/joint1": "joint1", - "arm/joint2": "joint2", - "arm/joint3": "joint3", - "arm/joint4": "joint4", - "arm/joint5": "joint5", - "arm/joint6": "joint6", - } + 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" - assert list(config.package_paths) == list(OPENYAM_PACKAGE_PATHS) - assert config.gripper_hardware_id is None def test_openyam_hardware_physical_mode_returns_one_whole_body( @@ -77,18 +65,12 @@ def test_openyam_hardware_physical_mode_returns_one_whole_body( hardware = openyam_hardware() - assert hardware.hardware_id == OPENYAM_HARDWARE_ID - assert hardware.hardware_type is HardwareType.WHOLE_BODY - assert hardware.adapter_type == "openyam_damiao" - assert hardware.joints == OPENYAM_JOINTS - assert hardware.gripper_joints == [] - assert hardware.wb_config is not None - assert hardware.wb_config.kp == (80.0, 80.0, 80.0, 10.0, 10.0, 10.0, 0.0) - assert hardware.wb_config.kd == (5.0, 5.0, 5.0, 1.5, 1.5, 1.5, 0.0) - runtime = hardware.adapter_kwargs["runtime_config"] - assert isinstance(runtime, DamiaoRuntimeConfig) - assert runtime.bus_addresses == {"openyam": "can1"} - assert runtime.gravity_comp is True + 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_hardware_simulation_mode_returns_generic_whole_body_mock( @@ -99,45 +81,31 @@ def test_openyam_hardware_simulation_mode_returns_generic_whole_body_mock( hardware = openyam_hardware() assert hardware.adapter_type == "mock_whole_body" - assert hardware.adapter_kwargs == {} - assert hardware.joints == OPENYAM_JOINTS -def test_openyam_planner_coordinator_trajectory_claims_only_arm_joints() -> None: - hardware = _coordinator_kwargs(openyam_planner_coordinator)["hardware"][0] - trajectory = _coordinator_kwargs(openyam_planner_coordinator)["tasks"][0] - - assert hardware.joints == OPENYAM_JOINTS - assert trajectory.type == "trajectory" - assert trajectory.joint_names == OPENYAM_ARM_JOINTS - assert OPENYAM_GRIPPER_JOINT not in trajectory.joint_names - - -def test_openyam_planner_coordinator_model_uses_arm_planning_group() -> None: - kwargs = _module_kwargs(openyam_planner_coordinator, ManipulationModule) - config = ManipulationModuleConfig(**kwargs).robots[0] - - assert config.name == "arm" - assert config.joint_names == ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] - - -def test_keyboard_teleop_openyam_planner_eef_task_controls_only_arm() -> None: - tasks = _coordinator_kwargs(keyboard_teleop_openyam_planner)["tasks"] - eef_twist = next(task for task in tasks if task.type == "eef_twist") +@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) + claimed_joints = [task.joint_names for task in kwargs["tasks"]] - assert eef_twist.joint_names == OPENYAM_ARM_JOINTS - assert eef_twist.params == { - "model_path": OPENYAM_GRAVITY_MODEL_PATH, - "ee_joint_id": OPENYAM_DOF, - } + 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_openyam_planner_gripper_task_is_independent_and_idle() -> None: - tasks = _coordinator_kwargs(keyboard_teleop_openyam_planner)["tasks"] +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] - assert gripper.params == {"timeout": 0.0} def test_keyboard_teleop_openyam_planner_trajectory_has_priority_over_eef_task() -> None: @@ -150,29 +118,9 @@ def test_keyboard_teleop_openyam_planner_trajectory_has_priority_over_eef_task() assert eef_twist.priority == 10 -def test_coordinator_openyam_arm_task_claims_six_of_seven_registered_joints() -> None: - kwargs = _coordinator_kwargs(coordinator_openyam) - - assert kwargs["hardware"][0].joints == OPENYAM_JOINTS - assert kwargs["tasks"][0].joint_names == OPENYAM_ARM_JOINTS - - -def test_keyboard_teleop_openyam_eef_task_controls_only_arm() -> None: - tasks = _coordinator_kwargs(keyboard_teleop_openyam)["tasks"] - eef_twist = next(task for task in tasks if task.type == "eef_twist") - - assert eef_twist.joint_names == OPENYAM_ARM_JOINTS - - 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 - - -def test_keyboard_teleop_openyam_visualization_uses_viser_backend() -> None: - visualization = _module_kwargs(keyboard_teleop_openyam, ManipulationModule)["visualization"] - - assert visualization == {"backend": "viser"} From 29fa5811609c4492e47e431c86009e9fccaa79c6 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 3 Aug 2026 16:31:37 -0700 Subject: [PATCH 30/44] feat: nest CAN commands under hardware --- dimos/cli/dimos.py | 4 ++- dimos/cli/test_can.py | 32 +++++++++++++------ .../manipulation/piper_integration.md | 6 ++-- 3 files changed, 29 insertions(+), 13 deletions(-) diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index 3db19fa3eb..dd30b475f0 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -172,7 +172,9 @@ def callback(**kwargs) -> None: # type: ignore[no-untyped-def] main.callback()(create_dynamic_callback()) # type: ignore[no-untyped-call] -main.add_typer(can_app, name="can") +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.command()(shell) main.add_typer(cache_app, name="cache") diff --git a/dimos/cli/test_can.py b/dimos/cli/test_can.py index 2b235351bc..509f58475e 100644 --- a/dimos/cli/test_can.py +++ b/dimos/cli/test_can.py @@ -15,17 +15,31 @@ 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.can import app +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: @@ -40,7 +54,7 @@ def test_setup_valid_options_configures_and_verifies_can_interface( ), ) - result = CliRunner().invoke(app, ["setup", "follower_l"]) + 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 @@ -78,14 +92,14 @@ def test_setup_valid_options_configures_and_verifies_can_interface( def test_setup_nonpositive_queue_length_returns_usage_error() -> None: - result = CliRunner().invoke(app, ["setup", "can0", "--txqueuelen", "0"]) + 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 = CliRunner().invoke(app, ["setup", "can0", "--bitrate", "0"]) + result = _invoke_can(["setup", "can0", "--bitrate", "0"]) assert result.exit_code == 2 assert "x>=1" in result.output @@ -97,7 +111,7 @@ def test_status_existing_interface_prints_detailed_state(mocker: MockerFixture) return_value=subprocess.CompletedProcess([], 0, stdout="can0: UP\n", stderr=""), ) - result = CliRunner().invoke(app, ["status", "can0"]) + result = _invoke_can(["status", "can0"]) assert result.exit_code == 0, result.output assert result.stdout == "can0: UP\n" @@ -118,7 +132,7 @@ def test_down_nonroot_user_runs_privileged_command_with_sudo( return_value=subprocess.CompletedProcess([], 0), ) - result = CliRunner().invoke(app, ["down", "can1"]) + result = _invoke_can(["down", "can1"]) assert result.exit_code == 0, result.output assert "CAN interface can1 is down" in result.stdout @@ -137,7 +151,7 @@ def test_up_root_user_runs_ip_without_sudo(mocker: MockerFixture) -> None: return_value=subprocess.CompletedProcess([], 0), ) - result = CliRunner().invoke(app, ["up", "can2"]) + result = _invoke_can(["up", "can2"]) assert result.exit_code == 0, result.output assert "CAN interface can2 is up" in result.stdout @@ -152,7 +166,7 @@ def test_up_root_user_runs_ip_without_sudo(mocker: MockerFixture) -> None: def test_status_missing_ip_command_returns_usage_error(mocker: MockerFixture) -> None: mocker.patch("dimos.cli.can.subprocess.run", side_effect=FileNotFoundError) - result = CliRunner().invoke(app, ["status", "can0"]) + result = _invoke_can(["status", "can0"]) assert result.exit_code == 2 assert "the 'ip' command is not installed" in result.output @@ -182,7 +196,7 @@ def test_status_failed_ip_command_reports_available_detail( ), ) - result = CliRunner().invoke(app, ["status", "can0"]) + result = _invoke_can(["status", "can0"]) assert result.exit_code == 1 assert f"CAN interface command failed: {expected_detail}" in result.output diff --git a/docs/capabilities/manipulation/piper_integration.md b/docs/capabilities/manipulation/piper_integration.md index 3e7a9158f8..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 can setup can0 +dimos hardware can setup can0 ``` For a non-default bitrate, pass `--bitrate` explicitly: ```bash -dimos can setup can0 --bitrate 500000 +dimos hardware can setup can0 --bitrate 500000 ``` The command prints each privileged operation before requesting sudo. Verify the interface before starting a blueprint: ```bash -dimos can status can0 +dimos hardware can status can0 ``` ## Run a Piper blueprint From 3a8f67c72f03c479d81930626c2a57ba4a255706 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Mon, 3 Aug 2026 21:46:15 -0700 Subject: [PATCH 31/44] feat(manipulation): drive bimanual OpenArm through the Damiao whole-body adapter Add OpenArmDamiaoAdapter: both v10 arms (2x DM8006, 2x DM4340, 3x DM4310, send ids 0x01..0x07) and both grippers (DM4310 at 0x08) as one whole-body device over two CAN buses (left=can1, right=can0), with gravity compensation from the bimanual URDF (14 joints, validated order left1..7 then right1..7). Rewrite the OpenArm hardware config on the OpenYAM pattern: one 16-joint WHOLE_BODY component, mock/real selection via global_config.simulation, hardware-measured MIT gains carried over from the legacy adapter, and per-side planning models gaining an explicit coordinator->URDF joint name mapping. Blueprints: coordinator-openarm, openarm-planner-coordinator, keyboard-teleop-openarm and keyboard-teleop-openarm-planner. The keyboard jogs the left arm while the right arm's twist task holds pose; a single servo task drives both grippers, enabled by a new KeyboardTeleopConfig.gripper_joint_names field. The e2e planning-groups test moves to openarm-planner-coordinator since the harness's --simulation flag now selects the in-memory adapter. --- .../test_manipulation_planning_groups.py | 4 +- dimos/hardware/test_adapter_registries.py | 1 + .../whole_body/openarm_damiao/_registry.py | 17 + .../whole_body/openarm_damiao/adapter.py | 111 +++++ .../whole_body/openarm_damiao/test_adapter.py | 67 +++ dimos/robot/all_blueprints.py | 10 +- .../manipulators/openarm/blueprints/basic.py | 72 ++-- .../openarm/blueprints/planner.py | 53 --- .../manipulators/openarm/blueprints/teleop.py | 102 +++-- dimos/robot/manipulators/openarm/config.py | 116 ++--- .../teleop/keyboard/keyboard_teleop_module.py | 7 +- docs/capabilities/manipulation/index.md | 7 + .../manipulation/openarm_integration.md | 400 ++---------------- 13 files changed, 414 insertions(+), 553 deletions(-) create mode 100644 dimos/hardware/whole_body/openarm_damiao/_registry.py create mode 100644 dimos/hardware/whole_body/openarm_damiao/adapter.py create mode 100644 dimos/hardware/whole_body/openarm_damiao/test_adapter.py delete mode 100644 dimos/robot/manipulators/openarm/blueprints/planner.py diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 390db63fde..1d8bc367a8 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -39,7 +39,9 @@ pytestmark = [pytest.mark.self_hosted_large] JOINT_STATE_TOPIC = "/coordinator_joint_state#sensor_msgs.JointState" -BLUEPRINT = "openarm-mock-planner-coordinator" +# The e2e harness always passes --simulation (DimosCliCall.simulator), so the +# blueprint's hardware selection resolves to the in-memory whole-body adapter. +BLUEPRINT = "openarm-planner-coordinator" def _wait_for_robot_info( diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 9b00544624..25b8a6feb3 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -67,6 +67,7 @@ }, "whole_body": { "mock_whole_body", + "openarm_damiao", "openyam_damiao", "sim_mujoco_g1", "transport_lcm", diff --git a/dimos/hardware/whole_body/openarm_damiao/_registry.py b/dimos/hardware/whole_body/openarm_damiao/_registry.py new file mode 100644 index 0000000000..0c16f69170 --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/_registry.py @@ -0,0 +1,17 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ADAPTER_FACTORIES = { + "openarm_damiao": ("dimos.hardware.whole_body.openarm_damiao.adapter:OpenArmDamiaoAdapter"), +} diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py new file mode 100644 index 0000000000..d7b195e6ab --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -0,0 +1,111 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm v10 bimanual physical topology for the generic Damiao whole-body adapter.""" + +from __future__ import annotations + +from pathlib import Path + +import can_motor_control +from can_motor_control import damiao + +from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter +from dimos.utils.data import LfsPath + +# Per-arm motor models, shoulder to wrist, from +# openarm_description/config/arm/v10/joint_limits.yaml. Both arms use the same +# CAN send ids 0x01..0x07 because each arm owns a dedicated bus. +_ARM_MOTOR_TYPES = ( + damiao.MotorType.DM8006, + damiao.MotorType.DM8006, + damiao.MotorType.DM4340, + damiao.MotorType.DM4340, + damiao.MotorType.DM4310, + damiao.MotorType.DM4310, + damiao.MotorType.DM4310, +) + + +def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: + return [ + can_motor_control.MotorSpec(f"openarm_{side}_joint{index}", motor_type, index, index | 0x10) + for index, motor_type in enumerate(_ARM_MOTOR_TYPES, start=1) + ] + + +def _gripper_motor(side: str) -> can_motor_control.MotorSpec: + return can_motor_control.MotorSpec( + f"openarm_{side}_gripper", + damiao.MotorType.DM4310, + 0x08, + 0x18, + ) + + +class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): + """Two OpenArm v10 arms with grippers, one CAN bus per arm.""" + + arm_joints = { + "left_arm": tuple(f"left_arm/joint{index}" for index in range(1, 8)), + "right_arm": tuple(f"right_arm/joint{index}" for index in range(1, 8)), + } + gripper_joints = { + "left_gripper": "left_arm/gripper", + "right_gripper": "right_arm/gripper", + } + # Linux assigns can0/can1 in USB enumeration order; remap a swapped rig + # through DamiaoRuntimeConfig.bus_addresses instead of editing topology. + bus_defaults = {"left": "can1", "right": "can0"} + gravity_joint_names = ( + *(f"openarm_left_joint{index}" for index in range(1, 8)), + *(f"openarm_right_joint{index}" for index in range(1, 8)), + ) + + @property + def gravity_model_path(self) -> Path: + """Return the lazy bimanual gravity-compensation URDF path.""" + return LfsPath("openarm_description") / "urdf/robot/openarm_v10_bimanual.urdf" + + def _build_robot(self) -> can_motor_control.Robot: + return ( + can_motor_control.Robot.builder() + .add_bus( + "left", + can_motor_control.SocketCanBus(self.bus_address("left")), + damiao.DamiaoCodec(), + ) + .add_bus( + "right", + can_motor_control.SocketCanBus(self.bus_address("right")), + damiao.DamiaoCodec(), + ) + .add_arm("left_arm", bus="left", motors=_arm_motors("left")) + .add_arm("right_arm", bus="right", motors=_arm_motors("right")) + .add_gripper( + "left_gripper", + bus="left", + motor=_gripper_motor("left"), + opening_direction="decreasing_position", + default_current=0.15, + ) + .add_gripper( + "right_gripper", + bus="right", + motor=_gripper_motor("right"), + opening_direction="decreasing_position", + default_current=0.15, + ) + .build() + ) diff --git a/dimos/hardware/whole_body/openarm_damiao/test_adapter.py b/dimos/hardware/whole_body/openarm_damiao/test_adapter.py new file mode 100644 index 0000000000..bddfa5cf96 --- /dev/null +++ b/dimos/hardware/whole_body/openarm_damiao/test_adapter.py @@ -0,0 +1,67 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from collections.abc import Iterator +import runpy + +import can_motor_control +import pytest +from pytest_mock import MockerFixture + +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.openarm_damiao import adapter as adapter_module +from dimos.hardware.whole_body.openarm_damiao.adapter import OpenArmDamiaoAdapter +from dimos.robot.manipulators.openarm.config import OPENARM_DOF, OPENARM_JOINTS + + +@pytest.fixture +def openarm_adapter(mocker: MockerFixture) -> Iterator[OpenArmDamiaoAdapter]: + mocker.patch.object(can_motor_control, "SocketCanBus", can_motor_control.MockCanBus) + adapter = OpenArmDamiaoAdapter( + runtime_config=DamiaoRuntimeConfig(gravity_comp=False), + ) + yield adapter + adapter.disconnect() + + +def test_import_lazy_gravity_model_does_not_resolve_lfs(mocker: MockerFixture) -> None: + get_data = mocker.patch("dimos.utils.data.get_data") + + runpy.run_path(adapter_module.__file__) + + get_data.assert_not_called() + + +def test_openarm_topology_connects_arms_and_grippers( + openarm_adapter: OpenArmDamiaoAdapter, +) -> None: + robot = openarm_adapter._build_robot() + + assert robot.group_names() == ["left_arm", "right_arm", "left_gripper", "right_gripper"] + assert robot.bus_names() == ["left", "right"] + assert isinstance(robot["left_arm"], can_motor_control.Arm) + assert isinstance(robot["right_arm"], can_motor_control.Arm) + assert len(robot["left_arm"]) == OPENARM_DOF + assert len(robot["right_arm"]) == OPENARM_DOF + assert isinstance(robot["left_gripper"], can_motor_control.Gripper) + assert isinstance(robot["right_gripper"], can_motor_control.Gripper) + assert openarm_adapter.connect() + + +def test_openarm_joint_order_matches_hardware_component( + openarm_adapter: OpenArmDamiaoAdapter, +) -> None: + """Commands are routed positionally: the config joint list must equal the + adapter's declared order or motors silently receive each other's targets.""" + assert list(openarm_adapter.joint_names) == OPENARM_JOINTS diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ae938521ef..7e583cb719 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -31,10 +31,7 @@ "coordinator-mobile-manip-mock": "dimos.control.blueprints.mobile:coordinator_mobile_manip_mock", "coordinator-mock": "dimos.robot.manipulators.common.mock:coordinator_mock", "coordinator-mock-twist-base": "dimos.control.blueprints.mobile:coordinator_mock_twist_base", - "coordinator-openarm-bimanual": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_bimanual", - "coordinator-openarm-left": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_left", - "coordinator-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_mock", - "coordinator-openarm-right": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm_right", + "coordinator-openarm": "dimos.robot.manipulators.openarm.blueprints.basic:coordinator_openarm", "coordinator-openyam": "dimos.robot.manipulators.openyam.blueprints.basic:coordinator_openyam", "coordinator-piper": "dimos.robot.manipulators.piper.blueprints.basic:coordinator_piper", "coordinator-piper-xarm": "dimos.robot.manipulators.common.mixed:coordinator_piper_xarm", @@ -70,7 +67,7 @@ "keyboard-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:keyboard_teleop_a1z", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints.teleop:keyboard_teleop_a750", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm", - "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm_mock", + "keyboard-teleop-openarm-planner": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm_planner", "keyboard-teleop-openyam": "dimos.robot.manipulators.openyam.blueprints.teleop:keyboard_teleop_openyam", "keyboard-teleop-openyam-planner": "dimos.robot.manipulators.openyam.blueprints.teleop:keyboard_teleop_openyam_planner", "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:keyboard_teleop_piper", @@ -87,8 +84,7 @@ "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", "mid360-realsense-record": "dimos.robot.assembly.mid360_realsense_30:mid360_realsense_record", "mid360-realsense-record-with-pcap": "dimos.robot.assembly.mid360_realsense_30:mid360_realsense_record_with_pcap", - "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", - "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", + "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.basic:openarm_planner_coordinator", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index 012d3ceb97..bf09190813 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -16,53 +16,45 @@ from __future__ import annotations -from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.robot.manipulators.common.blueprints import trajectory_task +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( - LEFT_CAN, - OPENARM_ADAPTER_KWARGS, - RIGHT_CAN, + OPENARM_ARM_JOINTS, openarm_hardware, + openarm_model_config, ) -def openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: - return trajectory_task(hw, name=name) - - -mock_left = openarm_hardware(side="left") -mock_right = openarm_hardware(side="right") - -coordinator_openarm_mock = ControlCoordinator.blueprint( - hardware=[mock_left, mock_right], - tasks=[trajectory_task(mock_left, mock_right)], +def _trajectory_task() -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=list(OPENARM_ARM_JOINTS), + priority=10, + params={"start_position_tolerance": 0.05}, + ) + + +_openarm_planner_hw = openarm_hardware() + +openarm_planner_coordinator = autoconnect( + planner( + robots=[ + openarm_model_config("left"), + openarm_model_config("right"), + ], + ), + coordinator( + hardware=[_openarm_planner_hw], + tasks=[_trajectory_task()], + ), ) -left_hw = openarm_hardware( - side="left", - address=LEFT_CAN, - adapter_type="openarm", - adapter_kwargs=OPENARM_ADAPTER_KWARGS, -) -right_hw = openarm_hardware( - side="right", - address=RIGHT_CAN, - adapter_type="openarm", - adapter_kwargs=OPENARM_ADAPTER_KWARGS, -) - -coordinator_openarm_left = ControlCoordinator.blueprint( - hardware=[left_hw], - tasks=[openarm_task(left_hw)], -) - -coordinator_openarm_right = ControlCoordinator.blueprint( - hardware=[right_hw], - tasks=[openarm_task(right_hw)], -) +_openarm_hw = openarm_hardware() -coordinator_openarm_bimanual = ControlCoordinator.blueprint( - hardware=[left_hw, right_hw], - tasks=[trajectory_task(left_hw, right_hw)], +coordinator_openarm = ControlCoordinator.blueprint( + hardware=[_openarm_hw], + tasks=[_trajectory_task()], ) diff --git a/dimos/robot/manipulators/openarm/blueprints/planner.py b/dimos/robot/manipulators/openarm/blueprints/planner.py deleted file mode 100644 index 6872b15157..0000000000 --- a/dimos/robot/manipulators/openarm/blueprints/planner.py +++ /dev/null @@ -1,53 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""OpenArm planner + coordinator blueprints.""" - -from __future__ import annotations - -from dimos.core.coordination.blueprints import autoconnect -from dimos.robot.manipulators.common.blueprints import coordinator, planner, trajectory_task -from dimos.robot.manipulators.openarm.blueprints.basic import ( - left_hw, - mock_left, - mock_right, - right_hw, -) -from dimos.robot.manipulators.openarm.config import openarm_model_config - -openarm_mock_planner_coordinator = autoconnect( - planner( - robots=[ - openarm_model_config("left"), - openarm_model_config("right"), - ], - ), - coordinator( - hardware=[mock_left, mock_right], - tasks=[trajectory_task(mock_left, mock_right)], - ), -) - -openarm_planner_coordinator = autoconnect( - planner( - robots=[ - openarm_model_config("left"), - openarm_model_config("right"), - ], - ), - coordinator( - hardware=[left_hw, right_hw], - tasks=[trajectory_task(left_hw, right_hw)], - ), -) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index 33e8b27f98..299ef9d85c 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -16,48 +16,96 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator +from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.robot.manipulators.common.blueprints import eef_twist_task +from dimos.robot.manipulators.common.blueprints import coordinator, planner +from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( - LEFT_CAN, - OPENARM_V10_FK_MODEL, - openarm_single_hardware, - openarm_single_model_config, + OPENARM_DOF, + OPENARM_GRIPPER_JOINTS, + OPENARM_LEFT_MODEL, + OPENARM_RIGHT_MODEL, + openarm_arm_joints, + openarm_hardware, + openarm_model_config, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -_teleop_hw = openarm_single_hardware() +# The keyboard publishes twists to one task by name; the other arm's task +# keeps holding its anchor pose. +KEYBOARD_EEF_TASK_NAME = "eef_twist_left_arm" -keyboard_teleop_openarm_mock = autoconnect( - KeyboardTeleopModule.blueprint(), +_openarm_keyboard_hw = openarm_hardware() + + +def _eef_twist_task(side: str, *, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=f"eef_twist_{side}_arm", + type="eef_twist", + joint_names=openarm_arm_joints(side), + priority=priority, + params={ + "model_path": OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, + "ee_joint_id": OPENARM_DOF, + }, + ) + + +def _trajectory_task(*, priority: int = 10) -> TaskConfig: + return TaskConfig( + name=DEFAULT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=[*openarm_arm_joints("left"), *openarm_arm_joints("right")], + priority=priority, + params={"start_position_tolerance": 0.05}, + ) + + +def _gripper_task() -> TaskConfig: + return TaskConfig( + name="servo_grippers", + type="servo", + joint_names=list(OPENARM_GRIPPER_JOINTS), + priority=20, + params={"timeout": 0.0}, + ) + + +keyboard_teleop_openarm = autoconnect( + KeyboardTeleopModule.blueprint( + task_name=KEYBOARD_EEF_TASK_NAME, + gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), + ), ControlCoordinator.blueprint( - hardware=[_teleop_hw], - tasks=[eef_twist_task(_teleop_hw, model_path=OPENARM_V10_FK_MODEL, ee_joint_id=7)], + hardware=[_openarm_keyboard_hw], + tasks=[ + _eef_twist_task("left"), + _eef_twist_task("right"), + _gripper_task(), + ], ), ManipulationModule.blueprint( - robots=[openarm_single_model_config()], - visualization={"backend": "meshcat"}, + robots=[openarm_model_config("left"), openarm_model_config("right")], + visualization={"backend": "viser"}, ), ) -_teleop_real_hw = openarm_single_hardware(adapter_type="openarm", address=LEFT_CAN) +_openarm_keyboard_planner_hw = openarm_hardware() -keyboard_teleop_openarm = autoconnect( - KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( - hardware=[_teleop_real_hw], +keyboard_teleop_openarm_planner = autoconnect( + KeyboardTeleopModule.blueprint( + task_name=KEYBOARD_EEF_TASK_NAME, + gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), + ), + planner(robots=[openarm_model_config("left"), openarm_model_config("right")]), + coordinator( + hardware=[_openarm_keyboard_planner_hw], tasks=[ - eef_twist_task( - _teleop_real_hw, - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, - ) + _eef_twist_task("left", priority=10), + _eef_twist_task("right", priority=10), + _gripper_task(), + _trajectory_task(priority=20), ], ), - ManipulationModule.blueprint( - robots=[openarm_single_model_config()], - visualization={"backend": "meshcat"}, - ), ) diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 307450d054..651c2e1f83 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -12,19 +12,32 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""OpenArm hardware and planning model configuration helpers.""" +"""OpenArm hardware and planning model configuration.""" from __future__ import annotations from pathlib import Path -from typing import Any from dimos.control.components import HardwareComponent, HardwareType +from dimos.core.global_config import global_config +from dimos.hardware.whole_body.damiao.config import DamiaoRuntimeConfig +from dimos.hardware.whole_body.spec import WholeBodyConfig from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import base_pose from dimos.utils.data import LfsPath +OPENARM_DOF = 7 +OPENARM_HARDWARE_ID = "openarm" +OPENARM_SIDES = ("left", "right") +# Order must match OpenArmDamiaoAdapter.joint_names: all arm groups in +# declaration order (left then right), then all grippers. +OPENARM_LEFT_ARM_JOINTS = [f"left_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +OPENARM_RIGHT_ARM_JOINTS = [f"right_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +OPENARM_ARM_JOINTS = [*OPENARM_LEFT_ARM_JOINTS, *OPENARM_RIGHT_ARM_JOINTS] +OPENARM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] +OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] + OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ ("openarm_left_link5", "openarm_left_link7"), ("openarm_right_link5", "openarm_right_link7"), @@ -34,55 +47,51 @@ OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" OPENARM_V10_FK_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" +OPENARM_GRAVITY_MODEL_PATH = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# Linux assigns can0/can1 in USB enumeration order, which is not guaranteed stable. -# Flip these if physical arms come up swapped. -LEFT_CAN = "can1" -RIGHT_CAN = "can0" - -# Leave true for normal operation; it is idempotent and ensures motors are in -# the expected CTRL_MODE=MIT mode at connect time. -AUTO_SET_MIT_MODE = True -OPENARM_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE} +# MIT gains measured on v10 hardware (legacy adapter): with gravity +# compensation active the PD terms only handle transient tracking, and high kd +# excites gearbox buzz. Gripper slots bypass MIT control, so their gains are 0. +_ARM_KP = (100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0) +_ARM_KD = (1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8) def validate_side(side: str) -> None: - if side not in ("left", "right"): + if side not in OPENARM_SIDES: raise ValueError(f"side must be 'left' or 'right', got {side!r}") -def openarm_joints(side: str) -> list[str]: +def openarm_arm_joints(side: str) -> list[str]: validate_side(side) - return [f"openarm_{side}_joint{i}" for i in range(1, 8)] + return [f"{side}_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] -def openarm_hardware( - side: str, - name: str | None = None, - *, - adapter_type: str = "mock", - address: str | None = None, - adapter_kwargs: dict[str, Any] | None = None, -) -> HardwareComponent: - validate_side(side) - kwargs = {"side": side} - if adapter_kwargs: - kwargs.update(adapter_kwargs) +def openarm_hardware() -> HardwareComponent: + """Select the physical or in-memory whole-body adapter for OpenArm.""" + adapter_type = "mock_whole_body" if global_config.simulation else "openarm_damiao" + adapter_kwargs: dict[str, object] = {} + if not global_config.simulation: + adapter_kwargs["runtime_config"] = DamiaoRuntimeConfig(gravity_comp=True) return HardwareComponent( - hardware_id=name or f"{side}_arm", - hardware_type=HardwareType.MANIPULATOR, - joints=openarm_joints(side), + hardware_id=OPENARM_HARDWARE_ID, + hardware_type=HardwareType.WHOLE_BODY, + joints=list(OPENARM_JOINTS), adapter_type=adapter_type, - address=address, - adapter_kwargs=kwargs, + auto_enable=True, + adapter_kwargs=adapter_kwargs, + wb_config=WholeBodyConfig( + kp=(*_ARM_KP, *_ARM_KP, 0.0, 0.0), + kd=(*_ARM_KD, *_ARM_KD, 0.0, 0.0), + ), ) def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: + """Build one side's seven-joint planning model.""" validate_side(side) resolved_name = name or f"{side}_arm" - local_joint_names = openarm_joints(side) + local_joint_names = [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] return RobotModelConfig( name=resolved_name, model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, @@ -102,42 +111,11 @@ def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, - home_joints=[0.0] * 7, - ) - - -def openarm_single_hardware( - *, - adapter_type: str = "mock", - address: str | None = None, -) -> HardwareComponent: - return openarm_hardware( - "left", - name="arm", - adapter_type=adapter_type, - address=address, - ) - - -def openarm_single_model_config() -> RobotModelConfig: - local_joint_names = openarm_joints("left") - return RobotModelConfig( - name="arm", - model_path=OPENARM_V10_FK_MODEL, - base_pose=base_pose(), - joint_names=local_joint_names, - base_link="openarm_body_link0", - planning_groups=[ - PlanningGroupDefinition( - name="manipulator", - joint_names=tuple(local_joint_names), - base_link="openarm_body_link0", - tip_link="openarm_left_link7", + joint_name_mapping={ + coordinator_name: urdf_name + for coordinator_name, urdf_name in zip( + openarm_arm_joints(side), local_joint_names, strict=True ) - ], - package_paths=OPENARM_PACKAGE_PATHS, - auto_convert_meshes=True, - max_velocity=0.5, - max_acceleration=1.0, - home_joints=[0.0] * 7, + }, + home_joints=[0.0] * OPENARM_DOF, ) diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index 80ad6e1471..a08ae1bdd6 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -63,7 +63,6 @@ # Normalized gripper command values. GRIPPER_OPEN_POSITION = 1.0 GRIPPER_CLOSED_POSITION = 0.0 -# TODO: Improve gripper handling. GRIPPER_JOINT_NAME = "arm/gripper" TwistVector = tuple[float, float, float] @@ -74,6 +73,9 @@ class KeyboardTeleopConfig(ModuleConfig): linear_speed: float = DEFAULT_LINEAR_SPEED angular_speed: float = DEFAULT_ANGULAR_SPEED gripper_open_position: float = GRIPPER_OPEN_POSITION + # All named joints receive the same opening; multi-gripper robots list + # every gripper joint here. + gripper_joint_names: list[str] = [GRIPPER_JOINT_NAME] def _motion_key_codes() -> frozenset[int]: @@ -260,7 +262,8 @@ def _set_gripper_position(self, position: float) -> None: if self._gripper_position == position: return self._gripper_position = position - self.joint_command.publish(JointState(name=[GRIPPER_JOINT_NAME], position=[position])) + names = list(self.config.gripper_joint_names) + self.joint_command.publish(JointState(name=names, position=[position] * len(names))) def _twist_from_keys( diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index bdf48b09f6..9a51f6fd2c 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -19,6 +19,7 @@ Each blueprint launches the full stack — keyboard UI, mock controller, IK solv ```bash dimos run keyboard-teleop-a750 # A-750 6-DOF +dimos run keyboard-teleop-openarm # OpenArm bimanual 2x(7-DOF + gripper) dimos run keyboard-teleop-piper # Piper 6-DOF dimos run keyboard-teleop-openyam # OpenYAM 6-DOF + gripper dimos run keyboard-teleop-xarm6 # XArm6 6-DOF @@ -32,6 +33,12 @@ mechanical endpoints, so clear the gripper jaws and workspace before startup. The gripper has no default startup target and moves only after joint control has an explicit target. +OpenArm follows the same whole-body model with both arms and both grippers in +one device: fourteen angular joints (`left_arm/joint1..7`, +`right_arm/joint1..7`) plus two normalized gripper joints (`left_arm/gripper`, +`right_arm/gripper`). The keyboard jogs the left arm; the right arm holds its +pose, and `[` / `]` drive both grippers together. + Open the Meshcat URL printed in the terminal (default `http://localhost:7000`) to see the robot. Keyboard controls: diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 4dd5b587dd..099f83f2b3 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -1,387 +1,79 @@ --- title: "OpenArm Integration" --- -Guide for running the **OpenArm** — an open-source bimanual 7-DOF research arm built from Damiao DM-J quasi-direct-drive motors — under the dimos manipulation + control stack. -**If you're standing in front of the hardware and just want to run it, skip to [Quick start](#quick-start).** +DimOS drives the [OpenArm](https://openarm.dev) bimanual platform (two 7-DOF +arms + grippers, Damiao motors, one CAN bus per arm) as a single whole-body +device through the generic Damiao adapter stack introduced for OpenYAM. Related: - Upstream hardware + C++ reference: [enactic/openarm_can](https://github.com/enactic/openarm_can) - How to integrate any new arm: [adding_a_custom_arm.md](/docs/capabilities/manipulation/adding_a_custom_arm.md) ---- - -## Why this integration is different - -Every other arm in dimos wraps a vendor Python SDK: - -| Arm | Transport | Python SDK | -|---|---|---| -| xArm | TCP/IP | `xarm-python-sdk` | -| Piper | CAN (via SDK) | `piper_sdk` | -| R1 Pro | Galaxea | Galaxea SDK | -| Go2 / G1 | WebRTC | Unitree SDK | -| Panda | FCI | `panda-py` | - -**OpenArm ships no Python SDK.** The only interface is raw CAN frames on the wire, speaking the Damiao MIT-mode protocol. So dimos includes a from-scratch driver that encodes/decodes the protocol directly on a SocketCAN bus. The reference implementation is the Enactic C++ library at [enactic/openarm_can](https://github.com/enactic/openarm_can) — we port the frame layout from there. - ## Architecture ``` -ManipulationModule → ControlCoordinator → OpenArmAdapter → OpenArmBus → SocketCAN → arm - (Drake plan) (100Hz tick loop) (dimos protocol) (CAN driver) +ControlCoordinator (100 Hz) + └── HardwareComponent "openarm" (WHOLE_BODY, 16 joints) + └── OpenArmDamiaoAdapter # dimos/hardware/whole_body/openarm_damiao/ + └── DamiaoWholeBodyAdapter # generic Damiao lifecycle + gravity comp + └── can-motor-control # Rust CAN transport + Damiao codec (PyPI) ``` -Code layout: - -``` -dimos/hardware/manipulators/openarm/ -├── driver.py # OpenArmBus, DamiaoMotor — pure CAN driver, no dimos deps -├── adapter.py # OpenArmAdapter — implements dimos ManipulatorAdapter protocol -├── test_driver.py # 13 unit tests (virtual CAN loopback, no hardware) -└── test_adapter.py # 11 unit tests (virtual CAN + mock state frames) +One adapter owns both arms: bus `left` (default `can1`) and bus `right` +(default `can0`) are commanded together in one synchronized tick per control +cycle. The command vector order is `left_arm/joint1..7`, `right_arm/joint1..7`, +`left_arm/gripper`, `right_arm/gripper`; gripper joints are normalized +(`0.0` closed, `1.0` open). -dimos/robot/manipulators/openarm/ -├── blueprints.py # coordinator-*, planner-*, keyboard-teleop-* blueprints and model config -└── scripts/ # bring-up + diagnostic scripts (run manually by humans) - ├── openarm_can_up.sh # bring SocketCAN interfaces up (needs sudo) - ├── openarm_can_probe.py # enumerate & read state from all 8 motors - ├── openarm_set_mit_mode.py # one-time CTRL_MODE=MIT write per motor - └── ... (diagnostics) +Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): +2x DM8006, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. -data/openarm_description/ # URDF + meshes (in-tree; may migrate to LFS) -└── urdf/robot/ - ├── openarm_v10_bimanual.urdf # both arms (14 DOF, used by coordinator) - ├── openarm_v10_left.urdf # left arm + torso (7 DOF, per-side planning) - ├── openarm_v10_right.urdf # right arm + torso (7 DOF) - └── openarm_v10_single.urdf # standalone arm (Pinocchio FK for teleop) -``` +Gravity compensation uses the bimanual URDF +(`openarm_description/urdf/robot/openarm_v10_bimanual.urdf`, resolved lazily +from LFS at connect time) and is preflighted against the declared joint order +before the motors enable. -Workspace analysis is generic and lives in [dimos/utils/workspace.py](/dimos/utils/workspace.py) — works for any URDF, not just OpenArm. - ---- - -## Quick start - -You need: - -- 2× **OpenArm v10** arms, wired to USB-CAN adapters -- 2× **USB-CAN adapters** (we used gs_usb family, VID:PID `1d50:606f`, e.g. CANable 2.0). Classical CAN @ 1 Mbit is enough; CAN-FD not required -- **Python 3.12 venv with dimos installed** plus `python-can >= 4.3` and `pinocchio` -- **sudo** on first run (to bring up the CAN interfaces) - -### 1. Bring up the CAN buses +## Bring-up ```bash -sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 can1 +dimos can setup can0 +dimos can setup can1 +dimos run keyboard-teleop-openarm ``` -This sets both interfaces to classical CAN @ 1 Mbit with a 1000-frame TX queue (enough headroom for the 100 Hz tick loop). If only one bus is present, pass just that one: `sudo ... openarm_can_up.sh can0`. +Linux assigns `can0`/`can1` in USB enumeration order. If the arms come up +swapped, override the mapping through +`DamiaoRuntimeConfig(bus_addresses={"left": ..., "right": ...})` rather than +editing the adapter topology. -**Troubleshooting:** -- `Operation not permitted` → you forgot `sudo`. -- `Operation not supported` on `fd on` → your adapter doesn't support CAN-FD. The script defaults to classical, so this shouldn't happen unless you set `MODE=fd`. -- Only one `can*` interface appears → the other adapter isn't enumerating. On gs_usb boards, the **blue LED** indicates USB enumeration. If one adapter only shows red/green, swap the USB cable (many USB-C cables are charge-only). +## Blueprints -### 2. Verify all 16 motors are alive - -```bash -python ./dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can0 -python ./dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can1 -``` - -Expected: `8/8 motors replied` on each bus, with plausible joint positions and rotor temps around 25–30 °C. - -### 3. (First time only) Put motors in MIT mode - -Damiao motors have a persistent `CTRL_MODE` register. They ship in POS_VEL mode by default, which means they will reply to enable/state queries but **silently ignore** any MIT control frames — the "motor doesn't move, error grows" failure. The adapter writes MIT on every `connect()` by default, so this step is usually automatic. If you want to set it explicitly once: - -```bash -python ./dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 -python ./dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can1 -``` - -The register is persistent across power cycles, so you only need this once per motor (or after a firmware reset). - -### 4. Run a blueprint - -| Blueprint | What it does | +| Blueprint | Contents | |---|---| -| `coordinator-openarm-mock` | Bimanual, mock adapters. No hardware. | -| `openarm-mock-planner-coordinator` | Drake planner + bimanual mock, Meshcat viz. Great smoke test. | -| `coordinator-openarm-left` / `coordinator-openarm-right` | Single arm, real hardware on can0 / can1. | -| `coordinator-openarm-bimanual` | Both arms, real hardware, no planner. | -| `openarm-planner-coordinator` | **Main usable blueprint** — Drake planner + both arms on real hardware. | -| `keyboard-teleop-openarm-mock` / `keyboard-teleop-openarm` | Single-arm Cartesian IK + pygame keyboard, mock / real. | - -**Safety before hot-plugging hardware:** hold the arms before starting. On connect, the adapter enables all motors and sends gravity-comp holds — the arms go slightly stiff but don't leap. Ctrl-C to cleanly disable and exit. - -First-time recommendation: mock planner to verify everything wires up, then real single-arm, then bimanual. - -```bash -# smoke test (no hardware) -dimos run openarm-mock-planner-coordinator - -# single-arm bring-up (hold the arm physically first) -dimos run coordinator-openarm-left - -# full bimanual with planner -dimos run openarm-planner-coordinator -``` - -Meshcat will appear at http://localhost:7000. - -### 5. Drive the arms from the manipulation client +| `coordinator-openarm` | coordinator + trajectory task over both arms | +| `openarm-planner-coordinator` | planner (per-side models) + coordinator | +| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + gripper servo + viser | +| `keyboard-teleop-openarm-planner` | teleop + planner + preempting trajectory task | -With `openarm-planner-coordinator` running in one terminal, open a second terminal and start the REPL client: +All blueprints run against the in-memory whole-body adapter under +`--simulation`; the physical adapter is selected automatically otherwise. -```bash -python -i -m dimos.manipulation.planning.examples.manipulation_client -``` +The keyboard jogs the left arm (`eef_twist_left_arm`); the right arm's twist +task holds its anchor pose. `[` opens and `]` closes both grippers together +via a single servo task over both gripper joints. -This gives you an interactive Python prompt with these functions: +## Files -| Function | Purpose | +| Path | Role | |---|---| -| `robots()` | List configured robots (here: `["left_arm", "right_arm"]`) | -| `joints(robot_name)` | Read current joint positions (7 floats) | -| `ee(robot_name)` | Read current end-effector pose | -| `state()` | Module state: `IDLE`, `PLANNING`, `EXECUTING`, `FAULT`, etc. | -| `plan([q1..q7], robot_name)` | Plan a collision-free trajectory to a joint configuration | -| `plan_pose(x, y, z, robot_name=...)` | Plan to a Cartesian EE pose (preserves current orientation) | -| `preview(robot_name)` | Animate the planned path in Meshcat without executing | -| `execute()` | Send the complete planned trajectory to the coordinator | -| `home(robot_name)` | Plan + execute to home joints | -| `commands()` | Print all available functions | - -#### Example session — simple joint moves - -```python skip ->>> robots() -['left_arm', 'right_arm'] - ->>> joints(robot_name="left_arm") -[0.02, -0.01, -0.13, 0.15, 0.17, -0.07, 0.10] - ->>> # One-liner: plan → preview in Meshcat → execute on hardware ->>> plan([0.3, 0, 0, 0, 0, 0, 0], robot_name="left_arm") and preview(robot_name="left_arm") and execute() -True - ->>> joints(robot_name="left_arm") -[0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00] # arm is now at the commanded pose -``` - -`plan()` returns `True` on success, `False` if planning failed (check the coordinator terminal for `COLLISION_AT_GOAL`, `INVALID_START`, `NO_SOLUTION`, etc). The `and` chaining is an idiom — if any step fails, the next one is short-circuited. - -If you ever get stuck in a `FAULT` state (e.g. an invalid plan was sent), reset the state machine: - -```python skip ->>> _client.reset() -'Reset to IDLE — ready for new commands' -``` - -#### Example session — bimanual - -```python skip ->>> # Move both arms to mirrored poses ->>> plan([0.5, 0, 0, 0, 0, 0, 0], robot_name="left_arm") and execute() -True ->>> plan([-0.5, 0, 0, 0, 0, 0, 0], robot_name="right_arm") and execute() -True -``` - -Each arm plans and executes independently — the coordinator runs both trajectories simultaneously on separate tick-loop tasks. - -#### Example session — Cartesian target - -```python skip ->>> ee(robot_name="left_arm") # see where the EE currently is ->>> plan_pose(0.1, 0.3, 0.5, robot_name="left_arm") and preview(robot_name="left_arm") -True ->>> execute() -True -``` - -If you don't know which Cartesian targets are reachable, check first with the workspace tool — see [Workspace analysis](#workspace-analysis) below. `plan_pose` will fail with `NO_SOLUTION` if the IK can't find a configuration reaching the target. - -#### Adding obstacles - -```python skip ->>> add_box("table", 0.4, 0.0, 0.1, w=0.6, h=0.4, d=0.05) # rectangular obstacle ->>> add_sphere("ball", 0.3, 0.2, 0.4, radius=0.05) ->>> plan_pose(0.4, 0.0, 0.3, robot_name="left_arm") # now plans around it ->>> remove("table") # id returned by add_* -``` - ---- - -## Configuration - -### Which CAN bus is which arm - -Linux assigns `can0`/`can1` in USB-enumeration order, which isn't guaranteed stable across reboots or cable swaps. If the arms come up "swapped" (commanding `left_arm` moves the physical right arm), flip these two constants in [config.py](/dimos/robot/manipulators/openarm/config.py): - -```python -LEFT_CAN = "can0" -RIGHT_CAN = "can1" -``` +| `dimos/hardware/whole_body/openarm_damiao/adapter.py` | physical topology (motors, buses, gravity URDF) | +| `dimos/robot/manipulators/openarm/config.py` | joints, gains, hardware + planning model configs | +| `dimos/robot/manipulators/openarm/blueprints/` | coordinator/planner/teleop blueprints | -No other code changes are needed. - -### Gain tuning (MIT kp/kd) - -Defaults live in [adapter.py](/dimos/hardware/manipulators/openarm/adapter.py). Gains are per-joint because the shoulder motors (DM8006, 40 Nm) tolerate higher kp than the wrist motors (DM4310, 10 Nm): - -```python -_DEFAULT_KP = [100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0] -_DEFAULT_KD = [1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8] -``` - -Guidelines: -- `kp ∈ [0, 500]` in MIT mode. Higher kp = stiffer position tracking; too high → oscillation. -- `kd ∈ [0, 5]`. Higher kd = more damping, but values above ~2 on these gearboxes cause high-frequency buzz/grinding. -- Gravity compensation is on by default (`gravity_comp=True`) — the adapter uses Pinocchio to compute `G(q)` and adds it as feedforward torque. This removes the need for very high kp to fight gravity, so prefer low kp + gravity comp over high kp. - -### Physical joint limits - -The URDFs use the xacro-generated limits (which include per-side offsets for mirroring). The adapter's `get_limits()` reports the same per-side limits. If you measure tighter physical limits and want to enforce them, edit the URDFs directly — the planner will respect them. - -### Disabling auto MIT-mode write - -The adapter writes `CTRL_MODE=MIT` to every motor at `connect()`. It's idempotent (writing the same value is a no-op), so this is safe to leave on. To verify that a previous write persisted across a power cycle, flip `AUTO_SET_MIT_MODE = False` in [config.py](/dimos/robot/manipulators/openarm/config.py) and restart — the arms should still respond. - ---- - -## Motor mapping (OpenArm v10) - -Derived from the URDF's `joint_limits.yaml` (effort column) cross-checked against the Damiao torque tables. Both arms are identical. - -| Send ID | Recv ID | Joint | Motor | vMax [rad/s] | tMax [Nm] | -|---|---|---|---|---|---| -| 0x01 | 0x11 | joint1 | DM8006 | 45 | 40 | -| 0x02 | 0x12 | joint2 | DM8006 | 45 | 40 | -| 0x03 | 0x13 | joint3 | DM4340 | 8 | 28 | -| 0x04 | 0x14 | joint4 | DM4340 | 8 | 28 | -| 0x05 | 0x15 | joint5 | DM4310 | 30 | 10 | -| 0x06 | 0x16 | joint6 | DM4310 | 30 | 10 | -| 0x07 | 0x17 | joint7 | DM4310 | 30 | 10 | -| 0x08 | 0x18 | gripper | DM4310 | 30 | 10 | - -Convention: `recv_id = send_id | 0x10`. - ---- - -## Damiao protocol essentials - -Ported from `enactic/openarm_can/src/openarm/damiao_motor/dm_motor_control.cpp`. You shouldn't need these unless you're modifying the driver. - -### Enable / disable / zero-position - -Send to the motor's send_id. 8-byte payload: - -``` -[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, CMD] - where CMD = 0xFC (enable) | 0xFD (disable) | 0xFE (zero current pose) -``` - -### MIT control frame (8 bytes) - -Bit layout: `q[16] | dq[12] | kp[12] | kd[12] | tau[12]`. Each float quantized via: - -```python -def float_to_uint(x, lo, hi, bits): - x = clamp(x, lo, hi) - return round((x - lo) / (hi - lo) * ((1 << bits) - 1)) -``` - -Gain ranges: `kp ∈ [0, 500]`, `kd ∈ [0, 5]`. Position/velocity/torque ranges come from the motor-type table above. - -Byte layout: -``` -byte0 = q_u >> 8 -byte1 = q_u & 0xFF -byte2 = dq_u >> 4 -byte3 = ((dq_u & 0xF) << 4) | ((kp_u >> 8) & 0xF) -byte4 = kp_u & 0xFF -byte5 = kd_u >> 4 -byte6 = ((kd_u & 0xF) << 4) | ((tau_u >> 8) & 0xF) -byte7 = tau_u & 0xFF -``` - -### State reply (8 bytes, on recv_id) - -Same `q | dq | tau` layout + 2 temperature bytes: - -``` -byte0 = motor_id_echo -byte1..5 = q | dq | tau (same packing as above) -byte6 = t_mos (°C) -byte7 = t_rotor (°C) -``` - -### CTRL_MODE register write - -Broadcast frame on CAN ID `0x7FF`: - -``` -data = [send_id_lo, send_id_hi, 0x55, RID=10, val[0..3]] - where val = 1 (MIT) | 2 (POS_VEL) | 3 (VEL) | 4 (POS_FORCE), little-endian uint32 -``` - -Persistent across power cycles. - ---- - -## Known gotchas - -- **`ip link ... fd on` → `Operation not supported`.** gs_usb firmware doesn't support CAN-FD. Use classical CAN @ 1 Mbit (our bringup script's default). -- **Motors reply to probes but commands do nothing.** CTRL_MODE is not MIT. The adapter now writes MIT on connect, but if you disabled that and motors got reset, run `openarm_set_mit_mode.py`. -- **`COLLISION_AT_START` during planning.** `link5` and `link7` collision meshes overlap by 3 mm at every configuration. Handled by `OPENARM_COLLISION_EXCLUSIONS` in the OpenArm config module. If you see it anyway, the exclusion pairs may not be getting applied — check that the collision filter log line appears during world build. -- **`INVALID_START` during planning.** Hardware encoder noise pushed a joint 1 mrad past a URDF limit. Joint4 used to be exactly `lower=0.0` which tripped this — it's now `-0.01` to give breathing room. If you see it on a different joint, widen that limit by ~10 mrad. -- **"Transmit buffer full" (ENOBUFS) at 100 Hz.** Kernel TX queue too small. The bringup script sets `txqueuelen 1000`; the driver also retries on ENOBUFS. If you still see the error, check `ip -details link show canX | grep qlen`. -- **Arms swap sides.** USB enumeration order flipped. Swap `LEFT_CAN` / `RIGHT_CAN` in [config.py](/dimos/robot/manipulators/openarm/config.py). - ---- - -## Design decisions - -- **Driver separate from adapter.** `driver.py` has zero dimos deps → unit-testable with a virtual CAN bus, reusable outside dimos. -- **MIT mode for everything.** MIT can emulate position (high kp), velocity (kp=0, nonzero kd+dq), and torque (kp=kd=0, nonzero tau). One code path. -- **Gravity compensation on by default.** Eliminates steady-state position error without needing high kp. Needs Pinocchio + the per-side URDFs. -- **One adapter per CAN bus, keyed by `address`.** Matches the Piper adapter pattern. Bimanual = two adapters with different `address` values. -- **Per-side URDFs for Drake planning.** Loading the full 14-DOF bimanual URDF twice (once per robot instance) creates phantom-arm collisions with the "other" arm frozen at zero. The per-side URDFs keep only one arm's links + the torso, avoiding the phantom collisions while matching the bimanual kinematics exactly. -- **URDF stays in-tree (`data/openarm_description/`) for now.** Can migrate to LFS later — only the path constants in the OpenArm blueprint module change. -- **CAN bringup stays manual (`sudo`).** Auto-bringup from `connect()` would need sudo-in-a-library or a systemd unit; the explicit script is clearer and testable. For production, add a oneshot systemd unit that runs the script at boot. - ---- - -## Workspace analysis - -For figuring out which targets are reachable before planning, use the generic workspace tool: - -```bash -# Visualize the left arm's reachable workspace as a point cloud -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf - -# Check if a specific target is reachable -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf query 0.1 0.3 0.5 - -# Get a list of reachable poses near a target, ranked by manipulability -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf suggest 0.1 0.3 0.5 - -# Interactive: visualize + type targets to query -python -m dimos.utils.workspace data/openarm_description/urdf/robot/openarm_v10_left.urdf interactive -``` - -Points are colored by Yoshikawa manipulability index: green = dexterous, red = near singularity. Avoid planning targets in the red regions. - ---- - -## Testing +## Validation ```bash -# Unit tests (no hardware, use virtual CAN) -.venv/bin/python -m pytest dimos/hardware/manipulators/openarm/ -v +uv run pytest dimos/hardware/whole_body/openarm_damiao \ + dimos/hardware/test_adapter_registries.py ``` - -Expected: 24 passed (13 driver + 11 adapter). All tests use `can.Bus(interface="virtual")` loopback — no real hardware needed. From 1358bb7d09372308fa87167d4f518e3a82beb705 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Mon, 3 Aug 2026 21:46:30 -0700 Subject: [PATCH 32/44] refactor(manipulation): remove the superseded OpenArm manipulator driver The hand-rolled Damiao CAN driver and manipulator-protocol adapter are fully replaced by OpenArmDamiaoAdapter on the whole-body path, which also wires the previously unimplemented grippers. Drop the legacy CAN bring-up scripts (superseded by 'dimos can setup') and the openarm manipulator registry entry. --- .../manipulators/openarm/_registry.py | 17 - .../hardware/manipulators/openarm/adapter.py | 430 ------------------ dimos/hardware/manipulators/openarm/driver.py | 329 -------------- .../manipulators/openarm/test_driver.py | 270 ----------- .../manipulators/test_adapter_lifecycle.py | 50 -- dimos/hardware/test_adapter_registries.py | 1 - .../openarm/scripts/openarm_can_probe.py | 206 --------- .../openarm/scripts/openarm_can_up.sh | 36 -- .../openarm/scripts/openarm_set_mit_mode.py | 140 ------ 9 files changed, 1479 deletions(-) delete mode 100644 dimos/hardware/manipulators/openarm/_registry.py delete mode 100644 dimos/hardware/manipulators/openarm/adapter.py delete mode 100644 dimos/hardware/manipulators/openarm/driver.py delete mode 100644 dimos/hardware/manipulators/openarm/test_driver.py delete mode 100755 dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py delete mode 100755 dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh delete mode 100755 dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py diff --git a/dimos/hardware/manipulators/openarm/_registry.py b/dimos/hardware/manipulators/openarm/_registry.py deleted file mode 100644 index eed680a4be..0000000000 --- a/dimos/hardware/manipulators/openarm/_registry.py +++ /dev/null @@ -1,17 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -ADAPTER_FACTORIES = { - "openarm": "dimos.hardware.manipulators.openarm.adapter:OpenArmAdapter", -} diff --git a/dimos/hardware/manipulators/openarm/adapter.py b/dimos/hardware/manipulators/openarm/adapter.py deleted file mode 100644 index 4881e03b50..0000000000 --- a/dimos/hardware/manipulators/openarm/adapter.py +++ /dev/null @@ -1,430 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""OpenArm ManipulatorAdapter — wraps the Damiao MIT-mode driver. SI units.""" - -from __future__ import annotations - -from pathlib import Path -import time -from typing import Any - -import numpy as np - -from dimos.hardware.manipulators.openarm.driver import ( - CTRL_MODE_MIT, - DamiaoMotor, - MotorType, - OpenArmBus, -) -from dimos.hardware.manipulators.spec import ( - ControlMode, - JointLimits, - ManipulatorInfo, -) -from dimos.utils.data import LfsPath - - -def _socketcan_iface_up(name: str) -> bool: - try: - flags_path = Path("/sys/class/net") / name / "flags" - if not flags_path.exists(): - return False - return (int(flags_path.read_text().strip(), 16) & 0x1) == 0x1 - except OSError: - return False - - -# OpenArm v10 BOM — (send_id, MotorType) per joint, derived from the torque -# column of data/openarm_description/config/arm/v10/joint_limits.yaml. -_OPENARM_V10_ARM_MOTORS: list[tuple[int, MotorType]] = [ - (0x01, MotorType.DM8006), # joint1 - (0x02, MotorType.DM8006), # joint2 - (0x03, MotorType.DM4340), # joint3 - (0x04, MotorType.DM4340), # joint4 - (0x05, MotorType.DM4310), # joint5 - (0x06, MotorType.DM4310), # joint6 - (0x07, MotorType.DM4310), # joint7 -] -# Gripper (motor id 0x08, DM4310) is on the bus but not currently wired up -# through the adapter — see the gripper-write methods which return None/False. - -# Physical joint limits (measured). Joints 1 & 2 are mirrored between sides. -_V10_POS_LOWER_LEFT = [-3.45, -3.30, -1.50, -0.01, -1.50, -0.75, -1.50] -_V10_POS_UPPER_LEFT = [1.35, 0.15, 1.50, 2.40, 1.50, 0.75, 1.50] -_V10_POS_LOWER_RIGHT = [-1.35, -0.15, -1.50, -0.01, -1.50, -0.75, -1.50] -_V10_POS_UPPER_RIGHT = [3.45, 3.30, 1.50, 2.40, 1.50, 0.75, 1.50] -_V10_VEL_MAX = [16.754666, 16.754666, 5.445426, 5.445426, 20.943946, 20.943946, 20.943946] - -# Default MIT gains per joint for POSITION mode. -# kp range is [0, 500], kd range is [0, 5]. -# With gravity compensation enabled, the PD gains only handle transient -# tracking — they don't fight gravity. Lower kp = smoother, less buzz. -# High kd causes high-frequency buzz/grinding from the gearbox. -_DEFAULT_KP = [100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0] -_DEFAULT_KD = [1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8] -_STATE_MAX_AGE_S = 0.1 - - -class OpenArmAdapter: - """7-DOF OpenArm on one SocketCAN bus. side=left|right picks URDF + limits.""" - - # Per-side URDFs for Pinocchio gravity model (LFS-backed) - _URDF_LEFT = LfsPath("openarm_description/urdf/robot/openarm_v10_left.urdf") - _URDF_RIGHT = LfsPath("openarm_description/urdf/robot/openarm_v10_right.urdf") - - def __init__( - self, - address: str = "can0", - dof: int = 7, - *, - side: str = "left", - fd: bool = False, - interface: str = "socketcan", - kp: list[float] | None = None, - kd: list[float] | None = None, - gravity_comp: bool = True, - auto_set_mit_mode: bool = True, - **_: Any, - ) -> None: - if dof != 7: - raise ValueError(f"OpenArmAdapter only supports 7 DOF (got {dof})") - if side not in ("left", "right"): - raise ValueError(f"side must be 'left' or 'right', got {side!r}") - self._address = address - self._dof = dof - self._side = side - self._fd = fd - self._interface = interface - self._kp = list(kp) if kp is not None else list(_DEFAULT_KP) - self._kd = list(kd) if kd is not None else list(_DEFAULT_KD) - if len(self._kp) != dof or len(self._kd) != dof: - raise ValueError("kp/kd must be length 7") - self._gravity_comp = gravity_comp - self._auto_set_mit_mode = auto_set_mit_mode - - self._motors = [DamiaoMotor(sid, mt) for sid, mt in _OPENARM_V10_ARM_MOTORS] - self._bus: OpenArmBus | None = None - self._control_mode: ControlMode = ControlMode.POSITION - self._enabled: bool = False - # Last successful position command — used as q_target for VELOCITY mode - self._last_cmd_q: list[float] | None = None - - # Pinocchio model for gravity compensation (loaded lazily in connect()) - self._pin_model: Any = None - self._pin_data: Any = None - - def connect(self) -> bool: - # Preflight: verify the SocketCAN interface is up before opening the bus. - # Bringing the interface up requires root privileges, so we don't do it - # here — just fail early with a helpful message. - if self._interface == "socketcan" and not _socketcan_iface_up(self._address): - print( - f"ERROR: SocketCAN interface '{self._address}' is not UP.\n" - f" Run: sudo ip link set {self._address} up type can bitrate 1000000\n" - f" (or: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {self._address})" - ) - return False - - try: - self._bus = OpenArmBus( - channel=self._address, - motors=self._motors, - fd=self._fd, - interface=self._interface, - ) - self._bus.open() - except Exception as e: - print(f"ERROR: OpenArm {self._side}@{self._address} connect failed: {e}") - self._bus = None - return False - - # Ensure every motor is in MIT control mode. The write is idempotent - # (setting CTRL_MODE=MIT when it's already MIT is a no-op), so we - # write unconditionally rather than query-then-write. - if self._auto_set_mit_mode: - try: - for m in self._motors: - self._bus.write_ctrl_mode(m.send_id, CTRL_MODE_MIT) - except Exception as e: - print(f"ERROR: failed to set MIT mode on {self._address}: {e}") - self._bus.close() - self._bus = None - return False - else: - print( - f"OpenArm {self._side}@{self._address}: " - "auto_set_mit_mode disabled — relying on persisted register" - ) - - # Load Pinocchio model for gravity compensation - if self._gravity_comp: - try: - import pinocchio - - urdf = str(self._URDF_LEFT if self._side == "left" else self._URDF_RIGHT) - self._pin_model = pinocchio.buildModelFromUrdf(urdf) - self._pin_data = self._pin_model.createData() - print( - f"OpenArm {self._side}: gravity compensation enabled (nq={self._pin_model.nq})" - ) - except Exception as e: - print(f"WARNING: gravity comp disabled — {e}") - self._pin_model = None - self._pin_data = None - - return True - - def disconnect(self) -> None: - if self._bus is None: - return - try: - self._bus.disable_all() - except Exception: - pass - self._enabled = False - self._bus.close() - self._bus = None - - def is_connected(self) -> bool: - return self._bus is not None - - def activate(self) -> bool: - return self.write_enable(True) - - def deactivate(self) -> bool: - stopped = self.write_stop() - disabled = self.write_enable(False) - return stopped and disabled - - def get_info(self) -> ManipulatorInfo: - return ManipulatorInfo( - vendor="Enactic", - model=f"OpenArm v10 ({self._side})", - dof=self._dof, - firmware_version=None, - serial_number=None, - ) - - def get_dof(self) -> int: - return self._dof - - def get_limits(self) -> JointLimits: - if self._side == "left": - lower, upper = _V10_POS_LOWER_LEFT, _V10_POS_UPPER_LEFT - else: - lower, upper = _V10_POS_LOWER_RIGHT, _V10_POS_UPPER_RIGHT - return JointLimits( - position_lower=list(lower), - position_upper=list(upper), - velocity_max=list(_V10_VEL_MAX), - ) - - def set_control_mode(self, mode: ControlMode) -> bool: - # OpenArm runs exclusively in Damiao MIT register mode; we emulate - # dimos ControlModes by tuning kp/kd/q/dq/tau on each MIT frame. - # Cartesian/impedance control are outside this adapter's scope. - if mode in ( - ControlMode.POSITION, - ControlMode.SERVO_POSITION, - ControlMode.VELOCITY, - ControlMode.TORQUE, - ): - self._control_mode = mode - return True - return False - - def get_control_mode(self) -> ControlMode: - return self._control_mode - - def _states_or_raise(self) -> list[Any]: - # Raises on missing or stale data so hardware_interface.py can retry - # (init) or skip the tick (steady-state). - if self._bus is None: - raise RuntimeError("OpenArmAdapter not connected") - now = time.monotonic() - states = self._bus.get_states() - for i, s in enumerate(states): - if s is None: - raise RuntimeError(f"motor {i + 1} has no state yet") - if now - s.timestamp > _STATE_MAX_AGE_S: - age_ms = (now - s.timestamp) * 1000 - raise RuntimeError(f"motor {i + 1} state stale ({age_ms:.0f} ms)") - return states - - def read_joint_positions(self) -> list[float]: - return [s.q for s in self._states_or_raise()] - - def read_joint_velocities(self) -> list[float]: - return [s.dq for s in self._states_or_raise()] - - def read_joint_efforts(self) -> list[float]: - return [s.tau for s in self._states_or_raise()] - - def read_state(self) -> dict[str, int]: - if self._bus is None: - return {"state": 0, "mode": 0} - states = self._bus.get_states() - # report the hottest rotor temperature so callers can monitor thermal - # stress with a single scalar - t_rotor = max((s.t_rotor for s in states if s is not None), default=0) - return { - "state": 1 if self._enabled else 0, - "mode": 1, # MIT - "t_rotor_max": int(t_rotor), - } - - def read_error(self) -> tuple[int, str]: - # The Damiao motors don't report a structured error code in the state - # frame; over-temperature / over-torque are detected by the host from - # the normal state fields. Surface a soft thermal warning here. - if self._bus is None: - return 0, "" - states = self._bus.get_states() - t_rotor = max((s.t_rotor for s in states if s is not None), default=0) - if t_rotor >= 85: - return 1, f"rotor over-temperature ({t_rotor}°C)" - return 0, "" - - def _compute_gravity_torques(self, q: list[float]) -> list[float]: - # Pinocchio G(q), clamped to motor torque limits. - if self._pin_model is None or self._pin_data is None: - return [0.0] * self._dof - import pinocchio - - q_arr = np.array(q, dtype=np.float64) - tau_g = pinocchio.computeGeneralizedGravity(self._pin_model, self._pin_data, q_arr) - # Clamp to motor torque limits for safety - limits = [m.limits for m in self._motors] # (p_max, v_max, t_max) - return [float(np.clip(tau_g[i], -lim[2], lim[2])) for i, lim in enumerate(limits)] - - def write_joint_positions( - self, - positions: list[float], - velocity: float = 1.0, - ) -> bool: - if self._bus is None or not self._enabled: - return False - if len(positions) != self._dof: - return False - velocity = max(0.0, min(1.0, velocity)) - # Gravity feedforward: compute tau needed to hold the arm at the - # current configuration. The PD gains handle the rest. Tolerate - # transient state-cache misses (e.g. startup, brief CAN gap) — fall - # back to commanded q with no feedforward instead of crashing. - try: - q_current = self.read_joint_positions() - tau_ff = self._compute_gravity_torques(q_current) - except RuntimeError: - tau_ff = [0.0] * self._dof - commands = [ - (q, 0.0, kp * velocity, kd, tau) - for q, kp, kd, tau in zip(positions, self._kp, self._kd, tau_ff, strict=False) - ] - self._bus.send_mit_many(commands) - self._last_cmd_q = list(positions) - return True - - def write_joint_velocities(self, velocities: list[float]) -> bool: - # MIT velocity tracking: kp=0, send dq directly, anchor q at the - # last-commanded position so the motor doesn't drift. Gravity - # feedforward is still needed — with kp=0 the only restoring force - # is damping, so without tau_ff the arm droops under its own weight. - if self._bus is None or not self._enabled: - return False - if len(velocities) != self._dof: - return False - # Seed anchor from current pose if we don't have a last-commanded one. - # If state isn't ready yet, can't safely anchor velocity tracking → bail. - if self._last_cmd_q is None: - try: - self._last_cmd_q = self.read_joint_positions() - except RuntimeError: - return False - anchor = self._last_cmd_q - try: - q_current = self.read_joint_positions() - tau_ff = self._compute_gravity_torques(q_current) - except RuntimeError: - tau_ff = [0.0] * self._dof - commands = [ - (q_anchor, dq, 0.0, kd, tau) - for q_anchor, dq, kd, tau in zip(anchor, velocities, self._kd, tau_ff, strict=False) - ] - self._bus.send_mit_many(commands) - return True - - def write_stop(self) -> bool: - if self._bus is None: - return False - # Without current positions we can't safely command "hold here" — sending - # any guessed q would torque the arm toward that pose. Bail out instead. - try: - q_now = self.read_joint_positions() - except RuntimeError: - return False - tau_ff = self._compute_gravity_torques(q_now) - commands = [ - (q, 0.0, kp, kd, tau) - for q, kp, kd, tau in zip(q_now, self._kp, self._kd, tau_ff, strict=False) - ] - self._bus.send_mit_many(commands) - self._last_cmd_q = q_now - return True - - def write_enable(self, enable: bool) -> bool: - if self._bus is None: - return False - self._enabled = False - try: - if enable: - self._bus.enable_all() - else: - self._bus.disable_all() - except Exception: - return False - self._enabled = enable - return True - - def read_enabled(self) -> bool: - return self._enabled - - def write_clear_errors(self) -> bool: - # Damiao motors have no separate clear-error command; re-enabling - # after a fault is the recovery path. - if self._bus is None: - return False - self._enabled = False - try: - self._bus.disable_all() - self._bus.enable_all() - except Exception: - return False - self._enabled = True - return True - - def read_cartesian_position(self) -> dict[str, float] | None: - return None - - def write_cartesian_position(self, pose: dict[str, float], velocity: float = 1.0) -> bool: - return False - - def read_gripper_position(self) -> float | None: - return None - - def write_gripper_position(self, position: float) -> bool: - return False - - def read_force_torque(self) -> list[float] | None: - return None diff --git a/dimos/hardware/manipulators/openarm/driver.py b/dimos/hardware/manipulators/openarm/driver.py deleted file mode 100644 index f7c9243cfa..0000000000 --- a/dimos/hardware/manipulators/openarm/driver.py +++ /dev/null @@ -1,329 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Damiao MIT-mode CAN driver for OpenArm. SI units throughout. - -Ported from ``enactic/openarm_can`` (C++). No dimos deps — testable with -``can.Bus(interface="virtual")``. -""" - -from __future__ import annotations - -from dataclasses import dataclass -import enum -import errno -import struct -import threading -import time -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - import can - - -class MotorType(str, enum.Enum): - """Damiao motor types used on OpenArm. Values match the reference library.""" - - DM3507 = "DM3507" - DM4310 = "DM4310" - DM4310_48V = "DM4310_48V" - DM4340 = "DM4340" - DM4340_48V = "DM4340_48V" - DM6006 = "DM6006" - DM8006 = "DM8006" - DM8009 = "DM8009" - DM10010L = "DM10010L" - DM10010 = "DM10010" - DMH3510 = "DMH3510" - DMH6215 = "DMH6215" - DMG6220 = "DMG6220" - - -# (p_max [rad], v_max [rad/s], t_max [Nm]) -_MOTOR_LIMITS: dict[MotorType, tuple[float, float, float]] = { - MotorType.DM3507: (12.5, 50.0, 5.0), - MotorType.DM4310: (12.5, 30.0, 10.0), - MotorType.DM4310_48V: (12.5, 50.0, 10.0), - MotorType.DM4340: (12.5, 8.0, 28.0), - MotorType.DM4340_48V: (12.5, 10.0, 28.0), - MotorType.DM6006: (12.5, 45.0, 20.0), - MotorType.DM8006: (12.5, 45.0, 40.0), - MotorType.DM8009: (12.5, 45.0, 54.0), - MotorType.DM10010L: (12.5, 25.0, 200.0), - MotorType.DM10010: (12.5, 20.0, 200.0), - MotorType.DMH3510: (12.5, 280.0, 1.0), - MotorType.DMH6215: (12.5, 45.0, 10.0), - MotorType.DMG6220: (12.5, 45.0, 10.0), -} - -# MIT gain ranges (protocol-fixed, same for every motor type) -KP_MIN, KP_MAX = 0.0, 500.0 -KD_MIN, KD_MAX = 0.0, 5.0 - -# Broadcast/control CAN IDs -_BROADCAST_ID = 0x7FF -_CMD_ENABLE = 0xFC -_CMD_DISABLE = 0xFD -_RID_CTRL_MODE = 10 -CTRL_MODE_MIT = 1 - - -def _clamp(x: float, lo: float, hi: float) -> float: - if x < lo: - return lo - if x > hi: - return hi - return x - - -def float_to_uint(x: float, lo: float, hi: float, bits: int) -> int: - x = _clamp(x, lo, hi) - return int((x - lo) / (hi - lo) * ((1 << bits) - 1)) - - -def uint_to_float(u: int, lo: float, hi: float, bits: int) -> float: - return u / ((1 << bits) - 1) * (hi - lo) + lo - - -def pack_mit_frame( - motor_type: MotorType, - q: float, - dq: float, - kp: float, - kd: float, - tau: float, -) -> bytes: - p_max, v_max, t_max = _MOTOR_LIMITS[motor_type] - q_u = float_to_uint(q, -p_max, p_max, 16) - dq_u = float_to_uint(dq, -v_max, v_max, 12) - kp_u = float_to_uint(kp, KP_MIN, KP_MAX, 12) - kd_u = float_to_uint(kd, KD_MIN, KD_MAX, 12) - tau_u = float_to_uint(tau, -t_max, t_max, 12) - return bytes( - [ - (q_u >> 8) & 0xFF, - q_u & 0xFF, - (dq_u >> 4) & 0xFF, - ((dq_u & 0xF) << 4) | ((kp_u >> 8) & 0xF), - kp_u & 0xFF, - (kd_u >> 4) & 0xFF, - ((kd_u & 0xF) << 4) | ((tau_u >> 8) & 0xF), - tau_u & 0xFF, - ] - ) - - -@dataclass(frozen=True) -class MotorState: - """Decoded state from a Damiao reply frame.""" - - q: float # rad - dq: float # rad/s - tau: float # Nm - t_mos: int # °C - t_rotor: int # °C - timestamp: float # monotonic seconds when received - - -def parse_state_frame(motor_type: MotorType, data: bytes) -> MotorState | None: - """Decode an 8-byte Damiao state reply. Returns None if too short.""" - if len(data) < 8: - return None - p_max, v_max, t_max = _MOTOR_LIMITS[motor_type] - q_u = (data[1] << 8) | data[2] - dq_u = (data[3] << 4) | (data[4] >> 4) - tau_u = ((data[4] & 0x0F) << 8) | data[5] - return MotorState( - q=uint_to_float(q_u, -p_max, p_max, 16), - dq=uint_to_float(dq_u, -v_max, v_max, 12), - tau=uint_to_float(tau_u, -t_max, t_max, 12), - t_mos=int(data[6]), - t_rotor=int(data[7]), - timestamp=time.monotonic(), - ) - - -def _pack_control_command(cmd: int) -> bytes: - return bytes([0xFF] * 7 + [cmd & 0xFF]) - - -def pack_write_param_frame(send_id: int, rid: int, value_u32: int) -> bytes: - """Broadcast parameter-write frame sent to CAN id 0x7FF.""" - val = struct.pack("> 8) & 0xFF, - 0x55, - rid & 0xFF, - val[0], - val[1], - val[2], - val[3], - ] - ) - - -@dataclass(frozen=True) -class DamiaoMotor: - """One Damiao motor on a CAN bus. recv_id defaults to send_id | 0x10.""" - - send_id: int - motor_type: MotorType - recv_id: int | None = None - - @property - def effective_recv_id(self) -> int: - return self.recv_id if self.recv_id is not None else (self.send_id | 0x10) - - @property - def limits(self) -> tuple[float, float, float]: - return _MOTOR_LIMITS[self.motor_type] - - -class OpenArmBus: - """One SocketCAN bus with a background RX thread caching latest state.""" - - def __init__( - self, - channel: str, - motors: list[DamiaoMotor], - *, - fd: bool = False, - interface: str = "socketcan", - ) -> None: - if not motors: - raise ValueError("OpenArmBus needs at least one motor") - # Enforce unique IDs — silent overlap would make state routing ambiguous. - send_ids = [m.send_id for m in motors] - if len(set(send_ids)) != len(send_ids): - raise ValueError(f"duplicate send_id in {send_ids}") - recv_ids = [m.effective_recv_id for m in motors] - if len(set(recv_ids)) != len(recv_ids): - raise ValueError(f"duplicate recv_id in {recv_ids}") - - self._channel = channel - self._motors = list(motors) - self._fd = fd - self._interface = interface - self._by_recv: dict[int, DamiaoMotor] = {m.effective_recv_id: m for m in motors} - - self._bus: can.BusABC | None = None - self._rx_thread: threading.Thread | None = None - self._rx_stop = threading.Event() - self._state_lock = threading.Lock() - self._states: dict[int, MotorState] = {} - - def open(self) -> None: - """Open the CAN bus and start the background RX thread.""" - if self._bus is not None: - return - import can # local import — python-can is optional - - self._bus = can.Bus(interface=self._interface, channel=self._channel, fd=self._fd) - self._rx_stop.clear() - self._rx_thread = threading.Thread( - target=self._rx_loop, name=f"openarm-rx-{self._channel}", daemon=True - ) - self._rx_thread.start() - - def close(self) -> None: - """Stop the RX thread and close the CAN bus.""" - self._rx_stop.set() - if self._rx_thread is not None: - self._rx_thread.join(timeout=1.0) - self._rx_thread = None - if self._bus is not None: - try: - self._bus.shutdown() - finally: - self._bus = None - - def enable_all(self) -> None: - for m in self._motors: - self._send_raw(m.send_id, _pack_control_command(_CMD_ENABLE)) - - def disable_all(self) -> None: - for m in self._motors: - self._send_raw(m.send_id, _pack_control_command(_CMD_DISABLE)) - - def write_ctrl_mode(self, send_id: int, mode: int = CTRL_MODE_MIT) -> None: - self._send_raw( - _BROADCAST_ID, - pack_write_param_frame(send_id, _RID_CTRL_MODE, mode), - ) - - def send_mit_many( - self, - commands: list[tuple[float, float, float, float, float]], - ) -> None: - """One MIT frame per motor; commands[i] → self.motors[i] = (q, dq, kp, kd, tau).""" - if len(commands) != len(self._motors): - raise ValueError(f"expected {len(self._motors)} commands, got {len(commands)}") - for motor, cmd in zip(self._motors, commands, strict=False): - q, dq, kp, kd, tau = cmd - data = pack_mit_frame(motor.motor_type, q, dq, kp, kd, tau) - self._send_raw(motor.send_id, data) - - def get_state(self, send_id: int) -> MotorState | None: - motor = next((m for m in self._motors if m.send_id == send_id), None) - if motor is None: - return None - with self._state_lock: - return self._states.get(motor.effective_recv_id) - - def get_states(self) -> list[MotorState | None]: - with self._state_lock: - return [self._states.get(m.effective_recv_id) for m in self._motors] - - def _send_raw(self, arbitration_id: int, data: bytes) -> None: - if self._bus is None: - raise RuntimeError("bus not open — call .open() first") - import can - - msg = can.Message( - arbitration_id=arbitration_id, - data=data, - is_extended_id=False, - is_fd=self._fd, - bitrate_switch=self._fd, - ) - # Retry on TX buffer full (ENOBUFS) — gs_usb's kernel-side TX queue - # is small. python-can chains the OSError via `raise ... from`, - # so the original errno is on __cause__. - for attempt in range(4): - try: - self._bus.send(msg) - return - except can.CanOperationError as e: - cause = e.__cause__ or e - if getattr(cause, "errno", None) == errno.ENOBUFS and attempt < 3: - time.sleep(0.001 * (attempt + 1)) - else: - raise - - def _rx_loop(self) -> None: - assert self._bus is not None - while not self._rx_stop.is_set(): - msg = self._bus.recv(timeout=0.05) - if msg is None: - continue - motor = self._by_recv.get(int(msg.arbitration_id)) - if motor is None: - continue - state = parse_state_frame(motor.motor_type, bytes(msg.data)) - if state is None: - continue - with self._state_lock: - self._states[motor.effective_recv_id] = state diff --git a/dimos/hardware/manipulators/openarm/test_driver.py b/dimos/hardware/manipulators/openarm/test_driver.py deleted file mode 100644 index c65a972bd6..0000000000 --- a/dimos/hardware/manipulators/openarm/test_driver.py +++ /dev/null @@ -1,270 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Unit tests for the Damiao MIT-mode driver — no hardware required. - -Uses ``can.Bus(interface="virtual")`` for loopback. -""" - -from __future__ import annotations - -import struct -import time - -import pytest - -can = pytest.importorskip("can") - -from dimos.hardware.manipulators.openarm.driver import ( - CTRL_MODE_MIT, - KD_MAX, - KP_MAX, - DamiaoMotor, - MotorType, - OpenArmBus, - float_to_uint, - pack_mit_frame, - pack_write_param_frame, - parse_state_frame, - uint_to_float, -) - - -def test_float_to_uint_endpoints_and_roundtrip() -> None: - # Endpoints - assert float_to_uint(-12.5, -12.5, 12.5, 16) == 0 - assert float_to_uint(12.5, -12.5, 12.5, 16) == (1 << 16) - 1 - # Midpoint is half the full range (rounded down) - mid = float_to_uint(0.0, -12.5, 12.5, 16) - assert mid in ((1 << 16) // 2 - 1, (1 << 16) // 2) - # Out-of-range clamps - assert float_to_uint(-100.0, -12.5, 12.5, 16) == 0 - assert float_to_uint(100.0, -12.5, 12.5, 16) == (1 << 16) - 1 - - -def test_roundtrip_all_gain_ranges() -> None: - # Quantization error should be tiny - for bits, lo, hi in [(16, -12.5, 12.5), (12, 0.0, KP_MAX), (12, 0.0, KD_MAX)]: - step = (hi - lo) / ((1 << bits) - 1) - for k in range(0, 1 << bits, max(1, (1 << bits) // 50)): - x = lo + k * step - u = float_to_uint(x, lo, hi, bits) - x2 = uint_to_float(u, lo, hi, bits) - assert abs(x - x2) <= step - - -def test_mit_frame_kp_kd_zero_and_pos_zero() -> None: - # q=dq=kp=kd=tau=0 → q_u = 32767 (16-bit midpoint), dq_u = 2047 (12-bit), - # tau_u = 2047. kp_u = kd_u = 0 (min of their 0-positive range). - data = pack_mit_frame(MotorType.DM4310, 0.0, 0.0, 0.0, 0.0, 0.0) - assert len(data) == 8 - # Reconstruct fields from bytes - q_u = (data[0] << 8) | data[1] - dq_u = (data[2] << 4) | (data[3] >> 4) - kp_u = ((data[3] & 0xF) << 8) | data[4] - kd_u = (data[5] << 4) | (data[6] >> 4) - tau_u = ((data[6] & 0xF) << 8) | data[7] - assert kp_u == 0 - assert kd_u == 0 - # 16-bit midpoint of symmetric range - assert q_u in (32767, 32768) - assert dq_u in (2047, 2048) - assert tau_u in (2047, 2048) - - -def test_mit_frame_full_positive() -> None: - # Command at every max → every _u field saturates. - data = pack_mit_frame(MotorType.DM4310, 12.5, 30.0, 500.0, 5.0, 10.0) - q_u = (data[0] << 8) | data[1] - dq_u = (data[2] << 4) | (data[3] >> 4) - kp_u = ((data[3] & 0xF) << 8) | data[4] - kd_u = (data[5] << 4) | (data[6] >> 4) - tau_u = ((data[6] & 0xF) << 8) | data[7] - assert q_u == 0xFFFF - assert dq_u == 0xFFF - assert kp_u == 0xFFF - assert kd_u == 0xFFF - assert tau_u == 0xFFF - - -def test_parse_state_roundtrip() -> None: - # Build a synthetic reply frame with known values and verify decode. - # Byte layout for state: [echo, q_hi, q_lo, dq_hi, dq_lo|tau_hi, tau_lo, t_mos, t_rotor] - motor = MotorType.DM4340 - p_max, v_max, t_max = 12.5, 8.0, 28.0 - q_u = float_to_uint(0.3, -p_max, p_max, 16) - dq_u = float_to_uint(-1.0, -v_max, v_max, 12) - tau_u = float_to_uint(2.0, -t_max, t_max, 12) - data = bytes( - [ - 0x03, - (q_u >> 8) & 0xFF, - q_u & 0xFF, - (dq_u >> 4) & 0xFF, - ((dq_u & 0xF) << 4) | ((tau_u >> 8) & 0xF), - tau_u & 0xFF, - 33, - 28, - ] - ) - state = parse_state_frame(motor, data) - assert state is not None - assert abs(state.q - 0.3) < 0.001 - assert abs(state.dq - (-1.0)) < 0.01 - assert abs(state.tau - 2.0) < 0.02 - assert state.t_mos == 33 - assert state.t_rotor == 28 - - -def test_parse_state_rejects_short_frames() -> None: - assert parse_state_frame(MotorType.DM4310, b"\x00" * 4) is None - - -def test_pack_write_param_ctrl_mode_mit() -> None: - data = pack_write_param_frame(0x05, 10, CTRL_MODE_MIT) - assert data[0] == 0x05 - assert data[1] == 0x00 - assert data[2] == 0x55 - assert data[3] == 10 - assert struct.unpack(" OpenArmBus: - return OpenArmBus(channel=channel, motors=motors, fd=False, interface="virtual") - - -def test_bus_validates_unique_ids() -> None: - with pytest.raises(ValueError, match="duplicate send_id"): - OpenArmBus( - channel="v0", - motors=[ - DamiaoMotor(0x01, MotorType.DM4310), - DamiaoMotor(0x01, MotorType.DM4310), - ], - fd=False, - interface="virtual", - ) - - -def test_bus_empty_motor_list_rejected() -> None: - with pytest.raises(ValueError): - OpenArmBus(channel="v0", motors=[], fd=False, interface="virtual") - - -def test_rx_thread_populates_state_cache() -> None: - # Two peers on the same virtual channel loop back to each other. - motors = [ - DamiaoMotor(0x01, MotorType.DM8006), - DamiaoMotor(0x05, MotorType.DM4310), - ] - bus = _make_bus("openarm-test-rx", motors) - # A raw sender on the same virtual channel injects state replies. - sender = can.Bus(interface="virtual", channel="openarm-test-rx") - try: - bus.open() - # Forge a reply for motor 0x01 (recv 0x11) at q = 0.25 rad - q_u = float_to_uint(0.25, -12.5, 12.5, 16) - dq_u = float_to_uint(0.0, -45.0, 45.0, 12) - tau_u = float_to_uint(0.0, -40.0, 40.0, 12) - payload = bytes( - [ - 0x01, - (q_u >> 8) & 0xFF, - q_u & 0xFF, - (dq_u >> 4) & 0xFF, - ((dq_u & 0xF) << 4) | ((tau_u >> 8) & 0xF), - tau_u & 0xFF, - 30, - 28, - ] - ) - sender.send(can.Message(arbitration_id=0x11, data=payload, is_extended_id=False)) - # Poll briefly for the RX thread to consume it - deadline = time.monotonic() + 0.5 - s = None - while s is None and time.monotonic() < deadline: - s = bus.get_state(0x01) - time.sleep(0.01) - assert s is not None, "RX thread did not pick up synthetic state reply" - assert abs(s.q - 0.25) < 0.001 - # Motor 0x05 never got a reply → state should still be None - assert bus.get_state(0x05) is None - finally: - bus.close() - sender.shutdown() - - -def test_send_mit_many_fans_out_one_per_motor() -> None: - motors = [ - DamiaoMotor(0x01, MotorType.DM8006), - DamiaoMotor(0x02, MotorType.DM8006), - DamiaoMotor(0x05, MotorType.DM4310), - ] - bus = _make_bus("openarm-test-send", motors) - listener = can.Bus(interface="virtual", channel="openarm-test-send") - try: - bus.open() - bus.send_mit_many( - [ - (0.1, 0.0, 10.0, 0.5, 0.0), - (0.2, 0.0, 10.0, 0.5, 0.0), - (0.3, 0.0, 10.0, 0.5, 0.0), - ] - ) - seen_ids: set[int] = set() - deadline = time.monotonic() + 0.5 - while len(seen_ids) < 3 and time.monotonic() < deadline: - msg = listener.recv(timeout=0.1) - if msg is not None: - seen_ids.add(int(msg.arbitration_id)) - assert seen_ids == {0x01, 0x02, 0x05} - finally: - bus.close() - listener.shutdown() - - -def test_send_mit_many_size_mismatch() -> None: - bus = _make_bus( - "openarm-test-mismatch", - [DamiaoMotor(0x01, MotorType.DM4310), DamiaoMotor(0x02, MotorType.DM4310)], - ) - try: - bus.open() - with pytest.raises(ValueError): - bus.send_mit_many([(0.0, 0.0, 0.0, 0.0, 0.0)]) - finally: - bus.close() - - -def test_enable_disable_frames_sent() -> None: - bus = _make_bus( - "openarm-test-enable", - [DamiaoMotor(0x01, MotorType.DM4310), DamiaoMotor(0x05, MotorType.DM4310)], - ) - listener = can.Bus(interface="virtual", channel="openarm-test-enable") - try: - bus.open() - bus.enable_all() - seen = {} - deadline = time.monotonic() + 0.3 - while len(seen) < 2 and time.monotonic() < deadline: - msg = listener.recv(timeout=0.1) - if msg is not None: - seen[int(msg.arbitration_id)] = bytes(msg.data) - assert set(seen) == {0x01, 0x05} - for data in seen.values(): - assert data == bytes([0xFF] * 7 + [0xFC]) - finally: - bus.close() - listener.shutdown() diff --git a/dimos/hardware/manipulators/test_adapter_lifecycle.py b/dimos/hardware/manipulators/test_adapter_lifecycle.py index d56f25c48a..c0ac135c1c 100644 --- a/dimos/hardware/manipulators/test_adapter_lifecycle.py +++ b/dimos/hardware/manipulators/test_adapter_lifecycle.py @@ -19,14 +19,12 @@ from typing import Any import pytest -from typing_extensions import override piper_sdk_module = ModuleType("piper_sdk") piper_sdk_module.__dict__["C_PiperInterface_V2"] = lambda **_: None sys.modules.setdefault("piper_sdk", piper_sdk_module) from dimos.hardware.manipulators.a750.adapter import A750Adapter -from dimos.hardware.manipulators.openarm.adapter import OpenArmAdapter from dimos.hardware.manipulators.piper import adapter as piper_adapter from dimos.hardware.manipulators.piper.adapter import PiperAdapter @@ -149,54 +147,6 @@ def test_piper_gripper_uses_sdk_units_and_clamps(piper_sdk: Any) -> None: assert piper_sdk.GripperCtrl.call_args.args[0] == 80_000 -class _OpenArmLifecycle: - def __init__(self) -> None: - self.actions: list[str] = [] - - def enable_all(self) -> None: - self.actions.append("enable") - - def disable_all(self) -> None: - self.actions.append("disable") - - -class _LifecycleOpenArmAdapter(OpenArmAdapter): - def __init__(self, lifecycle: _OpenArmLifecycle) -> None: - super().__init__() - self._lifecycle: _OpenArmLifecycle - self._lifecycle = lifecycle - - @override - def read_joint_positions(self) -> list[float]: - return [0.0] * 7 - - @override - def _compute_gravity_torques(self, q: list[float]) -> list[float]: - return [0.0] * len(q) - - @override - def write_enable(self, enable: bool) -> bool: - if enable: - self._lifecycle.enable_all() - else: - self._lifecycle.disable_all() - return True - - @override - def write_stop(self) -> bool: - self._lifecycle.actions.append("hold") - return True - - -def test_openarm_lifecycle_enables_then_holds_and_disables() -> None: - lifecycle = _OpenArmLifecycle() - adapter = _LifecycleOpenArmAdapter(lifecycle) - - assert adapter.activate() - assert adapter.deactivate() - assert lifecycle.actions == ["enable", "hold", "disable"] - - class _A750Robot: def __init__(self) -> None: self.actions: list[str] = [] diff --git a/dimos/hardware/test_adapter_registries.py b/dimos/hardware/test_adapter_registries.py index 25b8a6feb3..04dfb36aaa 100644 --- a/dimos/hardware/test_adapter_registries.py +++ b/dimos/hardware/test_adapter_registries.py @@ -53,7 +53,6 @@ "manipulators": { "a750", "mock", - "openarm", "piper", "sim_mujoco", "xarm", diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py b/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py deleted file mode 100755 index 9c740ef485..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Probe an OpenArm on a SocketCAN interface. - -Enumerates all 8 expected Damiao motors (7 arm joints + gripper) on one CAN bus -(classical by default, use --fd for CAN-FD), enables each, reads back one state -frame, then disables. Phase-0 hardware-verification script. - -Run AFTER bringing the bus up with dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh. - -Usage: - python dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can0 - python dimos/robot/manipulators/openarm/scripts/openarm_can_probe.py --channel can1 --ids 1,2,3,4,5,6,7 -""" - -from __future__ import annotations - -import argparse -import sys -import time - -try: - import can -except ImportError: - sys.exit("python-can not installed. Run: pip install 'python-can>=4.3'") - -# ---- Damiao motor limit tables (from enactic/openarm_can dm_motor_constants.hpp) -# [p_max rad, v_max rad/s, t_max Nm] -LIMITS: dict[str, tuple[float, float, float]] = { - "DM4310": (12.5, 30.0, 10.0), - "DM4340": (12.5, 8.0, 28.0), - "DM8006": (12.5, 45.0, 40.0), -} - -# OpenArm v10 per-joint motor assignment (derived from joint_limits.yaml effort column) -DEFAULT_MOTORS: list[tuple[int, str]] = [ - (0x01, "DM8006"), # joint1 - (0x02, "DM8006"), # joint2 - (0x03, "DM4340"), # joint3 - (0x04, "DM4340"), # joint4 - (0x05, "DM4310"), # joint5 - (0x06, "DM4310"), # joint6 - (0x07, "DM4310"), # joint7 - (0x08, "DM4310"), # gripper -] - -ENABLE = bytes([0xFF] * 7 + [0xFC]) -DISABLE = bytes([0xFF] * 7 + [0xFD]) - -FD = False # set by --fd at runtime; defaults to classical CAN @ 1 Mbit - - -def uint_to_float(x: int, lo: float, hi: float, bits: int) -> float: - return x / ((1 << bits) - 1) * (hi - lo) + lo - - -def parse_state(motor_type: str, data: bytes) -> tuple[float, float, float, int, int] | None: - """Decode an 8-byte DM motor state reply. Returns (q, dq, tau, t_mos, t_rotor).""" - if len(data) < 8: - return None - p_max, v_max, t_max = LIMITS[motor_type] - q_u = (data[1] << 8) | data[2] - dq_u = (data[3] << 4) | (data[4] >> 4) - tau_u = ((data[4] & 0x0F) << 8) | data[5] - q = uint_to_float(q_u, -p_max, p_max, 16) - dq = uint_to_float(dq_u, -v_max, v_max, 12) - tau = uint_to_float(tau_u, -t_max, t_max, 12) - return q, dq, tau, data[6], data[7] - - -def probe_motor( - bus: can.BusABC, send_id: int, recv_id: int, motor_type: str, timeout: float = 0.2 -) -> bool: - """Enable motor, wait for state reply on recv_id, print result, disable.""" - # Flush any stale frames - while bus.recv(0.0) is not None: - pass - - bus.send( - can.Message( - arbitration_id=send_id, data=ENABLE, is_extended_id=False, is_fd=FD, bitrate_switch=FD - ) - ) - t0 = time.monotonic() - while time.monotonic() - t0 < timeout: - msg = bus.recv(timeout - (time.monotonic() - t0)) - if msg is None: - break - if msg.arbitration_id != recv_id: - continue - parsed = parse_state(motor_type, bytes(msg.data)) - if parsed is None: - print(f" 0x{send_id:02X} ({motor_type}): short reply {list(msg.data)}") - bus.send( - can.Message( - arbitration_id=send_id, - data=DISABLE, - is_extended_id=False, - is_fd=FD, - bitrate_switch=FD, - ) - ) - return False - q, dq, tau, t_mos, t_rot = parsed - print( - f" 0x{send_id:02X} ({motor_type:>6}): " - f"q={q:+.3f} rad dq={dq:+.3f} rad/s tau={tau:+.3f} Nm " - f"T_mos={t_mos}C T_rotor={t_rot}C" - ) - bus.send( - can.Message( - arbitration_id=send_id, - data=DISABLE, - is_extended_id=False, - is_fd=FD, - bitrate_switch=FD, - ) - ) - return True - - print( - f" 0x{send_id:02X} ({motor_type:>6}): NO REPLY on 0x{recv_id:02X} within {timeout * 1e3:.0f}ms" - ) - bus.send( - can.Message( - arbitration_id=send_id, data=DISABLE, is_extended_id=False, is_fd=FD, bitrate_switch=FD - ) - ) - return False - - -def main() -> int: - ap = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - ap.add_argument("--channel", default="can0", help="SocketCAN interface (default: can0)") - ap.add_argument( - "--fd", - action="store_true", - help="Use CAN-FD (requires FD-capable adapter). Default is classical CAN @ 1 Mbit, which is what most gs_usb adapters support.", - ) - ap.add_argument("--ids", default=None, help="Comma-separated send IDs to probe (default: 1..8)") - ap.add_argument("--timeout", type=float, default=0.2, help="Reply timeout per motor (s)") - args = ap.parse_args() - - global FD - FD = args.fd - motors = DEFAULT_MOTORS - if args.ids: - wanted = {int(x, 0) for x in args.ids.split(",")} - motors = [m for m in DEFAULT_MOTORS if m[0] in wanted] - - # Preflight: is the interface up? - try: - flags = int(open(f"/sys/class/net/{args.channel}/flags").read().strip(), 16) - iface_up = bool(flags & 0x1) - except OSError: - print(f"ERROR: interface '{args.channel}' not found", file=sys.stderr) - return 1 - if not iface_up: - print(f"ERROR: SocketCAN interface '{args.channel}' is DOWN.", file=sys.stderr) - print( - f" Run: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {args.channel}", - file=sys.stderr, - ) - return 1 - - print(f"Opening {args.channel} ({'CAN-FD' if FD else 'classical CAN'})...") - try: - bus = can.Bus(interface="socketcan", channel=args.channel, fd=FD) - except Exception as e: - print(f"ERROR opening {args.channel}: {e}", file=sys.stderr) - print( - " Did you run 'sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh' first?", - file=sys.stderr, - ) - return 1 - - try: - print(f"Probing {len(motors)} motor(s) on {args.channel}:") - ok = 0 - for send_id, motor_type in motors: - recv_id = send_id | 0x10 - if probe_motor(bus, send_id, recv_id, motor_type, args.timeout): - ok += 1 - print(f"\n{ok}/{len(motors)} motors replied.") - return 0 if ok == len(motors) else 2 - finally: - bus.shutdown() - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh b/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh deleted file mode 100755 index d25fc41e43..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env bash -# Bring up CAN interfaces for OpenArm. Default is classical CAN @ 1 Mbit, -# which is what most gs_usb (OpenMoko / Geschwister Schneider) USB-CAN -# adapters support. Use MODE=fd if you have a CAN-FD-capable adapter. -# Run with sudo or as root. -# -# Usage: -# sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh # classical 1M, can0 and can1 -# sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 # single interface -# sudo MODE=fd ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh can0 # CAN-FD 1M/5M -set -euo pipefail - -BITRATE=1000000 -DBITRATE=5000000 -MODE="${MODE:-classical}" # classical | fd -IFACES_ARG="${*:-can0 can1}" -# shellcheck disable=SC2206 -IFACES=(${IFACES_ARG[@]}) - -for IF in "${IFACES[@]}"; do - if ! ip link show "$IF" >/dev/null 2>&1; then - echo "[skip] $IF not present" - continue - fi - ip link set "$IF" down || true - if [ "$MODE" = "classical" ]; then - echo "[up ] $IF ${BITRATE} (classical CAN)" - ip link set "$IF" type can bitrate "$BITRATE" - else - echo "[up ] $IF ${BITRATE}/${DBITRATE} fd on" - ip link set "$IF" type can bitrate "$BITRATE" dbitrate "$DBITRATE" fd on - fi - ip link set "$IF" up - ip link set "$IF" txqueuelen 1000 - ip -details link show "$IF" | grep -E "can |bitrate" || true -done diff --git a/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py b/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py deleted file mode 100755 index 04bf3912a3..0000000000 --- a/dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Write CTRL_MODE = MIT (1) to one or all OpenArm motors. - -Damiao motors have a persistent CTRL_MODE register (RID=10). If a motor was -previously configured in POS_VEL (2) / VEL (3) / POS_FORCE (4) mode, it will -respond to enable/disable but IGNORE MIT control frames — exactly the -"motor doesn't move, error grows" symptom. - -This script writes CTRL_MODE=1 (MIT) via the 0x7FF broadcast-write frame -format used by enactic/openarm_can: - - ID=0x7FF data = [id_lo, id_hi, 0x55, RID=10, val[0], val[1], val[2], val[3]] - -Run once per motor after CAN bring-up. The value is persistent across power -cycles. - -Usage: - # All 8 motors on can0 (classical CAN @ 1 Mbit, default) - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 - - # Single motor - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 --id 0x05 - - # CAN-FD (only if your adapter supports it) - python dimos/robot/manipulators/openarm/scripts/openarm_set_mit_mode.py --channel can0 --fd -""" - -from __future__ import annotations - -import argparse -import struct -import sys -import time - -try: - import can -except ImportError: - sys.exit("python-can not installed") - -RID_CTRL_MODE = 10 -MIT_MODE = 1 -DEFAULT_IDS = [0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08] - - -def write_ctrl_mode(bus: can.BusABC, send_id: int, fd: bool) -> bool: - val = struct.pack("> 8) & 0xFF, 0x55, RID_CTRL_MODE, val[0], val[1], val[2], val[3]] - ) - # Flush - while bus.recv(0.0) is not None: - pass - bus.send( - can.Message( - arbitration_id=0x7FF, data=data, is_extended_id=False, is_fd=fd, bitrate_switch=fd - ) - ) - # Wait for ack on 0x7FF (per openarm_can param response) - t0 = time.monotonic() - while time.monotonic() - t0 < 0.2: - msg = bus.recv(0.2 - (time.monotonic() - t0)) - if msg is None: - break - # Reply on 0x7FF: [id_lo, id_hi, 0x33|0x55, rid, value[0..3]] - if msg.arbitration_id != 0x7FF or len(msg.data) < 8: - continue - if msg.data[2] not in (0x33, 0x55): - continue - if msg.data[0] != (send_id & 0xFF) or msg.data[1] != ((send_id >> 8) & 0xFF): - continue # ack from a different motor - rid = msg.data[3] - if rid == RID_CTRL_MODE: - echoed = int(struct.unpack(" int: - ap = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - ap.add_argument("--channel", default="can0") - ap.add_argument("--fd", action="store_true", help="Use CAN-FD (default: classical CAN)") - ap.add_argument( - "--id", type=lambda s: int(s, 0), default=None, help="Single send ID (default: all 8)" - ) - args = ap.parse_args() - - fd = args.fd - ids = [args.id] if args.id is not None else DEFAULT_IDS - - # Preflight: is the interface up? - try: - flags = int(open(f"/sys/class/net/{args.channel}/flags").read().strip(), 16) - except OSError: - print(f"ERROR: interface '{args.channel}' not found", file=sys.stderr) - return 1 - if not (flags & 0x1): - print(f"ERROR: SocketCAN interface '{args.channel}' is DOWN.", file=sys.stderr) - print( - f" Run: sudo ./dimos/robot/manipulators/openarm/scripts/openarm_can_up.sh {args.channel}", - file=sys.stderr, - ) - return 1 - - print(f"Opening {args.channel} ({'CAN-FD' if fd else 'classical'})") - bus = can.Bus(interface="socketcan", channel=args.channel, fd=fd) - try: - ok = 0 - for i in ids: - if write_ctrl_mode(bus, i, fd): - ok += 1 - time.sleep(0.05) - print(f"\n{ok}/{len(ids)} motors set to MIT mode.") - return 0 if ok == len(ids) else 2 - finally: - bus.shutdown() - - -if __name__ == "__main__": - sys.exit(main()) From 7be88dfa56d7d949d3def7e263f5ed8424b38e6e Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 12:31:54 -0700 Subject: [PATCH 33/44] fix(manipulation): address OpenArm review round 1 DM8009 shoulders (DM8006 was a legacy typo), MotorSpecs as plain lists, keyboard teleop module restored to upstream with gripper bindings deferred to a follow-up PR, and dual-arm planning switched to a single bimanual robot model with left_manipulator and right_manipulator groups fed by a hand-written SRDF, since generated SRDFs cannot express cross-robot collision exclusions. Planner blueprint verified against the in-memory adapter in simulation. --- .../test_manipulation_planning_groups.py | 50 ++++++++-------- .../whole_body/openarm_damiao/adapter.py | 26 +++------ .../manipulators/openarm/blueprints/basic.py | 9 +-- .../manipulators/openarm/blueprints/teleop.py | 34 +++-------- dimos/robot/manipulators/openarm/config.py | 58 +++++++++++-------- .../openarm/openarm_v10_bimanual.srdf | 5 ++ .../teleop/keyboard/keyboard_teleop_module.py | 7 +-- docs/capabilities/manipulation/index.md | 4 +- .../manipulation/openarm_integration.md | 21 ++++--- 9 files changed, 103 insertions(+), 111 deletions(-) create mode 100644 dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 1d8bc367a8..7cbd5b68e0 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -42,6 +42,10 @@ # The e2e harness always passes --simulation (DimosCliCall.simulator), so the # blueprint's hardware selection resolves to the in-memory whole-body adapter. BLUEPRINT = "openarm-planner-coordinator" +# Both arms plan as one robot; joint order is left 1..7 then right 1..7. +ROBOT_NAME = "openarm" +LEFT_SLICE = slice(0, 7) +RIGHT_SLICE = slice(7, 14) def _wait_for_robot_info( @@ -130,21 +134,23 @@ def _prepare_for_planning(client: RPCClient, robot_names: tuple[str, ...]) -> No _wait_for_manipulation_state(client, "IDLE") -def _planning_group_id(info: dict[str, Any]) -> str: - groups = info["planning_groups"] - assert len(groups) == 1 - group = groups[0] - if isinstance(group, PlanningGroup): - return group.id - group_id = group["id"] - assert isinstance(group_id, str) - return group_id +def _planning_group_ids(info: dict[str, Any]) -> dict[str, str]: + ids: dict[str, str] = {} + for group in info["planning_groups"]: + if isinstance(group, PlanningGroup): + ids[group.group_name] = group.id + else: + group_id = group["id"] + assert isinstance(group_id, str) + ids[group["group_name"]] = group_id + assert set(ids) == {"left_manipulator", "right_manipulator"} + return ids -def _offset_target(client: RPCClient, robot_name: str, delta: float) -> JointState: - current = client.get_current_joints(robot_name) +def _offset_target(client: RPCClient, group_slice: slice, delta: float) -> JointState: + current = client.get_current_joints(ROBOT_NAME) assert current is not None - return JointState(position=[position + delta for position in current]) + return JointState(position=[position + delta for position in current[group_slice]]) def _start_openarm_mock_planner( @@ -165,15 +171,15 @@ def test_single_arm_plans_and_executes_through_control_coordinator( client = RPCClient(None, ManipulationModule) coordinator_client = RPCClient(None, ControlCoordinator) try: - left_info = _wait_for_robot_info(client, "left_arm") - left_id = _planning_group_id(left_info) + info = _wait_for_robot_info(client, ROBOT_NAME) + left_id = _planning_group_ids(info)["left_manipulator"] tasks = coordinator_client.list_tasks() assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME] - _prepare_for_planning(client, ("left_arm",)) + _prepare_for_planning(client, (ROBOT_NAME,)) - planned = client.plan_to_joint_targets({left_id: _offset_target(client, "left_arm", 0.02)}) + planned = client.plan_to_joint_targets({left_id: _offset_target(client, LEFT_SLICE, 0.02)}) assert planned, client.get_error() assert client.has_planned_path() assert client.execute_plan() @@ -194,20 +200,18 @@ def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( client = RPCClient(None, ManipulationModule) coordinator_client = RPCClient(None, ControlCoordinator) try: - left_info = _wait_for_robot_info(client, "left_arm") - right_info = _wait_for_robot_info(client, "right_arm") - left_id = _planning_group_id(left_info) - right_id = _planning_group_id(right_info) + info = _wait_for_robot_info(client, ROBOT_NAME) + group_ids = _planning_group_ids(info) tasks = coordinator_client.list_tasks() assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME] - _prepare_for_planning(client, ("left_arm", "right_arm")) + _prepare_for_planning(client, (ROBOT_NAME,)) planned = client.plan_to_joint_targets( { - left_id: _offset_target(client, "left_arm", 0.02), - right_id: _offset_target(client, "right_arm", -0.02), + group_ids["left_manipulator"]: _offset_target(client, LEFT_SLICE, 0.02), + group_ids["right_manipulator"]: _offset_target(client, RIGHT_SLICE, -0.02), } ) assert planned, client.get_error() diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py index d7b195e6ab..42b2fa748b 100644 --- a/dimos/hardware/whole_body/openarm_damiao/adapter.py +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -24,24 +24,16 @@ from dimos.hardware.whole_body.damiao.adapter import DamiaoWholeBodyAdapter from dimos.utils.data import LfsPath -# Per-arm motor models, shoulder to wrist, from -# openarm_description/config/arm/v10/joint_limits.yaml. Both arms use the same -# CAN send ids 0x01..0x07 because each arm owns a dedicated bus. -_ARM_MOTOR_TYPES = ( - damiao.MotorType.DM8006, - damiao.MotorType.DM8006, - damiao.MotorType.DM4340, - damiao.MotorType.DM4340, - damiao.MotorType.DM4310, - damiao.MotorType.DM4310, - damiao.MotorType.DM4310, -) - def _arm_motors(side: str) -> list[can_motor_control.MotorSpec]: return [ - can_motor_control.MotorSpec(f"openarm_{side}_joint{index}", motor_type, index, index | 0x10) - for index, motor_type in enumerate(_ARM_MOTOR_TYPES, start=1) + can_motor_control.MotorSpec(f"openarm_{side}_joint1", damiao.MotorType.DM8009, 0x01, 0x11), + can_motor_control.MotorSpec(f"openarm_{side}_joint2", damiao.MotorType.DM8009, 0x02, 0x12), + can_motor_control.MotorSpec(f"openarm_{side}_joint3", damiao.MotorType.DM4340, 0x03, 0x13), + can_motor_control.MotorSpec(f"openarm_{side}_joint4", damiao.MotorType.DM4340, 0x04, 0x14), + can_motor_control.MotorSpec(f"openarm_{side}_joint5", damiao.MotorType.DM4310, 0x05, 0x15), + can_motor_control.MotorSpec(f"openarm_{side}_joint6", damiao.MotorType.DM4310, 0x06, 0x16), + can_motor_control.MotorSpec(f"openarm_{side}_joint7", damiao.MotorType.DM4310, 0x07, 0x17), ] @@ -65,8 +57,8 @@ class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): "left_gripper": "left_arm/gripper", "right_gripper": "right_arm/gripper", } - # Linux assigns can0/can1 in USB enumeration order; remap a swapped rig - # through DamiaoRuntimeConfig.bus_addresses instead of editing topology. + # can0/can1 follow USB enumeration order; remap through + # DamiaoRuntimeConfig.bus_addresses if the rig comes up swapped. bus_defaults = {"left": "can1", "right": "can0"} gravity_joint_names = ( *(f"openarm_left_joint{index}" for index in range(1, 8)), diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index bf09190813..36afa57c52 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -22,8 +22,8 @@ from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( OPENARM_ARM_JOINTS, + openarm_bimanual_model_config, openarm_hardware, - openarm_model_config, ) @@ -40,12 +40,7 @@ def _trajectory_task() -> TaskConfig: _openarm_planner_hw = openarm_hardware() openarm_planner_coordinator = autoconnect( - planner( - robots=[ - openarm_model_config("left"), - openarm_model_config("right"), - ], - ), + planner(robots=[openarm_bimanual_model_config()]), coordinator( hardware=[_openarm_planner_hw], tasks=[_trajectory_task()], diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index 299ef9d85c..435f61a457 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -22,17 +22,17 @@ from dimos.robot.manipulators.common.blueprints import coordinator, planner from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME from dimos.robot.manipulators.openarm.config import ( + OPENARM_ARM_JOINTS, OPENARM_DOF, - OPENARM_GRIPPER_JOINTS, OPENARM_LEFT_MODEL, OPENARM_RIGHT_MODEL, openarm_arm_joints, + openarm_bimanual_model_config, openarm_hardware, - openarm_model_config, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -# The keyboard publishes twists to one task by name; the other arm's task +# The keyboard publishes twists to one task by name; the right arm's task # keeps holding its anchor pose. KEYBOARD_EEF_TASK_NAME = "eef_twist_left_arm" @@ -56,37 +56,23 @@ def _trajectory_task(*, priority: int = 10) -> TaskConfig: return TaskConfig( name=DEFAULT_TRAJECTORY_TASK_NAME, type="trajectory", - joint_names=[*openarm_arm_joints("left"), *openarm_arm_joints("right")], + joint_names=list(OPENARM_ARM_JOINTS), priority=priority, params={"start_position_tolerance": 0.05}, ) -def _gripper_task() -> TaskConfig: - return TaskConfig( - name="servo_grippers", - type="servo", - joint_names=list(OPENARM_GRIPPER_JOINTS), - priority=20, - params={"timeout": 0.0}, - ) - - keyboard_teleop_openarm = autoconnect( - KeyboardTeleopModule.blueprint( - task_name=KEYBOARD_EEF_TASK_NAME, - gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), - ), + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), ControlCoordinator.blueprint( hardware=[_openarm_keyboard_hw], tasks=[ _eef_twist_task("left"), _eef_twist_task("right"), - _gripper_task(), ], ), ManipulationModule.blueprint( - robots=[openarm_model_config("left"), openarm_model_config("right")], + robots=[openarm_bimanual_model_config()], visualization={"backend": "viser"}, ), ) @@ -94,17 +80,13 @@ def _gripper_task() -> TaskConfig: _openarm_keyboard_planner_hw = openarm_hardware() keyboard_teleop_openarm_planner = autoconnect( - KeyboardTeleopModule.blueprint( - task_name=KEYBOARD_EEF_TASK_NAME, - gripper_joint_names=list(OPENARM_GRIPPER_JOINTS), - ), - planner(robots=[openarm_model_config("left"), openarm_model_config("right")]), + KeyboardTeleopModule.blueprint(task_name=KEYBOARD_EEF_TASK_NAME), + planner(robots=[openarm_bimanual_model_config()]), coordinator( hardware=[_openarm_keyboard_planner_hw], tasks=[ _eef_twist_task("left", priority=10), _eef_twist_task("right", priority=10), - _gripper_task(), _trajectory_task(priority=20), ], ), diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 651c2e1f83..ebde7dd663 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -38,21 +38,16 @@ OPENARM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] -OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("openarm_left_link5", "openarm_left_link7"), - ("openarm_right_link5", "openarm_right_link7"), -] - OPENARM_PKG = LfsPath("openarm_description") OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" -OPENARM_V10_FK_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" -OPENARM_GRAVITY_MODEL_PATH = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" +OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" +OPENARM_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v10_bimanual.srdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# MIT gains measured on v10 hardware (legacy adapter): with gravity -# compensation active the PD terms only handle transient tracking, and high kd -# excites gearbox buzz. Gripper slots bypass MIT control, so their gains are 0. +# MIT gains measured on v10 hardware: with gravity compensation active the PD +# terms only handle transient tracking, and high kd excites gearbox buzz. +# Gripper slots bypass MIT control, so their gains are 0. _ARM_KP = (100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0) _ARM_KD = (1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8) @@ -67,6 +62,11 @@ def openarm_arm_joints(side: str) -> list[str]: return [f"{side}_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] +def openarm_urdf_joints(side: str) -> list[str]: + validate_side(side) + return [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] + + def openarm_hardware() -> HardwareComponent: """Select the physical or in-memory whole-body adapter for OpenArm.""" adapter_type = "mock_whole_body" if global_config.simulation else "openarm_damiao" @@ -87,35 +87,45 @@ def openarm_hardware() -> HardwareComponent: ) -def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: - """Build one side's seven-joint planning model.""" - validate_side(side) - resolved_name = name or f"{side}_arm" - local_joint_names = [f"openarm_{side}_joint{i}" for i in range(1, OPENARM_DOF + 1)] +def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModelConfig: + """Build the single fourteen-joint planning model with one group per arm. + + SRDF generation does not compose collision exclusions across robots, so + both arms plan as one robot and the exclusions come from a hand-written + SRDF. + """ + local_joint_names = [*openarm_urdf_joints("left"), *openarm_urdf_joints("right")] return RobotModelConfig( - name=resolved_name, - model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, + name=name, + model_path=OPENARM_BIMANUAL_MODEL, base_pose=base_pose(), joint_names=local_joint_names, base_link="openarm_body_link0", planning_groups=[ PlanningGroupDefinition( - name="manipulator", - joint_names=tuple(local_joint_names), + name="left_manipulator", + joint_names=tuple(openarm_urdf_joints("left")), base_link="openarm_body_link0", - tip_link=f"openarm_{side}_link7", - ) + tip_link="openarm_left_link7", + ), + PlanningGroupDefinition( + name="right_manipulator", + joint_names=tuple(openarm_urdf_joints("right")), + base_link="openarm_body_link0", + tip_link="openarm_right_link7", + ), ], package_paths=OPENARM_PACKAGE_PATHS, - collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, + srdf_path=OPENARM_BIMANUAL_SRDF, auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, joint_name_mapping={ coordinator_name: urdf_name + for side in OPENARM_SIDES for coordinator_name, urdf_name in zip( - openarm_arm_joints(side), local_joint_names, strict=True + openarm_arm_joints(side), openarm_urdf_joints(side), strict=True ) }, - home_joints=[0.0] * OPENARM_DOF, + home_joints=[0.0] * (2 * OPENARM_DOF), ) diff --git a/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf new file mode 100644 index 0000000000..68fd5991f8 --- /dev/null +++ b/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf @@ -0,0 +1,5 @@ + + + + + diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index a08ae1bdd6..80ad6e1471 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -63,6 +63,7 @@ # Normalized gripper command values. GRIPPER_OPEN_POSITION = 1.0 GRIPPER_CLOSED_POSITION = 0.0 +# TODO: Improve gripper handling. GRIPPER_JOINT_NAME = "arm/gripper" TwistVector = tuple[float, float, float] @@ -73,9 +74,6 @@ class KeyboardTeleopConfig(ModuleConfig): linear_speed: float = DEFAULT_LINEAR_SPEED angular_speed: float = DEFAULT_ANGULAR_SPEED gripper_open_position: float = GRIPPER_OPEN_POSITION - # All named joints receive the same opening; multi-gripper robots list - # every gripper joint here. - gripper_joint_names: list[str] = [GRIPPER_JOINT_NAME] def _motion_key_codes() -> frozenset[int]: @@ -262,8 +260,7 @@ def _set_gripper_position(self, position: float) -> None: if self._gripper_position == position: return self._gripper_position = position - names = list(self.config.gripper_joint_names) - self.joint_command.publish(JointState(name=names, position=[position] * len(names))) + self.joint_command.publish(JointState(name=[GRIPPER_JOINT_NAME], position=[position])) def _twist_from_keys( diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 9a51f6fd2c..7aaadd3c24 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -36,8 +36,8 @@ an explicit target. OpenArm follows the same whole-body model with both arms and both grippers in one device: fourteen angular joints (`left_arm/joint1..7`, `right_arm/joint1..7`) plus two normalized gripper joints (`left_arm/gripper`, -`right_arm/gripper`). The keyboard jogs the left arm; the right arm holds its -pose, and `[` / `]` drive both grippers together. +`right_arm/gripper`). The keyboard jogs the left arm while the right arm holds +its pose; keyboard gripper bindings are a follow-up. Open the Meshcat URL printed in the terminal (default `http://localhost:7000`) to see the robot. diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 099f83f2b3..517c81a5fb 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -27,18 +27,24 @@ cycle. The command vector order is `left_arm/joint1..7`, `right_arm/joint1..7`, (`0.0` closed, `1.0` open). Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): -2x DM8006, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. +2x DM8009, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. Gravity compensation uses the bimanual URDF (`openarm_description/urdf/robot/openarm_v10_bimanual.urdf`, resolved lazily from LFS at connect time) and is preflighted against the declared joint order before the motors enable. +Planning also uses the bimanual URDF: one robot model with a +`left_manipulator` and a `right_manipulator` planning group. Automatic SRDF +generation does not compose collision exclusions across robots, so the +exclusions come from a hand-written SRDF +(`dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf`). + ## Bring-up ```bash -dimos can setup can0 -dimos can setup can1 +dimos hardware can setup can0 +dimos hardware can setup can1 dimos run keyboard-teleop-openarm ``` @@ -52,16 +58,17 @@ editing the adapter topology. | Blueprint | Contents | |---|---| | `coordinator-openarm` | coordinator + trajectory task over both arms | -| `openarm-planner-coordinator` | planner (per-side models) + coordinator | -| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + gripper servo + viser | +| `openarm-planner-coordinator` | planner (bimanual model) + coordinator | +| `keyboard-teleop-openarm` | keyboard + per-arm EEF twist + viser | | `keyboard-teleop-openarm-planner` | teleop + planner + preempting trajectory task | All blueprints run against the in-memory whole-body adapter under `--simulation`; the physical adapter is selected automatically otherwise. The keyboard jogs the left arm (`eef_twist_left_arm`); the right arm's twist -task holds its anchor pose. `[` opens and `]` closes both grippers together -via a single servo task over both gripper joints. +task holds its anchor pose. Keyboard gripper bindings for the two grippers are +a follow-up; the gripper joints accept normalized `/joint_command` targets in +the meantime. ## Files From 678cc0514ac654f5e8b1d58636816d63d8343da1 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 13:33:13 -0700 Subject: [PATCH 34/44] feat(manipulation): use official OpenArm 2.0 description Replace the stale v1.0 data package with URDFs generated from the official OpenArm v2.0 preset pipeline (enactic/openarm_description @ 6c7b720f1ba, default_bimanual plus per-side pinch gripper presets). Pinch gripper finger joints are fixed in the generated models so each arm exposes exactly its seven driven joints while keeping gripper geometry and mass; mesh URIs stay package relative and ros2_control blocks are dropped. The package ships v2.0 meshes, the three URDFs, and a PROVENANCE file, and shrinks from 70 MB to 8 MB. The v2.0 generator collapses link7, so planning tips move to openarm_{side}_ee_base_link and the SRDF now disables the sibling finger pair per hand. Joint naming is unchanged from v1.0, so the adapter topology and gravity joint order carry over. Verified with pinocchio (14 and 7 DOF models, finite gravity) and a planner blueprint run in simulation. --- data/.lfs/openarm_description.tar.gz | 4 ++-- .../whole_body/openarm_damiao/adapter.py | 6 +++--- dimos/robot/manipulators/openarm/config.py | 19 ++++++++++--------- .../openarm/openarm_v10_bimanual.srdf | 5 ----- .../openarm/openarm_v20_bimanual.srdf | 5 +++++ .../manipulation/openarm_integration.md | 4 ++-- 6 files changed, 22 insertions(+), 21 deletions(-) delete mode 100644 dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf create mode 100644 dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf diff --git a/data/.lfs/openarm_description.tar.gz b/data/.lfs/openarm_description.tar.gz index 54aa76da41..74fe2a9ee4 100644 --- a/data/.lfs/openarm_description.tar.gz +++ b/data/.lfs/openarm_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4da176b6c210b9796bb2ee1a29c15ee9a67578b9ae906eb89a6ec8a44b7f303a -size 70064687 +oid sha256:b064cb32f95abb8b0d75c803d06246bbbf4c07a5226905f0e78297896a82fb45 +size 8095150 diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py index 42b2fa748b..4b64ae287a 100644 --- a/dimos/hardware/whole_body/openarm_damiao/adapter.py +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""OpenArm v10 bimanual physical topology for the generic Damiao whole-body adapter.""" +"""OpenArm v2.0 bimanual physical topology for the generic Damiao whole-body adapter.""" from __future__ import annotations @@ -47,7 +47,7 @@ def _gripper_motor(side: str) -> can_motor_control.MotorSpec: class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): - """Two OpenArm v10 arms with grippers, one CAN bus per arm.""" + """Two OpenArm v2.0 arms with grippers, one CAN bus per arm.""" arm_joints = { "left_arm": tuple(f"left_arm/joint{index}" for index in range(1, 8)), @@ -68,7 +68,7 @@ class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): @property def gravity_model_path(self) -> Path: """Return the lazy bimanual gravity-compensation URDF path.""" - return LfsPath("openarm_description") / "urdf/robot/openarm_v10_bimanual.urdf" + return LfsPath("openarm_description") / "urdf/robot/openarm_v20_bimanual.urdf" def _build_robot(self) -> can_motor_control.Robot: return ( diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index ebde7dd663..820c66a5e2 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -39,15 +39,16 @@ OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] OPENARM_PKG = LfsPath("openarm_description") -OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" -OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" -OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" -OPENARM_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v10_bimanual.srdf" +OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_left.urdf" +OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_right.urdf" +OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_bimanual.urdf" +OPENARM_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v20_bimanual.srdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} -# MIT gains measured on v10 hardware: with gravity compensation active the PD -# terms only handle transient tracking, and high kd excites gearbox buzz. -# Gripper slots bypass MIT control, so their gains are 0. +# MIT gains measured on v1.0 hardware, carried over as the v2.0 starting +# point: with gravity compensation active the PD terms only handle transient +# tracking, and high kd excites gearbox buzz. Gripper slots bypass MIT +# control, so their gains are 0. _ARM_KP = (100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0) _ARM_KD = (1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8) @@ -106,13 +107,13 @@ def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModel name="left_manipulator", joint_names=tuple(openarm_urdf_joints("left")), base_link="openarm_body_link0", - tip_link="openarm_left_link7", + tip_link="openarm_left_ee_base_link", ), PlanningGroupDefinition( name="right_manipulator", joint_names=tuple(openarm_urdf_joints("right")), base_link="openarm_body_link0", - tip_link="openarm_right_link7", + tip_link="openarm_right_ee_base_link", ), ], package_paths=OPENARM_PACKAGE_PATHS, diff --git a/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf deleted file mode 100644 index 68fd5991f8..0000000000 --- a/dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf new file mode 100644 index 0000000000..ddd46939c7 --- /dev/null +++ b/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf @@ -0,0 +1,5 @@ + + + + + diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 517c81a5fb..38f1e4161b 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -30,7 +30,7 @@ Per arm, shoulder to wrist (send ids `0x01..0x07`, feedback `send | 0x10`): 2x DM8009, 2x DM4340, 3x DM4310, plus a DM4310 gripper at `0x08`. Gravity compensation uses the bimanual URDF -(`openarm_description/urdf/robot/openarm_v10_bimanual.urdf`, resolved lazily +(`openarm_description/urdf/robot/openarm_v20_bimanual.urdf`, resolved lazily from LFS at connect time) and is preflighted against the declared joint order before the motors enable. @@ -38,7 +38,7 @@ Planning also uses the bimanual URDF: one robot model with a `left_manipulator` and a `right_manipulator` planning group. Automatic SRDF generation does not compose collision exclusions across robots, so the exclusions come from a hand-written SRDF -(`dimos/robot/manipulators/openarm/openarm_v10_bimanual.srdf`). +(`dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf`). ## Bring-up From 782e4b2d3ed3c23bd60f4a9d2b46924b0f4fdeb6 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 14:53:11 -0700 Subject: [PATCH 35/44] feat(manipulation): compose planning groups within one robot RoboPlan generated composite planning groups only for selections spanning two or more robots, so a bimanual robot modeled as one URDF with two planning groups could not plan both arms in a single request. Drop the robot-count restriction; the joint-disjointness requirement and the composite group cap still apply, and overlapping selections are already rejected at the selection layer. Verified in process on the OpenArm 2.0 bimanual model: left, right, and combined fourteen-joint plans all succeed. --- .../planning/world/roboplan_model.py | 2 -- dimos/manipulation/test_roboplan_world.py | 23 ++++++++++++++++--- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index 75f5b1d492..a6208abcf7 100644 --- a/dimos/manipulation/planning/world/roboplan_model.py +++ b/dimos/manipulation/planning/world/roboplan_model.py @@ -323,8 +323,6 @@ def _groups( generated = 0 for size in range(2, len(configured) + 1): for selected in combinations(configured, size): - if len({group.robot_name for group in selected}) < 2: - continue if len({name for group in selected for name in group.joint_names}) != sum( len(group.joint_names) for group in selected ): diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..9583a39f31 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -1394,7 +1394,7 @@ def test_native_selected_planner_accepts_local_joint_names( assert result.path[-1].position == [0.2, 0.4] -def test_native_selected_planner_rejects_multi_group_selection( +def test_native_selected_planner_composes_disjoint_groups_within_one_robot( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: config = robot_config.model_copy( @@ -1415,8 +1415,25 @@ def test_native_selected_planner_rejects_multi_group_selection( JointState(name=list(selection.joint_names), position=[0.1, 0.1]), ) - assert result.status == PlanningStatus.UNSUPPORTED - assert "no generated group" in result.message + assert result.status == PlanningStatus.SUCCESS + assert result.path + + +def test_overlapping_group_selection_rejected_before_planning( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + config = robot_config.model_copy( + update={ + "planning_groups": [ + PlanningGroupDefinition("left", ("joint1", "joint2"), "base", "left_tip"), + PlanningGroupDefinition("right", ("joint2",), "base", "right_tip"), + ] + } + ) + _make_world(fake_roboplan, config) + + with pytest.raises(ValueError, match="overlap"): + _selection((config,), "arm/left", "arm/right") def test_native_planner_coordinates_groups_across_two_robots( From 414620241873bce7a57e20c0f01ee166e79df540 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 18:40:29 -0700 Subject: [PATCH 36/44] fix(manipulation): keep converted meshes with equal stems distinct Converted OBJ files were named by stem only, so a robot whose visual and collision meshes share a stem (OpenArm v2.0 uses visual/link3.dae and collision/link3.stl) had the collision conversion overwrite the visual one, and viewers rendered collision geometry in visual mode. Suffix the converted name with a hash of the source path and pin the behavior with tests. --- .../manipulation/planning/utils/mesh_utils.py | 7 ++- .../planning/utils/test_mesh_utils.py | 59 +++++++++++++++++++ 2 files changed, 64 insertions(+), 2 deletions(-) create mode 100644 dimos/manipulation/planning/utils/test_mesh_utils.py diff --git a/dimos/manipulation/planning/utils/mesh_utils.py b/dimos/manipulation/planning/utils/mesh_utils.py index 33fce6d6c8..14c430780c 100644 --- a/dimos/manipulation/planning/utils/mesh_utils.py +++ b/dimos/manipulation/planning/utils/mesh_utils.py @@ -231,9 +231,12 @@ def convert_mesh(match: re.Match[str]) -> str: # Load mesh mesh = trimesh.load(original_path, force="mesh") - # Generate output path + # Generate output path. Include a source-path hash: different + # meshes may share a stem (visual/link3.dae vs collision/link3.stl) + # and stem-only names would overwrite each other. mesh_name = Path(original_path).stem - obj_path = mesh_dir / f"{mesh_name}.obj" + path_tag = hashlib.md5(original_path.encode()).hexdigest()[:8] + obj_path = mesh_dir / f"{mesh_name}_{path_tag}.obj" # Export as OBJ (trimesh.export returns None, ignore) mesh.export(str(obj_path), file_type="obj") # type: ignore[no-untyped-call] diff --git a/dimos/manipulation/planning/utils/test_mesh_utils.py b/dimos/manipulation/planning/utils/test_mesh_utils.py new file mode 100644 index 0000000000..f376f854d2 --- /dev/null +++ b/dimos/manipulation/planning/utils/test_mesh_utils.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import re + +import trimesh + +from dimos.manipulation.planning.utils.mesh_utils import _convert_meshes + + +def _write_box_mesh(path: Path, extents: tuple[float, float, float]) -> None: + trimesh.creation.box(extents=extents).export(str(path)) + + +def test_convert_meshes_same_stem_different_dirs_stay_distinct(tmp_path: Path) -> None: + """Visual and collision meshes often share a file stem; converted OBJs + must not overwrite each other.""" + visual_dir = tmp_path / "visual" + collision_dir = tmp_path / "collision" + visual_dir.mkdir() + collision_dir.mkdir() + _write_box_mesh(visual_dir / "link3.stl", (1.0, 1.0, 1.0)) + _write_box_mesh(collision_dir / "link3.stl", (2.0, 2.0, 2.0)) + + urdf = ( + f'' + f'' + ) + converted = _convert_meshes(urdf, tmp_path) + + obj_paths = [Path(p) for p in re.findall(r'filename="([^"]+\.obj)"', converted)] + assert len(obj_paths) == 2 + assert obj_paths[0] != obj_paths[1] + sizes = sorted(trimesh.load(str(p), force="mesh").extents[0] for p in obj_paths) + assert sizes[0] == 1.0 + assert sizes[1] == 2.0 + + +def test_convert_meshes_same_file_referenced_twice_converts_once(tmp_path: Path) -> None: + mesh = tmp_path / "part.stl" + _write_box_mesh(mesh, (1.0, 1.0, 1.0)) + + urdf = f'' + converted = _convert_meshes(urdf, tmp_path) + + obj_paths = set(re.findall(r'filename="([^"]+\.obj)"', converted)) + assert len(obj_paths) == 1 From caf14861366b78055afd00c5707a86224aa3bdc4 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 19:01:52 -0700 Subject: [PATCH 37/44] feat(manipulation): put OpenArm pose targets at the grasp frame Regenerate the v2.0 URDFs with emit_grasp_frame enabled and move the planning group tip links from the ee flange to openarm_{side}_grasp_frame, matching the OpenYAM gripper_tip convention. Model DOF, joint order, and gravity behavior are unchanged; planning verified for left, right, and combined requests. --- data/.lfs/openarm_description.tar.gz | 4 ++-- dimos/robot/manipulators/openarm/config.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/data/.lfs/openarm_description.tar.gz b/data/.lfs/openarm_description.tar.gz index 74fe2a9ee4..4a46e74a88 100644 --- a/data/.lfs/openarm_description.tar.gz +++ b/data/.lfs/openarm_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b064cb32f95abb8b0d75c803d06246bbbf4c07a5226905f0e78297896a82fb45 -size 8095150 +oid sha256:3e9a568ec8bded5ca32b2e3de92d27ab78732bb6f2bf4d3d6d16e5093ca30997 +size 8095302 diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 820c66a5e2..e90a498840 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -107,13 +107,13 @@ def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModel name="left_manipulator", joint_names=tuple(openarm_urdf_joints("left")), base_link="openarm_body_link0", - tip_link="openarm_left_ee_base_link", + tip_link="openarm_left_grasp_frame", ), PlanningGroupDefinition( name="right_manipulator", joint_names=tuple(openarm_urdf_joints("right")), base_link="openarm_body_link0", - tip_link="openarm_right_ee_base_link", + tip_link="openarm_right_grasp_frame", ), ], package_paths=OPENARM_PACKAGE_PATHS, From 64efdee7ada08a4323c6ff2f2d4d0ada2558c9c3 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Tue, 4 Aug 2026 20:21:53 -0700 Subject: [PATCH 38/44] fix(manipulation): merge target ghost state across groups on one robot The optimistic target ghost rebuilt its per-robot joint values from the current state for every selected group, so with two planning groups on one robot the group processed last discarded the other group's target and the ghost only ever showed one arm's goal. Seed the merge once per robot and overlay each group's target into the same values. --- dimos/manipulation/visualization/viser/gui.py | 4 ++- .../visualization/viser/test_gui.py | 32 +++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 02145d9fc5..1f90d4585c 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -969,7 +969,9 @@ def _target_ghost_states( current = self.get_current_joint_state(robot_name) if config is None or current is None: continue - values = self._local_values_for_robot(robot_name, current) + values = merged.get(robot_name) + if values is None: + values = self._local_values_for_robot(robot_name, current) target_raw = self._state_values_by_local_name(target) for local_name, global_name in zip( group.local_joint_names, group.joint_names, strict=True diff --git a/dimos/manipulation/visualization/viser/test_gui.py b/dimos/manipulation/visualization/viser/test_gui.py index 9d8be1e168..7e5d7b96c4 100644 --- a/dimos/manipulation/visualization/viser/test_gui.py +++ b/dimos/manipulation/visualization/viser/test_gui.py @@ -16,6 +16,7 @@ from collections.abc import Callable from dataclasses import dataclass +from types import SimpleNamespace import pytest @@ -470,3 +471,34 @@ def test_gui_ignores_stale_timed_out_operation_finish() -> None: assert gui.state.action_status == ActionStatus.FAILED assert gui.state.error == "Operation timed out after 5.0s" + + +def test_target_ghost_states_merge_groups_sharing_one_robot( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Two planning groups on one robot must both contribute to the ghost state.""" + gui = make_gui() + left = planning_group("bot", "left_manipulator", ("j1",)) + right = planning_group("bot", "right_manipulator", ("j2",)) + gui.state.selected_group_ids = (str(left.id), str(right.id)) + + monkeypatch.setattr(gui, "_groups_by_id", lambda: {str(left.id): left, str(right.id): right}) + monkeypatch.setattr( + gui, + "get_robot_config", + lambda _name: SimpleNamespace(joint_names=("j1", "j2")), + ) + monkeypatch.setattr( + gui, + "get_current_joint_state", + lambda _name: JointState({"name": ["bot/j1", "bot/j2"], "position": [0.0, 0.0]}), + ) + + targets = { + str(left.id): JointState({"name": ["bot/j1"], "position": [0.5]}), + str(right.id): JointState({"name": ["bot/j2"], "position": [-0.5]}), + } + ghost_states = gui._target_ghost_states(targets) + + assert list(ghost_states) == ["bot"] + assert list(ghost_states["bot"].position) == [0.5, -0.5] From 3565452cfd0d8e57dc37cd893805c291fb081da8 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Wed, 5 Aug 2026 10:01:55 -0700 Subject: [PATCH 39/44] refactor(manipulation): drop the unneeded OpenArm SRDF The pinch gripper finger joints are fixed in the generated models and their collision meshes do not intersect at the fixed pose, so the manual exclusions were dead weight. Verified left, right, and combined plans still succeed without the file. --- dimos/robot/manipulators/openarm/config.py | 6 +----- dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf | 5 ----- docs/capabilities/manipulation/openarm_integration.md | 6 ++---- 3 files changed, 3 insertions(+), 14 deletions(-) delete mode 100644 dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index e90a498840..cdfbef7a35 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -42,7 +42,6 @@ OPENARM_LEFT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_left.urdf" OPENARM_RIGHT_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_right.urdf" OPENARM_BIMANUAL_MODEL = OPENARM_PKG / "urdf/robot/openarm_v20_bimanual.urdf" -OPENARM_BIMANUAL_SRDF = Path(__file__).parent / "openarm_v20_bimanual.srdf" OPENARM_PACKAGE_PATHS: dict[str, Path] = {"openarm_description": OPENARM_PKG} # MIT gains measured on v1.0 hardware, carried over as the v2.0 starting @@ -91,9 +90,7 @@ def openarm_hardware() -> HardwareComponent: def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModelConfig: """Build the single fourteen-joint planning model with one group per arm. - SRDF generation does not compose collision exclusions across robots, so - both arms plan as one robot and the exclusions come from a hand-written - SRDF. + Collision exclusions cannot span robots, so both arms plan as one robot. """ local_joint_names = [*openarm_urdf_joints("left"), *openarm_urdf_joints("right")] return RobotModelConfig( @@ -117,7 +114,6 @@ def openarm_bimanual_model_config(name: str = OPENARM_HARDWARE_ID) -> RobotModel ), ], package_paths=OPENARM_PACKAGE_PATHS, - srdf_path=OPENARM_BIMANUAL_SRDF, auto_convert_meshes=True, max_velocity=0.5, max_acceleration=1.0, diff --git a/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf b/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf deleted file mode 100644 index ddd46939c7..0000000000 --- a/dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/docs/capabilities/manipulation/openarm_integration.md b/docs/capabilities/manipulation/openarm_integration.md index 38f1e4161b..ed8b1d07f1 100644 --- a/docs/capabilities/manipulation/openarm_integration.md +++ b/docs/capabilities/manipulation/openarm_integration.md @@ -35,10 +35,8 @@ from LFS at connect time) and is preflighted against the declared joint order before the motors enable. Planning also uses the bimanual URDF: one robot model with a -`left_manipulator` and a `right_manipulator` planning group. Automatic SRDF -generation does not compose collision exclusions across robots, so the -exclusions come from a hand-written SRDF -(`dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf`). +`left_manipulator` and a `right_manipulator` planning group, since collision +exclusions cannot span robots. ## Bring-up From 05ccdbd12d24bb93082d59221d356f7bc30078c6 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Wed, 5 Aug 2026 15:31:10 -0700 Subject: [PATCH 40/44] fix(manipulation): stream arm state before gripper calibration Gripper opening calibrates during activation, and reading it earlier raises, so a connected but not activated whole-body adapter failed the entire state read and read-only bring-up sessions (connect without enable) streamed nothing. Report placeholder gripper states until the adapter is active; arms stream immediately and real openings appear after activation. Found on OpenArm hardware during the read-only phase. --- dimos/hardware/whole_body/damiao/adapter.py | 5 +++++ dimos/hardware/whole_body/damiao/test_adapter.py | 14 ++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index 6cbe6d4eb2..87ecd1f47f 100644 --- a/dimos/hardware/whole_body/damiao/adapter.py +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -250,6 +250,11 @@ def read_motor_states(self) -> list[MotorState]: for position, velocity, effort in zip(q, dq, tau, strict=True) ) for name in self.gripper_joints: + if not self._active: + # Gripper opening calibrates during activation; report a + # placeholder so read-only sessions still stream arm state. + states.append(MotorState(q=0.0, dq=0.0, tau=0.0)) + continue opening = float(self._grippers[name].opening) if not np.isfinite(opening) or not 0.0 <= opening <= 1.0: raise RuntimeError(f"gripper {name!r} returned invalid opening {opening}") diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index cf3373edcd..2f88af2766 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -723,3 +723,17 @@ def test_write_motor_commands_gravity_enabled_adds_computed_torque( assert compute_gravity.call_count == 1 assert cast("FakeArm", dual_robot["left_arm"]).commands[-1][:, 4].tolist() == [1.5, 2.5] assert cast("FakeArm", dual_robot["right_arm"]).commands[-1][:, 4].tolist() == [3.5, 4.5] + + +def test_read_motor_states_inactive_gripper_reports_placeholder( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + """Gripper opening calibrates at activation; before that the read path + must not touch it so read-only bring-up sessions still stream arm state.""" + cast("FakeGripper", dual_robot["left_gripper"]).opening = None + cast("FakeGripper", dual_robot["right_gripper"]).opening = None + + states = connected_dual_adapter.read_motor_states() + + assert states[4:] == [MotorState(q=0.0), MotorState(q=0.0)] From 352e8bbcc7ec73364708d652ae12e82b4c2261aa Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Wed, 5 Aug 2026 15:35:46 -0700 Subject: [PATCH 41/44] fix(manipulation): pump feedback from the read path while inactive Damiao feedback only updates when the bus is ticked, which the write path does once per control cycle while the adapter is active. A connected but not activated adapter never ticked, so read-only sessions streamed the connect-time snapshot forever. Refresh from the read path whenever the adapter is inactive; the active path is unchanged and still ticks exactly once per cycle. --- dimos/hardware/whole_body/damiao/adapter.py | 5 +++++ dimos/hardware/whole_body/damiao/test_adapter.py | 16 ++++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/dimos/hardware/whole_body/damiao/adapter.py b/dimos/hardware/whole_body/damiao/adapter.py index 87ecd1f47f..102c3d277d 100644 --- a/dimos/hardware/whole_body/damiao/adapter.py +++ b/dimos/hardware/whole_body/damiao/adapter.py @@ -237,6 +237,11 @@ def has_motor_states(self) -> bool: def read_motor_states(self) -> list[MotorState]: if not self._connected: raise RuntimeError("Damiao whole-body adapter is not connected") + if not self._active: + # The write path pumps the bus once per control cycle while + # active; without it feedback would stay frozen at the connect + # snapshot, so keep it flowing for read-only sessions. + self._refresh() states: list[MotorState] = [] for name, expected_joints in self.arm_joints.items(): arm = self._arms[name] diff --git a/dimos/hardware/whole_body/damiao/test_adapter.py b/dimos/hardware/whole_body/damiao/test_adapter.py index 2f88af2766..951f604034 100644 --- a/dimos/hardware/whole_body/damiao/test_adapter.py +++ b/dimos/hardware/whole_body/damiao/test_adapter.py @@ -737,3 +737,19 @@ def test_read_motor_states_inactive_gripper_reports_placeholder( states = connected_dual_adapter.read_motor_states() assert states[4:] == [MotorState(q=0.0), MotorState(q=0.0)] + + +def test_read_motor_states_inactive_adapter_pumps_feedback( + connected_dual_adapter: DualAdapter, + dual_robot: FakeRobot, +) -> None: + """Without the active write path ticking the bus, the read path must + refresh feedback itself or read-only sessions stream a frozen snapshot.""" + refreshes_before = dual_robot.refresh_count + ticks_before = dual_robot.tick_count + + connected_dual_adapter.read_motor_states() + connected_dual_adapter.read_motor_states() + + assert dual_robot.refresh_count == refreshes_before + 2 + assert dual_robot.tick_count == ticks_before + 2 From 50ca351b262df44fcf4e44a8a7f1fc7a25faffa9 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Thu, 6 Aug 2026 15:52:21 -0700 Subject: [PATCH 42/44] feat(teleop): port openarm mini leader module to the OpenArm 2.0 stack Ports dimos/teleop/openarm_mini from cc/feat/openarm-mini at 1563ff173, including the multi-turn encoder wrap and the CLI tools. Adapted for the whole-body OpenArm follower: default follower joint names come from openarm_arm_joints, and the sender-side clamp limits mirror the OpenArm v2.0 URDF instead of the v1.0 measured limits. --- dimos/teleop/openarm_mini/calibration.py | 125 +++++ dimos/teleop/openarm_mini/cli/_errors.py | 27 + dimos/teleop/openarm_mini/cli/app.py | 28 + dimos/teleop/openarm_mini/cli/calibrate.py | 246 +++++++++ dimos/teleop/openarm_mini/cli/joint_tui.py | 203 ++++++++ .../teleop/openarm_mini/cli/setup_motor_id.py | 208 ++++++++ .../teleop/openarm_mini/cli/test_calibrate.py | 110 ++++ dimos/teleop/openarm_mini/cli/test_cli.py | 216 ++++++++ .../teleop/openarm_mini/cli/test_joint_tui.py | 131 +++++ .../openarm_mini/cli/test_setup_motor_id.py | 134 +++++ dimos/teleop/openarm_mini/feetech.py | 170 ++++++ dimos/teleop/openarm_mini/mapping.py | 132 +++++ dimos/teleop/openarm_mini/teleop_module.py | 248 +++++++++ dimos/teleop/openarm_mini/test_calibration.py | 124 +++++ dimos/teleop/openarm_mini/test_feetech.py | 133 +++++ dimos/teleop/openarm_mini/test_mapping.py | 108 ++++ .../teleop/openarm_mini/test_teleop_module.py | 482 ++++++++++++++++++ pyproject.toml | 5 + uv.lock | 29 +- 19 files changed, 2858 insertions(+), 1 deletion(-) create mode 100644 dimos/teleop/openarm_mini/calibration.py create mode 100644 dimos/teleop/openarm_mini/cli/_errors.py create mode 100644 dimos/teleop/openarm_mini/cli/app.py create mode 100644 dimos/teleop/openarm_mini/cli/calibrate.py create mode 100644 dimos/teleop/openarm_mini/cli/joint_tui.py create mode 100644 dimos/teleop/openarm_mini/cli/setup_motor_id.py create mode 100644 dimos/teleop/openarm_mini/cli/test_calibrate.py create mode 100644 dimos/teleop/openarm_mini/cli/test_cli.py create mode 100644 dimos/teleop/openarm_mini/cli/test_joint_tui.py create mode 100644 dimos/teleop/openarm_mini/cli/test_setup_motor_id.py create mode 100644 dimos/teleop/openarm_mini/feetech.py create mode 100644 dimos/teleop/openarm_mini/mapping.py create mode 100644 dimos/teleop/openarm_mini/teleop_module.py create mode 100644 dimos/teleop/openarm_mini/test_calibration.py create mode 100644 dimos/teleop/openarm_mini/test_feetech.py create mode 100644 dimos/teleop/openarm_mini/test_mapping.py create mode 100644 dimos/teleop/openarm_mini/test_teleop_module.py 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/pyproject.toml b/pyproject.toml index d449168da4..8b2482b347 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -298,6 +298,11 @@ manipulation = [ "roboplan>=0.5.1", ] +openarm-mini-teleop = [ + "ftservo-python-sdk", + "rich", +] + cpu = [ # CPU inference backends "onnxruntime", diff --git a/uv.lock b/uv.lock index dbd98ec0e6..96b20cb994 100644 --- a/uv.lock +++ b/uv.lock @@ -1772,6 +1772,10 @@ misc = [ { name = "torchreid" }, { name = "xarm-python-sdk" }, ] +openarm-mini-teleop = [ + { name = "ftservo-python-sdk" }, + { name = "rich" }, +] perception = [ { name = "chromadb" }, { name = "einops" }, @@ -2082,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" }, @@ -2145,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" }, @@ -2179,7 +2185,7 @@ 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" }] @@ -3022,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" @@ -7292,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" From 5dc4f30b279428fa2605ed9c71433b42a57d0bfb Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Thu, 6 Aug 2026 15:52:54 -0700 Subject: [PATCH 43/44] feat(openarm): add OpenArm Mini teleop blueprints for the bimanual follower mini-teleop-openarm drives both arms from two leader ports; the left and right variants drive one side while the other arm holds. Leader joint N maps to follower joint N with no wrist reordering, matching the OpenArm 2.0 conventions on both ends. --- dimos/robot/all_blueprints.py | 4 + .../openarm/blueprints/mini_teleop.py | 74 +++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 dimos/robot/manipulators/openarm/blueprints/mini_teleop.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 7e583cb719..12f947d0a4 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -84,6 +84,9 @@ "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", + "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", @@ -241,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/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"}, + ), +) From 3a6639ed3451d3267f0b601595f89d590d5a7f11 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Fri, 7 Aug 2026 16:32:36 -0700 Subject: [PATCH 44/44] chore(openarm): hardware bring-up configuration for leader teleop Soft arm gains and grippers removed from the topology, the exact configuration validated on both real arms during the leader teleop ladder. Drop or promote this commit before opening the PR. --- .../whole_body/openarm_damiao/adapter.py | 23 ++++--------------- dimos/robot/manipulators/openarm/config.py | 17 ++++++++++---- 2 files changed, 17 insertions(+), 23 deletions(-) diff --git a/dimos/hardware/whole_body/openarm_damiao/adapter.py b/dimos/hardware/whole_body/openarm_damiao/adapter.py index 4b64ae287a..d6029ec09b 100644 --- a/dimos/hardware/whole_body/openarm_damiao/adapter.py +++ b/dimos/hardware/whole_body/openarm_damiao/adapter.py @@ -53,10 +53,11 @@ class OpenArmDamiaoAdapter(DamiaoWholeBodyAdapter): "left_arm": tuple(f"left_arm/joint{index}" for index in range(1, 8)), "right_arm": tuple(f"right_arm/joint{index}" for index in range(1, 8)), } - gripper_joints = { - "left_gripper": "left_arm/gripper", - "right_gripper": "right_arm/gripper", - } + # 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"} @@ -85,19 +86,5 @@ def _build_robot(self) -> can_motor_control.Robot: ) .add_arm("left_arm", bus="left", motors=_arm_motors("left")) .add_arm("right_arm", bus="right", motors=_arm_motors("right")) - .add_gripper( - "left_gripper", - bus="left", - motor=_gripper_motor("left"), - opening_direction="decreasing_position", - default_current=0.15, - ) - .add_gripper( - "right_gripper", - bus="right", - motor=_gripper_motor("right"), - opening_direction="decreasing_position", - default_current=0.15, - ) .build() ) diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index cdfbef7a35..9cbfc9fa6d 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -36,7 +36,10 @@ OPENARM_RIGHT_ARM_JOINTS = [f"right_arm/joint{i}" for i in range(1, OPENARM_DOF + 1)] OPENARM_ARM_JOINTS = [*OPENARM_LEFT_ARM_JOINTS, *OPENARM_RIGHT_ARM_JOINTS] OPENARM_GRIPPER_JOINTS = ["left_arm/gripper", "right_arm/gripper"] -OPENARM_JOINTS = [*OPENARM_ARM_JOINTS, *OPENARM_GRIPPER_JOINTS] +# 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_v20_left.urdf" @@ -48,8 +51,10 @@ # point: with gravity compensation active the PD terms only handle transient # tracking, and high kd excites gearbox buzz. Gripper slots bypass MIT # control, so their gains are 0. -_ARM_KP = (100.0, 100.0, 80.0, 80.0, 60.0, 60.0, 60.0) -_ARM_KD = (1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.8) +# 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: @@ -80,9 +85,11 @@ def openarm_hardware() -> HardwareComponent: adapter_type=adapter_type, 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, 0.0, 0.0), - kd=(*_ARM_KD, *_ARM_KD, 0.0, 0.0), + kp=(*_ARM_KP, *_ARM_KP), + kd=(*_ARM_KD, *_ARM_KD), ), )