From fe7a7de5874ffbc4ba290e97aae6a863205dd4f0 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 4 Jun 2026 13:41:32 -0700 Subject: [PATCH 01/20] spec: openspec init --- .gitignore | 2 + 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 ++ 11 files changed, 396 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/.gitignore b/.gitignore index 42bdddfa45..787163e787 100644 --- a/.gitignore +++ b/.gitignore @@ -63,8 +63,10 @@ yolo11n.pt # symlink one of .envrc.* if you'd like to use .envrc .claude +.opencode/ **/CLAUDE.md .direnv/ +.omo/ /logs diff --git a/docs/coding-agents/index.md b/docs/coding-agents/index.md index ff778ac5cf..5ac7c854a7 100644 --- a/docs/coding-agents/index.md +++ b/docs/coding-agents/index.md @@ -3,6 +3,7 @@ ├── worktrees.md (creating provisioned worktrees with `bin/worktree`) ├── style.md (code style guidelines for dimos) ├── 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 58da2ff6a1..f0064c9ab9 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -144,6 +144,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 76158b261a9a4c3f0509bc7a218db7cfe1010e44 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 8 Jun 2026 16:20:39 -0700 Subject: [PATCH 02/20] 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 5ac7c854a7..ff778ac5cf 100644 --- a/docs/coding-agents/index.md +++ b/docs/coding-agents/index.md @@ -3,7 +3,6 @@ ├── worktrees.md (creating provisioned worktrees with `bin/worktree`) ├── style.md (code style guidelines for dimos) ├── 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 f0064c9ab9..58da2ff6a1 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -144,7 +144,6 @@ "group": "Development", "pages": [ "development/conventions", - "development/openspec", "development/testing", "development/docker", "development/grid_testing", From ba8ad05fed69458e845d4e8f2c74522d2d7703a4 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 17 Jun 2026 15:11:09 -0700 Subject: [PATCH 03/20] feat(manipulation): add VAMP planner backend --- CONTEXT.md | 109 ++++++ data/.lfs/franka_description.tar.gz | 3 + dimos/manipulation/manipulation_module.py | 86 +++-- dimos/manipulation/planning/factory.py | 87 ++++- .../planning/monitor/world_monitor.py | 7 +- .../manipulation/planning/planners/config.py | 65 ++++ .../planning/planners/vamp_planner.py | 169 +++++++++ dimos/manipulation/planning/vamp/__init__.py | 22 ++ dimos/manipulation/planning/vamp/errors.py | 32 ++ dimos/manipulation/planning/vamp/loader.py | 82 +++++ .../planning/vamp/test_vamp_backend.py | 324 ++++++++++++++++++ dimos/manipulation/planning/world/config.py | 83 +++++ .../manipulation/planning/world/vamp_world.py | 283 +++++++++++++++ dimos/manipulation/test_manipulation_unit.py | 101 +++++- dimos/robot/all_blueprints.py | 1 + dimos/robot/catalog/franka.py | 85 +++++ dimos/robot/catalog/test_franka.py | 100 ++++++ dimos/robot/config.py | 6 + dimos/robot/manipulators/franka/blueprints.py | 58 ++++ .../manipulators/franka/test_blueprints.py | 52 +++ docs/usage/README.md | 1 + docs/usage/manipulation_planning.md | 124 +++++++ .../add-vamp-planning-backend/.openspec.yaml | 2 + .../add-vamp-planning-backend/design.md | 229 +++++++++++++ .../changes/add-vamp-planning-backend/docs.md | 53 +++ .../add-vamp-planning-backend/proposal.md | 61 ++++ .../specs/manipulation-stack/spec.md | 92 +++++ .../specs/vamp-planning-backend/spec.md | 128 +++++++ .../add-vamp-planning-backend/tasks.md | 75 ++++ pyproject.toml | 6 + uv.lock | 15 +- 31 files changed, 2496 insertions(+), 45 deletions(-) create mode 100644 CONTEXT.md create mode 100644 data/.lfs/franka_description.tar.gz create mode 100644 dimos/manipulation/planning/planners/config.py create mode 100644 dimos/manipulation/planning/planners/vamp_planner.py create mode 100644 dimos/manipulation/planning/vamp/__init__.py create mode 100644 dimos/manipulation/planning/vamp/errors.py create mode 100644 dimos/manipulation/planning/vamp/loader.py create mode 100644 dimos/manipulation/planning/vamp/test_vamp_backend.py create mode 100644 dimos/manipulation/planning/world/config.py create mode 100644 dimos/manipulation/planning/world/vamp_world.py create mode 100644 dimos/robot/catalog/franka.py create mode 100644 dimos/robot/catalog/test_franka.py create mode 100644 dimos/robot/manipulators/franka/blueprints.py create mode 100644 dimos/robot/manipulators/franka/test_blueprints.py create mode 100644 docs/usage/manipulation_planning.md create mode 100644 openspec/changes/add-vamp-planning-backend/.openspec.yaml create mode 100644 openspec/changes/add-vamp-planning-backend/design.md create mode 100644 openspec/changes/add-vamp-planning-backend/docs.md create mode 100644 openspec/changes/add-vamp-planning-backend/proposal.md create mode 100644 openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md create mode 100644 openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md create mode 100644 openspec/changes/add-vamp-planning-backend/tasks.md diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..3b14dd690c --- /dev/null +++ b/CONTEXT.md @@ -0,0 +1,109 @@ +# DimOS Planning + +DimOS planning describes how robot motion-planning backends are represented, selected, and integrated into the manipulation framework. + +## Language + +**VAMP backend**: +An optional full planning backend in which VAMP owns the robot and environment representation used for planning. +_Avoid_: VAMP planner-only plugin, universal VAMP planner + +**VAMP robot artifact**: +A prepared robot-specific VAMP bundle generated from robot description resources and compiled into an importable VAMP robot module. +_Avoid_: raw URDF, runtime robot model + +**Artifact preparation**: +The offline process that turns URDF-derived robot resources into VAMP robot artifacts before planning runtime. +_Avoid_: runtime generation, dynamic URDF planning + +**VAMP artifact recipe**: +A reproducible DimOS-owned description of how to generate a VAMP robot artifact from robot resources. +_Avoid_: generated artifact source, manual VAMP fork patch + +**VAMP optional dependency**: +The pip-installable VAMP package used by DimOS only when the VAMP backend is selected. +_Avoid_: required manipulation dependency, vendored VAMP source + +**Pinned VAMP dependency**: +A commit-pinned VAMP optional dependency used while the backend integration depends on unreleased packaging or robot artifact support. +_Avoid_: floating Git dependency, permanent fork dependency + +**VAMP planner algorithm**: +The algorithm selected inside the VAMP backend after DimOS has selected the VAMP planner adapter. +_Avoid_: separate global planner name, backend name + +**World backend**: +The selected planning-world implementation that owns robot registration, obstacle representation, collision checks, and synchronization for manipulation planning. +_Avoid_: hidden planner world, implicit backend + +**Backend-agnostic pose planning**: +Pose planning in which DimOS converts a target pose into a goal joint state through a backend-agnostic IK solver before invoking the selected planner backend. +_Avoid_: VAMP-specific pose planner, backend-specific pose conversion + +**Planner-native VAMP backend**: +A VAMP backend that exposes DimOS functions only where they are supported by VAMP's native planning and robot APIs, such as joint-space planning, path validation, collision checking, and forward/end-effector kinematics. +_Avoid_: synthetic Jacobian support, pretending unsupported planner capabilities exist + +**VAMP kinematics boundary**: +The separation between VAMP-owned joint-space planning/world validity and DimOS-owned pose-to-joint kinematics. VAMP should not expose IK or Jacobian behavior unless VAMP or a compatible kinematics component naturally supports it. +_Avoid_: planner-owned IK, manufactured VAMP kinematics + +**VAMP pose planning availability**: +Pose planning with VAMP is available only when DimOS has an explicitly compatible kinematics component for the VAMP world surface. It is not implied by selecting the VAMP planner. +_Avoid_: implicit VAMP pose support, fake Jacobian fallback + +**Initial VAMP capability set**: +The first coherent VAMP integration surface: joint-space planning, VAMP algorithm selection, native path simplification/validation, official or user-prepared artifact loading, environment conversion, joint-configuration validity, joint limits when available, and native FK/end-effector pose queries. +_Avoid_: all-interface parity, synthetic unsupported methods + +**User-prepared VAMP artifact**: +A robot-specific VAMP artifact generated outside DimOS by the user or by an upstream VAMP distribution, then loaded by DimOS at runtime. +_Avoid_: DimOS-owned artifact generation pipeline, automatic arbitrary-robot compilation + +**VAMP artifact loading**: +The runtime mechanism by which DimOS selects either an official VAMP robot artifact or a user-specified custom artifact path for planning. +_Avoid_: dynamic artifact generation, implicit robot artifact synthesis + +**Custom VAMP artifact path**: +A local world configuration value that points DimOS to a user-prepared VAMP artifact for robots not covered by official VAMP artifacts. +_Avoid_: hidden local artifact cache, hardcoded custom robot module + +**Local world configuration**: +Backend-specific planning-world configuration attached to the world/robot planning context rather than to global `ManipulationModule` settings. VAMP artifact selection belongs here because it is part of how the local VAMP world is loaded. +_Avoid_: global VAMP artifact settings, module-wide custom artifact path + +**Typed backend configuration**: +A discriminated configuration object that selects a backend with a `backend` field and carries backend-specific options in the same local config object. +_Avoid_: unrelated flat module-level backend fields, untyped string-only configuration + +**Noisy config deprecation**: +A temporary compatibility path for legacy configuration that emits a visible deprecation warning when used, so future maintainers know the behavior is scheduled for removal. +_Avoid_: silent compatibility shim, permanent legacy alias + +**Nested-only VAMP settings**: +New VAMP-specific settings that live only inside typed nested world/planner backend configuration. VAMP should not introduce new flat `ManipulationModuleConfig` fields; only pre-existing flat fields may receive temporary warning-backed compatibility paths during migration. +_Avoid_: `vamp_*` module fields, silent legacy aliases + +**VAMP world/planner config split**: +The separation between VAMP world configuration, which owns artifact loading and world representation, and VAMP planner configuration, which owns algorithm selection and planner behavior. +_Avoid_: putting artifact paths in planner config, putting algorithm tuning in world config + +**Strict VAMP world/planner pairing**: +The validation rule that VAMP world configuration must be paired with VAMP planner configuration, because VAMP planning depends on VAMP-native robot artifacts and environment representation. +_Avoid_: VAMP planner over Drake world, generic planner over VAMP world + +**VAMP kinematics compatibility validation**: +The rule that VAMP pose planning is enabled only when the configured kinematics backend can operate on the VAMP world surface. Joint-space VAMP planning does not imply IK or Jacobian support. +_Avoid_: implicit VAMP pose planning, planner-owned Jacobian checks + +**Unsupported world capability**: +A clear failure mode for optional world operations that a backend does not natively support, such as a VAMP world rejecting Jacobian queries instead of manufacturing synthetic Jacobian behavior. +_Avoid_: fake interface completeness, planner-owned capability probing + +**Franka Panda mock support**: +A DimOS catalog/control-coordinator configuration for the Franka Panda arm that defaults to mock control while providing robot model metadata for manipulation planning tests and planner benchmarks. +_Avoid_: real hardware requirement, VAMP-only test robot + +**LFS-backed robot description**: +A robot model package stored through DimOS' existing `data/.lfs/*.tar.gz` asset pattern and referenced in code through `LfsPath`. +_Avoid_: runtime URDF download, generated robot description at import time diff --git a/data/.lfs/franka_description.tar.gz b/data/.lfs/franka_description.tar.gz new file mode 100644 index 0000000000..67083e58b5 --- /dev/null +++ b/data/.lfs/franka_description.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:105a3404b7795dfc4a0eae879adcb125a8491e99cae81c56a92be5d7bac9ae72 +size 3892483 diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 5ec58a2b74..3c01048603 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -27,10 +27,11 @@ from enum import Enum import threading import time -from typing import TYPE_CHECKING, Any, TypeAlias +from typing import TYPE_CHECKING, Any, Self, TypeAlias +import warnings import numpy as np -from pydantic import Field +from pydantic import Field, model_validator from dimos.agents.annotation import skill from dimos.agents.skill_result import SkillResult @@ -38,13 +39,22 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.manipulation.planning.factory import create_kinematics, create_planner +from dimos.manipulation.planning.factory import ( + create_kinematics, + create_planner, + validate_planning_stack_config, +) from dimos.manipulation.planning.kinematics.config import ( JacobianKinematicsConfig, ManipulationKinematicsConfig, kinematics_config_from_name, ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor +from dimos.manipulation.planning.planners.config import ( + ManipulationPlannerConfig, + RRTConnectPlannerConfig, + planner_config_from_name, +) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType from dimos.manipulation.planning.spec.models import ( @@ -58,8 +68,11 @@ from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( JointTrajectoryGenerator, ) +from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError +from dimos.manipulation.planning.world.config import DrakeWorldConfig, ManipulationWorldConfig from dimos.manipulation.skill_errors import ManipulationSkillError from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState @@ -101,7 +114,10 @@ class ManipulationModuleConfig(ModuleConfig): robots: list[RobotModelConfig] = Field(default_factory=list) planning_timeout: float = 10.0 enable_viz: bool = False - planner_name: str = "rrt_connect" # "rrt_connect" + world: ManipulationWorldConfig = Field(default_factory=DrakeWorldConfig) + planner: ManipulationPlannerConfig = Field(default_factory=RRTConnectPlannerConfig) + # Deprecated: use planner.backend instead. + planner_name: str | None = None kinematics: ManipulationKinematicsConfig = Field(default_factory=JacobianKinematicsConfig) # Deprecated: use kinematics.backend instead. kinematics_name: str | None = None # "jacobian", "drake_optimization", or "pink" @@ -110,6 +126,27 @@ class ManipulationModuleConfig(ModuleConfig): # Set to None to disable. floor_z: float | None = None + @model_validator(mode="after") + def apply_legacy_flat_backend_fields(self) -> Self: + """Apply deprecated flat backend fields as noisy compatibility shims.""" + if self.planner_name is not None: + warnings.warn( + "ManipulationModuleConfig.planner_name is deprecated; use " + "planner={'backend': ...} instead.", + DeprecationWarning, + stacklevel=3, + ) + self.planner = planner_config_from_name(self.planner_name) + if self.kinematics_name is not None: + warnings.warn( + "ManipulationModuleConfig.kinematics_name is deprecated; use " + "kinematics={'backend': ...} instead.", + DeprecationWarning, + stacklevel=3, + ) + self.kinematics = kinematics_config_from_name(self.kinematics_name) + return self + class ManipulationModule(Module): """Base motion planning module with ControlCoordinator execution. @@ -178,7 +215,16 @@ def _initialize_planning(self) -> None: logger.warning("No robots configured, planning disabled") return - self._world_monitor = WorldMonitor(enable_viz=self.config.enable_viz) + validate_planning_stack_config( + world=self.config.world, + planner=self.config.planner, + kinematics=self.config.kinematics, + ) + + self._world_monitor = WorldMonitor( + config=self.config.world, + enable_viz=self.config.enable_viz, + ) for robot_config in self.config.robots: robot_id = self._world_monitor.add_robot(robot_config) @@ -216,11 +262,8 @@ def _initialize_planning(self) -> None: if url := self._world_monitor.get_visualization_url(): logger.info(f"Visualization: {url}") - self._planner = create_planner(name=self.config.planner_name) - kinematics_config = self.config.kinematics - if self.config.kinematics_name is not None: - kinematics_config = kinematics_config_from_name(self.config.kinematics_name) - self._kinematics = create_kinematics(config=kinematics_config) + self._planner = create_planner(config=self.config.planner) + self._kinematics = create_kinematics(config=self.config.kinematics) # Start TF publishing thread if any robot has tf_extra_links if any(c.tf_extra_links for _, c, _ in self._robots.values()): @@ -470,22 +513,22 @@ def _solve_ik_for_pose( """Run the configured kinematics backend for a world-frame pose.""" assert self._world_monitor and self._kinematics - # Convert Pose to PoseStamped for the IK solver - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - target_pose = PoseStamped( frame_id="world", position=pose.position, orientation=pose.orientation, ) - return self._kinematics.solve( - world=self._world_monitor.world, - robot_id=robot_id, - target_pose=target_pose, - seed=seed, - check_collision=check_collision, - ) + try: + return self._kinematics.solve( + world=self._world_monitor.world, + robot_id=robot_id, + target_pose=target_pose, + seed=seed, + check_collision=check_collision, + ) + except UnsupportedWorldCapabilityError as exc: + return IKResult(status=IKStatus.NO_SOLUTION, message=str(exc)) @rpc def solve_ik( @@ -548,7 +591,8 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=True) if not ik.is_success() or ik.joint_state is None: - return self._fail(f"IK failed: {ik.status.name}") + detail = f": {ik.message}" if ik.message else "" + return self._fail(f"IK failed: {ik.status.name}{detail}") logger.info(f"IK solved, error: {ik.position_error:.4f}m") return self._plan_path_only(robot_name, robot_id, ik.joint_state) diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index 915feef8b1..8dbc092657 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -25,6 +25,18 @@ PinkKinematicsConfig, kinematics_config_from_name, ) +from dimos.manipulation.planning.planners.config import ( + ManipulationPlannerConfig, + RRTConnectPlannerConfig, + VampPlannerConfig, + planner_config_from_name, +) +from dimos.manipulation.planning.world.config import ( + DrakeWorldConfig, + ManipulationWorldConfig, + VampWorldConfig, + world_config_from_name, +) if TYPE_CHECKING: from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec, WorldSpec @@ -32,16 +44,23 @@ def create_world( backend: str = "drake", + config: ManipulationWorldConfig | None = None, enable_viz: bool = False, **kwargs: Any, ) -> WorldSpec: - """Create a world instance. backend='drake', enable_viz for Meshcat.""" - if backend == "drake": + """Create a world instance from a backend name or typed world config.""" + if config is None: + config = world_config_from_name(backend) + + if isinstance(config, DrakeWorldConfig): from dimos.manipulation.planning.world.drake_world import DrakeWorld return DrakeWorld(enable_viz=enable_viz, **kwargs) - else: - raise ValueError(f"Unknown backend: {backend}. Available: ['drake']") + if isinstance(config, VampWorldConfig): + from dimos.manipulation.planning.world.vamp_world import VampWorld + + return VampWorld(config=config, **kwargs) + raise TypeError(f"Unsupported world config: {type(config).__name__}") def create_kinematics( @@ -73,30 +92,68 @@ def create_kinematics( def create_planner( name: str = "rrt_connect", + config: ManipulationPlannerConfig | None = None, **kwargs: Any, ) -> PlannerSpec: - """Create motion planner. name='rrt_connect'.""" - if name == "rrt_connect": + """Create motion planner from a backend name or typed planner config.""" + if config is None: + config = planner_config_from_name(name) + + if isinstance(config, RRTConnectPlannerConfig): from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner - return RRTConnectPlanner(**kwargs) - else: - raise ValueError(f"Unknown planner: {name}. Available: ['rrt_connect']") + return RRTConnectPlanner( + step_size=config.step_size, + connect_step_size=config.connect_step_size, + goal_tolerance=config.goal_tolerance, + collision_step_size=config.collision_step_size, + **kwargs, + ) + if isinstance(config, VampPlannerConfig): + from dimos.manipulation.planning.planners.vamp_planner import VampPlanner + + return VampPlanner(config=config, **kwargs) + raise TypeError(f"Unsupported planner config: {type(config).__name__}") + + +def validate_planning_stack_config( + world: ManipulationWorldConfig, + planner: ManipulationPlannerConfig, + kinematics: ManipulationKinematicsConfig, +) -> None: + """Validate that selected world, planner, and kinematics backends can pair.""" + if isinstance(planner, VampPlannerConfig) and not isinstance(world, VampWorldConfig): + raise ValueError("VAMP planner requires world backend 'vamp'") + if isinstance(world, VampWorldConfig) and not isinstance(planner, VampPlannerConfig): + raise ValueError("VAMP world backend requires planner backend 'vamp'") + if isinstance(kinematics, DrakeOptimizationKinematicsConfig) and not isinstance( + world, DrakeWorldConfig + ): + raise ValueError("Drake optimization kinematics requires world backend 'drake'") def create_planning_stack( robot_config: Any, enable_viz: bool = False, + world: ManipulationWorldConfig | None = None, planner_name: str = "rrt_connect", + planner: ManipulationPlannerConfig | None = None, kinematics_name: str = "jacobian", kinematics: ManipulationKinematicsConfig | None = None, ) -> tuple[WorldSpec, KinematicsSpec, PlannerSpec, str]: """Create complete planning stack. Returns (world, kinematics, planner, robot_id).""" - world = create_world(backend="drake", enable_viz=enable_viz) - kinematics_solver = create_kinematics(name=kinematics_name, config=kinematics) - planner = create_planner(name=planner_name) + world_config = world if world is not None else DrakeWorldConfig() + planner_config = planner if planner is not None else planner_config_from_name(planner_name) + kinematics_config = ( + kinematics if kinematics is not None else kinematics_config_from_name(kinematics_name) + ) + validate_planning_stack_config(world_config, planner_config, kinematics_config) + + world_backend = create_world(config=world_config, enable_viz=enable_viz) + kinematics_solver = create_kinematics(name=kinematics_name, config=kinematics_config) + planner_backend = create_planner(name=planner_name, config=planner_config) - robot_id = world.add_robot(robot_config) - world.finalize() + robot_id = world_backend.add_robot(robot_config) + world_backend.finalize() - return world, kinematics_solver, planner, robot_id + return world_backend, kinematics_solver, planner_backend, robot_id diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 0bcb032b3a..c8a1ee20fc 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -25,6 +25,7 @@ from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor from dimos.manipulation.planning.spec.protocols import VisualizationSpec +from dimos.manipulation.planning.world.config import ManipulationWorldConfig, world_config_from_name from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -55,11 +56,13 @@ class WorldMonitor: def __init__( self, backend: str = "drake", + config: ManipulationWorldConfig | None = None, enable_viz: bool = False, **kwargs: Any, ) -> None: - self._backend = backend - self._world: WorldSpec = create_world(backend=backend, enable_viz=enable_viz, **kwargs) + world_config = config if config is not None else world_config_from_name(backend) + self._backend = world_config.backend + self._world: WorldSpec = create_world(config=world_config, enable_viz=enable_viz, **kwargs) self._visualization: VisualizationSpec | None = ( self._world if isinstance(self._world, VisualizationSpec) else None ) diff --git a/dimos/manipulation/planning/planners/config.py b/dimos/manipulation/planning/planners/config.py new file mode 100644 index 0000000000..915509e0ea --- /dev/null +++ b/dimos/manipulation/planning/planners/config.py @@ -0,0 +1,65 @@ +# 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. + +"""Configuration models for manipulation planner backends.""" + +from __future__ import annotations + +from typing import Annotated, Literal + +from pydantic import Field + +from dimos.protocol.service.spec import BaseConfig + + +class RRTConnectPlannerConfig(BaseConfig): + """Configuration for the backend-agnostic RRT-Connect planner.""" + + backend: Literal["rrt_connect"] = "rrt_connect" + step_size: float = 0.1 + connect_step_size: float = 0.05 + goal_tolerance: float = 0.1 + collision_step_size: float = 0.02 + + +class VampPlannerConfig(BaseConfig): + """Configuration for the VAMP-native joint-space planner adapter.""" + + backend: Literal["vamp"] = "vamp" + algorithm: Literal["rrtc", "prm", "fcit", "aorrtc"] = "rrtc" + simplify: bool = True + validate_path: bool = True + + +ManipulationPlannerConfig = Annotated[ + RRTConnectPlannerConfig | VampPlannerConfig, + Field(discriminator="backend"), +] + + +def planner_config_from_name(name: str) -> ManipulationPlannerConfig: + """Create a default planner config from a legacy planner name.""" + if name == "rrt_connect": + return RRTConnectPlannerConfig() + if name == "vamp": + return VampPlannerConfig() + raise ValueError(f"Unknown planner: {name}. Available: ['rrt_connect', 'vamp']") + + +__all__ = [ + "ManipulationPlannerConfig", + "RRTConnectPlannerConfig", + "VampPlannerConfig", + "planner_config_from_name", +] diff --git a/dimos/manipulation/planning/planners/vamp_planner.py b/dimos/manipulation/planning/planners/vamp_planner.py new file mode 100644 index 0000000000..14f9124de8 --- /dev/null +++ b/dimos/manipulation/planning/planners/vamp_planner.py @@ -0,0 +1,169 @@ +# 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. + +"""VAMP-native joint-space planner adapter.""" + +from __future__ import annotations + +from itertools import pairwise +import time +from typing import Any + +import numpy as np + +from dimos.manipulation.planning.planners.config import VampPlannerConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult, WorldRobotID +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.world.vamp_world import VampWorld +from dimos.msgs.sensor_msgs.JointState import JointState + + +class VampPlanner: + """Joint-space planner adapter for VAMP robot modules.""" + + def __init__(self, config: VampPlannerConfig) -> None: + self.config = config + + def plan_joint_path( + self, + world: WorldSpec, + robot_id: WorldRobotID, + start: JointState, + goal: JointState, + timeout: float = 10.0, + ) -> PlanningResult: + """Plan a VAMP-native joint-space path.""" + start_time = time.time() + if not isinstance(world, VampWorld): + raise ValueError("VampPlanner requires VampWorld") + if not world.is_finalized: + return _failure(PlanningStatus.NO_SOLUTION, "World must be finalized before planning") + if robot_id not in world.get_robot_ids(): + return _failure(PlanningStatus.NO_SOLUTION, f"Robot '{robot_id}' not found") + + if not world.check_config_collision_free(robot_id, start): + return _failure(PlanningStatus.COLLISION_AT_START, "Start configuration is invalid") + if not world.check_config_collision_free(robot_id, goal): + return _failure(PlanningStatus.COLLISION_AT_GOAL, "Goal configuration is invalid") + + robot_name = _robot_name(world) + robot_module, planner_func, plan_settings, simplify_settings = ( + world.vamp_module.configure_robot_and_planner_with_kwargs( + robot_name, + self.config.algorithm, + max_iterations=_timeout_to_iteration_budget(timeout), + ) + ) + sampler = robot_module.halton() + result = planner_func( + list(start.position), + list(goal.position), + world.environment, + plan_settings, + sampler, + ) + if not bool(getattr(result, "solved", False)): + return _failure( + PlanningStatus.NO_SOLUTION, + "VAMP planner did not find a path", + planning_time=time.time() - start_time, + iterations=int(getattr(result, "iterations", 0)), + ) + + path_source = result.path + if self.config.simplify: + simplified = robot_module.simplify( + path_source, world.environment, simplify_settings, sampler + ) + if bool(getattr(simplified, "solved", True)): + path_source = simplified.path + + path = _path_to_joint_states( + path_source, start.name or world.get_robot_config(robot_id).joint_names + ) + if self.config.validate_path and not _validate_path(world, robot_id, path): + return _failure( + PlanningStatus.NO_SOLUTION, + "VAMP returned a path that failed native validation", + planning_time=time.time() - start_time, + ) + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + planning_time=time.time() - start_time, + path_length=_path_length(path), + iterations=int(getattr(result, "iterations", 0)), + message="VAMP planning succeeded", + ) + + def get_name(self) -> str: + """Get planner name.""" + return f"VAMP/{self.config.algorithm}" + + +def _robot_name(world: VampWorld) -> str: + artifact = world.config.artifact + robot = getattr(artifact, "robot", None) + if isinstance(robot, str): + return robot + return world.robot_module.__name__.split(".")[-1] + + +def _timeout_to_iteration_budget(timeout: float) -> int: + return max(1, int(timeout * 1000)) + + +def _path_to_joint_states(path_source: Any, joint_names: list[str]) -> list[JointState]: + path_array = _path_to_array(path_source) + return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] + + +def _path_to_array(path_source: Any) -> np.ndarray: + if hasattr(path_source, "numpy"): + return np.asarray(path_source.numpy(), dtype=np.float64) + return np.asarray(path_source, dtype=np.float64) + + +def _validate_path(world: VampWorld, robot_id: WorldRobotID, path: list[JointState]) -> bool: + if not path: + return False + return all( + world.check_edge_collision_free(robot_id, before, after) for before, after in pairwise(path) + ) + + +def _path_length(path: list[JointState]) -> float: + if len(path) < 2: + return 0.0 + total = 0.0 + for before, after in pairwise(path): + q_before = np.array(before.position, dtype=np.float64) + q_after = np.array(after.position, dtype=np.float64) + total += float(np.linalg.norm(q_after - q_before)) + return total + + +def _failure( + status: PlanningStatus, + message: str, + planning_time: float = 0.0, + iterations: int = 0, +) -> PlanningResult: + return PlanningResult( + status=status, + planning_time=planning_time, + iterations=iterations, + message=message, + ) diff --git a/dimos/manipulation/planning/vamp/__init__.py b/dimos/manipulation/planning/vamp/__init__.py new file mode 100644 index 0000000000..b4b6858df7 --- /dev/null +++ b/dimos/manipulation/planning/vamp/__init__.py @@ -0,0 +1,22 @@ +# 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. + +"""Helpers for the optional VAMP manipulation planning backend.""" + +from dimos.manipulation.planning.vamp.errors import ( + UnsupportedWorldCapabilityError, + VampDependencyError, +) + +__all__ = ["UnsupportedWorldCapabilityError", "VampDependencyError"] diff --git a/dimos/manipulation/planning/vamp/errors.py b/dimos/manipulation/planning/vamp/errors.py new file mode 100644 index 0000000000..6070353d40 --- /dev/null +++ b/dimos/manipulation/planning/vamp/errors.py @@ -0,0 +1,32 @@ +# 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. + +"""Errors for the optional VAMP manipulation planning backend.""" + + +class VampDependencyError(ImportError): + """Raised when the VAMP backend is selected without the optional dependency.""" + + def __init__(self) -> None: + super().__init__( + "VAMP planning backend requires the optional 'vamp-planner' dependency. " + "Install the DimOS VAMP extra or install vamp-planner in this environment." + ) + + +class UnsupportedWorldCapabilityError(NotImplementedError): + """Raised when a world backend does not natively support a requested capability.""" + + def __init__(self, backend: str, capability: str) -> None: + super().__init__(f"World backend '{backend}' does not support capability: {capability}") diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py new file mode 100644 index 0000000000..1f933b86b4 --- /dev/null +++ b/dimos/manipulation/planning/vamp/loader.py @@ -0,0 +1,82 @@ +# 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. + +"""Lazy loaders for VAMP robot artifacts.""" + +from __future__ import annotations + +import importlib +import importlib.util +from pathlib import Path +import sys +from types import ModuleType + +from dimos.manipulation.planning.vamp.errors import VampDependencyError +from dimos.manipulation.planning.world.config import ( + CustomVampArtifactConfig, + OfficialVampArtifactConfig, + VampArtifactConfig, +) + + +def import_vamp() -> ModuleType: + """Import the optional VAMP package only when a VAMP backend is selected.""" + try: + return importlib.import_module("vamp") + except ImportError as exc: + raise VampDependencyError() from exc + + +def load_vamp_robot_module(artifact: VampArtifactConfig) -> tuple[ModuleType, ModuleType]: + """Load the VAMP package and configured robot module.""" + vamp_module = import_vamp() + if isinstance(artifact, OfficialVampArtifactConfig): + return vamp_module, _load_official_robot_module(vamp_module, artifact.robot) + if isinstance(artifact, CustomVampArtifactConfig): + return vamp_module, _load_custom_robot_module(artifact.path) + raise TypeError(f"Unsupported VAMP artifact config: {type(artifact).__name__}") + + +def _load_official_robot_module(vamp_module: ModuleType, robot: str) -> ModuleType: + robot_module = getattr(vamp_module, robot, None) + if isinstance(robot_module, ModuleType): + return robot_module + try: + imported = importlib.import_module(f"vamp.{robot}") + except ImportError as exc: + raise ValueError( + f"Installed VAMP package does not expose robot artifact '{robot}'" + ) from exc + return imported + + +def _load_custom_robot_module(path: Path) -> ModuleType: + artifact_path = path.expanduser().resolve() + if not artifact_path.exists(): + raise FileNotFoundError(f"VAMP custom artifact path does not exist: {artifact_path}") + + if artifact_path.is_dir(): + parent = str(artifact_path.parent) + if parent not in sys.path: + sys.path.insert(0, parent) + return importlib.import_module(artifact_path.name) + + module_name = artifact_path.stem + spec = importlib.util.spec_from_file_location(module_name, artifact_path) + if spec is None or spec.loader is None: + raise ImportError(f"Could not load VAMP custom artifact module: {artifact_path}") + module = importlib.util.module_from_spec(spec) + sys.modules[module_name] = module + spec.loader.exec_module(module) + return module diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py new file mode 100644 index 0000000000..23b2801a83 --- /dev/null +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -0,0 +1,324 @@ +# 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. + +"""Tests for the optional VAMP planning backend adapters.""" + +from __future__ import annotations + +from pathlib import Path +import sys +from types import ModuleType, SimpleNamespace + +import numpy as np +from pydantic import ValidationError +import pytest + +from dimos.manipulation.planning.factory import create_planner, create_world +from dimos.manipulation.planning.planners.config import RRTConnectPlannerConfig, VampPlannerConfig +from dimos.manipulation.planning.planners.vamp_planner import VampPlanner +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.vamp.errors import ( + UnsupportedWorldCapabilityError, + VampDependencyError, +) +from dimos.manipulation.planning.vamp.loader import import_vamp, load_vamp_robot_module +from dimos.manipulation.planning.world.config import ( + CustomVampArtifactConfig, + OfficialVampArtifactConfig, + VampWorldConfig, +) +from dimos.manipulation.planning.world.vamp_world import VampWorld +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.JointState import JointState + + +class FakeEnvironment: + """Small fake VAMP environment that records converted primitives.""" + + def __init__(self) -> None: + self.spheres: list[object] = [] + self.cuboids: list[object] = [] + self.capsules: list[object] = [] + + def add_sphere(self, sphere: object) -> None: + self.spheres.append(sphere) + + def add_cuboid(self, cuboid: object) -> None: + self.cuboids.append(cuboid) + + def add_capsule(self, capsule: object) -> None: + self.capsules.append(capsule) + + +class FakePath: + """Fake VAMP path object exposing the numpy() method used by bindings.""" + + def __init__(self, waypoints: list[list[float]]) -> None: + self._waypoints = np.array(waypoints, dtype=np.float64) + + def numpy(self) -> np.ndarray: + return self._waypoints + + +class FakePlanningResult: + """Fake VAMP planning result.""" + + def __init__(self, solved: bool, path: FakePath, iterations: int = 7) -> None: + self.solved = solved + self.path = path + self.iterations = iterations + + +class FakeRobotModule(ModuleType): + """Fake official VAMP robot module.""" + + def __init__(self) -> None: + super().__init__("vamp.panda") + self.validate_calls: list[tuple[list[float], bool]] = [] + self.motion_calls: list[tuple[list[float], list[float], bool]] = [] + self.simplify_calls: list[FakePath] = [] + self.valid = True + self.motion_valid = True + + def halton(self) -> str: + return "fake_sampler" + + def validate( + self, configuration: list[float], environment: FakeEnvironment, check_bounds: bool + ) -> bool: + del environment + self.validate_calls.append((configuration, check_bounds)) + return self.valid + + def validate_motion( + self, + configuration_in: list[float], + configuration_out: list[float], + environment: FakeEnvironment, + check_bounds: bool, + ) -> bool: + del environment + self.motion_calls.append((configuration_in, configuration_out, check_bounds)) + return self.motion_valid + + def eefk(self, configuration: list[float]) -> np.ndarray: + transform = np.eye(4, dtype=np.float64) + transform[0, 3] = configuration[0] + transform[1, 3] = configuration[1] + transform[2, 3] = configuration[2] + return transform + + def simplify( + self, + path: FakePath, + environment: FakeEnvironment, + settings: SimpleNamespace, + sampler: str, + ) -> FakePlanningResult: + del environment, settings, sampler + self.simplify_calls.append(path) + return FakePlanningResult(True, FakePath([[0.0, 0.0, 0.0], [1.0, 0.5, 0.25]]), 2) + + +class FakeVampModule(ModuleType): + """Fake top-level VAMP package module.""" + + def __init__(self, robot_module: FakeRobotModule) -> None: + super().__init__("vamp") + self.panda = robot_module + self.Environment = FakeEnvironment + self.Sphere = SimpleNamespace + self.Cuboid = SimpleNamespace + self.Cylinder = SimpleNamespace + self.configure_calls: list[tuple[str, str, int]] = [] + self.planner_calls: list[tuple[list[float], list[float], str]] = [] + self.planner_solved = True + + def configure_robot_and_planner_with_kwargs( + self, robot_name: str, planner_name: str, max_iterations: int + ) -> tuple[FakeRobotModule, object, SimpleNamespace, SimpleNamespace]: + self.configure_calls.append((robot_name, planner_name, max_iterations)) + + def planner_func( + start: list[float], + goal: list[float], + environment: FakeEnvironment, + settings: SimpleNamespace, + sampler: str, + ) -> FakePlanningResult: + del environment, settings + self.planner_calls.append((start, goal, sampler)) + return FakePlanningResult( + self.planner_solved, + FakePath([start, [0.5, 0.25, 0.125], goal]), + 11, + ) + + return self.panda, planner_func, SimpleNamespace(), SimpleNamespace() + + +@pytest.fixture +def fake_vamp_modules(mocker) -> tuple[FakeVampModule, FakeRobotModule]: + """Install fake VAMP modules into sys.modules.""" + robot_module = FakeRobotModule() + vamp_module = FakeVampModule(robot_module) + mocker.patch.dict(sys.modules, {"vamp": vamp_module, "vamp.panda": robot_module}) + return vamp_module, robot_module + + +def robot_config() -> RobotModelConfig: + """Create a minimal robot model config for VAMP adapter tests.""" + return RobotModelConfig( + name="panda", + model_path=Path("/tmp/panda.urdf"), + base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + joint_names=["joint1", "joint2", "joint3"], + end_effector_link="panda_hand", + base_link="panda_link0", + joint_limits_lower=[-1.0, -1.0, -1.0], + joint_limits_upper=[1.0, 1.0, 1.0], + home_joints=[0.0, 0.0, 0.0], + ) + + +def finalized_vamp_world() -> VampWorld: + """Create a finalized fake-backed VAMP world.""" + world = VampWorld(VampWorldConfig()) + world.add_robot(robot_config()) + world.finalize() + return world + + +def test_vamp_dependency_error_has_install_guidance(mocker) -> None: + """Selecting VAMP without the optional package raises an actionable error.""" + mocker.patch( + "dimos.manipulation.planning.vamp.loader.importlib.import_module", + side_effect=ImportError("missing vamp"), + ) + + with pytest.raises(VampDependencyError, match="vamp-planner"): + import_vamp() + + +def test_non_vamp_planner_creation_does_not_import_vamp(mocker) -> None: + """Default planner creation stays independent of the optional VAMP package.""" + mocker.patch( + "dimos.manipulation.planning.vamp.loader.import_vamp", + side_effect=AssertionError("VAMP import should stay lazy"), + ) + + planner = create_planner(config=RRTConnectPlannerConfig()) + + assert planner.get_name() == "RRTConnect" + + +def test_vamp_config_rejects_invalid_algorithm() -> None: + """VAMP planner config validates the finite algorithm set.""" + with pytest.raises(ValidationError): + VampPlannerConfig(algorithm="invalid") # type: ignore[arg-type] + + +def test_official_vamp_artifact_loading_uses_installed_robot_module(fake_vamp_modules) -> None: + """Official artifact mode loads robot modules exposed by the VAMP package.""" + vamp_module, robot_module = fake_vamp_modules + + loaded_vamp, loaded_robot = load_vamp_robot_module(OfficialVampArtifactConfig(robot="panda")) + + assert loaded_vamp is vamp_module + assert loaded_robot is robot_module + + +def test_custom_vamp_artifact_loading_uses_explicit_module_path( + fake_vamp_modules, tmp_path +) -> None: + """Custom artifact mode imports a user-prepared local Python robot module.""" + vamp_module, _ = fake_vamp_modules + artifact_path = tmp_path / "custom_panda.py" + artifact_path.write_text("ROBOT_NAME = 'custom_panda'\n", encoding="utf-8") + + loaded_vamp, loaded_robot = load_vamp_robot_module(CustomVampArtifactConfig(path=artifact_path)) + + assert loaded_vamp is vamp_module + assert loaded_robot.ROBOT_NAME == "custom_panda" + + +def test_create_world_and_planner_from_vamp_configs(fake_vamp_modules) -> None: + """Factory functions create VAMP world and planner adapters from typed configs.""" + world = create_world(config=VampWorldConfig()) + planner = create_planner(config=VampPlannerConfig(algorithm="fcit")) + + assert isinstance(world, VampWorld) + assert isinstance(planner, VampPlanner) + assert planner.get_name() == "VAMP/fcit" + + +def test_vamp_world_validity_fk_and_unsupported_jacobian(fake_vamp_modules) -> None: + """VAMP world delegates native validity/FK and rejects unsupported Jacobian.""" + _, robot_module = fake_vamp_modules + world = finalized_vamp_world() + robot_id = world.get_robot_ids()[0] + state = JointState(name=["joint1", "joint2", "joint3"], position=[0.1, 0.2, 0.3]) + + assert world.check_config_collision_free(robot_id, state) + assert robot_module.validate_calls[-1] == ([0.1, 0.2, 0.3], True) + + with world.scratch_context() as ctx: + world.set_joint_state(ctx, robot_id, state) + ee_pose = world.get_ee_pose(ctx, robot_id) + assert ee_pose.x == pytest.approx(0.1) + assert ee_pose.y == pytest.approx(0.2) + assert ee_pose.z == pytest.approx(0.3) + with pytest.raises(UnsupportedWorldCapabilityError, match="Jacobian"): + world.get_jacobian(ctx, robot_id) + + +def test_vamp_planner_dispatches_algorithm_simplifies_and_validates(fake_vamp_modules) -> None: + """VAMP planner uses configured algorithm and VAMP-native path utilities.""" + vamp_module, robot_module = fake_vamp_modules + world = finalized_vamp_world() + robot_id = world.get_robot_ids()[0] + planner = VampPlanner(VampPlannerConfig(algorithm="prm", simplify=True, validate_path=True)) + start = JointState(name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0]) + goal = JointState(name=["joint1", "joint2", "joint3"], position=[1.0, 0.5, 0.25]) + + result = planner.plan_joint_path(world, robot_id, start, goal, timeout=0.25) + + assert result.status == PlanningStatus.SUCCESS + assert [point.position for point in result.path] == [[0.0, 0.0, 0.0], [1.0, 0.5, 0.25]] + assert vamp_module.configure_calls == [("panda", "prm", 250)] + assert vamp_module.planner_calls == [([0.0, 0.0, 0.0], [1.0, 0.5, 0.25], "fake_sampler")] + assert len(robot_module.simplify_calls) == 1 + assert robot_module.motion_calls == [([0.0, 0.0, 0.0], [1.0, 0.5, 0.25], True)] + + +def test_vamp_planner_reports_native_planning_failure(fake_vamp_modules) -> None: + """Unsolved VAMP results map to a failed DimOS planning result.""" + vamp_module, _ = fake_vamp_modules + vamp_module.planner_solved = False + world = finalized_vamp_world() + robot_id = world.get_robot_ids()[0] + planner = VampPlanner(VampPlannerConfig(simplify=False, validate_path=False)) + + result = planner.plan_joint_path( + world, + robot_id, + JointState(name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0]), + JointState(name=["joint1", "joint2", "joint3"], position=[1.0, 0.5, 0.25]), + ) + + assert result.status == PlanningStatus.NO_SOLUTION + assert "did not find a path" in result.message diff --git a/dimos/manipulation/planning/world/config.py b/dimos/manipulation/planning/world/config.py new file mode 100644 index 0000000000..2b341509f4 --- /dev/null +++ b/dimos/manipulation/planning/world/config.py @@ -0,0 +1,83 @@ +# 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. + +"""Configuration models for manipulation world backends.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Annotated, Literal + +from pydantic import Field + +from dimos.protocol.service.spec import BaseConfig + + +class DrakeWorldConfig(BaseConfig): + """Configuration for the default Drake world backend.""" + + backend: Literal["drake"] = "drake" + + +class OfficialVampArtifactConfig(BaseConfig): + """Load a robot artifact exposed by the installed VAMP package.""" + + mode: Literal["official"] = "official" + robot: str = "panda" + + +class CustomVampArtifactConfig(BaseConfig): + """Load a user-prepared VAMP robot artifact from an explicit local path.""" + + mode: Literal["custom"] = "custom" + path: Path + + +VampArtifactConfig = Annotated[ + OfficialVampArtifactConfig | CustomVampArtifactConfig, + Field(discriminator="mode"), +] + + +class VampWorldConfig(BaseConfig): + """Configuration for the VAMP-native world backend.""" + + backend: Literal["vamp"] = "vamp" + artifact: VampArtifactConfig = Field(default_factory=OfficialVampArtifactConfig) + + +ManipulationWorldConfig = Annotated[ + DrakeWorldConfig | VampWorldConfig, + Field(discriminator="backend"), +] + + +def world_config_from_name(name: str) -> ManipulationWorldConfig: + """Create a default world config from a backend name.""" + if name == "drake": + return DrakeWorldConfig() + if name == "vamp": + return VampWorldConfig() + raise ValueError(f"Unknown backend: {name}. Available: ['drake', 'vamp']") + + +__all__ = [ + "CustomVampArtifactConfig", + "DrakeWorldConfig", + "ManipulationWorldConfig", + "OfficialVampArtifactConfig", + "VampArtifactConfig", + "VampWorldConfig", + "world_config_from_name", +] diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py new file mode 100644 index 0000000000..cf3c071970 --- /dev/null +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -0,0 +1,283 @@ +# 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. + +"""VAMP-native WorldSpec implementation.""" + +from __future__ import annotations + +from contextlib import contextmanager +from copy import deepcopy +from dataclasses import dataclass +from types import ModuleType +from typing import TYPE_CHECKING, Any + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import ObstacleType +from dimos.manipulation.planning.spec.models import Obstacle, WorldRobotID +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError +from dimos.manipulation.planning.vamp.loader import load_vamp_robot_module +from dimos.manipulation.planning.world.config import VampWorldConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.utils.transform_utils import matrix_to_pose + +if TYPE_CHECKING: + from collections.abc import Generator + + from numpy.typing import NDArray + + +@dataclass +class _VampContext: + joint_states: dict[WorldRobotID, JointState] + + +class VampWorld(WorldSpec): + """World adapter for VAMP-native robot artifacts and validity checking.""" + + def __init__(self, config: VampWorldConfig) -> None: + self.config = config + self._vamp_module, self._robot_module = load_vamp_robot_module(config.artifact) + self._environment = self._vamp_module.Environment() + self._robots: dict[WorldRobotID, RobotModelConfig] = {} + self._live_joint_states: dict[WorldRobotID, JointState] = {} + self._obstacles: dict[str, Obstacle] = {} + self._robot_counter = 0 + self._finalized = False + + @property + def vamp_module(self) -> ModuleType: + """The imported VAMP package module.""" + return self._vamp_module + + @property + def robot_module(self) -> ModuleType: + """The loaded VAMP robot module for this world.""" + return self._robot_module + + @property + def environment(self) -> Any: + """The current VAMP environment object.""" + return self._environment + + def add_robot(self, config: RobotModelConfig) -> WorldRobotID: + """Add a robot to the VAMP world.""" + if self._finalized: + raise RuntimeError("Cannot add robot after world is finalized") + if self._robots: + raise ValueError("VAMP world currently supports one robot per world") + self._robot_counter += 1 + robot_id = f"robot_{self._robot_counter}" + self._robots[robot_id] = config + home_positions = config.home_joints or [0.0] * len(config.joint_names) + self._live_joint_states[robot_id] = JointState( + name=config.joint_names, + position=home_positions, + ) + return robot_id + + def get_robot_ids(self) -> list[WorldRobotID]: + """Get all robot IDs.""" + return list(self._robots) + + def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: + """Get robot configuration.""" + return self._robots[robot_id] + + def get_joint_limits( + self, robot_id: WorldRobotID + ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: + """Get joint limits from config or conservative defaults.""" + config = self._robots[robot_id] + if config.joint_limits_lower is not None and config.joint_limits_upper is not None: + return ( + np.array(config.joint_limits_lower, dtype=np.float64), + np.array(config.joint_limits_upper, dtype=np.float64), + ) + n_joints = len(config.joint_names) + return (np.full(n_joints, -np.pi), np.full(n_joints, np.pi)) + + def add_obstacle(self, obstacle: Obstacle) -> str: + """Add an obstacle and rebuild the VAMP environment.""" + self._obstacles[obstacle.name] = obstacle + self._rebuild_environment() + return obstacle.name + + def remove_obstacle(self, obstacle_id: str) -> bool: + """Remove an obstacle.""" + existed = obstacle_id in self._obstacles + self._obstacles.pop(obstacle_id, None) + if existed: + self._rebuild_environment() + return existed + + def update_obstacle_pose(self, obstacle_id: str, pose: PoseStamped) -> bool: + """Update an obstacle pose.""" + if obstacle_id not in self._obstacles: + return False + self._obstacles[obstacle_id].pose = pose + self._rebuild_environment() + return True + + def clear_obstacles(self) -> None: + """Remove all obstacles.""" + self._obstacles.clear() + self._rebuild_environment() + + def get_obstacles(self) -> list[Obstacle]: + """Get all obstacles.""" + return list(self._obstacles.values()) + + def finalize(self) -> None: + """Finalize the VAMP world.""" + self._finalized = True + + @property + def is_finalized(self) -> bool: + """Check if the world is finalized.""" + return self._finalized + + def get_live_context(self) -> _VampContext: + """Get the live VAMP context.""" + return _VampContext(self._live_joint_states) + + @contextmanager + def scratch_context(self) -> Generator[_VampContext, None, None]: + """Get a scratch context with copied joint states.""" + yield _VampContext(deepcopy(self._live_joint_states)) + + def sync_from_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) -> None: + """Sync live state from a joint-state message.""" + self._live_joint_states[robot_id] = self._normalize_joint_state(robot_id, joint_state) + + def set_joint_state( + self, ctx: _VampContext, robot_id: WorldRobotID, joint_state: JointState + ) -> None: + """Set robot joint state in a context.""" + ctx.joint_states[robot_id] = self._normalize_joint_state(robot_id, joint_state) + + def get_joint_state(self, ctx: _VampContext, robot_id: WorldRobotID) -> JointState: + """Get robot joint state from a context.""" + return ctx.joint_states[robot_id] + + def is_collision_free(self, ctx: _VampContext, robot_id: WorldRobotID) -> bool: + """Check if current configuration is valid according to VAMP.""" + return self._validate_state(ctx.joint_states[robot_id], check_bounds=True) + + def get_min_distance(self, ctx: _VampContext, robot_id: WorldRobotID) -> float: + """Minimum distance is not exposed by VAMP's Python API.""" + raise UnsupportedWorldCapabilityError("vamp", "minimum distance query") + + def check_config_collision_free(self, robot_id: WorldRobotID, joint_state: JointState) -> bool: + """Check a joint state using VAMP native validation.""" + return self._validate_state( + self._normalize_joint_state(robot_id, joint_state), check_bounds=True + ) + + def check_edge_collision_free( + self, + robot_id: WorldRobotID, + start: JointState, + end: JointState, + step_size: float = 0.05, + ) -> bool: + """Check an edge using VAMP native motion validation.""" + del step_size + start_state = self._normalize_joint_state(robot_id, start) + end_state = self._normalize_joint_state(robot_id, end) + result = self._robot_module.validate_motion( + list(start_state.position), + list(end_state.position), + self._environment, + True, + ) + return bool(result) + + def get_ee_pose(self, ctx: _VampContext, robot_id: WorldRobotID) -> PoseStamped: + """Get end-effector pose from VAMP eefk.""" + joint_state = ctx.joint_states[robot_id] + transform = np.asarray( + self._robot_module.eefk(list(joint_state.position)), dtype=np.float64 + ) + pose = matrix_to_pose(transform) + return PoseStamped(position=pose.position, orientation=pose.orientation, frame_id="world") + + def get_link_pose( + self, ctx: _VampContext, robot_id: WorldRobotID, link_name: str + ) -> NDArray[np.float64]: + """Return EE pose only when the requested link is the configured EE link.""" + config = self._robots[robot_id] + if link_name != config.end_effector_link: + raise UnsupportedWorldCapabilityError("vamp", f"link pose for '{link_name}'") + joint_state = ctx.joint_states[robot_id] + return np.asarray(self._robot_module.eefk(list(joint_state.position)), dtype=np.float64) + + def get_jacobian(self, ctx: _VampContext, robot_id: WorldRobotID) -> NDArray[np.float64]: + """VAMP's Python API does not expose a Jacobian.""" + raise UnsupportedWorldCapabilityError("vamp", "end-effector Jacobian") + + def _normalize_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) -> JointState: + config = self._robots[robot_id] + positions = list(joint_state.position[: len(config.joint_names)]) + names = list(joint_state.name[: len(positions)]) if joint_state.name else config.joint_names + return JointState(name=names, position=positions) + + def _validate_state(self, joint_state: JointState, check_bounds: bool) -> bool: + return bool( + self._robot_module.validate( + list(joint_state.position), + self._environment, + check_bounds, + ) + ) + + def _rebuild_environment(self) -> None: + self._environment = self._vamp_module.Environment() + for obstacle in self._obstacles.values(): + self._add_obstacle_to_environment(obstacle) + + def _add_obstacle_to_environment(self, obstacle: Obstacle) -> None: + center = [obstacle.pose.position.x, obstacle.pose.position.y, obstacle.pose.position.z] + euler_xyz = ( + R.from_quat( + [ + obstacle.pose.orientation.x, + obstacle.pose.orientation.y, + obstacle.pose.orientation.z, + obstacle.pose.orientation.w, + ] + ) + .as_euler("xyz") + .tolist() + ) + if obstacle.obstacle_type == ObstacleType.SPHERE: + self._environment.add_sphere(self._vamp_module.Sphere(center, obstacle.dimensions[0])) + elif obstacle.obstacle_type == ObstacleType.BOX: + half_extents = [dimension / 2.0 for dimension in obstacle.dimensions] + self._environment.add_cuboid(self._vamp_module.Cuboid(center, euler_xyz, half_extents)) + elif obstacle.obstacle_type == ObstacleType.CYLINDER: + self._environment.add_capsule( + self._vamp_module.Cylinder( + center, + euler_xyz, + obstacle.dimensions[0], + obstacle.dimensions[1], + ) + ) + else: + raise UnsupportedWorldCapabilityError("vamp", f"{obstacle.obstacle_type.name} obstacle") diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 0f84bed419..2c53ce1777 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -29,10 +29,13 @@ ) from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor +from dimos.manipulation.planning.planners.config import RRTConnectPlannerConfig, VampPlannerConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.protocols import VisualizationSpec +from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError +from dimos.manipulation.planning.world.config import DrakeWorldConfig, VampWorldConfig from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -225,17 +228,19 @@ def test_kinematics_config_is_passed_to_factory(self, robot_config): ): module._initialize_planning() - mock_planner.assert_called_once_with(name="rrt_connect") + planner_config = mock_planner.call_args.kwargs["config"] + assert isinstance(planner_config, RRTConnectPlannerConfig) mock_kinematics.assert_called_once_with(config=kinematics) def test_legacy_kinematics_name_still_selects_backend(self, robot_config): """The old kinematics_name field remains a compatibility shim.""" module = _make_module() - module.config = ManipulationModuleConfig( - robots=[robot_config], - kinematics_name="pink", - enable_viz=False, - ) + with pytest.warns(DeprecationWarning, match="kinematics_name is deprecated"): + module.config = ManipulationModuleConfig( + robots=[robot_config], + kinematics_name="pink", + enable_viz=False, + ) mock_world_monitor = MagicMock(spec=WorldMonitor) mock_world_monitor.add_robot.return_value = "robot_id" @@ -269,6 +274,69 @@ def test_nested_kinematics_config_parses_cli_override_shape(self) -> None: assert config.kinematics.dt == 0.02 assert config.kinematics.posture_cost == 0.0 + def test_nested_world_and_planner_config_parses_cli_override_shape(self) -> None: + """Pydantic parses nested world/planner config shapes used by -o overrides.""" + config = ManipulationModuleConfig( + world={ + "backend": "vamp", + "artifact": { + "mode": "official", + "robot": "panda", + }, + }, + planner={ + "backend": "vamp", + "algorithm": "prm", + "simplify": "false", + "validate_path": "true", + }, + ) + + assert isinstance(config.world, VampWorldConfig) + assert config.world.artifact.mode == "official" + assert config.world.artifact.robot == "panda" + assert isinstance(config.planner, VampPlannerConfig) + assert config.planner.algorithm == "prm" + assert config.planner.simplify is False + assert config.planner.validate_path is True + + def test_default_world_and_planner_config_preserves_drake_rrt_behavior(self) -> None: + """Default config remains Drake world plus RRT-Connect planner.""" + config = ManipulationModuleConfig() + + assert isinstance(config.world, DrakeWorldConfig) + assert isinstance(config.planner, RRTConnectPlannerConfig) + assert config.kinematics.backend == "jacobian" + + def test_legacy_planner_name_still_selects_backend(self) -> None: + """The old planner_name field remains a noisy compatibility shim.""" + with pytest.warns(DeprecationWarning, match="planner_name is deprecated"): + config = ManipulationModuleConfig(planner_name="vamp") + + assert isinstance(config.planner, VampPlannerConfig) + + def test_vamp_planner_requires_vamp_world(self, robot_config) -> None: + """Planning initialization fails early for invalid VAMP planner pairing.""" + module = _make_module() + module.config = ManipulationModuleConfig( + robots=[robot_config], + planner={"backend": "vamp"}, + ) + + with pytest.raises(ValueError, match="VAMP planner requires world backend 'vamp'"): + module._initialize_planning() + + def test_vamp_world_requires_vamp_planner(self, robot_config) -> None: + """Planning initialization fails early for invalid VAMP world pairing.""" + module = _make_module() + module.config = ManipulationModuleConfig( + robots=[robot_config], + world={"backend": "vamp"}, + ) + + with pytest.raises(ValueError, match="VAMP world backend requires planner backend 'vamp'"): + module._initialize_planning() + def test_solve_ik_rpc_calls_configured_backend(self, robot_config): """solve_ik returns the backend IKResult without path planning.""" module = _make_module() @@ -341,6 +409,27 @@ def test_solve_ik_rpc_uses_explicit_seed(self, robot_config): assert kwargs["seed"] is explicit_seed module._world_monitor.get_current_joint_state.assert_not_called() + def test_solve_ik_rpc_reports_unsupported_world_capability(self, robot_config): + """Pose planning surfaces incompatible world/kinematics capabilities clearly.""" + module = _make_module() + module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._world_monitor = MagicMock() + module._world_monitor.world = MagicMock() + module._world_monitor.get_current_joint_state.return_value = JointState( + name=robot_config.joint_names, position=[0.0, 0.0, 0.0] + ) + module._kinematics = MagicMock() + module._kinematics.solve.side_effect = UnsupportedWorldCapabilityError( + "vamp", "end-effector Jacobian" + ) + + pose = Pose(position=Vector3(x=0.45, y=0.0, z=0.25), orientation=Quaternion()) + result = module.solve_ik(pose) + + assert result.status == IKStatus.NO_SOLUTION + assert "end-effector Jacobian" in result.message + assert module._state == ManipulationState.IDLE + class TestJointNameTranslation: """Test trajectory joint name translation for coordinator.""" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 24d90dd7fe..421ec80a1f 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -71,6 +71,7 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", + "panda-coordinator": "dimos.robot.manipulators.franka.blueprints:panda_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-xarm7": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_xarm7", diff --git a/dimos/robot/catalog/franka.py b/dimos/robot/catalog/franka.py new file mode 100644 index 0000000000..902586e2f2 --- /dev/null +++ b/dimos/robot/catalog/franka.py @@ -0,0 +1,85 @@ +# 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. + +"""Franka Panda robot configurations.""" + +from __future__ import annotations + +from typing import Any + +from dimos.robot.config import RobotConfig +from dimos.utils.data import LfsPath + +# LFS-backed: data/.lfs/franka_description.tar.gz extracts to data/franka_description/. +_FRANKA_DESCRIPTION_PKG = LfsPath("franka_description") + +# Keep URDF/SRDF paths explicit so VAMP tests/benchmarks can validate that the +# DimOS robot model matches the official VAMP Panda artifact joint order. +FRANKA_PANDA_MODEL = _FRANKA_DESCRIPTION_PKG / "urdf/panda.urdf.xacro" +FRANKA_PANDA_FK_MODEL = _FRANKA_DESCRIPTION_PKG / "urdf/panda.urdf" +FRANKA_PANDA_SRDF = _FRANKA_DESCRIPTION_PKG / "srdf/panda.srdf" + +FRANKA_PANDA_JOINT_NAMES = [f"panda_joint{i}" for i in range(1, 8)] +FRANKA_PANDA_HOME_JOINTS = [0.0, -0.7853981634, 0.0, -2.35619449, 0.0, 1.5707963268, 0.7853981634] +FRANKA_PANDA_JOINT_LIMITS_LOWER = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] +FRANKA_PANDA_JOINT_LIMITS_UPPER = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] + + +def franka_panda( + name: str = "panda", + *, + adapter_type: str = "mock", + address: str | None = None, + **overrides: Any, +) -> RobotConfig: + """Franka Panda config for mock-control planning tests and benchmarks.""" + defaults: dict[str, Any] = { + "name": name, + "model_path": FRANKA_PANDA_MODEL, + "end_effector_link": "panda_hand", + "adapter_type": adapter_type, + "address": address, + "joint_names": FRANKA_PANDA_JOINT_NAMES, + "base_link": "panda_link0", + "home_joints": FRANKA_PANDA_HOME_JOINTS, + "joint_limits_lower": FRANKA_PANDA_JOINT_LIMITS_LOWER, + "joint_limits_upper": FRANKA_PANDA_JOINT_LIMITS_UPPER, + "package_paths": { + "franka_description": _FRANKA_DESCRIPTION_PKG, + "moveit_resources_panda_description": _FRANKA_DESCRIPTION_PKG, + }, + "auto_convert_meshes": True, + "max_velocity": 1.0, + "max_acceleration": 2.0, + "adapter_kwargs": {"srdf_path": FRANKA_PANDA_SRDF}, + } + if "adapter_kwargs" in overrides: + defaults["adapter_kwargs"] = { + **defaults["adapter_kwargs"], + **overrides.pop("adapter_kwargs"), + } + defaults.update(overrides) + return RobotConfig(**defaults) + + +__all__ = [ + "FRANKA_PANDA_FK_MODEL", + "FRANKA_PANDA_HOME_JOINTS", + "FRANKA_PANDA_JOINT_LIMITS_LOWER", + "FRANKA_PANDA_JOINT_LIMITS_UPPER", + "FRANKA_PANDA_JOINT_NAMES", + "FRANKA_PANDA_MODEL", + "FRANKA_PANDA_SRDF", + "franka_panda", +] diff --git a/dimos/robot/catalog/test_franka.py b/dimos/robot/catalog/test_franka.py new file mode 100644 index 0000000000..d710430ad3 --- /dev/null +++ b/dimos/robot/catalog/test_franka.py @@ -0,0 +1,100 @@ +# 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. + +"""Tests for the Franka Panda robot catalog.""" + +from __future__ import annotations + +from dimos.control.components import HardwareType +from dimos.robot.catalog.franka import ( + FRANKA_PANDA_FK_MODEL, + FRANKA_PANDA_JOINT_LIMITS_LOWER, + FRANKA_PANDA_JOINT_LIMITS_UPPER, + FRANKA_PANDA_JOINT_NAMES, + FRANKA_PANDA_MODEL, + FRANKA_PANDA_SRDF, + franka_panda, +) + + +def _lfs_filename(path: object) -> object: + return object.__getattribute__(path, "_lfs_filename") + + +def test_franka_panda_catalog_defaults_to_mock_control() -> None: + """The Panda catalog config is mock-control first.""" + config = franka_panda() + + assert config.name == "panda" + assert config.adapter_type == "mock" + assert config.address is None + assert config.joint_names == FRANKA_PANDA_JOINT_NAMES + assert config.end_effector_link == "panda_hand" + assert config.base_link == "panda_link0" + assert config.joint_limits_lower == FRANKA_PANDA_JOINT_LIMITS_LOWER + assert config.joint_limits_upper == FRANKA_PANDA_JOINT_LIMITS_UPPER + + +def test_franka_panda_uses_lfs_backed_model_and_srdf_paths() -> None: + """Panda URDF/SRDF resources follow the repo LFS-backed data pattern.""" + assert _lfs_filename(FRANKA_PANDA_MODEL) == "franka_description/urdf/panda.urdf.xacro" + assert _lfs_filename(FRANKA_PANDA_FK_MODEL) == "franka_description/urdf/panda.urdf" + assert _lfs_filename(FRANKA_PANDA_SRDF) == "franka_description/srdf/panda.srdf" + + +def test_franka_panda_hardware_component_uses_mock_adapter_and_prefixed_joints() -> None: + """RobotConfig conversion feeds ControlCoordinator mock hardware.""" + config = franka_panda() + + component = config.to_hardware_component() + + assert component.hardware_id == "panda" + assert component.hardware_type == HardwareType.MANIPULATOR + assert component.adapter_type == "mock" + assert component.joints == [f"panda/{joint}" for joint in FRANKA_PANDA_JOINT_NAMES] + assert component.adapter_kwargs["initial_positions"] == config.home_joints + + +def test_franka_panda_robot_model_config_preserves_vamp_joint_order() -> None: + """Manipulation robot model config keeps the official Panda joint order.""" + config = franka_panda() + + model = config.to_robot_model_config() + + assert model.name == "panda" + assert _lfs_filename(model.model_path) == _lfs_filename(FRANKA_PANDA_MODEL) + assert model.joint_names == FRANKA_PANDA_JOINT_NAMES + assert model.joint_limits_lower == FRANKA_PANDA_JOINT_LIMITS_LOWER + assert model.joint_limits_upper == FRANKA_PANDA_JOINT_LIMITS_UPPER + assert model.joint_name_mapping == { + f"panda/{joint}": joint for joint in FRANKA_PANDA_JOINT_NAMES + } + + +def test_franka_panda_task_config_supports_mock_coordinator_benchmark_path() -> None: + """Catalog output can build the same mock coordinator task shape as xArm/Piper.""" + config = franka_panda() + + task = config.to_task_config( + task_type="cartesian_ik", + task_name="cartesian_ik_panda", + model_path=FRANKA_PANDA_FK_MODEL, + ee_joint_id=config.dof, + ) + + assert task.name == "cartesian_ik_panda" + assert task.type == "cartesian_ik" + assert task.joint_names == [f"panda/{joint}" for joint in FRANKA_PANDA_JOINT_NAMES] + assert _lfs_filename(task.params["model_path"]) == _lfs_filename(FRANKA_PANDA_FK_MODEL) + assert task.params["ee_joint_id"] == 7 diff --git a/dimos/robot/config.py b/dimos/robot/config.py index d0922cb842..af4bb92a50 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -83,6 +83,9 @@ class RobotConfig(BaseModel): base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # Planning + joint_limits_lower: list[float] | None = None + joint_limits_upper: list[float] | None = None + velocity_limits: list[float] | None = None max_velocity: float = 1.0 max_acceleration: float = 2.0 pre_grasp_offset: float = 0.10 @@ -232,6 +235,9 @@ def to_robot_model_config(self) -> RobotModelConfig: auto_convert_meshes=self.auto_convert_meshes, max_velocity=self.max_velocity, max_acceleration=self.max_acceleration, + joint_limits_lower=self.joint_limits_lower, + joint_limits_upper=self.joint_limits_upper, + velocity_limits=self.velocity_limits, joint_name_mapping=self.joint_name_mapping, coordinator_task_name=self.coordinator_task_name, gripper_hardware_id=self.name if self.gripper else None, diff --git a/dimos/robot/manipulators/franka/blueprints.py b/dimos/robot/manipulators/franka/blueprints.py new file mode 100644 index 0000000000..75abc25cbe --- /dev/null +++ b/dimos/robot/manipulators/franka/blueprints.py @@ -0,0 +1,58 @@ +# 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. + +"""Mock-control blueprint for Franka Panda planning. + +Usage: + uv run --extra manipulation --extra vamp dimos run panda-coordinator \ + -o manipulationmodule.world.backend=vamp \ + -o manipulationmodule.world.artifact.mode=official \ + -o manipulationmodule.world.artifact.robot=panda \ + -o manipulationmodule.planner.backend=vamp \ + -o manipulationmodule.planner.algorithm=rrtc +""" + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.coordination.blueprints import autoconnect +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.robot.catalog.franka import FRANKA_PANDA_FK_MODEL, franka_panda + +_panda_cfg = franka_panda(name="panda") + +panda_coordinator = autoconnect( + ManipulationModule.blueprint( + robots=[_panda_cfg.to_robot_model_config()], + planning_timeout=10.0, + enable_viz=False, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_panda_cfg.to_hardware_component()], + tasks=[ + _panda_cfg.to_task_config( + task_type="cartesian_ik", + task_name="cartesian_ik_panda", + model_path=FRANKA_PANDA_FK_MODEL, + ee_joint_id=_panda_cfg.dof, + ), + ], + ), +) + +# Alias matching existing xArm naming style. +panda_planner_coordinator = panda_coordinator + +__all__ = ["panda_coordinator", "panda_planner_coordinator"] diff --git a/dimos/robot/manipulators/franka/test_blueprints.py b/dimos/robot/manipulators/franka/test_blueprints.py new file mode 100644 index 0000000000..1dc7380ad9 --- /dev/null +++ b/dimos/robot/manipulators/franka/test_blueprints.py @@ -0,0 +1,52 @@ +# 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. + +"""Tests for Franka Panda blueprints.""" + +from __future__ import annotations + +from dimos.robot.all_blueprints import all_blueprints +from dimos.robot.manipulators.franka.blueprints import panda_coordinator + + +def test_panda_coordinator_is_registered() -> None: + assert ( + all_blueprints["panda-coordinator"] + == "dimos.robot.manipulators.franka.blueprints:panda_coordinator" + ) + + +def test_panda_coordinator_accepts_vamp_cli_override_shape() -> None: + config = panda_coordinator.config()( + manipulationmodule={ + "world": { + "backend": "vamp", + "artifact": {"mode": "official", "robot": "panda"}, + }, + "planner": { + "backend": "vamp", + "algorithm": "rrtc", + "simplify": "true", + "validate_path": "true", + }, + } + ) + + assert config.manipulationmodule is not None + assert config.manipulationmodule.world.backend == "vamp" + assert config.manipulationmodule.world.artifact.robot == "panda" + assert config.manipulationmodule.planner.backend == "vamp" + assert config.manipulationmodule.planner.algorithm == "rrtc" + assert config.manipulationmodule.planner.simplify is True + assert config.manipulationmodule.planner.validate_path is True diff --git a/docs/usage/README.md b/docs/usage/README.md index 071b6fc0b2..1b09a6c15c 100644 --- a/docs/usage/README.md +++ b/docs/usage/README.md @@ -7,6 +7,7 @@ This page explains general concepts. - [Modules](/docs/usage/modules.md): The primary units of deployment in DimOS, modules run in parallel and are python classes. - [Streams](/docs/usage/sensor_streams/README.md): How modules communicate, a Pub / Sub system. - [Blueprints](/docs/usage/blueprints.md): a way to group modules together and define their connections to each other. +- [Manipulation planning](/docs/usage/manipulation_planning.md): world, planner, and kinematics backend configuration. - [RPC](/docs/usage/blueprints.md#calling-the-methods-of-other-modules): how one module can call a method on another module (arguments get serialized to JSON-like binary data). - [Skills](/docs/usage/blueprints.md#defining-skills): An RPC function, except it can be called by an AI agent (a tool for an AI). - Agents: AI that has an objective, access to stream data, and is capable of calling skills as tools. diff --git a/docs/usage/manipulation_planning.md b/docs/usage/manipulation_planning.md new file mode 100644 index 0000000000..0904d22347 --- /dev/null +++ b/docs/usage/manipulation_planning.md @@ -0,0 +1,124 @@ +# Manipulation planning backends + +DimOS manipulation planning is configured as a stack: + +- `world`: robot/environment representation and collision or validity checks +- `planner`: joint-space path planning +- `kinematics`: pose-to-joint solving for pose goals + +The default stack remains Drake world + RRT-Connect planner + Jacobian IK. + +## VAMP backend + +VAMP is an optional joint-space planning backend. Install it only when needed: + +```bash +uv sync --extra vamp +``` + +VAMP owns its robot artifact and environment representation, so select the VAMP +world and VAMP planner together: + +```python +from dimos.manipulation.manipulation_module import ManipulationModule + +module = ManipulationModule.blueprint( + world={"backend": "vamp", "artifact": {"mode": "official", "robot": "panda"}}, + planner={"backend": "vamp", "algorithm": "rrtc", "simplify": True}, +) +``` + +Supported VAMP algorithms are `rrtc`, `prm`, `fcit`, and `aorrtc`. + +For a user-prepared custom robot artifact, point the world config at a local +Python module/package produced outside DimOS: + +```python +module = ManipulationModule.blueprint( + world={"backend": "vamp", "artifact": {"mode": "custom", "path": "/path/to/vamp_robot"}}, + planner={"backend": "vamp", "algorithm": "prm"}, +) +``` + +CLI/config overrides follow the same nested field shape, for example: + +```bash +uv run --extra manipulation --extra vamp dimos run panda-coordinator \ + -o manipulationmodule.world.backend=vamp \ + -o manipulationmodule.world.artifact.mode=official \ + -o manipulationmodule.world.artifact.robot=panda \ + -o manipulationmodule.planner.backend=vamp \ + -o manipulationmodule.planner.algorithm=rrtc +``` + +## Artifact scope + +DimOS does not generate VAMP artifacts at runtime. Official artifacts are loaded +from the installed `vamp-planner` package. Custom robot artifacts must be +prepared by the user/upstream VAMP tooling before DimOS starts. + +## Pose planning with VAMP + +The VAMP planner is joint-space only. It does not run IK, convert poses, or +probe Jacobians. Pose planning is available only if the configured kinematics +backend can solve against the selected world. A VAMP world currently raises a +clear unsupported-capability error for Jacobian requests. + +## Franka Panda mock-control support + +`dimos.robot.catalog.franka.franka_panda()` provides a mock-control Franka Panda +configuration for VAMP tests and planner benchmarks. It uses: + +- mock manipulator control by default (`adapter_type="mock"`) +- Panda arm joint order `panda_joint1` through `panda_joint7` +- LFS-backed model resources under `franka_description` +- `RobotConfig.to_hardware_component()` for `ControlCoordinator` +- `RobotConfig.to_robot_model_config()` for `ManipulationModule` + +The Panda model constants are: + +- `FRANKA_PANDA_MODEL`: `franka_description/urdf/panda.urdf.xacro` +- `FRANKA_PANDA_FK_MODEL`: `franka_description/urdf/panda.urdf` +- `FRANKA_PANDA_SRDF`: `franka_description/srdf/panda.srdf` + +The description package should be stored using the repository LFS data pattern: +`data/.lfs/franka_description.tar.gz` extracts to `data/franka_description/`. +Do not download or generate the Panda URDF/SRDF at import time. + +Start the registered mock coordinator/planner blueprint with: + +```bash +uv run --extra manipulation --extra vamp dimos run panda-coordinator \ + -o manipulationmodule.world.backend=vamp \ + -o manipulationmodule.world.artifact.mode=official \ + -o manipulationmodule.world.artifact.robot=panda \ + -o manipulationmodule.planner.backend=vamp \ + -o manipulationmodule.planner.algorithm=rrtc +``` + +Then use the manipulation client in another terminal and plan joint-space Panda +motions with `plan([...], "panda")`. + +## Failure modes + +- Missing VAMP dependency: selecting a VAMP world raises an install hint for + `vamp-planner` / `dimos[vamp]`. +- Invalid pairing: VAMP world and VAMP planner must be selected together. +- Incompatible kinematics: Drake optimization IK requires a Drake world; VAMP + pose planning requires a kinematics backend that naturally supports the VAMP + world capabilities it needs. +- Unsupported capability: VAMP does not expose Jacobians or minimum-distance + queries through the current Python API. +- Model/artifact mismatch: Panda benchmarks should verify URDF joint order and + limits against the official VAMP Panda artifact. + +## Contributor notes + +- Keep optional planner imports lazy and backend-scoped. +- Add new backends through typed `world`, `planner`, or `kinematics` config + variants with discriminator fields. +- Use explicit `DeprecationWarning` shims for migrated config fields. +- Raise clear unsupported-capability errors instead of synthesizing planner + features the backend does not expose. +- Prefer mock-control catalog fixtures for backend tests and benchmarks before + adding real hardware adapters. diff --git a/openspec/changes/add-vamp-planning-backend/.openspec.yaml b/openspec/changes/add-vamp-planning-backend/.openspec.yaml new file mode 100644 index 0000000000..c2d56cecef --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-06-17 diff --git a/openspec/changes/add-vamp-planning-backend/design.md b/openspec/changes/add-vamp-planning-backend/design.md new file mode 100644 index 0000000000..0fab0e80b5 --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/design.md @@ -0,0 +1,229 @@ +## Context + +DimOS manipulation planning is currently centered on the existing planning contracts in `dimos/manipulation/planning/spec/protocols.py`: + +- `WorldSpec` owns robot/world state, collision checks, FK/link poses, and currently includes Jacobian access. +- `PlannerSpec` owns joint-space path planning from start to goal joint states. +- `KinematicsSpec` owns pose-to-joint-state solving. +- `VisualizationSpec` is optional and currently implemented by `DrakeWorld` through Meshcat. + +The current module wiring in `dimos/manipulation/manipulation_module.py` initializes a `WorldMonitor`, adds configured `RobotModelConfig` objects, finalizes the world, then creates planner and kinematics backends. `WorldMonitor` already has a backend seam (`WorldMonitor(backend="drake", enable_viz=False, **kwargs)`) that delegates to `create_world(...)` in `dimos/manipulation/planning/factory.py`. + +VAMP has a different model than the current Drake-backed world. Its Python package exposes robot-specific planning modules and utilities such as joint-space planning algorithms, validation/debug helpers, `fk`, and `eefk`. It does not expose a general runtime URDF-to-planner pipeline through the runtime binding, and it does not expose a public Jacobian API. Custom robot support is an offline/user-owned artifact-generation concern upstream of runtime planning. + +PR #2481 for Pink IK establishes the configuration pattern this change should follow: typed discriminated backend config objects with `backend` as the discriminator, backend-specific settings in the nested config, and deprecated flat compatibility shims that emit warnings when used. + +## Goals / Non-Goals + +**Goals:** + +- Add VAMP as an optional manipulation planning backend for joint-space planning. +- Load VAMP robot artifacts from either official VAMP robot modules/artifacts or a user-provided custom artifact path. +- Keep VAMP dependency loading lazy and backend-scoped. +- Add typed nested world and planner config objects instead of new flat `ManipulationModuleConfig.vamp_*` fields. +- Validate VAMP world/planner pairing eagerly during planning-stack initialization. +- Keep pose planning conditional on an explicitly compatible kinematics backend. +- Add a Franka Panda catalog/configuration target for mock-control planning tests and future planner benchmarks. +- Fail clearly when a selected backend or kinematics solver calls an unsupported world capability. +- Preserve existing Drake/default manipulation behavior unless users opt into VAMP or migrated config fields. + +**Non-Goals:** + +- DimOS will not generate VAMP artifacts from URDF/SRDF/meshes. +- DimOS will not own a foam/cricket/fkcc_gen artifact-preparation pipeline in this change. +- VAMP will not expose synthetic Jacobian support. +- VAMP will not pretend to satisfy unsupported `WorldSpec` behavior just to match the Drake surface. +- VAMP planner selection will not imply pose-planning support. +- Real Franka Panda hardware control is not required in this change. +- No new runnable robot blueprint is required unless implementation later chooses to add a VAMP/Panda demo or benchmark blueprint. + +## DimOS Architecture + +### Configuration + +Introduce typed nested planning config objects analogous to the Pink IK pattern: + +```python +world={ + "backend": "vamp", + "artifact": { + "mode": "official", + "robot": "panda", + }, +} + +planner={ + "backend": "vamp", + "algorithm": "rrtc", + "simplify": True, + "validate_path": True, +} +``` + +Custom artifact mode uses an explicit user-prepared path: + +```python +world={ + "backend": "vamp", + "artifact": { + "mode": "custom", + "path": "/path/to/user/prepared/artifact", + }, +} +``` + +VAMP-specific settings must be nested-only. Do not add fields such as `vamp_robot` or `vamp_artifact_path` directly to `ManipulationModuleConfig`. + +Existing flat fields that are migrated, such as `planner_name`, `kinematics_name`, or `enable_viz`, may remain temporarily as compatibility shims. Those shims must emit a visible `DeprecationWarning` when used. The `Deprecated` package is appropriate for deprecated callable APIs, but config-field migration should use explicit warnings in config normalization/validation because config fields are data, not callables. + +### World backend + +Add a VAMP world backend that is responsible for: + +- Loading official VAMP artifacts/modules by configured robot name. +- Loading custom user-prepared artifacts by explicit configured path. +- Holding VAMP-native robot/environment representation. +- Exposing native VAMP validity/FK/EE pose behavior where supported. +- Converting supported DimOS obstacle/environment inputs into VAMP environment data. +- Raising a clear unsupported-capability error for operations that are not natively available. + +The VAMP world must not dynamically generate artifacts or invoke the VAMP artifact-generation toolchain. If a robot is not covered by official VAMP artifacts, the user owns generating/building the artifact and provides the path/config needed to load it. + +### Planner backend + +Add a VAMP planner backend that is responsible for: + +- Joint-space planning only. +- Configurable algorithm selection (`rrtc`, `prm`, `fcit`, `aorrtc`) with default `rrtc`. +- Native path simplification when enabled and available. +- Native path validation when enabled and available. +- Backend-scoped lazy imports and actionable dependency errors. + +`VampPlanner` must not call `WorldSpec.get_jacobian()`, solve IK, or own pose-to-joint conversion. Jacobian and IK are kinematics concerns, not planner concerns. + +### Kinematics and pose planning + +Pose planning remains a DimOS-level flow: + +```text +target pose -> KinematicsSpec -> goal joint state -> PlannerSpec -> joint path +``` + +For VAMP, this flow is enabled only when the selected `KinematicsSpec` declares or demonstrates compatibility with the VAMP world surface. The initial VAMP scope is joint-space planning. A future FK-only or derivative-free IK backend could enable VAMP pose planning without requiring VAMP to manufacture Jacobian behavior. + +If `kinematics={"backend": "jacobian"}` is paired with `world={"backend": "vamp"}` and the VAMP world does not support Jacobian, initialization or the first compatibility check should fail with an explicit incompatibility message rather than a generic attribute error. + +### Factory and validation + +Update planning factory wiring to dispatch from typed config objects rather than only string names. The factory should validate cross-backend combinations before planning: + +- `VampWorldConfig` requires `VampPlannerConfig`. +- `VampPlannerConfig` requires `VampWorldConfig`. +- VAMP world/planner pairs are invalid with incompatible kinematics backends. +- Drake/default behavior remains valid without VAMP installed. + +### Streams, blueprints, skills, and CLI + +No new streams are required. Existing manipulation RPCs and skills continue to use the existing planning module surface. VAMP affects the planning implementation selected under the module, not the LCM/SHM/ROS/DDS transport topology. + +Blueprints can opt into VAMP through nested config. CLI examples should use nested override paths, such as: + +```bash +-o manipulationmodule.world.backend=vamp +-o manipulationmodule.world.artifact.mode=official +-o manipulationmodule.world.artifact.robot=panda +-o manipulationmodule.planner.backend=vamp +-o manipulationmodule.planner.algorithm=rrtc +``` + +If no new blueprints are added, `dimos/robot/all_blueprints.py` should not need regeneration. If implementation adds or renames blueprint variables, run `pytest dimos/robot/test_all_blueprints_generation.py`. + +### Franka Panda mock-control support + +Add Franka Panda support through the existing robot catalog and coordinator patterns, not as a VAMP-only special case. The shape should mirror `dimos.robot.catalog.ufactory` and `dimos.robot.catalog.piper`: + +- Provide a catalog function such as `franka_panda(...) -> RobotConfig` in a Franka/Panda catalog module. +- Default `adapter_type` to `"mock"` so the `ControlCoordinator` uses the existing mock manipulator adapter unless a future real adapter is explicitly selected. +- Provide model/FK constants for the Panda robot model used by manipulation planning, cartesian IK tasks, tests, and benchmarks. +- Store Panda URDF/SRDF/model resources in a repository LFS-backed robot description package, following the existing `data/.lfs/_description.tar.gz` pattern used by catalog assets and referencing the extracted package through `LfsPath`. +- Configure the Panda `RobotConfig` with explicit arm joint names, base link, end-effector link, home joints, package paths, and collision exclusions if needed. +- Let `RobotConfig.to_hardware_component()` feed the coordinator hardware list and `RobotConfig.to_robot_model_config()` feed the `ManipulationModule`, matching the xArm/Piper pattern. + +The initial Panda scope is mock control plus planning metadata. A real Panda hardware adapter may be introduced later behind the same `adapter_type` seam, but VAMP testing and benchmarking should not depend on real hardware availability. + +## Decisions + +### Q&A decision record + +1. **VAMP is a full optional backend, not a generic planner-only plugin.** It owns a VAMP-native robot/environment representation and planner behavior when selected. +2. **DimOS will not dynamically generate VAMP artifacts.** Artifact generation is too large for this integration. Users either use official VAMP artifacts or provide a custom user-prepared artifact path. +3. **VAMP artifacts are runtime-loaded, not maintained as checked-in generated source by DimOS.** Official artifacts come from the VAMP distribution; custom artifacts come from the user. +4. **VAMP remains an optional dependency.** Adapter imports should be lazy and raise actionable installation errors when VAMP is selected but unavailable. +5. **`planner.backend="vamp"` selects the DimOS VAMP planner adapter.** The specific VAMP algorithm is a planner config option, not separate global planner names like `vamp_rrtc`. +6. **Config uses typed nested objects.** This follows PR #2481's Pink IK pattern and avoids new flat VAMP fields on `ManipulationModuleConfig`. +7. **VAMP world and planner must be strictly paired.** Mixed VAMP/Drake world-planner combinations are invalid unless a future bridge explicitly supports them. +8. **Pose planning is not VAMP planner behavior.** It remains a `KinematicsSpec` concern that converts pose goals to joint goals before planning. +9. **Do not expose synthetic Jacobian support.** VAMP has `fk`/`eefk` utilities but no public Jacobian API; the adapter should not invent one. +10. **Initial VAMP capability set is minimal and native.** Joint-space planning, algorithm selection, native validation/simplification, official/custom artifact loading, environment conversion, joint-configuration validity, joint limits when available, and native FK/EE pose where available. +11. **Pose planning with VAMP is conditional.** It is available only if an explicitly compatible kinematics backend is configured. +12. **Unsupported world capabilities fail clearly.** Backends may raise a dedicated unsupported-capability error rather than faking interface completeness. +13. **Legacy config compatibility must be noisy.** Existing flat migrated fields may be accepted temporarily but must emit deprecation warnings. +14. **Franka Panda support is mock-control first.** The Panda catalog target exists to test and benchmark planning flows without requiring physical Panda hardware. +15. **Panda robot descriptions follow the repo LFS pattern.** Panda URDF/SRDF/model assets should be checked in as LFS-backed data package contents and referenced via `LfsPath`, not fetched or generated at runtime. + +### Alternatives considered + +- **Generate VAMP artifacts in DimOS:** rejected as too complex for this change because it would require spherization, tracing/codegen, native compilation, and local cache semantics. +- **Expose VAMP as planner-only over arbitrary `WorldSpec`:** rejected because VAMP planning depends on VAMP-native robot/environment artifacts. +- **Add a numerical Jacobian wrapper over `eefk`:** rejected because it manufactures a capability not inherently supported by the planner/backend. +- **Add flat VAMP module fields:** rejected in favor of typed nested backend config, matching the Pink IK direction. +- **Require real Panda hardware support before benchmarking:** rejected because planning tests and benchmarks need a repeatable mock-control target first. +- **Fetch Panda URDF/SRDF dynamically during tests:** rejected because DimOS catalog robots should follow the existing LFS-backed description package pattern for reproducible offline tests. + +## Safety / Simulation / Replay + +VAMP changes planning, not control execution. Generated trajectories still flow through existing manipulation execution and coordinator surfaces. Safety depends on correct VAMP artifact loading, obstacle conversion, collision validity, and trajectory execution checks. + +Hardware safety constraints: + +- VAMP must fail closed on invalid artifact config, missing dependency, invalid backend pairing, or unsupported kinematics combination. +- VAMP should not silently skip collision/path validation when validation is configured. +- User-prepared artifacts must be treated as trusted local planning inputs; DimOS should validate presence/loadability but cannot prove the artifact accurately models the robot. + +Simulation/replay: + +- Non-VAMP replay and simulation stacks must not import or initialize VAMP. +- VAMP behavior should be testable with fake or official lightweight artifacts before hardware use. +- Franka Panda mock-control support should allow planner testing and benchmarking without commanding physical hardware. +- Manual QA should include a joint-space plan on an official VAMP robot artifact and verification that existing Drake planning still works without VAMP installed. + +## Risks / Trade-offs + +- **Artifact mismatch risk:** A user-prepared artifact may not match the physical robot or DimOS `RobotModelConfig`. Mitigate with explicit config, load-time validation, and documentation. +- **Protocol mismatch risk:** Existing `WorldSpec` includes methods VAMP should not implement. Mitigate with explicit unsupported-capability errors and compatibility validation. +- **Dependency risk:** VAMP is native/compiled. Mitigate with lazy imports and optional extras. +- **Config migration risk:** Moving toward nested config may disturb existing blueprints. Mitigate with deprecation warnings and tests covering legacy shims. +- **Environment conversion risk:** VAMP supports specific environment representations. Start with documented/supported obstacle conversions and fail clearly for unsupported obstacle types. +- **Panda model mismatch risk:** The DimOS Panda catalog model, mock coordinator joint names, and VAMP official Panda artifact may diverge. Mitigate with explicit joint-name/model validation in tests and documentation. +- **LFS asset drift risk:** The Panda URDF/SRDF package may diverge from catalog constants or benchmark expectations. Mitigate by resolving assets through `LfsPath` in tests and validating expected files/joints during catalog tests. + +## Migration / Rollout + +1. Add typed world and planner config objects with default Drake-compatible behavior. +2. Add temporary compatibility shims for existing flat fields that are being migrated, each with `DeprecationWarning`. +3. Add VAMP world/planner adapters behind lazy imports. +4. Add cross-config validation before planning begins. +5. Update docs with official/custom artifact loading examples and CLI override examples. +6. Add the LFS-backed Franka Panda robot description package with URDF/SRDF/model assets and catalog references through `LfsPath`. +7. Add Franka Panda mock-control catalog support and tests for coordinator/manipulation config generation. +8. Add tests before enabling any blueprint-level VAMP examples. +9. If new blueprints are introduced, regenerate and verify `dimos/robot/all_blueprints.py` with the registry test. + +Rollback is straightforward if VAMP remains optional and default config remains Drake-compatible: remove the VAMP config/adapters and keep or revert the generic typed-config migration separately. + +## Open Questions + +- Which official VAMP robots should be documented as supported at first depends on the installed `vamp-planner` distribution used during implementation. +- The exact custom artifact loading shape depends on how user-prepared VAMP artifacts are importable from the Python binding or local path. +- The initial supported obstacle/environment conversion set should be confirmed against VAMP's current Python API during implementation. +- The exact Franka Panda LFS package name, internal URDF/SRDF paths, and package source should be selected during implementation, then validated against the official VAMP Panda artifact joint order. diff --git a/openspec/changes/add-vamp-planning-backend/docs.md b/openspec/changes/add-vamp-planning-backend/docs.md new file mode 100644 index 0000000000..b0ab4c681a --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/docs.md @@ -0,0 +1,53 @@ +## User-Facing Docs + +- Update manipulation planning documentation under `docs/capabilities/` or `docs/usage/` to describe optional VAMP planning backend support. +- Document that VAMP supports initial joint-space planning only, and that pose planning requires an explicitly compatible kinematics backend. +- Add nested config examples for official artifacts: + + ```python + world={"backend": "vamp", "artifact": {"mode": "official", "robot": "panda"}} + planner={"backend": "vamp", "algorithm": "rrtc"} + ``` + +- Add nested config examples for custom user-prepared artifacts: + + ```python + world={"backend": "vamp", "artifact": {"mode": "custom", "path": "/path/to/artifact"}} + planner={"backend": "vamp", "algorithm": "rrtc"} + ``` + +- Document that DimOS does not generate VAMP artifacts. Users who need unsupported robots must generate/build VAMP artifacts themselves and provide the custom path. +- Document Franka Panda mock-control support as the recommended initial robot target for VAMP planning tests and benchmarks, including catalog usage, LFS-backed URDF/SRDF asset expectations, and the fact that control remains mock by default. +- Document CLI override examples using nested config paths, including world backend, artifact mode, official robot name or custom path, planner backend, and algorithm. +- Document expected failure modes: missing optional VAMP dependency, invalid world/planner pairing, invalid artifact config, unsupported obstacle/environment conversion, and incompatible kinematics. + +## Contributor Docs + +- Update planning backend contributor guidance if present, or add a short section to manipulation planning docs explaining the typed backend config pattern. +- Mention that backend imports should be lazy and adapter-owned, with actionable dependency errors. +- Mention that migrated flat config fields must emit `DeprecationWarning` and should be scheduled for removal. +- Mention that VAMP artifact generation is intentionally out of scope for DimOS; contributor work should focus on loading official or user-prepared artifacts. +- Mention that Franka Panda support should follow the shared `RobotConfig` catalog pattern, store URDF/SRDF resources through the repo's LFS-backed data package pattern, and should not require a real hardware adapter for tests or benchmarks. + +## Coding-Agent Docs + +- No AGENTS.md update is required unless implementation introduces new recurring coding-agent rules. +- If docs under `docs/coding-agents/` include manipulation planning guidance, update them with the VAMP boundaries: + - no synthetic Jacobian support, + - no DimOS artifact-generation pipeline, + - VAMP config lives in typed nested world/planner config, + - VAMP planner does not own IK. + - Franka Panda mock support is a catalog/coordinator test target, not a VAMP-specific hardware driver. + - Panda model resources should be `LfsPath`-referenced LFS assets, not runtime downloads. + +## Doc Validation + +- Run any repository doc link validation command documented for DimOS if docs are changed. +- For Markdown examples with runnable Python snippets, run the documented `md-babel-py run ` command where applicable. +- If diagrams are added or changed, run the documented diagram generation command such as `bin/gen-diagrams` if required by the touched docs. +- At minimum, run targeted tests that cover any code examples embedded in documentation or keep examples clearly illustrative if they cannot run without optional VAMP artifacts. +- Validate any Panda examples against the selected LFS-backed Panda model/SRDF assets and mock coordinator joint naming. + +## No Docs Needed + +Documentation is needed because the change adds a user-visible optional planning backend, new nested configuration shape, dependency behavior, artifact-loading expectations, explicit unsupported-capability semantics, and a mock-control Franka Panda target for tests/benchmarks. diff --git a/openspec/changes/add-vamp-planning-backend/proposal.md b/openspec/changes/add-vamp-planning-backend/proposal.md new file mode 100644 index 0000000000..d796a15696 --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/proposal.md @@ -0,0 +1,61 @@ +## Why + +DimOS manipulation currently has a Drake-centered planning stack with generic world, planner, and kinematics protocols. VAMP offers a fast optional motion-planning backend for supported robot artifacts, but it has a different capability model: it plans over VAMP-native robot/environment representations and exposes native validation/FK utilities rather than arbitrary runtime URDF ingestion or a full kinematics interface. + +This change adds VAMP as an optional manipulation planning backend while preserving DimOS' backend boundaries. It should let users run joint-space planning through official VAMP robot artifacts or their own user-prepared artifacts, without making non-VAMP stacks pay dependency, import, or configuration costs. + +The change also adds Franka Panda mock-control support so VAMP can be tested and benchmarked against an official VAMP robot without requiring real hardware. + +## What Changes + +- Add a VAMP world backend for loading official or user-prepared VAMP artifacts and representing VAMP-native planning state. +- Add a VAMP planner backend for joint-space planning with configurable VAMP algorithms such as `rrtc`, `prm`, `fcit`, and `aorrtc`. +- Add typed nested world/planner configuration for backend selection and VAMP-specific options, following the Pink IK configuration pattern. +- Require strict VAMP world/planner pairing during planning-stack initialization. +- Keep VAMP pose planning conditional on an explicitly compatible kinematics backend; joint-space VAMP planning must not imply IK or Jacobian support. +- Provide clear unsupported-capability failures when a backend-specific world operation is not natively supported. +- Keep VAMP optional and lazily imported with actionable dependency errors. +- Do not generate VAMP artifacts in DimOS; users must rely on official VAMP artifacts or provide a custom user-prepared artifact path. +- Add a Franka Panda robot catalog entry and mock-control coordinator path for planning tests and later planner benchmarking, with Panda URDF/SRDF assets stored through the existing LFS-backed robot description pattern. +- Add noisy deprecation warnings for migrated pre-existing flat configuration fields where compatibility shims remain. + +## Affected DimOS Surfaces + +- Modules/streams: + - `ManipulationModule` planning-stack initialization and validation. + - `WorldMonitor` world backend creation path. + - Manipulation planning factory functions for world, planner, and config dispatch. + - Planning world/planner/kinematics protocols where unsupported optional capabilities need clear failure behavior. +- Blueprints/CLI: + - Manipulation blueprints may configure nested world/planner backend objects. + - CLI override examples should use nested config paths such as `manipulationmodule.world.backend=vamp` and `manipulationmodule.planner.backend=vamp`. +- Skills/MCP: + - No direct skill or MCP tool behavior changes are expected; existing manipulation RPCs/skills should fail clearly for unsupported pose-planning combinations. +- Hardware/simulation/replay: + - VAMP is an optional planning backend for manipulation stacks. + - Franka Panda support should default to the existing mock manipulator adapter pattern for control. + - Franka Panda URDF/SRDF assets should live in an LFS-backed robot description package and be referenced through `LfsPath`, matching existing catalog assets such as xArm, Piper, A-750, and OpenArm descriptions. + - A future real Panda hardware adapter may be added behind the same catalog/config seam, but real hardware control is not required for this change. + - Runtime planning behavior changes only when VAMP is selected. +- Docs/generated registries: + - Planning backend documentation and manipulation capability docs need updates. + - No blueprint registry generation change is expected unless new runnable blueprints are added. + +## Capabilities + +### New Capabilities + +- `vamp-planning-backend`: Behavior for optional VAMP world/planner configuration, artifact loading, joint-space planning, validation, and unsupported-capability handling. +- `manipulation-stack`: Behavior for typed manipulation planning backend configuration, compatibility validation, and legacy config migration. + +### Modified Capabilities + +- None. + +## Impact + +Users gain an optional VAMP planning path for fast joint-space manipulation planning when official or custom user-prepared VAMP artifacts are available. Developers gain a typed backend configuration pattern for world and planner options rather than adding VAMP-specific flat module fields. Test and benchmarking workflows gain a mock-control Franka Panda catalog target aligned with VAMP's official Panda artifact. + +Compatibility risk is concentrated around migrating existing flat fields such as planner and visualization selection into nested config objects. Any retained compatibility path should emit a visible `DeprecationWarning` so maintainers know it is temporary. VAMP adds optional runtime dependencies only when selected; missing dependencies or invalid backend combinations should fail at planning-stack initialization with actionable messages. + +Testing should cover config parsing, factory dispatch, lazy import errors, VAMP world/planner pairing validation, artifact-mode validation, joint-space planning adapter behavior with fakes/mocks, Franka Panda mock catalog/coordinator wiring, LFS-backed Panda URDF/SRDF asset resolution, unsupported kinematics combinations, and preservation of existing Drake/default behavior. diff --git a/openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md b/openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md new file mode 100644 index 0000000000..acb063e70b --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md @@ -0,0 +1,92 @@ +## ADDED Requirements + +### Requirement: Typed manipulation planning backend configuration + +DimOS MUST support typed nested configuration for manipulation world, planner, and kinematics backend selection. + +#### Scenario: nested backend config selects planning components +- **GIVEN** a manipulation module configuration with nested `world`, `planner`, and `kinematics` backend objects +- **WHEN** the manipulation planning stack is initialized +- **THEN** DimOS SHALL use the configured backend discriminators to create the selected planning components +- **AND** backend-specific options SHALL be read from the matching nested backend config object. + +#### Scenario: backend-specific options stay local +- **GIVEN** a backend-specific option for world loading or planner tuning +- **WHEN** the option is configured +- **THEN** DimOS MUST read world-loading options from the world config +- **AND** DimOS MUST read planner behavior options from the planner config. + +### Requirement: Planning stack compatibility validation + +DimOS MUST validate incompatible world, planner, and kinematics backend combinations before unsafe or unsupported planning proceeds. + +#### Scenario: invalid backend combination +- **GIVEN** a manipulation planning configuration with incompatible world and planner backends +- **WHEN** the planning stack is initialized +- **THEN** DimOS MUST reject the configuration before executing a plan +- **AND** the error message MUST identify the incompatible backends. + +#### Scenario: invalid kinematics combination +- **GIVEN** a manipulation planning configuration with a kinematics backend that requires unsupported world capabilities +- **WHEN** pose-planning compatibility is checked +- **THEN** DimOS MUST reject the pose-planning configuration clearly +- **AND** joint-space-only planning modes SHALL not be advertised as pose-planning support. + +### Requirement: Legacy flat config migration warnings + +DimOS MUST emit visible deprecation warnings when pre-existing flat manipulation planning config fields are accepted as temporary compatibility shims. + +#### Scenario: legacy planner field used +- **GIVEN** a user configures a pre-existing flat planner-selection field that has a nested replacement +- **WHEN** the config is normalized or the planning stack is initialized +- **THEN** DimOS SHALL preserve the legacy behavior for the compatibility period +- **AND** DimOS MUST emit a `DeprecationWarning` that identifies the nested replacement. + +#### Scenario: new backend settings avoid flat fields +- **GIVEN** a new backend-specific setting is introduced for manipulation planning +- **WHEN** the setting is exposed to users +- **THEN** DimOS MUST expose it through the typed nested backend configuration +- **AND** DimOS SHALL avoid introducing new flat module-level backend fields. + +### Requirement: Default manipulation planning compatibility + +DimOS MUST preserve the default manipulation planning behavior for users who do not opt into new backend config. + +#### Scenario: existing default config +- **GIVEN** a manipulation module configuration that relies on current defaults +- **WHEN** the manipulation module starts +- **THEN** DimOS SHALL initialize the default world, planner, and kinematics behavior +- **AND** optional backend dependencies SHALL not be required. + +#### Scenario: existing non-VAMP blueprint +- **GIVEN** a manipulation blueprint that does not select VAMP +- **WHEN** the blueprint is run +- **THEN** DimOS SHALL not perform VAMP artifact loading +- **AND** DimOS SHALL not require VAMP-specific dependencies. + +### Requirement: Franka Panda mock-control catalog support + +DimOS MUST provide Franka Panda catalog support that can be used for manipulation planning tests and planner benchmarks without requiring physical Panda hardware. + +#### Scenario: Panda catalog creates mock hardware config +- **GIVEN** a user or test constructs the Franka Panda catalog configuration with default control settings +- **WHEN** the configuration is converted to a `HardwareComponent` +- **THEN** DimOS SHALL configure a manipulator hardware component using the mock adapter +- **AND** the component SHALL expose the Panda arm joints expected by the coordinator. + +#### Scenario: Panda catalog creates manipulation model config +- **GIVEN** a user or test constructs the Franka Panda catalog configuration +- **WHEN** the configuration is converted to a manipulation robot model config +- **THEN** DimOS SHALL provide the Panda model path, base link, end-effector link, joint names, package paths, and home joints needed by the manipulation planning stack. + +#### Scenario: Panda model assets resolve from LFS-backed data +- **GIVEN** the Franka Panda catalog configuration references Panda URDF/SRDF resources +- **WHEN** tests or blueprints resolve those resources +- **THEN** DimOS SHALL resolve them from an LFS-backed robot description package through `LfsPath` +- **AND** DimOS SHALL avoid downloading or generating Panda robot descriptions at runtime. + +#### Scenario: Panda target supports VAMP planning tests +- **GIVEN** VAMP is configured with an official Panda artifact and the DimOS Panda catalog robot model +- **WHEN** a joint-space planning test or benchmark initializes the manipulation stack +- **THEN** DimOS SHALL support running the flow through mock control surfaces +- **AND** DimOS SHALL not require a real Panda hardware adapter. diff --git a/openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md b/openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md new file mode 100644 index 0000000000..302f2addcd --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md @@ -0,0 +1,128 @@ +## ADDED Requirements + +### Requirement: VAMP backend selection + +DimOS MUST allow users to select VAMP as an optional manipulation planning backend through typed world and planner backend configuration. + +#### Scenario: select VAMP stack +- **GIVEN** a manipulation module configuration with `world.backend` set to `vamp` +- **AND** `planner.backend` set to `vamp` +- **WHEN** the planning stack is initialized +- **THEN** DimOS SHALL initialize the VAMP planning path instead of the default non-VAMP planning path +- **AND** non-VAMP backends SHALL remain usable without VAMP being installed. + +#### Scenario: reject mixed VAMP and non-VAMP stack +- **GIVEN** a manipulation module configuration with exactly one of the world or planner backends set to `vamp` +- **WHEN** the planning stack is initialized +- **THEN** DimOS MUST reject the configuration before planning begins +- **AND** the error message MUST identify the incompatible world/planner pairing. + +### Requirement: VAMP artifact loading + +DimOS MUST load VAMP robot artifacts from either official VAMP artifacts or a user-provided custom artifact path. + +#### Scenario: load official artifact +- **GIVEN** `world.backend` is `vamp` +- **AND** the VAMP artifact mode is `official` +- **AND** an official robot artifact name is configured +- **WHEN** the planning stack is initialized +- **THEN** DimOS SHALL attempt to load the named official VAMP artifact +- **AND** initialization MUST fail clearly if the artifact is unavailable. + +#### Scenario: load custom user-prepared artifact +- **GIVEN** `world.backend` is `vamp` +- **AND** the VAMP artifact mode is `custom` +- **AND** a custom artifact path is configured +- **WHEN** the planning stack is initialized +- **THEN** DimOS SHALL attempt to load the user-prepared artifact from that path +- **AND** initialization MUST fail clearly if the path is missing, invalid, or not loadable. + +### Requirement: No artifact generation by DimOS + +DimOS MUST treat VAMP artifact generation as outside the runtime planning backend. + +#### Scenario: unsupported robot requires custom artifact +- **GIVEN** a robot that is not available as an official VAMP artifact +- **WHEN** a user selects VAMP planning for that robot +- **THEN** DimOS MUST require a loadable user-prepared custom artifact +- **AND** DimOS SHALL report that artifact generation must be performed outside DimOS when no loadable artifact is provided. + +### Requirement: VAMP joint-space planning + +The VAMP planner backend MUST support joint-space planning with a configured VAMP planning algorithm. + +#### Scenario: plan between joint states +- **GIVEN** a valid VAMP world and planner configuration +- **AND** a start joint state and goal joint state for the configured robot +- **WHEN** joint-space planning is requested +- **THEN** DimOS SHALL invoke the configured VAMP planning algorithm +- **AND** the result MUST report either a collision-free joint path or a clear planning failure. + +#### Scenario: configure planner algorithm +- **GIVEN** a valid VAMP planner configuration with an algorithm value such as `rrtc`, `prm`, `fcit`, or `aorrtc` +- **WHEN** the planning stack is initialized +- **THEN** DimOS SHALL use that algorithm for VAMP joint-space planning +- **AND** DimOS MUST reject unsupported algorithm values before planning begins. + +### Requirement: VAMP native validation and simplification + +DimOS MUST use VAMP-native validation and simplification behavior only when it is available and configured. + +#### Scenario: validate planned path +- **GIVEN** VAMP path validation is enabled +- **AND** VAMP exposes validation for the planned path or sampled path states +- **WHEN** a VAMP plan is produced +- **THEN** DimOS SHALL validate the path before reporting success +- **AND** DimOS MUST report failure if validation detects an invalid path. + +#### Scenario: unavailable validation capability +- **GIVEN** VAMP path validation is enabled +- **AND** the loaded VAMP artifact does not provide the needed validation capability +- **WHEN** the planning stack or plan result is validated +- **THEN** DimOS MUST fail clearly instead of silently skipping validation. + +### Requirement: VAMP pose planning compatibility + +VAMP pose planning MUST be available only when the configured kinematics backend is compatible with the VAMP world surface. + +#### Scenario: incompatible kinematics backend +- **GIVEN** a VAMP world and planner configuration +- **AND** a configured kinematics backend requires a world capability that the VAMP world does not support +- **WHEN** the planning stack is initialized or pose planning compatibility is checked +- **THEN** DimOS MUST reject the pose-planning combination clearly +- **AND** joint-space VAMP planning SHALL remain the supported initial VAMP planning mode. + +#### Scenario: compatible future kinematics backend +- **GIVEN** a VAMP world and planner configuration +- **AND** a kinematics backend that is compatible with the VAMP world surface +- **WHEN** pose planning is requested +- **THEN** DimOS SHALL convert the target pose to a goal joint state through the configured kinematics backend +- **AND** DimOS SHALL then plan the joint-space path with the configured VAMP planner. + +### Requirement: Unsupported VAMP world capabilities + +DimOS MUST provide a clear unsupported-capability failure when a VAMP world operation is not natively supported. + +#### Scenario: unsupported Jacobian request +- **GIVEN** a VAMP world that does not provide Jacobian support +- **WHEN** a kinematics backend requests the end-effector Jacobian +- **THEN** DimOS MUST raise or surface a clear unsupported-capability error +- **AND** the error MUST identify the incompatible requested capability. + +### Requirement: Optional VAMP dependency behavior + +DimOS MUST keep VAMP dependency loading scoped to VAMP selection. + +#### Scenario: default stack without VAMP installed +- **GIVEN** VAMP is not installed +- **AND** the user selects a non-VAMP planning stack +- **WHEN** the manipulation module starts +- **THEN** DimOS SHALL start without importing VAMP +- **AND** non-VAMP planning behavior SHALL remain available. + +#### Scenario: VAMP selected without dependency +- **GIVEN** VAMP is not installed +- **AND** the user selects a VAMP planning stack +- **WHEN** the planning stack is initialized +- **THEN** DimOS MUST fail with an actionable dependency error +- **AND** the error SHOULD identify the optional dependency or installation path needed for VAMP planning. diff --git a/openspec/changes/add-vamp-planning-backend/tasks.md b/openspec/changes/add-vamp-planning-backend/tasks.md new file mode 100644 index 0000000000..754a194079 --- /dev/null +++ b/openspec/changes/add-vamp-planning-backend/tasks.md @@ -0,0 +1,75 @@ +## 1. Configuration and validation + +- [x] 1.1 Add typed nested manipulation world config with default Drake-compatible behavior and a `backend` discriminator. +- [x] 1.2 Add typed nested manipulation planner config with default RRT-compatible behavior and a `backend` discriminator. +- [x] 1.3 Add VAMP world config variants for official artifact loading and custom user-prepared artifact path loading. +- [x] 1.4 Add VAMP planner config for algorithm selection, path simplification, and path validation behavior. +- [x] 1.5 Update manipulation planning factory wiring to create world and planner backends from typed config objects. +- [x] 1.6 Add planning-stack compatibility validation for VAMP world/planner pairing and incompatible kinematics combinations. +- [x] 1.7 Add warning-backed compatibility shims for migrated pre-existing flat config fields, using `DeprecationWarning` with replacement guidance. +- [x] 1.8 Add tests that nested world/planner/kinematics config parses from dict/CLI override shapes and preserves current defaults. +- [x] 1.9 Add tests that legacy flat config shims preserve behavior and emit deprecation warnings. + +## 2. VAMP backend implementation + +- [x] 2.1 Add optional VAMP dependency wiring in packaging, keeping VAMP imports lazy and backend-scoped. +- [x] 2.2 Add VAMP-specific dependency error type or error path with actionable install guidance. +- [x] 2.3 Implement VAMP artifact loading for official robot artifacts exposed by the installed VAMP package. +- [x] 2.4 Implement VAMP artifact loading for user-prepared custom artifact paths. +- [x] 2.5 Implement the VAMP world adapter surface for native validity, FK/end-effector pose, joint limits when available, and supported environment conversion. +- [x] 2.6 Add a clear unsupported-world-capability error for operations the VAMP world does not natively support, including Jacobian requests. +- [x] 2.7 Implement the VAMP planner adapter for joint-space planning with configured algorithm selection. +- [x] 2.8 Implement configured VAMP path simplification and path validation only through VAMP-native capabilities. +- [x] 2.9 Ensure `VampPlanner` does not perform IK, pose conversion, or Jacobian probing. + +## 3. Manipulation module integration + +- [x] 3.1 Update `WorldMonitor`/planning initialization to pass typed world config and backend-specific options into world creation. +- [x] 3.2 Update `ManipulationModule` planning initialization to use typed world, planner, and kinematics config consistently. +- [x] 3.3 Ensure non-VAMP manipulation stacks do not import VAMP, load VAMP artifacts, or require VAMP dependencies. +- [x] 3.4 Ensure pose-planning entry points fail clearly when VAMP is selected with an incompatible kinematics backend. +- [x] 3.5 Preserve existing Drake/default planning behavior and existing blueprint behavior unless a blueprint explicitly opts into VAMP. + +## 4. Franka Panda mock support + +- [x] 4.1 Add a Franka/Panda robot catalog module with a `franka_panda(...) -> RobotConfig` helper and exported Panda model/FK constants. +- [x] 4.2 Add Panda URDF/SRDF/model resources through the existing repository LFS data package pattern, e.g. a `data/.lfs/.tar.gz` package resolved by `LfsPath`. +- [x] 4.3 Configure the Panda catalog helper with explicit arm joint names, base link, end-effector link, home joints, package paths, LFS-backed model/SRDF paths, and collision exclusions if required by the selected model. +- [x] 4.4 Keep Panda control mock by default through `adapter_type="mock"`; reserve any real Panda adapter selection for explicit future configuration. +- [x] 4.5 Add a mock-control Panda coordinator path for tests/benchmarks, using `RobotConfig.to_hardware_component()` and `RobotConfig.to_task_config()` in the same style as xArm/Piper. +- [x] 4.6 Ensure Panda manipulation planning setup uses `RobotConfig.to_robot_model_config()` and can pair with the official VAMP Panda artifact for joint-space tests. +- [x] 4.7 If a runnable Panda blueprint is added, add it through normal blueprint registration and regenerate `dimos/robot/all_blueprints.py` with the registry test. + +## 5. Tests + +- [x] 5.1 Add factory/config tests for VAMP world/planner creation, invalid pairings, invalid algorithms, and invalid artifact configs. +- [x] 5.2 Add lazy-import tests proving non-VAMP stacks do not require VAMP to be installed. +- [x] 5.3 Add dependency-error tests for selecting VAMP without the optional dependency installed. +- [x] 5.4 Add VAMP world adapter tests using fakes/mocks for official artifact loading, custom artifact loading, supported queries, and unsupported capability errors. +- [x] 5.5 Add VAMP planner adapter tests using fakes/mocks for algorithm dispatch, planning result conversion, simplification, validation, and planning failure reporting. +- [x] 5.6 Add manipulation module wiring tests for nested config propagation and VAMP kinematics compatibility validation. +- [x] 5.7 Add Franka Panda catalog tests covering default mock adapter selection, coordinator joint names, hardware component conversion, manipulation robot model conversion, and LFS-backed URDF/SRDF resolution. +- [x] 5.8 Add a Panda-backed VAMP joint-space planning test or benchmark harness using fakes/mocks where the optional VAMP dependency or official artifact is unavailable. +- [x] 5.9 Add regression tests showing existing Drake/default manipulation tests still pass without VAMP installed. + +## 6. Documentation + +- [x] 6.1 Update user-facing manipulation planning docs with VAMP backend overview, initial joint-space-only scope, official artifact config, and custom artifact path config. +- [x] 6.2 Document that DimOS does not generate VAMP artifacts and that users must prepare custom artifacts outside DimOS. +- [x] 6.3 Document nested CLI override examples for VAMP world artifact settings and VAMP planner algorithm settings. +- [x] 6.4 Document Franka Panda mock-control catalog usage for planning tests and planner benchmarks, including the LFS-backed URDF/SRDF/model asset location. +- [x] 6.5 Document failure modes for missing VAMP dependency, invalid world/planner pairing, invalid artifact config, unsupported world capabilities, incompatible kinematics, and Panda model/artifact mismatch. +- [x] 6.6 Update contributor planning-backend guidance with lazy-import, typed-config, deprecation-warning, unsupported-capability, and mock robot catalog expectations. + +## 7. Verification + +- [x] 7.1 Run `openspec validate add-vamp-planning-backend`. +- [x] 7.2 Run focused manipulation planning config/factory tests. +- [x] 7.3 Run focused VAMP world/planner adapter tests. +- [x] 7.4 Run focused manipulation module wiring tests. +- [x] 7.5 Run focused Franka Panda catalog/coordinator conversion tests. +- [x] 7.6 Run existing default/Drake manipulation tests to verify compatibility. +- [x] 7.7 Run docs validation commands for changed docs, including documented link/snippet validation where applicable. +- [x] 7.8 Manually QA a VAMP joint-space planning flow through the manipulation module user surface using an official Panda VAMP artifact or a fake artifact-backed Panda test harness. +- [x] 7.9 Manually QA that selecting an incompatible VAMP pose-planning stack fails clearly before executing robot motion. +- [x] 7.10 If new blueprints are added or renamed, run `pytest dimos/robot/test_all_blueprints_generation.py` and commit generated registry changes. diff --git a/pyproject.toml b/pyproject.toml index 55e7ffcb73..0edb934b42 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -275,6 +275,12 @@ manipulation = [ "pyyaml>=6.0", ] +vamp = [ + # Optional VAMP planning backend. Keep out of broad extras to avoid forcing + # native planner bindings into non-VAMP manipulation environments. + "vamp-planner @ git+https://github.com/TomCC7/vamp.git@2b59552179c2af473c59c8617519e4663497fbe6", +] + cpu = [ # CPU inference backends "onnxruntime", diff --git a/uv.lock b/uv.lock index 0736ec2961..cfda49fe11 100644 --- a/uv.lock +++ b/uv.lock @@ -2171,6 +2171,9 @@ unitree-dds = [ { name = "unitree-webrtc-connect-leshy" }, { name = "uvicorn" }, ] +vamp = [ + { name = "vamp-planner" }, +] visualization = [ { name = "dimos-viewer" }, { name = "rerun-sdk" }, @@ -2455,13 +2458,14 @@ requires-dist = [ { name = "unitree-sdk2py-dimos", marker = "extra == 'unitree-dds'", specifier = ">=1.0.2" }, { name = "unitree-webrtc-connect-leshy", marker = "extra == 'unitree'", specifier = ">=2.0.7" }, { name = "uvicorn", marker = "extra == 'web'", specifier = ">=0.34.0" }, + { name = "vamp-planner", marker = "extra == 'vamp'", git = "https://github.com/TomCC7/vamp.git?rev=2b59552179c2af473c59c8617519e4663497fbe6" }, { name = "xacro", marker = "extra == 'manipulation'" }, { name = "xarm-python-sdk", marker = "extra == 'manipulation'", specifier = ">=1.17.0" }, { name = "xarm-python-sdk", marker = "extra == 'misc'", specifier = ">=1.17.0" }, { name = "xformers", marker = "platform_machine == 'x86_64' and extra == 'cuda'", specifier = ">=0.0.20" }, { name = "yapf", marker = "extra == 'misc'", specifier = "==0.40.2" }, ] -provides-extras = ["misc", "visualization", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "cpu", "cuda", "psql", "sim", "mapping", "drone", "dds", "base", "apriltag", "all"] +provides-extras = ["misc", "visualization", "agents", "web", "perception", "unitree", "unitree-dds", "manipulation", "vamp", "cpu", "cuda", "psql", "sim", "mapping", "drone", "dds", "base", "apriltag", "all"] [package.metadata.requires-dev] autofix = [{ name = "ruff", specifier = "==0.14.3" }] @@ -11175,6 +11179,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/e4/16/c1fd27e9549f3c4baf1dc9c20c456cd2f822dbf8de9f463824b0c0357e06/uvloop-0.22.1-cp314-cp314t-musllinux_1_2_x86_64.whl", hash = "sha256:6cde23eeda1a25c75b2e07d39970f3374105d5eafbaab2a4482be82f272d5a5e", size = 4296730, upload-time = "2025-10-16T22:17:00.744Z" }, ] +[[package]] +name = "vamp-planner" +version = "0.6.4" +source = { git = "https://github.com/TomCC7/vamp.git?rev=2b59552179c2af473c59c8617519e4663497fbe6#2b59552179c2af473c59c8617519e4663497fbe6" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, +] + [[package]] name = "virtualenv" version = "20.36.1" From cbb59b20ce7da49c136b41d6d3d8117c5ad4f2c6 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 13:17:52 -0700 Subject: [PATCH 04/20] fix(manipulation): encapsulate VAMP planner state --- .../manipulation/planning/planners/config.py | 12 +- .../planning/planners/vamp_planner.py | 124 +-------------- dimos/manipulation/planning/world/config.py | 10 +- .../manipulation/planning/world/vamp_world.py | 146 +++++++++++++++--- 4 files changed, 139 insertions(+), 153 deletions(-) diff --git a/dimos/manipulation/planning/planners/config.py b/dimos/manipulation/planning/planners/config.py index 915509e0ea..880ad777bd 100644 --- a/dimos/manipulation/planning/planners/config.py +++ b/dimos/manipulation/planning/planners/config.py @@ -18,7 +18,7 @@ from typing import Annotated, Literal -from pydantic import Field +from pydantic import Field, TypeAdapter from dimos.protocol.service.spec import BaseConfig @@ -47,14 +47,14 @@ class VampPlannerConfig(BaseConfig): Field(discriminator="backend"), ] +_PLANNER_CONFIG_ADAPTER: TypeAdapter[ManipulationPlannerConfig] = TypeAdapter( + ManipulationPlannerConfig +) + def planner_config_from_name(name: str) -> ManipulationPlannerConfig: """Create a default planner config from a legacy planner name.""" - if name == "rrt_connect": - return RRTConnectPlannerConfig() - if name == "vamp": - return VampPlannerConfig() - raise ValueError(f"Unknown planner: {name}. Available: ['rrt_connect', 'vamp']") + return _PLANNER_CONFIG_ADAPTER.validate_python({"backend": name}) __all__ = [ diff --git a/dimos/manipulation/planning/planners/vamp_planner.py b/dimos/manipulation/planning/planners/vamp_planner.py index 14f9124de8..5acc7c8938 100644 --- a/dimos/manipulation/planning/planners/vamp_planner.py +++ b/dimos/manipulation/planning/planners/vamp_planner.py @@ -16,14 +16,7 @@ from __future__ import annotations -from itertools import pairwise -import time -from typing import Any - -import numpy as np - from dimos.manipulation.planning.planners.config import VampPlannerConfig -from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import PlanningResult, WorldRobotID from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.world.vamp_world import VampWorld @@ -45,125 +38,10 @@ def plan_joint_path( timeout: float = 10.0, ) -> PlanningResult: """Plan a VAMP-native joint-space path.""" - start_time = time.time() if not isinstance(world, VampWorld): raise ValueError("VampPlanner requires VampWorld") - if not world.is_finalized: - return _failure(PlanningStatus.NO_SOLUTION, "World must be finalized before planning") - if robot_id not in world.get_robot_ids(): - return _failure(PlanningStatus.NO_SOLUTION, f"Robot '{robot_id}' not found") - - if not world.check_config_collision_free(robot_id, start): - return _failure(PlanningStatus.COLLISION_AT_START, "Start configuration is invalid") - if not world.check_config_collision_free(robot_id, goal): - return _failure(PlanningStatus.COLLISION_AT_GOAL, "Goal configuration is invalid") - - robot_name = _robot_name(world) - robot_module, planner_func, plan_settings, simplify_settings = ( - world.vamp_module.configure_robot_and_planner_with_kwargs( - robot_name, - self.config.algorithm, - max_iterations=_timeout_to_iteration_budget(timeout), - ) - ) - sampler = robot_module.halton() - result = planner_func( - list(start.position), - list(goal.position), - world.environment, - plan_settings, - sampler, - ) - if not bool(getattr(result, "solved", False)): - return _failure( - PlanningStatus.NO_SOLUTION, - "VAMP planner did not find a path", - planning_time=time.time() - start_time, - iterations=int(getattr(result, "iterations", 0)), - ) - - path_source = result.path - if self.config.simplify: - simplified = robot_module.simplify( - path_source, world.environment, simplify_settings, sampler - ) - if bool(getattr(simplified, "solved", True)): - path_source = simplified.path - - path = _path_to_joint_states( - path_source, start.name or world.get_robot_config(robot_id).joint_names - ) - if self.config.validate_path and not _validate_path(world, robot_id, path): - return _failure( - PlanningStatus.NO_SOLUTION, - "VAMP returned a path that failed native validation", - planning_time=time.time() - start_time, - ) - return PlanningResult( - status=PlanningStatus.SUCCESS, - path=path, - planning_time=time.time() - start_time, - path_length=_path_length(path), - iterations=int(getattr(result, "iterations", 0)), - message="VAMP planning succeeded", - ) + return world.plan_joint_path(self.config, robot_id, start, goal, timeout=timeout) def get_name(self) -> str: """Get planner name.""" return f"VAMP/{self.config.algorithm}" - - -def _robot_name(world: VampWorld) -> str: - artifact = world.config.artifact - robot = getattr(artifact, "robot", None) - if isinstance(robot, str): - return robot - return world.robot_module.__name__.split(".")[-1] - - -def _timeout_to_iteration_budget(timeout: float) -> int: - return max(1, int(timeout * 1000)) - - -def _path_to_joint_states(path_source: Any, joint_names: list[str]) -> list[JointState]: - path_array = _path_to_array(path_source) - return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] - - -def _path_to_array(path_source: Any) -> np.ndarray: - if hasattr(path_source, "numpy"): - return np.asarray(path_source.numpy(), dtype=np.float64) - return np.asarray(path_source, dtype=np.float64) - - -def _validate_path(world: VampWorld, robot_id: WorldRobotID, path: list[JointState]) -> bool: - if not path: - return False - return all( - world.check_edge_collision_free(robot_id, before, after) for before, after in pairwise(path) - ) - - -def _path_length(path: list[JointState]) -> float: - if len(path) < 2: - return 0.0 - total = 0.0 - for before, after in pairwise(path): - q_before = np.array(before.position, dtype=np.float64) - q_after = np.array(after.position, dtype=np.float64) - total += float(np.linalg.norm(q_after - q_before)) - return total - - -def _failure( - status: PlanningStatus, - message: str, - planning_time: float = 0.0, - iterations: int = 0, -) -> PlanningResult: - return PlanningResult( - status=status, - planning_time=planning_time, - iterations=iterations, - message=message, - ) diff --git a/dimos/manipulation/planning/world/config.py b/dimos/manipulation/planning/world/config.py index 2b341509f4..57605b7fe3 100644 --- a/dimos/manipulation/planning/world/config.py +++ b/dimos/manipulation/planning/world/config.py @@ -19,7 +19,7 @@ from pathlib import Path from typing import Annotated, Literal -from pydantic import Field +from pydantic import Field, TypeAdapter from dimos.protocol.service.spec import BaseConfig @@ -62,14 +62,12 @@ class VampWorldConfig(BaseConfig): Field(discriminator="backend"), ] +_WORLD_CONFIG_ADAPTER: TypeAdapter[ManipulationWorldConfig] = TypeAdapter(ManipulationWorldConfig) + def world_config_from_name(name: str) -> ManipulationWorldConfig: """Create a default world config from a backend name.""" - if name == "drake": - return DrakeWorldConfig() - if name == "vamp": - return VampWorldConfig() - raise ValueError(f"Unknown backend: {name}. Available: ['drake', 'vamp']") + return _WORLD_CONFIG_ADAPTER.validate_python({"backend": name}) __all__ = [ diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index cf3c071970..f541057da9 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -19,15 +19,17 @@ from contextlib import contextmanager from copy import deepcopy from dataclasses import dataclass -from types import ModuleType +from itertools import pairwise +import time from typing import TYPE_CHECKING, Any import numpy as np from scipy.spatial.transform import Rotation as R +from dimos.manipulation.planning.planners.config import VampPlannerConfig from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.enums import ObstacleType -from dimos.manipulation.planning.spec.models import Obstacle, WorldRobotID +from dimos.manipulation.planning.spec.enums import ObstacleType, PlanningStatus +from dimos.manipulation.planning.spec.models import Obstacle, PlanningResult, WorldRobotID from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError from dimos.manipulation.planning.vamp.loader import load_vamp_robot_module @@ -60,21 +62,6 @@ def __init__(self, config: VampWorldConfig) -> None: self._robot_counter = 0 self._finalized = False - @property - def vamp_module(self) -> ModuleType: - """The imported VAMP package module.""" - return self._vamp_module - - @property - def robot_module(self) -> ModuleType: - """The loaded VAMP robot module for this world.""" - return self._robot_module - - @property - def environment(self) -> Any: - """The current VAMP environment object.""" - return self._environment - def add_robot(self, config: RobotModelConfig) -> WorldRobotID: """Add a robot to the VAMP world.""" if self._finalized: @@ -231,12 +218,95 @@ def get_jacobian(self, ctx: _VampContext, robot_id: WorldRobotID) -> NDArray[np. """VAMP's Python API does not expose a Jacobian.""" raise UnsupportedWorldCapabilityError("vamp", "end-effector Jacobian") + def plan_joint_path( + self, + planner_config: VampPlannerConfig, + robot_id: WorldRobotID, + start: JointState, + goal: JointState, + timeout: float = 10.0, + ) -> PlanningResult: + """Plan a VAMP-native joint-space path inside the VAMP world adapter.""" + start_time = time.time() + if not self.is_finalized: + return _failure(PlanningStatus.NO_SOLUTION, "World must be finalized before planning") + if robot_id not in self.get_robot_ids(): + return _failure(PlanningStatus.NO_SOLUTION, f"Robot '{robot_id}' not found") + + if not self.check_config_collision_free(robot_id, start): + return _failure(PlanningStatus.COLLISION_AT_START, "Start configuration is invalid") + if not self.check_config_collision_free(robot_id, goal): + return _failure(PlanningStatus.COLLISION_AT_GOAL, "Goal configuration is invalid") + + robot_module, planner_func, plan_settings, simplify_settings = ( + self._vamp_module.configure_robot_and_planner_with_kwargs( + self._robot_name(), + planner_config.algorithm, + max_iterations=_timeout_to_iteration_budget(timeout), + ) + ) + sampler = robot_module.halton() + result = planner_func( + list(start.position), + list(goal.position), + self._environment, + plan_settings, + sampler, + ) + if not bool(getattr(result, "solved", False)): + return _failure( + PlanningStatus.NO_SOLUTION, + "VAMP planner did not find a path", + planning_time=time.time() - start_time, + iterations=int(getattr(result, "iterations", 0)), + ) + + path_source = result.path + if planner_config.simplify: + simplified = robot_module.simplify( + path_source, self._environment, simplify_settings, sampler + ) + if bool(getattr(simplified, "solved", True)): + path_source = simplified.path + + path = _path_to_joint_states( + path_source, start.name or self.get_robot_config(robot_id).joint_names + ) + if planner_config.validate_path and not self._validate_path(robot_id, path): + return _failure( + PlanningStatus.NO_SOLUTION, + "VAMP returned a path that failed native validation", + planning_time=time.time() - start_time, + ) + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + planning_time=time.time() - start_time, + path_length=_path_length(path), + iterations=int(getattr(result, "iterations", 0)), + message="VAMP planning succeeded", + ) + def _normalize_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) -> JointState: config = self._robots[robot_id] positions = list(joint_state.position[: len(config.joint_names)]) names = list(joint_state.name[: len(positions)]) if joint_state.name else config.joint_names return JointState(name=names, position=positions) + def _robot_name(self) -> str: + robot = getattr(self.config.artifact, "robot", None) + if isinstance(robot, str): + return robot + return self._robot_module.__name__.split(".")[-1] + + def _validate_path(self, robot_id: WorldRobotID, path: list[JointState]) -> bool: + if not path: + return False + return all( + self.check_edge_collision_free(robot_id, before, after) + for before, after in pairwise(path) + ) + def _validate_state(self, joint_state: JointState, check_bounds: bool) -> bool: return bool( self._robot_module.validate( @@ -281,3 +351,43 @@ def _add_obstacle_to_environment(self, obstacle: Obstacle) -> None: ) else: raise UnsupportedWorldCapabilityError("vamp", f"{obstacle.obstacle_type.name} obstacle") + + +def _timeout_to_iteration_budget(timeout: float) -> int: + return max(1, int(timeout * 1000)) + + +def _path_to_joint_states(path_source: Any, joint_names: list[str]) -> list[JointState]: + path_array = _path_to_array(path_source) + return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] + + +def _path_to_array(path_source: Any) -> np.ndarray: + if hasattr(path_source, "numpy"): + return np.asarray(path_source.numpy(), dtype=np.float64) + return np.asarray(path_source, dtype=np.float64) + + +def _path_length(path: list[JointState]) -> float: + if len(path) < 2: + return 0.0 + total = 0.0 + for before, after in pairwise(path): + q_before = np.array(before.position, dtype=np.float64) + q_after = np.array(after.position, dtype=np.float64) + total += float(np.linalg.norm(q_after - q_before)) + return total + + +def _failure( + status: PlanningStatus, + message: str, + planning_time: float = 0.0, + iterations: int = 0, +) -> PlanningResult: + return PlanningResult( + status=status, + planning_time=planning_time, + iterations=iterations, + message=message, + ) From 1351162bf89433620f54aed71951a78dfab32cbe Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 13:29:11 -0700 Subject: [PATCH 05/20] fix(manipulation): remove backend config helper functions --- dimos/manipulation/manipulation_module.py | 6 ++++-- dimos/manipulation/planning/factory.py | 14 +++++++++----- .../manipulation/planning/monitor/world_monitor.py | 11 +++++++++-- dimos/manipulation/planning/planners/config.py | 9 ++------- dimos/manipulation/planning/world/config.py | 11 ++++------- 5 files changed, 28 insertions(+), 23 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 3c01048603..bd1133535c 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -51,9 +51,9 @@ ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.planners.config import ( + MANIPULATION_PLANNER_CONFIG_ADAPTER, ManipulationPlannerConfig, RRTConnectPlannerConfig, - planner_config_from_name, ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType @@ -136,7 +136,9 @@ def apply_legacy_flat_backend_fields(self) -> Self: DeprecationWarning, stacklevel=3, ) - self.planner = planner_config_from_name(self.planner_name) + self.planner = MANIPULATION_PLANNER_CONFIG_ADAPTER.validate_python( + {"backend": self.planner_name} + ) if self.kinematics_name is not None: warnings.warn( "ManipulationModuleConfig.kinematics_name is deprecated; use " diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index 8dbc092657..7a1b62bff4 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -26,16 +26,16 @@ kinematics_config_from_name, ) from dimos.manipulation.planning.planners.config import ( + MANIPULATION_PLANNER_CONFIG_ADAPTER, ManipulationPlannerConfig, RRTConnectPlannerConfig, VampPlannerConfig, - planner_config_from_name, ) from dimos.manipulation.planning.world.config import ( + MANIPULATION_WORLD_CONFIG_ADAPTER, DrakeWorldConfig, ManipulationWorldConfig, VampWorldConfig, - world_config_from_name, ) if TYPE_CHECKING: @@ -50,7 +50,7 @@ def create_world( ) -> WorldSpec: """Create a world instance from a backend name or typed world config.""" if config is None: - config = world_config_from_name(backend) + config = MANIPULATION_WORLD_CONFIG_ADAPTER.validate_python({"backend": backend}) if isinstance(config, DrakeWorldConfig): from dimos.manipulation.planning.world.drake_world import DrakeWorld @@ -97,7 +97,7 @@ def create_planner( ) -> PlannerSpec: """Create motion planner from a backend name or typed planner config.""" if config is None: - config = planner_config_from_name(name) + config = MANIPULATION_PLANNER_CONFIG_ADAPTER.validate_python({"backend": name}) if isinstance(config, RRTConnectPlannerConfig): from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner @@ -143,7 +143,11 @@ def create_planning_stack( ) -> tuple[WorldSpec, KinematicsSpec, PlannerSpec, str]: """Create complete planning stack. Returns (world, kinematics, planner, robot_id).""" world_config = world if world is not None else DrakeWorldConfig() - planner_config = planner if planner is not None else planner_config_from_name(planner_name) + planner_config = ( + planner + if planner is not None + else MANIPULATION_PLANNER_CONFIG_ADAPTER.validate_python({"backend": planner_name}) + ) kinematics_config = ( kinematics if kinematics is not None else kinematics_config_from_name(kinematics_name) ) diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index c8a1ee20fc..90eff53f31 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -25,7 +25,10 @@ from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor from dimos.manipulation.planning.spec.protocols import VisualizationSpec -from dimos.manipulation.planning.world.config import ManipulationWorldConfig, world_config_from_name +from dimos.manipulation.planning.world.config import ( + MANIPULATION_WORLD_CONFIG_ADAPTER, + ManipulationWorldConfig, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -60,7 +63,11 @@ def __init__( enable_viz: bool = False, **kwargs: Any, ) -> None: - world_config = config if config is not None else world_config_from_name(backend) + world_config = ( + config + if config is not None + else MANIPULATION_WORLD_CONFIG_ADAPTER.validate_python({"backend": backend}) + ) self._backend = world_config.backend self._world: WorldSpec = create_world(config=world_config, enable_viz=enable_viz, **kwargs) self._visualization: VisualizationSpec | None = ( diff --git a/dimos/manipulation/planning/planners/config.py b/dimos/manipulation/planning/planners/config.py index 880ad777bd..3b83357d0f 100644 --- a/dimos/manipulation/planning/planners/config.py +++ b/dimos/manipulation/planning/planners/config.py @@ -47,19 +47,14 @@ class VampPlannerConfig(BaseConfig): Field(discriminator="backend"), ] -_PLANNER_CONFIG_ADAPTER: TypeAdapter[ManipulationPlannerConfig] = TypeAdapter( +MANIPULATION_PLANNER_CONFIG_ADAPTER: TypeAdapter[ManipulationPlannerConfig] = TypeAdapter( ManipulationPlannerConfig ) -def planner_config_from_name(name: str) -> ManipulationPlannerConfig: - """Create a default planner config from a legacy planner name.""" - return _PLANNER_CONFIG_ADAPTER.validate_python({"backend": name}) - - __all__ = [ + "MANIPULATION_PLANNER_CONFIG_ADAPTER", "ManipulationPlannerConfig", "RRTConnectPlannerConfig", "VampPlannerConfig", - "planner_config_from_name", ] diff --git a/dimos/manipulation/planning/world/config.py b/dimos/manipulation/planning/world/config.py index 57605b7fe3..d0b320bff6 100644 --- a/dimos/manipulation/planning/world/config.py +++ b/dimos/manipulation/planning/world/config.py @@ -62,20 +62,17 @@ class VampWorldConfig(BaseConfig): Field(discriminator="backend"), ] -_WORLD_CONFIG_ADAPTER: TypeAdapter[ManipulationWorldConfig] = TypeAdapter(ManipulationWorldConfig) - - -def world_config_from_name(name: str) -> ManipulationWorldConfig: - """Create a default world config from a backend name.""" - return _WORLD_CONFIG_ADAPTER.validate_python({"backend": name}) +MANIPULATION_WORLD_CONFIG_ADAPTER: TypeAdapter[ManipulationWorldConfig] = TypeAdapter( + ManipulationWorldConfig +) __all__ = [ + "MANIPULATION_WORLD_CONFIG_ADAPTER", "CustomVampArtifactConfig", "DrakeWorldConfig", "ManipulationWorldConfig", "OfficialVampArtifactConfig", "VampArtifactConfig", "VampWorldConfig", - "world_config_from_name", ] From 8c7744d4a2cfef929b29e4afbc905bd452e0f694 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 21:17:01 -0700 Subject: [PATCH 06/20] fix(manipulation): address VAMP review feedback --- dimos/manipulation/planning/vamp/__init__.py | 22 ---- dimos/manipulation/planning/vamp/loader.py | 38 +++--- dimos/manipulation/planning/vamp/protocols.py | 108 ++++++++++++++++++ .../planning/vamp/test_vamp_backend.py | 18 ++- dimos/manipulation/planning/vamp/utils.py | 42 +++++++ .../manipulation/planning/world/vamp_world.py | 101 ++++++++-------- dimos/robot/catalog/franka.py | 6 - dimos/robot/catalog/test_franka.py | 6 - dimos/robot/config.py | 7 -- 9 files changed, 230 insertions(+), 118 deletions(-) delete mode 100644 dimos/manipulation/planning/vamp/__init__.py create mode 100644 dimos/manipulation/planning/vamp/protocols.py create mode 100644 dimos/manipulation/planning/vamp/utils.py diff --git a/dimos/manipulation/planning/vamp/__init__.py b/dimos/manipulation/planning/vamp/__init__.py deleted file mode 100644 index b4b6858df7..0000000000 --- a/dimos/manipulation/planning/vamp/__init__.py +++ /dev/null @@ -1,22 +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. - -"""Helpers for the optional VAMP manipulation planning backend.""" - -from dimos.manipulation.planning.vamp.errors import ( - UnsupportedWorldCapabilityError, - VampDependencyError, -) - -__all__ = ["UnsupportedWorldCapabilityError", "VampDependencyError"] diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py index 1f933b86b4..5b6c3a704a 100644 --- a/dimos/manipulation/planning/vamp/loader.py +++ b/dimos/manipulation/planning/vamp/loader.py @@ -20,27 +20,29 @@ import importlib.util from pathlib import Path import sys -from types import ModuleType +from typing import cast from dimos.manipulation.planning.vamp.errors import VampDependencyError +from dimos.manipulation.planning.vamp.protocols import VampModuleProtocol, VampRobotModuleProtocol from dimos.manipulation.planning.world.config import ( CustomVampArtifactConfig, OfficialVampArtifactConfig, VampArtifactConfig, ) - -def import_vamp() -> ModuleType: - """Import the optional VAMP package only when a VAMP backend is selected.""" - try: - return importlib.import_module("vamp") - except ImportError as exc: - raise VampDependencyError() from exc +try: + import vamp as _vamp_module +except ImportError: + _vamp_module = None -def load_vamp_robot_module(artifact: VampArtifactConfig) -> tuple[ModuleType, ModuleType]: +def load_vamp_robot_module( + artifact: VampArtifactConfig, +) -> tuple[VampModuleProtocol, VampRobotModuleProtocol]: """Load the VAMP package and configured robot module.""" - vamp_module = import_vamp() + if _vamp_module is None: + raise VampDependencyError() + vamp_module = cast("VampModuleProtocol", _vamp_module) if isinstance(artifact, OfficialVampArtifactConfig): return vamp_module, _load_official_robot_module(vamp_module, artifact.robot) if isinstance(artifact, CustomVampArtifactConfig): @@ -48,20 +50,20 @@ def load_vamp_robot_module(artifact: VampArtifactConfig) -> tuple[ModuleType, Mo raise TypeError(f"Unsupported VAMP artifact config: {type(artifact).__name__}") -def _load_official_robot_module(vamp_module: ModuleType, robot: str) -> ModuleType: - robot_module = getattr(vamp_module, robot, None) - if isinstance(robot_module, ModuleType): - return robot_module +def _load_official_robot_module( + vamp_module: VampModuleProtocol, robot: str +) -> VampRobotModuleProtocol: + del vamp_module try: imported = importlib.import_module(f"vamp.{robot}") except ImportError as exc: raise ValueError( f"Installed VAMP package does not expose robot artifact '{robot}'" ) from exc - return imported + return cast("VampRobotModuleProtocol", imported) -def _load_custom_robot_module(path: Path) -> ModuleType: +def _load_custom_robot_module(path: Path) -> VampRobotModuleProtocol: artifact_path = path.expanduser().resolve() if not artifact_path.exists(): raise FileNotFoundError(f"VAMP custom artifact path does not exist: {artifact_path}") @@ -70,7 +72,7 @@ def _load_custom_robot_module(path: Path) -> ModuleType: parent = str(artifact_path.parent) if parent not in sys.path: sys.path.insert(0, parent) - return importlib.import_module(artifact_path.name) + return cast("VampRobotModuleProtocol", importlib.import_module(artifact_path.name)) module_name = artifact_path.stem spec = importlib.util.spec_from_file_location(module_name, artifact_path) @@ -79,4 +81,4 @@ def _load_custom_robot_module(path: Path) -> ModuleType: module = importlib.util.module_from_spec(spec) sys.modules[module_name] = module spec.loader.exec_module(module) - return module + return cast("VampRobotModuleProtocol", module) diff --git a/dimos/manipulation/planning/vamp/protocols.py b/dimos/manipulation/planning/vamp/protocols.py new file mode 100644 index 0000000000..d3f8d9b9db --- /dev/null +++ b/dimos/manipulation/planning/vamp/protocols.py @@ -0,0 +1,108 @@ +# 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. + +"""Protocols for the optional VAMP Python bindings.""" + +from __future__ import annotations + +from collections.abc import Callable, Sequence +from typing import Protocol + +import numpy as np +from numpy.typing import NDArray + + +class VampPathProtocol(Protocol): + """Path object returned by VAMP bindings.""" + + def numpy(self) -> NDArray[np.float64]: + """Return path waypoints as an array.""" + ... + + +class VampPlanningResultProtocol(Protocol): + """Planning or simplification result returned by VAMP bindings.""" + + solved: bool + path: object + iterations: int + + +class VampEnvironmentProtocol(Protocol): + """VAMP collision environment.""" + + def add_sphere(self, sphere: object) -> None: ... + + def add_cuboid(self, cuboid: object) -> None: ... + + def add_capsule(self, capsule: object) -> None: ... + + +VampPlannerFunction = Callable[ + [Sequence[float], Sequence[float], VampEnvironmentProtocol, object, object], + VampPlanningResultProtocol, +] + + +class VampRobotModuleProtocol(Protocol): + """Robot-specific VAMP module such as ``vamp.panda``.""" + + __name__: str + + def halton(self) -> object: ... + + def validate( + self, + configuration: Sequence[float], + environment: VampEnvironmentProtocol, + check_bounds: bool, + ) -> bool: ... + + def validate_motion( + self, + configuration_in: Sequence[float], + configuration_out: Sequence[float], + environment: VampEnvironmentProtocol, + check_bounds: bool, + ) -> bool: ... + + def eefk(self, configuration: Sequence[float]) -> NDArray[np.float64]: ... + + def simplify( + self, + path: object, + environment: VampEnvironmentProtocol, + settings: object, + sampler: object, + ) -> VampPlanningResultProtocol: ... + + +class VampModuleProtocol(Protocol): + """Top-level VAMP module.""" + + def Environment(self) -> VampEnvironmentProtocol: ... + + def Sphere(self, center: Sequence[float], radius: float) -> object: ... + + def Cuboid( + self, center: Sequence[float], euler_xyz: Sequence[float], half_extents: Sequence[float] + ) -> object: ... + + def Cylinder( + self, center: Sequence[float], euler_xyz: Sequence[float], radius: float, length: float + ) -> object: ... + + def configure_robot_and_planner_with_kwargs( + self, robot_name: str, planner_name: str, max_iterations: int + ) -> tuple[VampRobotModuleProtocol, VampPlannerFunction, object, object]: ... diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index 23b2801a83..de74233d41 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -19,6 +19,7 @@ from pathlib import Path import sys from types import ModuleType, SimpleNamespace +from typing import cast import numpy as np from pydantic import ValidationError @@ -33,7 +34,7 @@ UnsupportedWorldCapabilityError, VampDependencyError, ) -from dimos.manipulation.planning.vamp.loader import import_vamp, load_vamp_robot_module +from dimos.manipulation.planning.vamp.loader import load_vamp_robot_module from dimos.manipulation.planning.world.config import ( CustomVampArtifactConfig, OfficialVampArtifactConfig, @@ -177,6 +178,7 @@ def fake_vamp_modules(mocker) -> tuple[FakeVampModule, FakeRobotModule]: robot_module = FakeRobotModule() vamp_module = FakeVampModule(robot_module) mocker.patch.dict(sys.modules, {"vamp": vamp_module, "vamp.panda": robot_module}) + mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", vamp_module) return vamp_module, robot_module @@ -205,21 +207,15 @@ def finalized_vamp_world() -> VampWorld: def test_vamp_dependency_error_has_install_guidance(mocker) -> None: """Selecting VAMP without the optional package raises an actionable error.""" - mocker.patch( - "dimos.manipulation.planning.vamp.loader.importlib.import_module", - side_effect=ImportError("missing vamp"), - ) + mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", None) with pytest.raises(VampDependencyError, match="vamp-planner"): - import_vamp() + load_vamp_robot_module(OfficialVampArtifactConfig(robot="panda")) def test_non_vamp_planner_creation_does_not_import_vamp(mocker) -> None: """Default planner creation stays independent of the optional VAMP package.""" - mocker.patch( - "dimos.manipulation.planning.vamp.loader.import_vamp", - side_effect=AssertionError("VAMP import should stay lazy"), - ) + mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", None) planner = create_planner(config=RRTConnectPlannerConfig()) @@ -253,7 +249,7 @@ def test_custom_vamp_artifact_loading_uses_explicit_module_path( loaded_vamp, loaded_robot = load_vamp_robot_module(CustomVampArtifactConfig(path=artifact_path)) assert loaded_vamp is vamp_module - assert loaded_robot.ROBOT_NAME == "custom_panda" + assert cast("ModuleType", loaded_robot).ROBOT_NAME == "custom_panda" def test_create_world_and_planner_from_vamp_configs(fake_vamp_modules) -> None: diff --git a/dimos/manipulation/planning/vamp/utils.py b/dimos/manipulation/planning/vamp/utils.py new file mode 100644 index 0000000000..c1584f3992 --- /dev/null +++ b/dimos/manipulation/planning/vamp/utils.py @@ -0,0 +1,42 @@ +# 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. + +"""Utilities for adapting VAMP binding values to DimOS models.""" + +from __future__ import annotations + +from typing import TypeGuard + +import numpy as np +from numpy.typing import NDArray + +from dimos.manipulation.planning.vamp.protocols import VampPathProtocol +from dimos.msgs.sensor_msgs.JointState import JointState + + +def path_to_joint_states(path_source: object, joint_names: list[str]) -> list[JointState]: + """Convert a VAMP path object or numeric waypoint array into joint states.""" + path_array = path_to_array(path_source) + return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] + + +def path_to_array(path_source: object) -> NDArray[np.float64]: + """Convert a VAMP path object or sequence into a float waypoint array.""" + if _has_numpy(path_source): + return np.asarray(path_source.numpy(), dtype=np.float64) + return np.asarray(path_source, dtype=np.float64) + + +def _has_numpy(path_source: object) -> TypeGuard[VampPathProtocol]: + return hasattr(path_source, "numpy") diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index f541057da9..699e8da09c 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -21,7 +21,7 @@ from dataclasses import dataclass from itertools import pairwise import time -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Protocol import numpy as np from scipy.spatial.transform import Rotation as R @@ -31,8 +31,10 @@ from dimos.manipulation.planning.spec.enums import ObstacleType, PlanningStatus from dimos.manipulation.planning.spec.models import Obstacle, PlanningResult, WorldRobotID from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.utils.path_utils import compute_path_length from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError from dimos.manipulation.planning.vamp.loader import load_vamp_robot_module +from dimos.manipulation.planning.vamp.utils import path_to_joint_states from dimos.manipulation.planning.world.config import VampWorldConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -44,9 +46,18 @@ from numpy.typing import NDArray +class _VampContext(Protocol): + """Typed context shape used by the VAMP world adapter.""" + + # TODO: Replace the loose WorldSpec context object with a typed ContextProtocol. + joint_state: JointState + + @dataclass -class _VampContext: - joint_states: dict[WorldRobotID, JointState] +class _SingleRobotVampContext(_VampContext): + """Concrete VAMP context for the current single-robot backend.""" + + joint_state: JointState class VampWorld(WorldSpec): @@ -57,7 +68,7 @@ def __init__(self, config: VampWorldConfig) -> None: self._vamp_module, self._robot_module = load_vamp_robot_module(config.artifact) self._environment = self._vamp_module.Environment() self._robots: dict[WorldRobotID, RobotModelConfig] = {} - self._live_joint_states: dict[WorldRobotID, JointState] = {} + self._live_joint_state: JointState | None = None self._obstacles: dict[str, Obstacle] = {} self._robot_counter = 0 self._finalized = False @@ -72,7 +83,7 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: robot_id = f"robot_{self._robot_counter}" self._robots[robot_id] = config home_positions = config.home_joints or [0.0] * len(config.joint_names) - self._live_joint_states[robot_id] = JointState( + self._live_joint_state = JointState( name=config.joint_names, position=home_positions, ) @@ -141,30 +152,34 @@ def is_finalized(self) -> bool: def get_live_context(self) -> _VampContext: """Get the live VAMP context.""" - return _VampContext(self._live_joint_states) + return _SingleRobotVampContext(self._require_live_joint_state()) @contextmanager def scratch_context(self) -> Generator[_VampContext, None, None]: """Get a scratch context with copied joint states.""" - yield _VampContext(deepcopy(self._live_joint_states)) + yield _SingleRobotVampContext(deepcopy(self._require_live_joint_state())) def sync_from_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) -> None: """Sync live state from a joint-state message.""" - self._live_joint_states[robot_id] = self._normalize_joint_state(robot_id, joint_state) + self._assert_robot_id(robot_id) + self._live_joint_state = self._joint_state_for_robot_order(robot_id, joint_state) def set_joint_state( self, ctx: _VampContext, robot_id: WorldRobotID, joint_state: JointState ) -> None: """Set robot joint state in a context.""" - ctx.joint_states[robot_id] = self._normalize_joint_state(robot_id, joint_state) + self._assert_robot_id(robot_id) + ctx.joint_state = self._joint_state_for_robot_order(robot_id, joint_state) def get_joint_state(self, ctx: _VampContext, robot_id: WorldRobotID) -> JointState: """Get robot joint state from a context.""" - return ctx.joint_states[robot_id] + self._assert_robot_id(robot_id) + return ctx.joint_state def is_collision_free(self, ctx: _VampContext, robot_id: WorldRobotID) -> bool: """Check if current configuration is valid according to VAMP.""" - return self._validate_state(ctx.joint_states[robot_id], check_bounds=True) + self._assert_robot_id(robot_id) + return self._validate_state(ctx.joint_state, check_bounds=True) def get_min_distance(self, ctx: _VampContext, robot_id: WorldRobotID) -> float: """Minimum distance is not exposed by VAMP's Python API.""" @@ -173,7 +188,7 @@ def get_min_distance(self, ctx: _VampContext, robot_id: WorldRobotID) -> float: def check_config_collision_free(self, robot_id: WorldRobotID, joint_state: JointState) -> bool: """Check a joint state using VAMP native validation.""" return self._validate_state( - self._normalize_joint_state(robot_id, joint_state), check_bounds=True + self._joint_state_for_robot_order(robot_id, joint_state), check_bounds=True ) def check_edge_collision_free( @@ -185,8 +200,8 @@ def check_edge_collision_free( ) -> bool: """Check an edge using VAMP native motion validation.""" del step_size - start_state = self._normalize_joint_state(robot_id, start) - end_state = self._normalize_joint_state(robot_id, end) + start_state = self._joint_state_for_robot_order(robot_id, start) + end_state = self._joint_state_for_robot_order(robot_id, end) result = self._robot_module.validate_motion( list(start_state.position), list(end_state.position), @@ -197,7 +212,8 @@ def check_edge_collision_free( def get_ee_pose(self, ctx: _VampContext, robot_id: WorldRobotID) -> PoseStamped: """Get end-effector pose from VAMP eefk.""" - joint_state = ctx.joint_states[robot_id] + self._assert_robot_id(robot_id) + joint_state = ctx.joint_state transform = np.asarray( self._robot_module.eefk(list(joint_state.position)), dtype=np.float64 ) @@ -211,7 +227,7 @@ def get_link_pose( config = self._robots[robot_id] if link_name != config.end_effector_link: raise UnsupportedWorldCapabilityError("vamp", f"link pose for '{link_name}'") - joint_state = ctx.joint_states[robot_id] + joint_state = ctx.joint_state return np.asarray(self._robot_module.eefk(list(joint_state.position)), dtype=np.float64) def get_jacobian(self, ctx: _VampContext, robot_id: WorldRobotID) -> NDArray[np.float64]: @@ -253,12 +269,12 @@ def plan_joint_path( plan_settings, sampler, ) - if not bool(getattr(result, "solved", False)): + if not result.solved: return _failure( PlanningStatus.NO_SOLUTION, "VAMP planner did not find a path", planning_time=time.time() - start_time, - iterations=int(getattr(result, "iterations", 0)), + iterations=result.iterations, ) path_source = result.path @@ -266,10 +282,10 @@ def plan_joint_path( simplified = robot_module.simplify( path_source, self._environment, simplify_settings, sampler ) - if bool(getattr(simplified, "solved", True)): + if simplified.solved: path_source = simplified.path - path = _path_to_joint_states( + path = path_to_joint_states( path_source, start.name or self.get_robot_config(robot_id).joint_names ) if planner_config.validate_path and not self._validate_path(robot_id, path): @@ -282,23 +298,34 @@ def plan_joint_path( status=PlanningStatus.SUCCESS, path=path, planning_time=time.time() - start_time, - path_length=_path_length(path), - iterations=int(getattr(result, "iterations", 0)), + path_length=compute_path_length(path), + iterations=result.iterations, message="VAMP planning succeeded", ) - def _normalize_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) -> JointState: + def _joint_state_for_robot_order( + self, robot_id: WorldRobotID, joint_state: JointState + ) -> JointState: + """Return a joint state truncated to VAMP's configured robot joint order.""" config = self._robots[robot_id] positions = list(joint_state.position[: len(config.joint_names)]) names = list(joint_state.name[: len(positions)]) if joint_state.name else config.joint_names return JointState(name=names, position=positions) def _robot_name(self) -> str: - robot = getattr(self.config.artifact, "robot", None) - if isinstance(robot, str): - return robot + if self.config.artifact.mode == "official": + return self.config.artifact.robot return self._robot_module.__name__.split(".")[-1] + def _assert_robot_id(self, robot_id: WorldRobotID) -> None: + if robot_id not in self._robots: + raise KeyError(robot_id) + + def _require_live_joint_state(self) -> JointState: + if self._live_joint_state is None: + raise RuntimeError("VAMP world has no robot joint state") + return self._live_joint_state + def _validate_path(self, robot_id: WorldRobotID, path: list[JointState]) -> bool: if not path: return False @@ -357,28 +384,6 @@ def _timeout_to_iteration_budget(timeout: float) -> int: return max(1, int(timeout * 1000)) -def _path_to_joint_states(path_source: Any, joint_names: list[str]) -> list[JointState]: - path_array = _path_to_array(path_source) - return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] - - -def _path_to_array(path_source: Any) -> np.ndarray: - if hasattr(path_source, "numpy"): - return np.asarray(path_source.numpy(), dtype=np.float64) - return np.asarray(path_source, dtype=np.float64) - - -def _path_length(path: list[JointState]) -> float: - if len(path) < 2: - return 0.0 - total = 0.0 - for before, after in pairwise(path): - q_before = np.array(before.position, dtype=np.float64) - q_after = np.array(after.position, dtype=np.float64) - total += float(np.linalg.norm(q_after - q_before)) - return total - - def _failure( status: PlanningStatus, message: str, diff --git a/dimos/robot/catalog/franka.py b/dimos/robot/catalog/franka.py index 902586e2f2..41d96f165c 100644 --- a/dimos/robot/catalog/franka.py +++ b/dimos/robot/catalog/franka.py @@ -32,8 +32,6 @@ FRANKA_PANDA_JOINT_NAMES = [f"panda_joint{i}" for i in range(1, 8)] FRANKA_PANDA_HOME_JOINTS = [0.0, -0.7853981634, 0.0, -2.35619449, 0.0, 1.5707963268, 0.7853981634] -FRANKA_PANDA_JOINT_LIMITS_LOWER = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] -FRANKA_PANDA_JOINT_LIMITS_UPPER = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] def franka_panda( @@ -53,8 +51,6 @@ def franka_panda( "joint_names": FRANKA_PANDA_JOINT_NAMES, "base_link": "panda_link0", "home_joints": FRANKA_PANDA_HOME_JOINTS, - "joint_limits_lower": FRANKA_PANDA_JOINT_LIMITS_LOWER, - "joint_limits_upper": FRANKA_PANDA_JOINT_LIMITS_UPPER, "package_paths": { "franka_description": _FRANKA_DESCRIPTION_PKG, "moveit_resources_panda_description": _FRANKA_DESCRIPTION_PKG, @@ -76,8 +72,6 @@ def franka_panda( __all__ = [ "FRANKA_PANDA_FK_MODEL", "FRANKA_PANDA_HOME_JOINTS", - "FRANKA_PANDA_JOINT_LIMITS_LOWER", - "FRANKA_PANDA_JOINT_LIMITS_UPPER", "FRANKA_PANDA_JOINT_NAMES", "FRANKA_PANDA_MODEL", "FRANKA_PANDA_SRDF", diff --git a/dimos/robot/catalog/test_franka.py b/dimos/robot/catalog/test_franka.py index d710430ad3..44fa79e52c 100644 --- a/dimos/robot/catalog/test_franka.py +++ b/dimos/robot/catalog/test_franka.py @@ -19,8 +19,6 @@ from dimos.control.components import HardwareType from dimos.robot.catalog.franka import ( FRANKA_PANDA_FK_MODEL, - FRANKA_PANDA_JOINT_LIMITS_LOWER, - FRANKA_PANDA_JOINT_LIMITS_UPPER, FRANKA_PANDA_JOINT_NAMES, FRANKA_PANDA_MODEL, FRANKA_PANDA_SRDF, @@ -42,8 +40,6 @@ def test_franka_panda_catalog_defaults_to_mock_control() -> None: assert config.joint_names == FRANKA_PANDA_JOINT_NAMES assert config.end_effector_link == "panda_hand" assert config.base_link == "panda_link0" - assert config.joint_limits_lower == FRANKA_PANDA_JOINT_LIMITS_LOWER - assert config.joint_limits_upper == FRANKA_PANDA_JOINT_LIMITS_UPPER def test_franka_panda_uses_lfs_backed_model_and_srdf_paths() -> None: @@ -75,8 +71,6 @@ def test_franka_panda_robot_model_config_preserves_vamp_joint_order() -> None: assert model.name == "panda" assert _lfs_filename(model.model_path) == _lfs_filename(FRANKA_PANDA_MODEL) assert model.joint_names == FRANKA_PANDA_JOINT_NAMES - assert model.joint_limits_lower == FRANKA_PANDA_JOINT_LIMITS_LOWER - assert model.joint_limits_upper == FRANKA_PANDA_JOINT_LIMITS_UPPER assert model.joint_name_mapping == { f"panda/{joint}": joint for joint in FRANKA_PANDA_JOINT_NAMES } diff --git a/dimos/robot/config.py b/dimos/robot/config.py index af4bb92a50..9c7e292beb 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -82,10 +82,6 @@ class RobotConfig(BaseModel): joint_prefix: str | None = None # defaults to "{name}_" base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) - # Planning - joint_limits_lower: list[float] | None = None - joint_limits_upper: list[float] | None = None - velocity_limits: list[float] | None = None max_velocity: float = 1.0 max_acceleration: float = 2.0 pre_grasp_offset: float = 0.10 @@ -235,9 +231,6 @@ def to_robot_model_config(self) -> RobotModelConfig: auto_convert_meshes=self.auto_convert_meshes, max_velocity=self.max_velocity, max_acceleration=self.max_acceleration, - joint_limits_lower=self.joint_limits_lower, - joint_limits_upper=self.joint_limits_upper, - velocity_limits=self.velocity_limits, joint_name_mapping=self.joint_name_mapping, coordinator_task_name=self.coordinator_task_name, gripper_hardware_id=self.name if self.gripper else None, From 821eea95e0c8702484dd01f6894b4622ea548dd6 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 21:31:39 -0700 Subject: [PATCH 07/20] test(manipulation): tighten VAMP PR tests --- .../planning/vamp/test_vamp_backend.py | 21 +++++++++++++------ dimos/robot/catalog/test_franka.py | 6 ++++-- .../manipulators/franka/test_blueprints.py | 13 ++++++------ 3 files changed, 26 insertions(+), 14 deletions(-) diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index de74233d41..d8059e0a94 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -16,12 +16,13 @@ from __future__ import annotations +from collections.abc import Callable from pathlib import Path import sys from types import ModuleType, SimpleNamespace -from typing import cast import numpy as np +from numpy.typing import NDArray from pydantic import ValidationError import pytest @@ -71,7 +72,7 @@ class FakePath: def __init__(self, waypoints: list[list[float]]) -> None: self._waypoints = np.array(waypoints, dtype=np.float64) - def numpy(self) -> np.ndarray: + def numpy(self) -> NDArray[np.float64]: return self._waypoints @@ -151,7 +152,14 @@ def __init__(self, robot_module: FakeRobotModule) -> None: def configure_robot_and_planner_with_kwargs( self, robot_name: str, planner_name: str, max_iterations: int - ) -> tuple[FakeRobotModule, object, SimpleNamespace, SimpleNamespace]: + ) -> tuple[ + FakeRobotModule, + Callable[ + [list[float], list[float], FakeEnvironment, SimpleNamespace, str], FakePlanningResult + ], + SimpleNamespace, + SimpleNamespace, + ]: self.configure_calls.append((robot_name, planner_name, max_iterations)) def planner_func( @@ -224,8 +232,8 @@ def test_non_vamp_planner_creation_does_not_import_vamp(mocker) -> None: def test_vamp_config_rejects_invalid_algorithm() -> None: """VAMP planner config validates the finite algorithm set.""" - with pytest.raises(ValidationError): - VampPlannerConfig(algorithm="invalid") # type: ignore[arg-type] + with pytest.raises(ValidationError, match="algorithm"): + VampPlannerConfig.model_validate({"backend": "vamp", "algorithm": "invalid"}) def test_official_vamp_artifact_loading_uses_installed_robot_module(fake_vamp_modules) -> None: @@ -249,7 +257,8 @@ def test_custom_vamp_artifact_loading_uses_explicit_module_path( loaded_vamp, loaded_robot = load_vamp_robot_module(CustomVampArtifactConfig(path=artifact_path)) assert loaded_vamp is vamp_module - assert cast("ModuleType", loaded_robot).ROBOT_NAME == "custom_panda" + assert isinstance(loaded_robot, ModuleType) + assert loaded_robot.ROBOT_NAME == "custom_panda" def test_create_world_and_planner_from_vamp_configs(fake_vamp_modules) -> None: diff --git a/dimos/robot/catalog/test_franka.py b/dimos/robot/catalog/test_franka.py index 44fa79e52c..289d53ff45 100644 --- a/dimos/robot/catalog/test_franka.py +++ b/dimos/robot/catalog/test_franka.py @@ -26,8 +26,10 @@ ) -def _lfs_filename(path: object) -> object: - return object.__getattribute__(path, "_lfs_filename") +def _lfs_filename(path: object) -> str: + filename = object.__getattribute__(path, "_lfs_filename") + assert isinstance(filename, str) + return filename def test_franka_panda_catalog_defaults_to_mock_control() -> None: diff --git a/dimos/robot/manipulators/franka/test_blueprints.py b/dimos/robot/manipulators/franka/test_blueprints.py index 1dc7380ad9..4ec8a9b7e8 100644 --- a/dimos/robot/manipulators/franka/test_blueprints.py +++ b/dimos/robot/manipulators/franka/test_blueprints.py @@ -44,9 +44,10 @@ def test_panda_coordinator_accepts_vamp_cli_override_shape() -> None: ) assert config.manipulationmodule is not None - assert config.manipulationmodule.world.backend == "vamp" - assert config.manipulationmodule.world.artifact.robot == "panda" - assert config.manipulationmodule.planner.backend == "vamp" - assert config.manipulationmodule.planner.algorithm == "rrtc" - assert config.manipulationmodule.planner.simplify is True - assert config.manipulationmodule.planner.validate_path is True + module_config = config.manipulationmodule + assert module_config.world.backend == "vamp" + assert module_config.world.artifact.robot == "panda" + assert module_config.planner.backend == "vamp" + assert module_config.planner.algorithm == "rrtc" + assert module_config.planner.simplify is True + assert module_config.planner.validate_path is True From 40723e9a65d4a8a7ce26046325de573082969322 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 21:32:35 -0700 Subject: [PATCH 08/20] test(manipulation): assert unsupported VAMP IK contract --- dimos/manipulation/test_manipulation_unit.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 2c53ce1777..fe1e5a9bdd 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -429,6 +429,8 @@ def test_solve_ik_rpc_reports_unsupported_world_capability(self, robot_config): assert result.status == IKStatus.NO_SOLUTION assert "end-effector Jacobian" in result.message assert module._state == ManipulationState.IDLE + assert module._planned_paths == {} + module._kinematics.solve.assert_called_once() class TestJointNameTranslation: From 7747b4192cfbe11bcd383d93b47299c0ce5031f6 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 21:37:48 -0700 Subject: [PATCH 09/20] test(manipulation): address unit test review --- .../planning/vamp/test_vamp_backend.py | 4 +- dimos/manipulation/test_manipulation_unit.py | 69 +++++++++---------- dimos/robot/catalog/test_franka.py | 5 +- 3 files changed, 38 insertions(+), 40 deletions(-) diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index d8059e0a94..5fb0783544 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -221,8 +221,8 @@ def test_vamp_dependency_error_has_install_guidance(mocker) -> None: load_vamp_robot_module(OfficialVampArtifactConfig(robot="panda")) -def test_non_vamp_planner_creation_does_not_import_vamp(mocker) -> None: - """Default planner creation stays independent of the optional VAMP package.""" +def test_rrt_planner_creation_works_when_vamp_unavailable(mocker) -> None: + """Default planner creation still works when the optional VAMP package is unavailable.""" mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", None) planner = create_planner(config=RRTConnectPlannerConfig()) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index fe1e5a9bdd..17889d1d0a 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -17,11 +17,11 @@ from __future__ import annotations from pathlib import Path -import threading from unittest.mock import MagicMock, patch import pytest +from dimos.core.module import Module from dimos.manipulation.manipulation_module import ( ManipulationModule, ManipulationModuleConfig, @@ -97,20 +97,9 @@ def simple_trajectory(): def _make_module(): - """Create a ManipulationModule instance with mocked __init__.""" - with patch.object(ManipulationModule, "__init__", lambda self: None): - module = ManipulationModule.__new__(ManipulationModule) - module._state = ManipulationState.IDLE - module._lock = threading.Lock() - module._error_message = "" - module._robots = {} - module._planned_paths = {} - module._planned_trajectories = {} - module._world_monitor = None - module._planner = None - module._kinematics = None - module._coordinator_client = None - return module + """Create a ManipulationModule while skipping only base Module side effects.""" + with patch.object(Module, "__init__", return_value=None): + return ManipulationModule() class TestStateMachine: @@ -205,7 +194,7 @@ def test_multiple_robots_require_name(self, robot_config): class TestPlanningInitialization: """Test planning backend configuration wiring.""" - def test_kinematics_config_is_passed_to_factory(self, robot_config): + def test_kinematics_config_is_passed_to_factory(self, robot_config, mocker): """ManipulationModule config selects the requested IK backend.""" module = _make_module() kinematics = PinkKinematicsConfig(max_iterations=100, dt=0.02) @@ -217,22 +206,22 @@ def test_kinematics_config_is_passed_to_factory(self, robot_config): mock_world_monitor = MagicMock(spec=WorldMonitor) mock_world_monitor.add_robot.return_value = "robot_id" - with ( - patch( - "dimos.manipulation.manipulation_module.WorldMonitor", - return_value=mock_world_monitor, - ), - patch("dimos.manipulation.manipulation_module.JointTrajectoryGenerator"), - patch("dimos.manipulation.manipulation_module.create_planner") as mock_planner, - patch("dimos.manipulation.manipulation_module.create_kinematics") as mock_kinematics, - ): - module._initialize_planning() + mocker.patch( + "dimos.manipulation.manipulation_module.WorldMonitor", + return_value=mock_world_monitor, + ) + mocker.patch("dimos.manipulation.manipulation_module.JointTrajectoryGenerator") + mock_planner = mocker.patch("dimos.manipulation.manipulation_module.create_planner") + mock_kinematics = mocker.patch("dimos.manipulation.manipulation_module.create_kinematics") + + module._initialize_planning() + mock_planner.assert_called_once() planner_config = mock_planner.call_args.kwargs["config"] assert isinstance(planner_config, RRTConnectPlannerConfig) mock_kinematics.assert_called_once_with(config=kinematics) - def test_legacy_kinematics_name_still_selects_backend(self, robot_config): + def test_legacy_kinematics_name_still_selects_backend(self, robot_config, mocker): """The old kinematics_name field remains a compatibility shim.""" module = _make_module() with pytest.warns(DeprecationWarning, match="kinematics_name is deprecated"): @@ -244,17 +233,17 @@ def test_legacy_kinematics_name_still_selects_backend(self, robot_config): mock_world_monitor = MagicMock(spec=WorldMonitor) mock_world_monitor.add_robot.return_value = "robot_id" - with ( - patch( - "dimos.manipulation.manipulation_module.WorldMonitor", - return_value=mock_world_monitor, - ), - patch("dimos.manipulation.manipulation_module.JointTrajectoryGenerator"), - patch("dimos.manipulation.manipulation_module.create_planner"), - patch("dimos.manipulation.manipulation_module.create_kinematics") as mock_kinematics, - ): - module._initialize_planning() + mocker.patch( + "dimos.manipulation.manipulation_module.WorldMonitor", + return_value=mock_world_monitor, + ) + mocker.patch("dimos.manipulation.manipulation_module.JointTrajectoryGenerator") + mocker.patch("dimos.manipulation.manipulation_module.create_planner") + mock_kinematics = mocker.patch("dimos.manipulation.manipulation_module.create_kinematics") + + module._initialize_planning() + mock_kinematics.assert_called_once() call_config = mock_kinematics.call_args.kwargs["config"] assert isinstance(call_config, PinkKinematicsConfig) @@ -676,6 +665,9 @@ def test_no_monitor_returns_early(self, robot_config_with_mapping): ) module._on_joint_state(msg) + assert module._world_monitor is None + assert module._init_joints == {} + class TestWorldMonitorVisualization: def test_visualization_routing_and_stop_all_monitors(self): @@ -720,6 +712,9 @@ def test_dismiss_preview_noop_without_monitor(self): module._dismiss_preview("robot_id") + assert module._world_monitor is None + assert module._state == ManipulationState.IDLE + def test_dismiss_preview_routes_to_monitor(self): module = _make_module() module._world_monitor = MagicMock() diff --git a/dimos/robot/catalog/test_franka.py b/dimos/robot/catalog/test_franka.py index 289d53ff45..834ca0f38f 100644 --- a/dimos/robot/catalog/test_franka.py +++ b/dimos/robot/catalog/test_franka.py @@ -24,9 +24,10 @@ FRANKA_PANDA_SRDF, franka_panda, ) +from dimos.utils.data import LfsPath -def _lfs_filename(path: object) -> str: +def _lfs_filename(path: LfsPath) -> str: filename = object.__getattribute__(path, "_lfs_filename") assert isinstance(filename, str) return filename @@ -71,6 +72,7 @@ def test_franka_panda_robot_model_config_preserves_vamp_joint_order() -> None: model = config.to_robot_model_config() assert model.name == "panda" + assert isinstance(model.model_path, LfsPath) assert _lfs_filename(model.model_path) == _lfs_filename(FRANKA_PANDA_MODEL) assert model.joint_names == FRANKA_PANDA_JOINT_NAMES assert model.joint_name_mapping == { @@ -92,5 +94,6 @@ def test_franka_panda_task_config_supports_mock_coordinator_benchmark_path() -> assert task.name == "cartesian_ik_panda" assert task.type == "cartesian_ik" assert task.joint_names == [f"panda/{joint}" for joint in FRANKA_PANDA_JOINT_NAMES] + assert isinstance(task.params["model_path"], LfsPath) assert _lfs_filename(task.params["model_path"]) == _lfs_filename(FRANKA_PANDA_FK_MODEL) assert task.params["ee_joint_id"] == 7 From e4323c389b7a99252b4bf4190384edf8c311f833 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 22:18:03 -0700 Subject: [PATCH 10/20] fix(manipulation): tighten VAMP imports and typing --- dimos/manipulation/planning/factory.py | 6 +- dimos/manipulation/planning/vamp/loader.py | 30 ++++-- dimos/manipulation/planning/vamp/protocols.py | 66 +++++++++--- .../planning/vamp/test_vamp_backend.py | 101 ++++++++++++++---- dimos/manipulation/planning/vamp/utils.py | 12 +-- .../manipulation/planning/world/vamp_world.py | 2 +- dimos/robot/catalog/franka.py | 34 ++++-- dimos/robot/catalog/test_franka.py | 2 +- dimos/utils/data.py | 17 ++- 9 files changed, 205 insertions(+), 65 deletions(-) diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index 7a1b62bff4..822695fc9f 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -31,12 +31,14 @@ RRTConnectPlannerConfig, VampPlannerConfig, ) +from dimos.manipulation.planning.planners.vamp_planner import VampPlanner from dimos.manipulation.planning.world.config import ( MANIPULATION_WORLD_CONFIG_ADAPTER, DrakeWorldConfig, ManipulationWorldConfig, VampWorldConfig, ) +from dimos.manipulation.planning.world.vamp_world import VampWorld if TYPE_CHECKING: from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec, WorldSpec @@ -57,8 +59,6 @@ def create_world( return DrakeWorld(enable_viz=enable_viz, **kwargs) if isinstance(config, VampWorldConfig): - from dimos.manipulation.planning.world.vamp_world import VampWorld - return VampWorld(config=config, **kwargs) raise TypeError(f"Unsupported world config: {type(config).__name__}") @@ -110,8 +110,6 @@ def create_planner( **kwargs, ) if isinstance(config, VampPlannerConfig): - from dimos.manipulation.planning.planners.vamp_planner import VampPlanner - return VampPlanner(config=config, **kwargs) raise TypeError(f"Unsupported planner config: {type(config).__name__}") diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py index 5b6c3a704a..32cf64e1bc 100644 --- a/dimos/manipulation/planning/vamp/loader.py +++ b/dimos/manipulation/planning/vamp/loader.py @@ -31,9 +31,26 @@ ) try: - import vamp as _vamp_module -except ImportError: + import vamp as _imported_vamp + import vamp.baxter as _vamp_baxter + import vamp.fetch as _vamp_fetch + import vamp.panda as _vamp_panda + import vamp.sphere as _vamp_sphere + import vamp.ur5 as _vamp_ur5 +except ImportError as exc: + _vamp_import_error: ImportError | None = exc _vamp_module = None + _VAMP_OFFICIAL_ROBOT_MODULES: dict[str, VampRobotModuleProtocol] = {} +else: + _vamp_import_error = None + _vamp_module: VampModuleProtocol | None = cast("VampModuleProtocol", _imported_vamp) + _VAMP_OFFICIAL_ROBOT_MODULES = { + "baxter": cast("VampRobotModuleProtocol", _vamp_baxter), + "fetch": cast("VampRobotModuleProtocol", _vamp_fetch), + "panda": cast("VampRobotModuleProtocol", _vamp_panda), + "sphere": cast("VampRobotModuleProtocol", _vamp_sphere), + "ur5": cast("VampRobotModuleProtocol", _vamp_ur5), + } def load_vamp_robot_module( @@ -41,8 +58,8 @@ def load_vamp_robot_module( ) -> tuple[VampModuleProtocol, VampRobotModuleProtocol]: """Load the VAMP package and configured robot module.""" if _vamp_module is None: - raise VampDependencyError() - vamp_module = cast("VampModuleProtocol", _vamp_module) + raise VampDependencyError() from _vamp_import_error + vamp_module = _vamp_module if isinstance(artifact, OfficialVampArtifactConfig): return vamp_module, _load_official_robot_module(vamp_module, artifact.robot) if isinstance(artifact, CustomVampArtifactConfig): @@ -55,12 +72,11 @@ def _load_official_robot_module( ) -> VampRobotModuleProtocol: del vamp_module try: - imported = importlib.import_module(f"vamp.{robot}") - except ImportError as exc: + return _VAMP_OFFICIAL_ROBOT_MODULES[robot] + except KeyError as exc: raise ValueError( f"Installed VAMP package does not expose robot artifact '{robot}'" ) from exc - return cast("VampRobotModuleProtocol", imported) def _load_custom_robot_module(path: Path) -> VampRobotModuleProtocol: diff --git a/dimos/manipulation/planning/vamp/protocols.py b/dimos/manipulation/planning/vamp/protocols.py index d3f8d9b9db..e35fda7824 100644 --- a/dimos/manipulation/planning/vamp/protocols.py +++ b/dimos/manipulation/planning/vamp/protocols.py @@ -24,33 +24,66 @@ class VampPathProtocol(Protocol): - """Path object returned by VAMP bindings.""" + """Path value returned by VAMP bindings.""" def numpy(self) -> NDArray[np.float64]: """Return path waypoints as an array.""" ... +VampPathSource = VampPathProtocol | Sequence[Sequence[float]] | NDArray[np.float64] + + +class VampSphereProtocol(Protocol): + """Sphere primitive handle accepted by VAMP environments.""" + + +class VampCuboidProtocol(Protocol): + """Cuboid primitive handle accepted by VAMP environments.""" + + +class VampCylinderProtocol(Protocol): + """Cylinder primitive handle accepted by VAMP environments.""" + + +class VampPlannerSettingsProtocol(Protocol): + """Planner settings handle returned by VAMP.""" + + +class VampSimplifySettingsProtocol(Protocol): + """Simplification settings handle returned by VAMP.""" + + +class VampSamplerProtocol(Protocol): + """Sampler handle returned by robot-specific VAMP modules.""" + + class VampPlanningResultProtocol(Protocol): """Planning or simplification result returned by VAMP bindings.""" solved: bool - path: object + path: VampPathSource iterations: int class VampEnvironmentProtocol(Protocol): """VAMP collision environment.""" - def add_sphere(self, sphere: object) -> None: ... + def add_sphere(self, sphere: VampSphereProtocol) -> None: ... - def add_cuboid(self, cuboid: object) -> None: ... + def add_cuboid(self, cuboid: VampCuboidProtocol) -> None: ... - def add_capsule(self, capsule: object) -> None: ... + def add_capsule(self, capsule: VampCylinderProtocol) -> None: ... VampPlannerFunction = Callable[ - [Sequence[float], Sequence[float], VampEnvironmentProtocol, object, object], + [ + Sequence[float], + Sequence[float], + VampEnvironmentProtocol, + VampPlannerSettingsProtocol, + VampSamplerProtocol, + ], VampPlanningResultProtocol, ] @@ -60,7 +93,7 @@ class VampRobotModuleProtocol(Protocol): __name__: str - def halton(self) -> object: ... + def halton(self) -> VampSamplerProtocol: ... def validate( self, @@ -81,10 +114,10 @@ def eefk(self, configuration: Sequence[float]) -> NDArray[np.float64]: ... def simplify( self, - path: object, + path: VampPathSource, environment: VampEnvironmentProtocol, - settings: object, - sampler: object, + settings: VampSimplifySettingsProtocol, + sampler: VampSamplerProtocol, ) -> VampPlanningResultProtocol: ... @@ -93,16 +126,21 @@ class VampModuleProtocol(Protocol): def Environment(self) -> VampEnvironmentProtocol: ... - def Sphere(self, center: Sequence[float], radius: float) -> object: ... + def Sphere(self, center: Sequence[float], radius: float) -> VampSphereProtocol: ... def Cuboid( self, center: Sequence[float], euler_xyz: Sequence[float], half_extents: Sequence[float] - ) -> object: ... + ) -> VampCuboidProtocol: ... def Cylinder( self, center: Sequence[float], euler_xyz: Sequence[float], radius: float, length: float - ) -> object: ... + ) -> VampCylinderProtocol: ... def configure_robot_and_planner_with_kwargs( self, robot_name: str, planner_name: str, max_iterations: int - ) -> tuple[VampRobotModuleProtocol, VampPlannerFunction, object, object]: ... + ) -> tuple[ + VampRobotModuleProtocol, + VampPlannerFunction, + VampPlannerSettingsProtocol, + VampSimplifySettingsProtocol, + ]: ... diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index 5fb0783544..ffa282b0d2 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -17,9 +17,10 @@ from __future__ import annotations from collections.abc import Callable +from dataclasses import dataclass from pathlib import Path import sys -from types import ModuleType, SimpleNamespace +from types import ModuleType import numpy as np from numpy.typing import NDArray @@ -48,26 +49,62 @@ from dimos.msgs.sensor_msgs.JointState import JointState +@dataclass(frozen=True) +class FakeSphere: + center: list[float] + radius: float + + +@dataclass(frozen=True) +class FakeCuboid: + center: list[float] + euler_xyz: list[float] + half_extents: list[float] + + +@dataclass(frozen=True) +class FakeCylinder: + center: list[float] + euler_xyz: list[float] + radius: float + length: float + + +@dataclass(frozen=True) +class FakePlannerSettings: + max_iterations: int + + +@dataclass(frozen=True) +class FakeSimplifySettings: + enabled: bool = True + + +@dataclass(frozen=True) +class FakeSampler: + name: str + + class FakeEnvironment: """Small fake VAMP environment that records converted primitives.""" def __init__(self) -> None: - self.spheres: list[object] = [] - self.cuboids: list[object] = [] - self.capsules: list[object] = [] + self.spheres: list[FakeSphere] = [] + self.cuboids: list[FakeCuboid] = [] + self.capsules: list[FakeCylinder] = [] - def add_sphere(self, sphere: object) -> None: + def add_sphere(self, sphere: FakeSphere) -> None: self.spheres.append(sphere) - def add_cuboid(self, cuboid: object) -> None: + def add_cuboid(self, cuboid: FakeCuboid) -> None: self.cuboids.append(cuboid) - def add_capsule(self, capsule: object) -> None: + def add_capsule(self, capsule: FakeCylinder) -> None: self.capsules.append(capsule) class FakePath: - """Fake VAMP path object exposing the numpy() method used by bindings.""" + """Fake VAMP path value exposing the numpy() method used by bindings.""" def __init__(self, waypoints: list[list[float]]) -> None: self._waypoints = np.array(waypoints, dtype=np.float64) @@ -96,8 +133,8 @@ def __init__(self) -> None: self.valid = True self.motion_valid = True - def halton(self) -> str: - return "fake_sampler" + def halton(self) -> FakeSampler: + return FakeSampler("fake_sampler") def validate( self, configuration: list[float], environment: FakeEnvironment, check_bounds: bool @@ -128,8 +165,8 @@ def simplify( self, path: FakePath, environment: FakeEnvironment, - settings: SimpleNamespace, - sampler: str, + settings: FakeSimplifySettings, + sampler: FakeSampler, ) -> FakePlanningResult: del environment, settings, sampler self.simplify_calls.append(path) @@ -143,11 +180,11 @@ def __init__(self, robot_module: FakeRobotModule) -> None: super().__init__("vamp") self.panda = robot_module self.Environment = FakeEnvironment - self.Sphere = SimpleNamespace - self.Cuboid = SimpleNamespace - self.Cylinder = SimpleNamespace + self.Sphere = FakeSphere + self.Cuboid = FakeCuboid + self.Cylinder = FakeCylinder self.configure_calls: list[tuple[str, str, int]] = [] - self.planner_calls: list[tuple[list[float], list[float], str]] = [] + self.planner_calls: list[tuple[list[float], list[float], FakeSampler]] = [] self.planner_solved = True def configure_robot_and_planner_with_kwargs( @@ -155,10 +192,17 @@ def configure_robot_and_planner_with_kwargs( ) -> tuple[ FakeRobotModule, Callable[ - [list[float], list[float], FakeEnvironment, SimpleNamespace, str], FakePlanningResult + [ + list[float], + list[float], + FakeEnvironment, + FakePlannerSettings, + FakeSampler, + ], + FakePlanningResult, ], - SimpleNamespace, - SimpleNamespace, + FakePlannerSettings, + FakeSimplifySettings, ]: self.configure_calls.append((robot_name, planner_name, max_iterations)) @@ -166,8 +210,8 @@ def planner_func( start: list[float], goal: list[float], environment: FakeEnvironment, - settings: SimpleNamespace, - sampler: str, + settings: FakePlannerSettings, + sampler: FakeSampler, ) -> FakePlanningResult: del environment, settings self.planner_calls.append((start, goal, sampler)) @@ -177,7 +221,12 @@ def planner_func( 11, ) - return self.panda, planner_func, SimpleNamespace(), SimpleNamespace() + return ( + self.panda, + planner_func, + FakePlannerSettings(max_iterations=max_iterations), + FakeSimplifySettings(), + ) @pytest.fixture @@ -187,6 +236,10 @@ def fake_vamp_modules(mocker) -> tuple[FakeVampModule, FakeRobotModule]: vamp_module = FakeVampModule(robot_module) mocker.patch.dict(sys.modules, {"vamp": vamp_module, "vamp.panda": robot_module}) mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", vamp_module) + mocker.patch( + "dimos.manipulation.planning.vamp.loader._VAMP_OFFICIAL_ROBOT_MODULES", + {"panda": robot_module}, + ) return vamp_module, robot_module @@ -305,7 +358,9 @@ def test_vamp_planner_dispatches_algorithm_simplifies_and_validates(fake_vamp_mo assert result.status == PlanningStatus.SUCCESS assert [point.position for point in result.path] == [[0.0, 0.0, 0.0], [1.0, 0.5, 0.25]] assert vamp_module.configure_calls == [("panda", "prm", 250)] - assert vamp_module.planner_calls == [([0.0, 0.0, 0.0], [1.0, 0.5, 0.25], "fake_sampler")] + assert vamp_module.planner_calls == [ + ([0.0, 0.0, 0.0], [1.0, 0.5, 0.25], FakeSampler("fake_sampler")) + ] assert len(robot_module.simplify_calls) == 1 assert robot_module.motion_calls == [([0.0, 0.0, 0.0], [1.0, 0.5, 0.25], True)] diff --git a/dimos/manipulation/planning/vamp/utils.py b/dimos/manipulation/planning/vamp/utils.py index c1584f3992..dde0303ce8 100644 --- a/dimos/manipulation/planning/vamp/utils.py +++ b/dimos/manipulation/planning/vamp/utils.py @@ -21,22 +21,22 @@ import numpy as np from numpy.typing import NDArray -from dimos.manipulation.planning.vamp.protocols import VampPathProtocol +from dimos.manipulation.planning.vamp.protocols import VampPathProtocol, VampPathSource from dimos.msgs.sensor_msgs.JointState import JointState -def path_to_joint_states(path_source: object, joint_names: list[str]) -> list[JointState]: - """Convert a VAMP path object or numeric waypoint array into joint states.""" +def path_to_joint_states(path_source: VampPathSource, joint_names: list[str]) -> list[JointState]: + """Convert VAMP path data or numeric waypoints into joint states.""" path_array = path_to_array(path_source) return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] -def path_to_array(path_source: object) -> NDArray[np.float64]: - """Convert a VAMP path object or sequence into a float waypoint array.""" +def path_to_array(path_source: VampPathSource) -> NDArray[np.float64]: + """Convert VAMP path data into a float waypoint array.""" if _has_numpy(path_source): return np.asarray(path_source.numpy(), dtype=np.float64) return np.asarray(path_source, dtype=np.float64) -def _has_numpy(path_source: object) -> TypeGuard[VampPathProtocol]: +def _has_numpy(path_source: VampPathSource) -> TypeGuard[VampPathProtocol]: return hasattr(path_source, "numpy") diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index 699e8da09c..be95370970 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -49,7 +49,7 @@ class _VampContext(Protocol): """Typed context shape used by the VAMP world adapter.""" - # TODO: Replace the loose WorldSpec context object with a typed ContextProtocol. + # TODO: Replace the loose WorldSpec context payload with a typed ContextProtocol. joint_state: JointState diff --git a/dimos/robot/catalog/franka.py b/dimos/robot/catalog/franka.py index 41d96f165c..9dca67f9e2 100644 --- a/dimos/robot/catalog/franka.py +++ b/dimos/robot/catalog/franka.py @@ -16,7 +16,8 @@ from __future__ import annotations -from typing import Any +from pathlib import Path +from typing import TypeAlias from dimos.robot.config import RobotConfig from dimos.utils.data import LfsPath @@ -33,16 +34,32 @@ FRANKA_PANDA_JOINT_NAMES = [f"panda_joint{i}" for i in range(1, 8)] FRANKA_PANDA_HOME_JOINTS = [0.0, -0.7853981634, 0.0, -2.35619449, 0.0, 1.5707963268, 0.7853981634] +FrankaAdapterKwargs: TypeAlias = dict[str, str | bool | float | Path | LfsPath | list[float] | None] +FrankaOverrideValue: TypeAlias = ( + str + | bool + | int + | float + | Path + | LfsPath + | list[str] + | list[float] + | dict[str, Path | LfsPath] + | FrankaAdapterKwargs + | None +) + def franka_panda( name: str = "panda", *, adapter_type: str = "mock", address: str | None = None, - **overrides: Any, + **overrides: FrankaOverrideValue, ) -> RobotConfig: """Franka Panda config for mock-control planning tests and benchmarks.""" - defaults: dict[str, Any] = { + base_adapter_kwargs: FrankaAdapterKwargs = {"srdf_path": FRANKA_PANDA_SRDF} + defaults: dict[str, FrankaOverrideValue] = { "name": name, "model_path": FRANKA_PANDA_MODEL, "end_effector_link": "panda_hand", @@ -58,12 +75,15 @@ def franka_panda( "auto_convert_meshes": True, "max_velocity": 1.0, "max_acceleration": 2.0, - "adapter_kwargs": {"srdf_path": FRANKA_PANDA_SRDF}, + "adapter_kwargs": base_adapter_kwargs, } - if "adapter_kwargs" in overrides: + override_adapter_kwargs = overrides.pop("adapter_kwargs", None) + if override_adapter_kwargs is not None: + if not isinstance(override_adapter_kwargs, dict): + raise TypeError("adapter_kwargs override must be a dictionary") defaults["adapter_kwargs"] = { - **defaults["adapter_kwargs"], - **overrides.pop("adapter_kwargs"), + **base_adapter_kwargs, + **override_adapter_kwargs, } defaults.update(overrides) return RobotConfig(**defaults) diff --git a/dimos/robot/catalog/test_franka.py b/dimos/robot/catalog/test_franka.py index 834ca0f38f..d8a57db724 100644 --- a/dimos/robot/catalog/test_franka.py +++ b/dimos/robot/catalog/test_franka.py @@ -28,7 +28,7 @@ def _lfs_filename(path: LfsPath) -> str: - filename = object.__getattribute__(path, "_lfs_filename") + filename = path.lfs_filename assert isinstance(filename, str) return filename diff --git a/dimos/utils/data.py b/dimos/utils/data.py index b4b7e2210a..6238718a99 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -348,6 +348,14 @@ def _ensure_downloaded(self) -> Path: object.__setattr__(self, "_lfs_resolved_cache", cache) return cache + @property + def lfs_filename(self) -> str | Path: + """Return the configured LFS resource without resolving the path.""" + filename = self._lfs_filename + if isinstance(filename, Path): + return filename + return str(filename) + def __getattribute__(self, name: str) -> object: # During Path.__new__(), _lfs_filename hasn't been set yet. # Fall through to normal Path behavior until construction is complete. @@ -357,8 +365,13 @@ def __getattribute__(self, name: str) -> object: return object.__getattribute__(self, name) # After construction, allow access to our internal attributes directly - if name in ("_lfs_filename", "_lfs_resolved_cache", "_ensure_downloaded"): - return object.__getattribute__(self, name) + if name in ( + "_lfs_filename", + "_lfs_resolved_cache", + "_ensure_downloaded", + "lfs_filename", + ): + return super().__getattribute__(name) # For all other attributes, ensure download first then delegate to resolved path resolved = object.__getattribute__(self, "_ensure_downloaded")() From aa1343929585690af607c4283f37d272992d58a4 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 22:58:32 -0700 Subject: [PATCH 11/20] fix(manipulation): use singular VAMP robot state --- .../planning/vamp/test_vamp_backend.py | 14 +++++++++ .../manipulation/planning/world/vamp_world.py | 31 +++++++++++-------- 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index ffa282b0d2..4bc47c645a 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -344,6 +344,20 @@ def test_vamp_world_validity_fk_and_unsupported_jacobian(fake_vamp_modules) -> N world.get_jacobian(ctx, robot_id) +def test_vamp_world_accepts_one_robot(fake_vamp_modules) -> None: + """VAMP world exposes WorldSpec robot APIs through a single robot slot.""" + world = VampWorld(VampWorldConfig()) + config = robot_config() + + robot_id = world.add_robot(config) + + assert robot_id == "robot_1" + assert world.get_robot_ids() == ["robot_1"] + assert world.get_robot_config(robot_id) is config + with pytest.raises(ValueError, match="one robot"): + world.add_robot(robot_config()) + + def test_vamp_planner_dispatches_algorithm_simplifies_and_validates(fake_vamp_modules) -> None: """VAMP planner uses configured algorithm and VAMP-native path utilities.""" vamp_module, robot_module = fake_vamp_modules diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index be95370970..8dc9f824de 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -67,21 +67,21 @@ def __init__(self, config: VampWorldConfig) -> None: self.config = config self._vamp_module, self._robot_module = load_vamp_robot_module(config.artifact) self._environment = self._vamp_module.Environment() - self._robots: dict[WorldRobotID, RobotModelConfig] = {} + self._robot_id: WorldRobotID | None = None + self._robot_config: RobotModelConfig | None = None self._live_joint_state: JointState | None = None self._obstacles: dict[str, Obstacle] = {} - self._robot_counter = 0 self._finalized = False def add_robot(self, config: RobotModelConfig) -> WorldRobotID: """Add a robot to the VAMP world.""" if self._finalized: raise RuntimeError("Cannot add robot after world is finalized") - if self._robots: + if self._robot_config is not None: raise ValueError("VAMP world currently supports one robot per world") - self._robot_counter += 1 - robot_id = f"robot_{self._robot_counter}" - self._robots[robot_id] = config + robot_id = "robot_1" + self._robot_id = robot_id + self._robot_config = config home_positions = config.home_joints or [0.0] * len(config.joint_names) self._live_joint_state = JointState( name=config.joint_names, @@ -91,17 +91,22 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: def get_robot_ids(self) -> list[WorldRobotID]: """Get all robot IDs.""" - return list(self._robots) + if self._robot_id is None: + return [] + return [self._robot_id] def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: """Get robot configuration.""" - return self._robots[robot_id] + self._assert_robot_id(robot_id) + if self._robot_config is None: + raise RuntimeError("VAMP world has no robot config") + return self._robot_config def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: """Get joint limits from config or conservative defaults.""" - config = self._robots[robot_id] + config = self.get_robot_config(robot_id) if config.joint_limits_lower is not None and config.joint_limits_upper is not None: return ( np.array(config.joint_limits_lower, dtype=np.float64), @@ -224,7 +229,7 @@ def get_link_pose( self, ctx: _VampContext, robot_id: WorldRobotID, link_name: str ) -> NDArray[np.float64]: """Return EE pose only when the requested link is the configured EE link.""" - config = self._robots[robot_id] + config = self.get_robot_config(robot_id) if link_name != config.end_effector_link: raise UnsupportedWorldCapabilityError("vamp", f"link pose for '{link_name}'") joint_state = ctx.joint_state @@ -246,7 +251,7 @@ def plan_joint_path( start_time = time.time() if not self.is_finalized: return _failure(PlanningStatus.NO_SOLUTION, "World must be finalized before planning") - if robot_id not in self.get_robot_ids(): + if robot_id != self._robot_id: return _failure(PlanningStatus.NO_SOLUTION, f"Robot '{robot_id}' not found") if not self.check_config_collision_free(robot_id, start): @@ -307,7 +312,7 @@ def _joint_state_for_robot_order( self, robot_id: WorldRobotID, joint_state: JointState ) -> JointState: """Return a joint state truncated to VAMP's configured robot joint order.""" - config = self._robots[robot_id] + config = self.get_robot_config(robot_id) positions = list(joint_state.position[: len(config.joint_names)]) names = list(joint_state.name[: len(positions)]) if joint_state.name else config.joint_names return JointState(name=names, position=positions) @@ -318,7 +323,7 @@ def _robot_name(self) -> str: return self._robot_module.__name__.split(".")[-1] def _assert_robot_id(self, robot_id: WorldRobotID) -> None: - if robot_id not in self._robots: + if robot_id != self._robot_id: raise KeyError(robot_id) def _require_live_joint_state(self) -> JointState: From 924cd5255472709a418430cb98b13d70c2b2084e Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 23:22:53 -0700 Subject: [PATCH 12/20] fix(robot): simplify Franka catalog config --- dimos/robot/catalog/franka.py | 60 ++++++++---------------------- dimos/robot/catalog/test_franka.py | 14 ++----- dimos/utils/data.py | 9 ----- 3 files changed, 18 insertions(+), 65 deletions(-) diff --git a/dimos/robot/catalog/franka.py b/dimos/robot/catalog/franka.py index 9dca67f9e2..8bf62d934b 100644 --- a/dimos/robot/catalog/franka.py +++ b/dimos/robot/catalog/franka.py @@ -16,9 +16,6 @@ from __future__ import annotations -from pathlib import Path -from typing import TypeAlias - from dimos.robot.config import RobotConfig from dimos.utils.data import LfsPath @@ -34,59 +31,32 @@ FRANKA_PANDA_JOINT_NAMES = [f"panda_joint{i}" for i in range(1, 8)] FRANKA_PANDA_HOME_JOINTS = [0.0, -0.7853981634, 0.0, -2.35619449, 0.0, 1.5707963268, 0.7853981634] -FrankaAdapterKwargs: TypeAlias = dict[str, str | bool | float | Path | LfsPath | list[float] | None] -FrankaOverrideValue: TypeAlias = ( - str - | bool - | int - | float - | Path - | LfsPath - | list[str] - | list[float] - | dict[str, Path | LfsPath] - | FrankaAdapterKwargs - | None -) - def franka_panda( name: str = "panda", *, adapter_type: str = "mock", address: str | None = None, - **overrides: FrankaOverrideValue, ) -> RobotConfig: """Franka Panda config for mock-control planning tests and benchmarks.""" - base_adapter_kwargs: FrankaAdapterKwargs = {"srdf_path": FRANKA_PANDA_SRDF} - defaults: dict[str, FrankaOverrideValue] = { - "name": name, - "model_path": FRANKA_PANDA_MODEL, - "end_effector_link": "panda_hand", - "adapter_type": adapter_type, - "address": address, - "joint_names": FRANKA_PANDA_JOINT_NAMES, - "base_link": "panda_link0", - "home_joints": FRANKA_PANDA_HOME_JOINTS, - "package_paths": { + return RobotConfig( + name=name, + model_path=FRANKA_PANDA_MODEL, + end_effector_link="panda_hand", + adapter_type=adapter_type, + address=address, + joint_names=FRANKA_PANDA_JOINT_NAMES, + base_link="panda_link0", + home_joints=FRANKA_PANDA_HOME_JOINTS, + package_paths={ "franka_description": _FRANKA_DESCRIPTION_PKG, "moveit_resources_panda_description": _FRANKA_DESCRIPTION_PKG, }, - "auto_convert_meshes": True, - "max_velocity": 1.0, - "max_acceleration": 2.0, - "adapter_kwargs": base_adapter_kwargs, - } - override_adapter_kwargs = overrides.pop("adapter_kwargs", None) - if override_adapter_kwargs is not None: - if not isinstance(override_adapter_kwargs, dict): - raise TypeError("adapter_kwargs override must be a dictionary") - defaults["adapter_kwargs"] = { - **base_adapter_kwargs, - **override_adapter_kwargs, - } - defaults.update(overrides) - return RobotConfig(**defaults) + auto_convert_meshes=True, + max_velocity=1.0, + max_acceleration=2.0, + adapter_kwargs={"srdf_path": FRANKA_PANDA_SRDF}, + ) __all__ = [ diff --git a/dimos/robot/catalog/test_franka.py b/dimos/robot/catalog/test_franka.py index d8a57db724..578685852c 100644 --- a/dimos/robot/catalog/test_franka.py +++ b/dimos/robot/catalog/test_franka.py @@ -27,12 +27,6 @@ from dimos.utils.data import LfsPath -def _lfs_filename(path: LfsPath) -> str: - filename = path.lfs_filename - assert isinstance(filename, str) - return filename - - def test_franka_panda_catalog_defaults_to_mock_control() -> None: """The Panda catalog config is mock-control first.""" config = franka_panda() @@ -47,9 +41,9 @@ def test_franka_panda_catalog_defaults_to_mock_control() -> None: def test_franka_panda_uses_lfs_backed_model_and_srdf_paths() -> None: """Panda URDF/SRDF resources follow the repo LFS-backed data pattern.""" - assert _lfs_filename(FRANKA_PANDA_MODEL) == "franka_description/urdf/panda.urdf.xacro" - assert _lfs_filename(FRANKA_PANDA_FK_MODEL) == "franka_description/urdf/panda.urdf" - assert _lfs_filename(FRANKA_PANDA_SRDF) == "franka_description/srdf/panda.srdf" + assert isinstance(FRANKA_PANDA_MODEL, LfsPath) + assert isinstance(FRANKA_PANDA_FK_MODEL, LfsPath) + assert isinstance(FRANKA_PANDA_SRDF, LfsPath) def test_franka_panda_hardware_component_uses_mock_adapter_and_prefixed_joints() -> None: @@ -73,7 +67,6 @@ def test_franka_panda_robot_model_config_preserves_vamp_joint_order() -> None: assert model.name == "panda" assert isinstance(model.model_path, LfsPath) - assert _lfs_filename(model.model_path) == _lfs_filename(FRANKA_PANDA_MODEL) assert model.joint_names == FRANKA_PANDA_JOINT_NAMES assert model.joint_name_mapping == { f"panda/{joint}": joint for joint in FRANKA_PANDA_JOINT_NAMES @@ -95,5 +88,4 @@ def test_franka_panda_task_config_supports_mock_coordinator_benchmark_path() -> assert task.type == "cartesian_ik" assert task.joint_names == [f"panda/{joint}" for joint in FRANKA_PANDA_JOINT_NAMES] assert isinstance(task.params["model_path"], LfsPath) - assert _lfs_filename(task.params["model_path"]) == _lfs_filename(FRANKA_PANDA_FK_MODEL) assert task.params["ee_joint_id"] == 7 diff --git a/dimos/utils/data.py b/dimos/utils/data.py index 6238718a99..991352b9be 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -348,14 +348,6 @@ def _ensure_downloaded(self) -> Path: object.__setattr__(self, "_lfs_resolved_cache", cache) return cache - @property - def lfs_filename(self) -> str | Path: - """Return the configured LFS resource without resolving the path.""" - filename = self._lfs_filename - if isinstance(filename, Path): - return filename - return str(filename) - def __getattribute__(self, name: str) -> object: # During Path.__new__(), _lfs_filename hasn't been set yet. # Fall through to normal Path behavior until construction is complete. @@ -369,7 +361,6 @@ def __getattribute__(self, name: str) -> object: "_lfs_filename", "_lfs_resolved_cache", "_ensure_downloaded", - "lfs_filename", ): return super().__getattribute__(name) From c9810931cae33bff2da7ddf9d4b3468f7f401d41 Mon Sep 17 00:00:00 2001 From: cc <55869557+TomCC7@users.noreply.github.com> Date: Thu, 18 Jun 2026 23:24:25 -0700 Subject: [PATCH 13/20] Delete docs/usage/manipulation_planning.md --- docs/usage/manipulation_planning.md | 124 ---------------------------- 1 file changed, 124 deletions(-) delete mode 100644 docs/usage/manipulation_planning.md diff --git a/docs/usage/manipulation_planning.md b/docs/usage/manipulation_planning.md deleted file mode 100644 index 0904d22347..0000000000 --- a/docs/usage/manipulation_planning.md +++ /dev/null @@ -1,124 +0,0 @@ -# Manipulation planning backends - -DimOS manipulation planning is configured as a stack: - -- `world`: robot/environment representation and collision or validity checks -- `planner`: joint-space path planning -- `kinematics`: pose-to-joint solving for pose goals - -The default stack remains Drake world + RRT-Connect planner + Jacobian IK. - -## VAMP backend - -VAMP is an optional joint-space planning backend. Install it only when needed: - -```bash -uv sync --extra vamp -``` - -VAMP owns its robot artifact and environment representation, so select the VAMP -world and VAMP planner together: - -```python -from dimos.manipulation.manipulation_module import ManipulationModule - -module = ManipulationModule.blueprint( - world={"backend": "vamp", "artifact": {"mode": "official", "robot": "panda"}}, - planner={"backend": "vamp", "algorithm": "rrtc", "simplify": True}, -) -``` - -Supported VAMP algorithms are `rrtc`, `prm`, `fcit`, and `aorrtc`. - -For a user-prepared custom robot artifact, point the world config at a local -Python module/package produced outside DimOS: - -```python -module = ManipulationModule.blueprint( - world={"backend": "vamp", "artifact": {"mode": "custom", "path": "/path/to/vamp_robot"}}, - planner={"backend": "vamp", "algorithm": "prm"}, -) -``` - -CLI/config overrides follow the same nested field shape, for example: - -```bash -uv run --extra manipulation --extra vamp dimos run panda-coordinator \ - -o manipulationmodule.world.backend=vamp \ - -o manipulationmodule.world.artifact.mode=official \ - -o manipulationmodule.world.artifact.robot=panda \ - -o manipulationmodule.planner.backend=vamp \ - -o manipulationmodule.planner.algorithm=rrtc -``` - -## Artifact scope - -DimOS does not generate VAMP artifacts at runtime. Official artifacts are loaded -from the installed `vamp-planner` package. Custom robot artifacts must be -prepared by the user/upstream VAMP tooling before DimOS starts. - -## Pose planning with VAMP - -The VAMP planner is joint-space only. It does not run IK, convert poses, or -probe Jacobians. Pose planning is available only if the configured kinematics -backend can solve against the selected world. A VAMP world currently raises a -clear unsupported-capability error for Jacobian requests. - -## Franka Panda mock-control support - -`dimos.robot.catalog.franka.franka_panda()` provides a mock-control Franka Panda -configuration for VAMP tests and planner benchmarks. It uses: - -- mock manipulator control by default (`adapter_type="mock"`) -- Panda arm joint order `panda_joint1` through `panda_joint7` -- LFS-backed model resources under `franka_description` -- `RobotConfig.to_hardware_component()` for `ControlCoordinator` -- `RobotConfig.to_robot_model_config()` for `ManipulationModule` - -The Panda model constants are: - -- `FRANKA_PANDA_MODEL`: `franka_description/urdf/panda.urdf.xacro` -- `FRANKA_PANDA_FK_MODEL`: `franka_description/urdf/panda.urdf` -- `FRANKA_PANDA_SRDF`: `franka_description/srdf/panda.srdf` - -The description package should be stored using the repository LFS data pattern: -`data/.lfs/franka_description.tar.gz` extracts to `data/franka_description/`. -Do not download or generate the Panda URDF/SRDF at import time. - -Start the registered mock coordinator/planner blueprint with: - -```bash -uv run --extra manipulation --extra vamp dimos run panda-coordinator \ - -o manipulationmodule.world.backend=vamp \ - -o manipulationmodule.world.artifact.mode=official \ - -o manipulationmodule.world.artifact.robot=panda \ - -o manipulationmodule.planner.backend=vamp \ - -o manipulationmodule.planner.algorithm=rrtc -``` - -Then use the manipulation client in another terminal and plan joint-space Panda -motions with `plan([...], "panda")`. - -## Failure modes - -- Missing VAMP dependency: selecting a VAMP world raises an install hint for - `vamp-planner` / `dimos[vamp]`. -- Invalid pairing: VAMP world and VAMP planner must be selected together. -- Incompatible kinematics: Drake optimization IK requires a Drake world; VAMP - pose planning requires a kinematics backend that naturally supports the VAMP - world capabilities it needs. -- Unsupported capability: VAMP does not expose Jacobians or minimum-distance - queries through the current Python API. -- Model/artifact mismatch: Panda benchmarks should verify URDF joint order and - limits against the official VAMP Panda artifact. - -## Contributor notes - -- Keep optional planner imports lazy and backend-scoped. -- Add new backends through typed `world`, `planner`, or `kinematics` config - variants with discriminator fields. -- Use explicit `DeprecationWarning` shims for migrated config fields. -- Raise clear unsupported-capability errors instead of synthesizing planner - features the backend does not expose. -- Prefer mock-control catalog fixtures for backend tests and benchmarks before - adding real hardware adapters. From 6c423bd9bbed79b68a3ab8508cb7f9bd01a00573 Mon Sep 17 00:00:00 2001 From: cc <55869557+TomCC7@users.noreply.github.com> Date: Thu, 18 Jun 2026 23:24:58 -0700 Subject: [PATCH 14/20] Update README.md --- docs/usage/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/usage/README.md b/docs/usage/README.md index 1b09a6c15c..071b6fc0b2 100644 --- a/docs/usage/README.md +++ b/docs/usage/README.md @@ -7,7 +7,6 @@ This page explains general concepts. - [Modules](/docs/usage/modules.md): The primary units of deployment in DimOS, modules run in parallel and are python classes. - [Streams](/docs/usage/sensor_streams/README.md): How modules communicate, a Pub / Sub system. - [Blueprints](/docs/usage/blueprints.md): a way to group modules together and define their connections to each other. -- [Manipulation planning](/docs/usage/manipulation_planning.md): world, planner, and kinematics backend configuration. - [RPC](/docs/usage/blueprints.md#calling-the-methods-of-other-modules): how one module can call a method on another module (arguments get serialized to JSON-like binary data). - [Skills](/docs/usage/blueprints.md#defining-skills): An RPC function, except it can be called by an AI agent (a tool for an AI). - Agents: AI that has an objective, access to stream data, and is capable of calling skills as tools. From 8f2944c8f59333a144c54cf4b4ab6aadb148a3e5 Mon Sep 17 00:00:00 2001 From: cc <55869557+TomCC7@users.noreply.github.com> Date: Thu, 18 Jun 2026 23:26:07 -0700 Subject: [PATCH 15/20] Apply suggestions from code review Co-authored-by: cc <55869557+TomCC7@users.noreply.github.com> --- dimos/utils/data.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/dimos/utils/data.py b/dimos/utils/data.py index 991352b9be..b4b7e2210a 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -357,12 +357,8 @@ def __getattribute__(self, name: str) -> object: return object.__getattribute__(self, name) # After construction, allow access to our internal attributes directly - if name in ( - "_lfs_filename", - "_lfs_resolved_cache", - "_ensure_downloaded", - ): - return super().__getattribute__(name) + if name in ("_lfs_filename", "_lfs_resolved_cache", "_ensure_downloaded"): + return object.__getattribute__(self, name) # For all other attributes, ensure download first then delegate to resolved path resolved = object.__getattribute__(self, "_ensure_downloaded")() From 79fefc8dd5fe759437e0bef6dc64b999f6e6a754 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 23:27:18 -0700 Subject: [PATCH 16/20] openspec remove --- CONTEXT.md | 109 --------- .../add-vamp-planning-backend/.openspec.yaml | 2 - .../add-vamp-planning-backend/design.md | 229 ------------------ .../changes/add-vamp-planning-backend/docs.md | 53 ---- .../add-vamp-planning-backend/proposal.md | 61 ----- .../specs/manipulation-stack/spec.md | 92 ------- .../specs/vamp-planning-backend/spec.md | 128 ---------- .../add-vamp-planning-backend/tasks.md | 75 ------ 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 -- 15 files changed, 1039 deletions(-) delete mode 100644 CONTEXT.md delete mode 100644 openspec/changes/add-vamp-planning-backend/.openspec.yaml delete mode 100644 openspec/changes/add-vamp-planning-backend/design.md delete mode 100644 openspec/changes/add-vamp-planning-backend/docs.md delete mode 100644 openspec/changes/add-vamp-planning-backend/proposal.md delete mode 100644 openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md delete mode 100644 openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md delete mode 100644 openspec/changes/add-vamp-planning-backend/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/CONTEXT.md b/CONTEXT.md deleted file mode 100644 index 3b14dd690c..0000000000 --- a/CONTEXT.md +++ /dev/null @@ -1,109 +0,0 @@ -# DimOS Planning - -DimOS planning describes how robot motion-planning backends are represented, selected, and integrated into the manipulation framework. - -## Language - -**VAMP backend**: -An optional full planning backend in which VAMP owns the robot and environment representation used for planning. -_Avoid_: VAMP planner-only plugin, universal VAMP planner - -**VAMP robot artifact**: -A prepared robot-specific VAMP bundle generated from robot description resources and compiled into an importable VAMP robot module. -_Avoid_: raw URDF, runtime robot model - -**Artifact preparation**: -The offline process that turns URDF-derived robot resources into VAMP robot artifacts before planning runtime. -_Avoid_: runtime generation, dynamic URDF planning - -**VAMP artifact recipe**: -A reproducible DimOS-owned description of how to generate a VAMP robot artifact from robot resources. -_Avoid_: generated artifact source, manual VAMP fork patch - -**VAMP optional dependency**: -The pip-installable VAMP package used by DimOS only when the VAMP backend is selected. -_Avoid_: required manipulation dependency, vendored VAMP source - -**Pinned VAMP dependency**: -A commit-pinned VAMP optional dependency used while the backend integration depends on unreleased packaging or robot artifact support. -_Avoid_: floating Git dependency, permanent fork dependency - -**VAMP planner algorithm**: -The algorithm selected inside the VAMP backend after DimOS has selected the VAMP planner adapter. -_Avoid_: separate global planner name, backend name - -**World backend**: -The selected planning-world implementation that owns robot registration, obstacle representation, collision checks, and synchronization for manipulation planning. -_Avoid_: hidden planner world, implicit backend - -**Backend-agnostic pose planning**: -Pose planning in which DimOS converts a target pose into a goal joint state through a backend-agnostic IK solver before invoking the selected planner backend. -_Avoid_: VAMP-specific pose planner, backend-specific pose conversion - -**Planner-native VAMP backend**: -A VAMP backend that exposes DimOS functions only where they are supported by VAMP's native planning and robot APIs, such as joint-space planning, path validation, collision checking, and forward/end-effector kinematics. -_Avoid_: synthetic Jacobian support, pretending unsupported planner capabilities exist - -**VAMP kinematics boundary**: -The separation between VAMP-owned joint-space planning/world validity and DimOS-owned pose-to-joint kinematics. VAMP should not expose IK or Jacobian behavior unless VAMP or a compatible kinematics component naturally supports it. -_Avoid_: planner-owned IK, manufactured VAMP kinematics - -**VAMP pose planning availability**: -Pose planning with VAMP is available only when DimOS has an explicitly compatible kinematics component for the VAMP world surface. It is not implied by selecting the VAMP planner. -_Avoid_: implicit VAMP pose support, fake Jacobian fallback - -**Initial VAMP capability set**: -The first coherent VAMP integration surface: joint-space planning, VAMP algorithm selection, native path simplification/validation, official or user-prepared artifact loading, environment conversion, joint-configuration validity, joint limits when available, and native FK/end-effector pose queries. -_Avoid_: all-interface parity, synthetic unsupported methods - -**User-prepared VAMP artifact**: -A robot-specific VAMP artifact generated outside DimOS by the user or by an upstream VAMP distribution, then loaded by DimOS at runtime. -_Avoid_: DimOS-owned artifact generation pipeline, automatic arbitrary-robot compilation - -**VAMP artifact loading**: -The runtime mechanism by which DimOS selects either an official VAMP robot artifact or a user-specified custom artifact path for planning. -_Avoid_: dynamic artifact generation, implicit robot artifact synthesis - -**Custom VAMP artifact path**: -A local world configuration value that points DimOS to a user-prepared VAMP artifact for robots not covered by official VAMP artifacts. -_Avoid_: hidden local artifact cache, hardcoded custom robot module - -**Local world configuration**: -Backend-specific planning-world configuration attached to the world/robot planning context rather than to global `ManipulationModule` settings. VAMP artifact selection belongs here because it is part of how the local VAMP world is loaded. -_Avoid_: global VAMP artifact settings, module-wide custom artifact path - -**Typed backend configuration**: -A discriminated configuration object that selects a backend with a `backend` field and carries backend-specific options in the same local config object. -_Avoid_: unrelated flat module-level backend fields, untyped string-only configuration - -**Noisy config deprecation**: -A temporary compatibility path for legacy configuration that emits a visible deprecation warning when used, so future maintainers know the behavior is scheduled for removal. -_Avoid_: silent compatibility shim, permanent legacy alias - -**Nested-only VAMP settings**: -New VAMP-specific settings that live only inside typed nested world/planner backend configuration. VAMP should not introduce new flat `ManipulationModuleConfig` fields; only pre-existing flat fields may receive temporary warning-backed compatibility paths during migration. -_Avoid_: `vamp_*` module fields, silent legacy aliases - -**VAMP world/planner config split**: -The separation between VAMP world configuration, which owns artifact loading and world representation, and VAMP planner configuration, which owns algorithm selection and planner behavior. -_Avoid_: putting artifact paths in planner config, putting algorithm tuning in world config - -**Strict VAMP world/planner pairing**: -The validation rule that VAMP world configuration must be paired with VAMP planner configuration, because VAMP planning depends on VAMP-native robot artifacts and environment representation. -_Avoid_: VAMP planner over Drake world, generic planner over VAMP world - -**VAMP kinematics compatibility validation**: -The rule that VAMP pose planning is enabled only when the configured kinematics backend can operate on the VAMP world surface. Joint-space VAMP planning does not imply IK or Jacobian support. -_Avoid_: implicit VAMP pose planning, planner-owned Jacobian checks - -**Unsupported world capability**: -A clear failure mode for optional world operations that a backend does not natively support, such as a VAMP world rejecting Jacobian queries instead of manufacturing synthetic Jacobian behavior. -_Avoid_: fake interface completeness, planner-owned capability probing - -**Franka Panda mock support**: -A DimOS catalog/control-coordinator configuration for the Franka Panda arm that defaults to mock control while providing robot model metadata for manipulation planning tests and planner benchmarks. -_Avoid_: real hardware requirement, VAMP-only test robot - -**LFS-backed robot description**: -A robot model package stored through DimOS' existing `data/.lfs/*.tar.gz` asset pattern and referenced in code through `LfsPath`. -_Avoid_: runtime URDF download, generated robot description at import time diff --git a/openspec/changes/add-vamp-planning-backend/.openspec.yaml b/openspec/changes/add-vamp-planning-backend/.openspec.yaml deleted file mode 100644 index c2d56cecef..0000000000 --- a/openspec/changes/add-vamp-planning-backend/.openspec.yaml +++ /dev/null @@ -1,2 +0,0 @@ -schema: dimos-capability -created: 2026-06-17 diff --git a/openspec/changes/add-vamp-planning-backend/design.md b/openspec/changes/add-vamp-planning-backend/design.md deleted file mode 100644 index 0fab0e80b5..0000000000 --- a/openspec/changes/add-vamp-planning-backend/design.md +++ /dev/null @@ -1,229 +0,0 @@ -## Context - -DimOS manipulation planning is currently centered on the existing planning contracts in `dimos/manipulation/planning/spec/protocols.py`: - -- `WorldSpec` owns robot/world state, collision checks, FK/link poses, and currently includes Jacobian access. -- `PlannerSpec` owns joint-space path planning from start to goal joint states. -- `KinematicsSpec` owns pose-to-joint-state solving. -- `VisualizationSpec` is optional and currently implemented by `DrakeWorld` through Meshcat. - -The current module wiring in `dimos/manipulation/manipulation_module.py` initializes a `WorldMonitor`, adds configured `RobotModelConfig` objects, finalizes the world, then creates planner and kinematics backends. `WorldMonitor` already has a backend seam (`WorldMonitor(backend="drake", enable_viz=False, **kwargs)`) that delegates to `create_world(...)` in `dimos/manipulation/planning/factory.py`. - -VAMP has a different model than the current Drake-backed world. Its Python package exposes robot-specific planning modules and utilities such as joint-space planning algorithms, validation/debug helpers, `fk`, and `eefk`. It does not expose a general runtime URDF-to-planner pipeline through the runtime binding, and it does not expose a public Jacobian API. Custom robot support is an offline/user-owned artifact-generation concern upstream of runtime planning. - -PR #2481 for Pink IK establishes the configuration pattern this change should follow: typed discriminated backend config objects with `backend` as the discriminator, backend-specific settings in the nested config, and deprecated flat compatibility shims that emit warnings when used. - -## Goals / Non-Goals - -**Goals:** - -- Add VAMP as an optional manipulation planning backend for joint-space planning. -- Load VAMP robot artifacts from either official VAMP robot modules/artifacts or a user-provided custom artifact path. -- Keep VAMP dependency loading lazy and backend-scoped. -- Add typed nested world and planner config objects instead of new flat `ManipulationModuleConfig.vamp_*` fields. -- Validate VAMP world/planner pairing eagerly during planning-stack initialization. -- Keep pose planning conditional on an explicitly compatible kinematics backend. -- Add a Franka Panda catalog/configuration target for mock-control planning tests and future planner benchmarks. -- Fail clearly when a selected backend or kinematics solver calls an unsupported world capability. -- Preserve existing Drake/default manipulation behavior unless users opt into VAMP or migrated config fields. - -**Non-Goals:** - -- DimOS will not generate VAMP artifacts from URDF/SRDF/meshes. -- DimOS will not own a foam/cricket/fkcc_gen artifact-preparation pipeline in this change. -- VAMP will not expose synthetic Jacobian support. -- VAMP will not pretend to satisfy unsupported `WorldSpec` behavior just to match the Drake surface. -- VAMP planner selection will not imply pose-planning support. -- Real Franka Panda hardware control is not required in this change. -- No new runnable robot blueprint is required unless implementation later chooses to add a VAMP/Panda demo or benchmark blueprint. - -## DimOS Architecture - -### Configuration - -Introduce typed nested planning config objects analogous to the Pink IK pattern: - -```python -world={ - "backend": "vamp", - "artifact": { - "mode": "official", - "robot": "panda", - }, -} - -planner={ - "backend": "vamp", - "algorithm": "rrtc", - "simplify": True, - "validate_path": True, -} -``` - -Custom artifact mode uses an explicit user-prepared path: - -```python -world={ - "backend": "vamp", - "artifact": { - "mode": "custom", - "path": "/path/to/user/prepared/artifact", - }, -} -``` - -VAMP-specific settings must be nested-only. Do not add fields such as `vamp_robot` or `vamp_artifact_path` directly to `ManipulationModuleConfig`. - -Existing flat fields that are migrated, such as `planner_name`, `kinematics_name`, or `enable_viz`, may remain temporarily as compatibility shims. Those shims must emit a visible `DeprecationWarning` when used. The `Deprecated` package is appropriate for deprecated callable APIs, but config-field migration should use explicit warnings in config normalization/validation because config fields are data, not callables. - -### World backend - -Add a VAMP world backend that is responsible for: - -- Loading official VAMP artifacts/modules by configured robot name. -- Loading custom user-prepared artifacts by explicit configured path. -- Holding VAMP-native robot/environment representation. -- Exposing native VAMP validity/FK/EE pose behavior where supported. -- Converting supported DimOS obstacle/environment inputs into VAMP environment data. -- Raising a clear unsupported-capability error for operations that are not natively available. - -The VAMP world must not dynamically generate artifacts or invoke the VAMP artifact-generation toolchain. If a robot is not covered by official VAMP artifacts, the user owns generating/building the artifact and provides the path/config needed to load it. - -### Planner backend - -Add a VAMP planner backend that is responsible for: - -- Joint-space planning only. -- Configurable algorithm selection (`rrtc`, `prm`, `fcit`, `aorrtc`) with default `rrtc`. -- Native path simplification when enabled and available. -- Native path validation when enabled and available. -- Backend-scoped lazy imports and actionable dependency errors. - -`VampPlanner` must not call `WorldSpec.get_jacobian()`, solve IK, or own pose-to-joint conversion. Jacobian and IK are kinematics concerns, not planner concerns. - -### Kinematics and pose planning - -Pose planning remains a DimOS-level flow: - -```text -target pose -> KinematicsSpec -> goal joint state -> PlannerSpec -> joint path -``` - -For VAMP, this flow is enabled only when the selected `KinematicsSpec` declares or demonstrates compatibility with the VAMP world surface. The initial VAMP scope is joint-space planning. A future FK-only or derivative-free IK backend could enable VAMP pose planning without requiring VAMP to manufacture Jacobian behavior. - -If `kinematics={"backend": "jacobian"}` is paired with `world={"backend": "vamp"}` and the VAMP world does not support Jacobian, initialization or the first compatibility check should fail with an explicit incompatibility message rather than a generic attribute error. - -### Factory and validation - -Update planning factory wiring to dispatch from typed config objects rather than only string names. The factory should validate cross-backend combinations before planning: - -- `VampWorldConfig` requires `VampPlannerConfig`. -- `VampPlannerConfig` requires `VampWorldConfig`. -- VAMP world/planner pairs are invalid with incompatible kinematics backends. -- Drake/default behavior remains valid without VAMP installed. - -### Streams, blueprints, skills, and CLI - -No new streams are required. Existing manipulation RPCs and skills continue to use the existing planning module surface. VAMP affects the planning implementation selected under the module, not the LCM/SHM/ROS/DDS transport topology. - -Blueprints can opt into VAMP through nested config. CLI examples should use nested override paths, such as: - -```bash --o manipulationmodule.world.backend=vamp --o manipulationmodule.world.artifact.mode=official --o manipulationmodule.world.artifact.robot=panda --o manipulationmodule.planner.backend=vamp --o manipulationmodule.planner.algorithm=rrtc -``` - -If no new blueprints are added, `dimos/robot/all_blueprints.py` should not need regeneration. If implementation adds or renames blueprint variables, run `pytest dimos/robot/test_all_blueprints_generation.py`. - -### Franka Panda mock-control support - -Add Franka Panda support through the existing robot catalog and coordinator patterns, not as a VAMP-only special case. The shape should mirror `dimos.robot.catalog.ufactory` and `dimos.robot.catalog.piper`: - -- Provide a catalog function such as `franka_panda(...) -> RobotConfig` in a Franka/Panda catalog module. -- Default `adapter_type` to `"mock"` so the `ControlCoordinator` uses the existing mock manipulator adapter unless a future real adapter is explicitly selected. -- Provide model/FK constants for the Panda robot model used by manipulation planning, cartesian IK tasks, tests, and benchmarks. -- Store Panda URDF/SRDF/model resources in a repository LFS-backed robot description package, following the existing `data/.lfs/_description.tar.gz` pattern used by catalog assets and referencing the extracted package through `LfsPath`. -- Configure the Panda `RobotConfig` with explicit arm joint names, base link, end-effector link, home joints, package paths, and collision exclusions if needed. -- Let `RobotConfig.to_hardware_component()` feed the coordinator hardware list and `RobotConfig.to_robot_model_config()` feed the `ManipulationModule`, matching the xArm/Piper pattern. - -The initial Panda scope is mock control plus planning metadata. A real Panda hardware adapter may be introduced later behind the same `adapter_type` seam, but VAMP testing and benchmarking should not depend on real hardware availability. - -## Decisions - -### Q&A decision record - -1. **VAMP is a full optional backend, not a generic planner-only plugin.** It owns a VAMP-native robot/environment representation and planner behavior when selected. -2. **DimOS will not dynamically generate VAMP artifacts.** Artifact generation is too large for this integration. Users either use official VAMP artifacts or provide a custom user-prepared artifact path. -3. **VAMP artifacts are runtime-loaded, not maintained as checked-in generated source by DimOS.** Official artifacts come from the VAMP distribution; custom artifacts come from the user. -4. **VAMP remains an optional dependency.** Adapter imports should be lazy and raise actionable installation errors when VAMP is selected but unavailable. -5. **`planner.backend="vamp"` selects the DimOS VAMP planner adapter.** The specific VAMP algorithm is a planner config option, not separate global planner names like `vamp_rrtc`. -6. **Config uses typed nested objects.** This follows PR #2481's Pink IK pattern and avoids new flat VAMP fields on `ManipulationModuleConfig`. -7. **VAMP world and planner must be strictly paired.** Mixed VAMP/Drake world-planner combinations are invalid unless a future bridge explicitly supports them. -8. **Pose planning is not VAMP planner behavior.** It remains a `KinematicsSpec` concern that converts pose goals to joint goals before planning. -9. **Do not expose synthetic Jacobian support.** VAMP has `fk`/`eefk` utilities but no public Jacobian API; the adapter should not invent one. -10. **Initial VAMP capability set is minimal and native.** Joint-space planning, algorithm selection, native validation/simplification, official/custom artifact loading, environment conversion, joint-configuration validity, joint limits when available, and native FK/EE pose where available. -11. **Pose planning with VAMP is conditional.** It is available only if an explicitly compatible kinematics backend is configured. -12. **Unsupported world capabilities fail clearly.** Backends may raise a dedicated unsupported-capability error rather than faking interface completeness. -13. **Legacy config compatibility must be noisy.** Existing flat migrated fields may be accepted temporarily but must emit deprecation warnings. -14. **Franka Panda support is mock-control first.** The Panda catalog target exists to test and benchmark planning flows without requiring physical Panda hardware. -15. **Panda robot descriptions follow the repo LFS pattern.** Panda URDF/SRDF/model assets should be checked in as LFS-backed data package contents and referenced via `LfsPath`, not fetched or generated at runtime. - -### Alternatives considered - -- **Generate VAMP artifacts in DimOS:** rejected as too complex for this change because it would require spherization, tracing/codegen, native compilation, and local cache semantics. -- **Expose VAMP as planner-only over arbitrary `WorldSpec`:** rejected because VAMP planning depends on VAMP-native robot/environment artifacts. -- **Add a numerical Jacobian wrapper over `eefk`:** rejected because it manufactures a capability not inherently supported by the planner/backend. -- **Add flat VAMP module fields:** rejected in favor of typed nested backend config, matching the Pink IK direction. -- **Require real Panda hardware support before benchmarking:** rejected because planning tests and benchmarks need a repeatable mock-control target first. -- **Fetch Panda URDF/SRDF dynamically during tests:** rejected because DimOS catalog robots should follow the existing LFS-backed description package pattern for reproducible offline tests. - -## Safety / Simulation / Replay - -VAMP changes planning, not control execution. Generated trajectories still flow through existing manipulation execution and coordinator surfaces. Safety depends on correct VAMP artifact loading, obstacle conversion, collision validity, and trajectory execution checks. - -Hardware safety constraints: - -- VAMP must fail closed on invalid artifact config, missing dependency, invalid backend pairing, or unsupported kinematics combination. -- VAMP should not silently skip collision/path validation when validation is configured. -- User-prepared artifacts must be treated as trusted local planning inputs; DimOS should validate presence/loadability but cannot prove the artifact accurately models the robot. - -Simulation/replay: - -- Non-VAMP replay and simulation stacks must not import or initialize VAMP. -- VAMP behavior should be testable with fake or official lightweight artifacts before hardware use. -- Franka Panda mock-control support should allow planner testing and benchmarking without commanding physical hardware. -- Manual QA should include a joint-space plan on an official VAMP robot artifact and verification that existing Drake planning still works without VAMP installed. - -## Risks / Trade-offs - -- **Artifact mismatch risk:** A user-prepared artifact may not match the physical robot or DimOS `RobotModelConfig`. Mitigate with explicit config, load-time validation, and documentation. -- **Protocol mismatch risk:** Existing `WorldSpec` includes methods VAMP should not implement. Mitigate with explicit unsupported-capability errors and compatibility validation. -- **Dependency risk:** VAMP is native/compiled. Mitigate with lazy imports and optional extras. -- **Config migration risk:** Moving toward nested config may disturb existing blueprints. Mitigate with deprecation warnings and tests covering legacy shims. -- **Environment conversion risk:** VAMP supports specific environment representations. Start with documented/supported obstacle conversions and fail clearly for unsupported obstacle types. -- **Panda model mismatch risk:** The DimOS Panda catalog model, mock coordinator joint names, and VAMP official Panda artifact may diverge. Mitigate with explicit joint-name/model validation in tests and documentation. -- **LFS asset drift risk:** The Panda URDF/SRDF package may diverge from catalog constants or benchmark expectations. Mitigate by resolving assets through `LfsPath` in tests and validating expected files/joints during catalog tests. - -## Migration / Rollout - -1. Add typed world and planner config objects with default Drake-compatible behavior. -2. Add temporary compatibility shims for existing flat fields that are being migrated, each with `DeprecationWarning`. -3. Add VAMP world/planner adapters behind lazy imports. -4. Add cross-config validation before planning begins. -5. Update docs with official/custom artifact loading examples and CLI override examples. -6. Add the LFS-backed Franka Panda robot description package with URDF/SRDF/model assets and catalog references through `LfsPath`. -7. Add Franka Panda mock-control catalog support and tests for coordinator/manipulation config generation. -8. Add tests before enabling any blueprint-level VAMP examples. -9. If new blueprints are introduced, regenerate and verify `dimos/robot/all_blueprints.py` with the registry test. - -Rollback is straightforward if VAMP remains optional and default config remains Drake-compatible: remove the VAMP config/adapters and keep or revert the generic typed-config migration separately. - -## Open Questions - -- Which official VAMP robots should be documented as supported at first depends on the installed `vamp-planner` distribution used during implementation. -- The exact custom artifact loading shape depends on how user-prepared VAMP artifacts are importable from the Python binding or local path. -- The initial supported obstacle/environment conversion set should be confirmed against VAMP's current Python API during implementation. -- The exact Franka Panda LFS package name, internal URDF/SRDF paths, and package source should be selected during implementation, then validated against the official VAMP Panda artifact joint order. diff --git a/openspec/changes/add-vamp-planning-backend/docs.md b/openspec/changes/add-vamp-planning-backend/docs.md deleted file mode 100644 index b0ab4c681a..0000000000 --- a/openspec/changes/add-vamp-planning-backend/docs.md +++ /dev/null @@ -1,53 +0,0 @@ -## User-Facing Docs - -- Update manipulation planning documentation under `docs/capabilities/` or `docs/usage/` to describe optional VAMP planning backend support. -- Document that VAMP supports initial joint-space planning only, and that pose planning requires an explicitly compatible kinematics backend. -- Add nested config examples for official artifacts: - - ```python - world={"backend": "vamp", "artifact": {"mode": "official", "robot": "panda"}} - planner={"backend": "vamp", "algorithm": "rrtc"} - ``` - -- Add nested config examples for custom user-prepared artifacts: - - ```python - world={"backend": "vamp", "artifact": {"mode": "custom", "path": "/path/to/artifact"}} - planner={"backend": "vamp", "algorithm": "rrtc"} - ``` - -- Document that DimOS does not generate VAMP artifacts. Users who need unsupported robots must generate/build VAMP artifacts themselves and provide the custom path. -- Document Franka Panda mock-control support as the recommended initial robot target for VAMP planning tests and benchmarks, including catalog usage, LFS-backed URDF/SRDF asset expectations, and the fact that control remains mock by default. -- Document CLI override examples using nested config paths, including world backend, artifact mode, official robot name or custom path, planner backend, and algorithm. -- Document expected failure modes: missing optional VAMP dependency, invalid world/planner pairing, invalid artifact config, unsupported obstacle/environment conversion, and incompatible kinematics. - -## Contributor Docs - -- Update planning backend contributor guidance if present, or add a short section to manipulation planning docs explaining the typed backend config pattern. -- Mention that backend imports should be lazy and adapter-owned, with actionable dependency errors. -- Mention that migrated flat config fields must emit `DeprecationWarning` and should be scheduled for removal. -- Mention that VAMP artifact generation is intentionally out of scope for DimOS; contributor work should focus on loading official or user-prepared artifacts. -- Mention that Franka Panda support should follow the shared `RobotConfig` catalog pattern, store URDF/SRDF resources through the repo's LFS-backed data package pattern, and should not require a real hardware adapter for tests or benchmarks. - -## Coding-Agent Docs - -- No AGENTS.md update is required unless implementation introduces new recurring coding-agent rules. -- If docs under `docs/coding-agents/` include manipulation planning guidance, update them with the VAMP boundaries: - - no synthetic Jacobian support, - - no DimOS artifact-generation pipeline, - - VAMP config lives in typed nested world/planner config, - - VAMP planner does not own IK. - - Franka Panda mock support is a catalog/coordinator test target, not a VAMP-specific hardware driver. - - Panda model resources should be `LfsPath`-referenced LFS assets, not runtime downloads. - -## Doc Validation - -- Run any repository doc link validation command documented for DimOS if docs are changed. -- For Markdown examples with runnable Python snippets, run the documented `md-babel-py run ` command where applicable. -- If diagrams are added or changed, run the documented diagram generation command such as `bin/gen-diagrams` if required by the touched docs. -- At minimum, run targeted tests that cover any code examples embedded in documentation or keep examples clearly illustrative if they cannot run without optional VAMP artifacts. -- Validate any Panda examples against the selected LFS-backed Panda model/SRDF assets and mock coordinator joint naming. - -## No Docs Needed - -Documentation is needed because the change adds a user-visible optional planning backend, new nested configuration shape, dependency behavior, artifact-loading expectations, explicit unsupported-capability semantics, and a mock-control Franka Panda target for tests/benchmarks. diff --git a/openspec/changes/add-vamp-planning-backend/proposal.md b/openspec/changes/add-vamp-planning-backend/proposal.md deleted file mode 100644 index d796a15696..0000000000 --- a/openspec/changes/add-vamp-planning-backend/proposal.md +++ /dev/null @@ -1,61 +0,0 @@ -## Why - -DimOS manipulation currently has a Drake-centered planning stack with generic world, planner, and kinematics protocols. VAMP offers a fast optional motion-planning backend for supported robot artifacts, but it has a different capability model: it plans over VAMP-native robot/environment representations and exposes native validation/FK utilities rather than arbitrary runtime URDF ingestion or a full kinematics interface. - -This change adds VAMP as an optional manipulation planning backend while preserving DimOS' backend boundaries. It should let users run joint-space planning through official VAMP robot artifacts or their own user-prepared artifacts, without making non-VAMP stacks pay dependency, import, or configuration costs. - -The change also adds Franka Panda mock-control support so VAMP can be tested and benchmarked against an official VAMP robot without requiring real hardware. - -## What Changes - -- Add a VAMP world backend for loading official or user-prepared VAMP artifacts and representing VAMP-native planning state. -- Add a VAMP planner backend for joint-space planning with configurable VAMP algorithms such as `rrtc`, `prm`, `fcit`, and `aorrtc`. -- Add typed nested world/planner configuration for backend selection and VAMP-specific options, following the Pink IK configuration pattern. -- Require strict VAMP world/planner pairing during planning-stack initialization. -- Keep VAMP pose planning conditional on an explicitly compatible kinematics backend; joint-space VAMP planning must not imply IK or Jacobian support. -- Provide clear unsupported-capability failures when a backend-specific world operation is not natively supported. -- Keep VAMP optional and lazily imported with actionable dependency errors. -- Do not generate VAMP artifacts in DimOS; users must rely on official VAMP artifacts or provide a custom user-prepared artifact path. -- Add a Franka Panda robot catalog entry and mock-control coordinator path for planning tests and later planner benchmarking, with Panda URDF/SRDF assets stored through the existing LFS-backed robot description pattern. -- Add noisy deprecation warnings for migrated pre-existing flat configuration fields where compatibility shims remain. - -## Affected DimOS Surfaces - -- Modules/streams: - - `ManipulationModule` planning-stack initialization and validation. - - `WorldMonitor` world backend creation path. - - Manipulation planning factory functions for world, planner, and config dispatch. - - Planning world/planner/kinematics protocols where unsupported optional capabilities need clear failure behavior. -- Blueprints/CLI: - - Manipulation blueprints may configure nested world/planner backend objects. - - CLI override examples should use nested config paths such as `manipulationmodule.world.backend=vamp` and `manipulationmodule.planner.backend=vamp`. -- Skills/MCP: - - No direct skill or MCP tool behavior changes are expected; existing manipulation RPCs/skills should fail clearly for unsupported pose-planning combinations. -- Hardware/simulation/replay: - - VAMP is an optional planning backend for manipulation stacks. - - Franka Panda support should default to the existing mock manipulator adapter pattern for control. - - Franka Panda URDF/SRDF assets should live in an LFS-backed robot description package and be referenced through `LfsPath`, matching existing catalog assets such as xArm, Piper, A-750, and OpenArm descriptions. - - A future real Panda hardware adapter may be added behind the same catalog/config seam, but real hardware control is not required for this change. - - Runtime planning behavior changes only when VAMP is selected. -- Docs/generated registries: - - Planning backend documentation and manipulation capability docs need updates. - - No blueprint registry generation change is expected unless new runnable blueprints are added. - -## Capabilities - -### New Capabilities - -- `vamp-planning-backend`: Behavior for optional VAMP world/planner configuration, artifact loading, joint-space planning, validation, and unsupported-capability handling. -- `manipulation-stack`: Behavior for typed manipulation planning backend configuration, compatibility validation, and legacy config migration. - -### Modified Capabilities - -- None. - -## Impact - -Users gain an optional VAMP planning path for fast joint-space manipulation planning when official or custom user-prepared VAMP artifacts are available. Developers gain a typed backend configuration pattern for world and planner options rather than adding VAMP-specific flat module fields. Test and benchmarking workflows gain a mock-control Franka Panda catalog target aligned with VAMP's official Panda artifact. - -Compatibility risk is concentrated around migrating existing flat fields such as planner and visualization selection into nested config objects. Any retained compatibility path should emit a visible `DeprecationWarning` so maintainers know it is temporary. VAMP adds optional runtime dependencies only when selected; missing dependencies or invalid backend combinations should fail at planning-stack initialization with actionable messages. - -Testing should cover config parsing, factory dispatch, lazy import errors, VAMP world/planner pairing validation, artifact-mode validation, joint-space planning adapter behavior with fakes/mocks, Franka Panda mock catalog/coordinator wiring, LFS-backed Panda URDF/SRDF asset resolution, unsupported kinematics combinations, and preservation of existing Drake/default behavior. diff --git a/openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md b/openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md deleted file mode 100644 index acb063e70b..0000000000 --- a/openspec/changes/add-vamp-planning-backend/specs/manipulation-stack/spec.md +++ /dev/null @@ -1,92 +0,0 @@ -## ADDED Requirements - -### Requirement: Typed manipulation planning backend configuration - -DimOS MUST support typed nested configuration for manipulation world, planner, and kinematics backend selection. - -#### Scenario: nested backend config selects planning components -- **GIVEN** a manipulation module configuration with nested `world`, `planner`, and `kinematics` backend objects -- **WHEN** the manipulation planning stack is initialized -- **THEN** DimOS SHALL use the configured backend discriminators to create the selected planning components -- **AND** backend-specific options SHALL be read from the matching nested backend config object. - -#### Scenario: backend-specific options stay local -- **GIVEN** a backend-specific option for world loading or planner tuning -- **WHEN** the option is configured -- **THEN** DimOS MUST read world-loading options from the world config -- **AND** DimOS MUST read planner behavior options from the planner config. - -### Requirement: Planning stack compatibility validation - -DimOS MUST validate incompatible world, planner, and kinematics backend combinations before unsafe or unsupported planning proceeds. - -#### Scenario: invalid backend combination -- **GIVEN** a manipulation planning configuration with incompatible world and planner backends -- **WHEN** the planning stack is initialized -- **THEN** DimOS MUST reject the configuration before executing a plan -- **AND** the error message MUST identify the incompatible backends. - -#### Scenario: invalid kinematics combination -- **GIVEN** a manipulation planning configuration with a kinematics backend that requires unsupported world capabilities -- **WHEN** pose-planning compatibility is checked -- **THEN** DimOS MUST reject the pose-planning configuration clearly -- **AND** joint-space-only planning modes SHALL not be advertised as pose-planning support. - -### Requirement: Legacy flat config migration warnings - -DimOS MUST emit visible deprecation warnings when pre-existing flat manipulation planning config fields are accepted as temporary compatibility shims. - -#### Scenario: legacy planner field used -- **GIVEN** a user configures a pre-existing flat planner-selection field that has a nested replacement -- **WHEN** the config is normalized or the planning stack is initialized -- **THEN** DimOS SHALL preserve the legacy behavior for the compatibility period -- **AND** DimOS MUST emit a `DeprecationWarning` that identifies the nested replacement. - -#### Scenario: new backend settings avoid flat fields -- **GIVEN** a new backend-specific setting is introduced for manipulation planning -- **WHEN** the setting is exposed to users -- **THEN** DimOS MUST expose it through the typed nested backend configuration -- **AND** DimOS SHALL avoid introducing new flat module-level backend fields. - -### Requirement: Default manipulation planning compatibility - -DimOS MUST preserve the default manipulation planning behavior for users who do not opt into new backend config. - -#### Scenario: existing default config -- **GIVEN** a manipulation module configuration that relies on current defaults -- **WHEN** the manipulation module starts -- **THEN** DimOS SHALL initialize the default world, planner, and kinematics behavior -- **AND** optional backend dependencies SHALL not be required. - -#### Scenario: existing non-VAMP blueprint -- **GIVEN** a manipulation blueprint that does not select VAMP -- **WHEN** the blueprint is run -- **THEN** DimOS SHALL not perform VAMP artifact loading -- **AND** DimOS SHALL not require VAMP-specific dependencies. - -### Requirement: Franka Panda mock-control catalog support - -DimOS MUST provide Franka Panda catalog support that can be used for manipulation planning tests and planner benchmarks without requiring physical Panda hardware. - -#### Scenario: Panda catalog creates mock hardware config -- **GIVEN** a user or test constructs the Franka Panda catalog configuration with default control settings -- **WHEN** the configuration is converted to a `HardwareComponent` -- **THEN** DimOS SHALL configure a manipulator hardware component using the mock adapter -- **AND** the component SHALL expose the Panda arm joints expected by the coordinator. - -#### Scenario: Panda catalog creates manipulation model config -- **GIVEN** a user or test constructs the Franka Panda catalog configuration -- **WHEN** the configuration is converted to a manipulation robot model config -- **THEN** DimOS SHALL provide the Panda model path, base link, end-effector link, joint names, package paths, and home joints needed by the manipulation planning stack. - -#### Scenario: Panda model assets resolve from LFS-backed data -- **GIVEN** the Franka Panda catalog configuration references Panda URDF/SRDF resources -- **WHEN** tests or blueprints resolve those resources -- **THEN** DimOS SHALL resolve them from an LFS-backed robot description package through `LfsPath` -- **AND** DimOS SHALL avoid downloading or generating Panda robot descriptions at runtime. - -#### Scenario: Panda target supports VAMP planning tests -- **GIVEN** VAMP is configured with an official Panda artifact and the DimOS Panda catalog robot model -- **WHEN** a joint-space planning test or benchmark initializes the manipulation stack -- **THEN** DimOS SHALL support running the flow through mock control surfaces -- **AND** DimOS SHALL not require a real Panda hardware adapter. diff --git a/openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md b/openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md deleted file mode 100644 index 302f2addcd..0000000000 --- a/openspec/changes/add-vamp-planning-backend/specs/vamp-planning-backend/spec.md +++ /dev/null @@ -1,128 +0,0 @@ -## ADDED Requirements - -### Requirement: VAMP backend selection - -DimOS MUST allow users to select VAMP as an optional manipulation planning backend through typed world and planner backend configuration. - -#### Scenario: select VAMP stack -- **GIVEN** a manipulation module configuration with `world.backend` set to `vamp` -- **AND** `planner.backend` set to `vamp` -- **WHEN** the planning stack is initialized -- **THEN** DimOS SHALL initialize the VAMP planning path instead of the default non-VAMP planning path -- **AND** non-VAMP backends SHALL remain usable without VAMP being installed. - -#### Scenario: reject mixed VAMP and non-VAMP stack -- **GIVEN** a manipulation module configuration with exactly one of the world or planner backends set to `vamp` -- **WHEN** the planning stack is initialized -- **THEN** DimOS MUST reject the configuration before planning begins -- **AND** the error message MUST identify the incompatible world/planner pairing. - -### Requirement: VAMP artifact loading - -DimOS MUST load VAMP robot artifacts from either official VAMP artifacts or a user-provided custom artifact path. - -#### Scenario: load official artifact -- **GIVEN** `world.backend` is `vamp` -- **AND** the VAMP artifact mode is `official` -- **AND** an official robot artifact name is configured -- **WHEN** the planning stack is initialized -- **THEN** DimOS SHALL attempt to load the named official VAMP artifact -- **AND** initialization MUST fail clearly if the artifact is unavailable. - -#### Scenario: load custom user-prepared artifact -- **GIVEN** `world.backend` is `vamp` -- **AND** the VAMP artifact mode is `custom` -- **AND** a custom artifact path is configured -- **WHEN** the planning stack is initialized -- **THEN** DimOS SHALL attempt to load the user-prepared artifact from that path -- **AND** initialization MUST fail clearly if the path is missing, invalid, or not loadable. - -### Requirement: No artifact generation by DimOS - -DimOS MUST treat VAMP artifact generation as outside the runtime planning backend. - -#### Scenario: unsupported robot requires custom artifact -- **GIVEN** a robot that is not available as an official VAMP artifact -- **WHEN** a user selects VAMP planning for that robot -- **THEN** DimOS MUST require a loadable user-prepared custom artifact -- **AND** DimOS SHALL report that artifact generation must be performed outside DimOS when no loadable artifact is provided. - -### Requirement: VAMP joint-space planning - -The VAMP planner backend MUST support joint-space planning with a configured VAMP planning algorithm. - -#### Scenario: plan between joint states -- **GIVEN** a valid VAMP world and planner configuration -- **AND** a start joint state and goal joint state for the configured robot -- **WHEN** joint-space planning is requested -- **THEN** DimOS SHALL invoke the configured VAMP planning algorithm -- **AND** the result MUST report either a collision-free joint path or a clear planning failure. - -#### Scenario: configure planner algorithm -- **GIVEN** a valid VAMP planner configuration with an algorithm value such as `rrtc`, `prm`, `fcit`, or `aorrtc` -- **WHEN** the planning stack is initialized -- **THEN** DimOS SHALL use that algorithm for VAMP joint-space planning -- **AND** DimOS MUST reject unsupported algorithm values before planning begins. - -### Requirement: VAMP native validation and simplification - -DimOS MUST use VAMP-native validation and simplification behavior only when it is available and configured. - -#### Scenario: validate planned path -- **GIVEN** VAMP path validation is enabled -- **AND** VAMP exposes validation for the planned path or sampled path states -- **WHEN** a VAMP plan is produced -- **THEN** DimOS SHALL validate the path before reporting success -- **AND** DimOS MUST report failure if validation detects an invalid path. - -#### Scenario: unavailable validation capability -- **GIVEN** VAMP path validation is enabled -- **AND** the loaded VAMP artifact does not provide the needed validation capability -- **WHEN** the planning stack or plan result is validated -- **THEN** DimOS MUST fail clearly instead of silently skipping validation. - -### Requirement: VAMP pose planning compatibility - -VAMP pose planning MUST be available only when the configured kinematics backend is compatible with the VAMP world surface. - -#### Scenario: incompatible kinematics backend -- **GIVEN** a VAMP world and planner configuration -- **AND** a configured kinematics backend requires a world capability that the VAMP world does not support -- **WHEN** the planning stack is initialized or pose planning compatibility is checked -- **THEN** DimOS MUST reject the pose-planning combination clearly -- **AND** joint-space VAMP planning SHALL remain the supported initial VAMP planning mode. - -#### Scenario: compatible future kinematics backend -- **GIVEN** a VAMP world and planner configuration -- **AND** a kinematics backend that is compatible with the VAMP world surface -- **WHEN** pose planning is requested -- **THEN** DimOS SHALL convert the target pose to a goal joint state through the configured kinematics backend -- **AND** DimOS SHALL then plan the joint-space path with the configured VAMP planner. - -### Requirement: Unsupported VAMP world capabilities - -DimOS MUST provide a clear unsupported-capability failure when a VAMP world operation is not natively supported. - -#### Scenario: unsupported Jacobian request -- **GIVEN** a VAMP world that does not provide Jacobian support -- **WHEN** a kinematics backend requests the end-effector Jacobian -- **THEN** DimOS MUST raise or surface a clear unsupported-capability error -- **AND** the error MUST identify the incompatible requested capability. - -### Requirement: Optional VAMP dependency behavior - -DimOS MUST keep VAMP dependency loading scoped to VAMP selection. - -#### Scenario: default stack without VAMP installed -- **GIVEN** VAMP is not installed -- **AND** the user selects a non-VAMP planning stack -- **WHEN** the manipulation module starts -- **THEN** DimOS SHALL start without importing VAMP -- **AND** non-VAMP planning behavior SHALL remain available. - -#### Scenario: VAMP selected without dependency -- **GIVEN** VAMP is not installed -- **AND** the user selects a VAMP planning stack -- **WHEN** the planning stack is initialized -- **THEN** DimOS MUST fail with an actionable dependency error -- **AND** the error SHOULD identify the optional dependency or installation path needed for VAMP planning. diff --git a/openspec/changes/add-vamp-planning-backend/tasks.md b/openspec/changes/add-vamp-planning-backend/tasks.md deleted file mode 100644 index 754a194079..0000000000 --- a/openspec/changes/add-vamp-planning-backend/tasks.md +++ /dev/null @@ -1,75 +0,0 @@ -## 1. Configuration and validation - -- [x] 1.1 Add typed nested manipulation world config with default Drake-compatible behavior and a `backend` discriminator. -- [x] 1.2 Add typed nested manipulation planner config with default RRT-compatible behavior and a `backend` discriminator. -- [x] 1.3 Add VAMP world config variants for official artifact loading and custom user-prepared artifact path loading. -- [x] 1.4 Add VAMP planner config for algorithm selection, path simplification, and path validation behavior. -- [x] 1.5 Update manipulation planning factory wiring to create world and planner backends from typed config objects. -- [x] 1.6 Add planning-stack compatibility validation for VAMP world/planner pairing and incompatible kinematics combinations. -- [x] 1.7 Add warning-backed compatibility shims for migrated pre-existing flat config fields, using `DeprecationWarning` with replacement guidance. -- [x] 1.8 Add tests that nested world/planner/kinematics config parses from dict/CLI override shapes and preserves current defaults. -- [x] 1.9 Add tests that legacy flat config shims preserve behavior and emit deprecation warnings. - -## 2. VAMP backend implementation - -- [x] 2.1 Add optional VAMP dependency wiring in packaging, keeping VAMP imports lazy and backend-scoped. -- [x] 2.2 Add VAMP-specific dependency error type or error path with actionable install guidance. -- [x] 2.3 Implement VAMP artifact loading for official robot artifacts exposed by the installed VAMP package. -- [x] 2.4 Implement VAMP artifact loading for user-prepared custom artifact paths. -- [x] 2.5 Implement the VAMP world adapter surface for native validity, FK/end-effector pose, joint limits when available, and supported environment conversion. -- [x] 2.6 Add a clear unsupported-world-capability error for operations the VAMP world does not natively support, including Jacobian requests. -- [x] 2.7 Implement the VAMP planner adapter for joint-space planning with configured algorithm selection. -- [x] 2.8 Implement configured VAMP path simplification and path validation only through VAMP-native capabilities. -- [x] 2.9 Ensure `VampPlanner` does not perform IK, pose conversion, or Jacobian probing. - -## 3. Manipulation module integration - -- [x] 3.1 Update `WorldMonitor`/planning initialization to pass typed world config and backend-specific options into world creation. -- [x] 3.2 Update `ManipulationModule` planning initialization to use typed world, planner, and kinematics config consistently. -- [x] 3.3 Ensure non-VAMP manipulation stacks do not import VAMP, load VAMP artifacts, or require VAMP dependencies. -- [x] 3.4 Ensure pose-planning entry points fail clearly when VAMP is selected with an incompatible kinematics backend. -- [x] 3.5 Preserve existing Drake/default planning behavior and existing blueprint behavior unless a blueprint explicitly opts into VAMP. - -## 4. Franka Panda mock support - -- [x] 4.1 Add a Franka/Panda robot catalog module with a `franka_panda(...) -> RobotConfig` helper and exported Panda model/FK constants. -- [x] 4.2 Add Panda URDF/SRDF/model resources through the existing repository LFS data package pattern, e.g. a `data/.lfs/.tar.gz` package resolved by `LfsPath`. -- [x] 4.3 Configure the Panda catalog helper with explicit arm joint names, base link, end-effector link, home joints, package paths, LFS-backed model/SRDF paths, and collision exclusions if required by the selected model. -- [x] 4.4 Keep Panda control mock by default through `adapter_type="mock"`; reserve any real Panda adapter selection for explicit future configuration. -- [x] 4.5 Add a mock-control Panda coordinator path for tests/benchmarks, using `RobotConfig.to_hardware_component()` and `RobotConfig.to_task_config()` in the same style as xArm/Piper. -- [x] 4.6 Ensure Panda manipulation planning setup uses `RobotConfig.to_robot_model_config()` and can pair with the official VAMP Panda artifact for joint-space tests. -- [x] 4.7 If a runnable Panda blueprint is added, add it through normal blueprint registration and regenerate `dimos/robot/all_blueprints.py` with the registry test. - -## 5. Tests - -- [x] 5.1 Add factory/config tests for VAMP world/planner creation, invalid pairings, invalid algorithms, and invalid artifact configs. -- [x] 5.2 Add lazy-import tests proving non-VAMP stacks do not require VAMP to be installed. -- [x] 5.3 Add dependency-error tests for selecting VAMP without the optional dependency installed. -- [x] 5.4 Add VAMP world adapter tests using fakes/mocks for official artifact loading, custom artifact loading, supported queries, and unsupported capability errors. -- [x] 5.5 Add VAMP planner adapter tests using fakes/mocks for algorithm dispatch, planning result conversion, simplification, validation, and planning failure reporting. -- [x] 5.6 Add manipulation module wiring tests for nested config propagation and VAMP kinematics compatibility validation. -- [x] 5.7 Add Franka Panda catalog tests covering default mock adapter selection, coordinator joint names, hardware component conversion, manipulation robot model conversion, and LFS-backed URDF/SRDF resolution. -- [x] 5.8 Add a Panda-backed VAMP joint-space planning test or benchmark harness using fakes/mocks where the optional VAMP dependency or official artifact is unavailable. -- [x] 5.9 Add regression tests showing existing Drake/default manipulation tests still pass without VAMP installed. - -## 6. Documentation - -- [x] 6.1 Update user-facing manipulation planning docs with VAMP backend overview, initial joint-space-only scope, official artifact config, and custom artifact path config. -- [x] 6.2 Document that DimOS does not generate VAMP artifacts and that users must prepare custom artifacts outside DimOS. -- [x] 6.3 Document nested CLI override examples for VAMP world artifact settings and VAMP planner algorithm settings. -- [x] 6.4 Document Franka Panda mock-control catalog usage for planning tests and planner benchmarks, including the LFS-backed URDF/SRDF/model asset location. -- [x] 6.5 Document failure modes for missing VAMP dependency, invalid world/planner pairing, invalid artifact config, unsupported world capabilities, incompatible kinematics, and Panda model/artifact mismatch. -- [x] 6.6 Update contributor planning-backend guidance with lazy-import, typed-config, deprecation-warning, unsupported-capability, and mock robot catalog expectations. - -## 7. Verification - -- [x] 7.1 Run `openspec validate add-vamp-planning-backend`. -- [x] 7.2 Run focused manipulation planning config/factory tests. -- [x] 7.3 Run focused VAMP world/planner adapter tests. -- [x] 7.4 Run focused manipulation module wiring tests. -- [x] 7.5 Run focused Franka Panda catalog/coordinator conversion tests. -- [x] 7.6 Run existing default/Drake manipulation tests to verify compatibility. -- [x] 7.7 Run docs validation commands for changed docs, including documented link/snippet validation where applicable. -- [x] 7.8 Manually QA a VAMP joint-space planning flow through the manipulation module user surface using an official Panda VAMP artifact or a fake artifact-backed Panda test harness. -- [x] 7.9 Manually QA that selecting an incompatible VAMP pose-planning stack fails clearly before executing robot motion. -- [x] 7.10 If new blueprints are added or renamed, run `pytest dimos/robot/test_all_blueprints_generation.py` and commit generated registry changes. 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 a3678f86124fb2f64831e1418645a71000627177 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 23:43:20 -0700 Subject: [PATCH 17/20] fix(manipulation): satisfy mypy for optional VAMP imports --- dimos/manipulation/planning/vamp/loader.py | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py index 32cf64e1bc..4907249df7 100644 --- a/dimos/manipulation/planning/vamp/loader.py +++ b/dimos/manipulation/planning/vamp/loader.py @@ -30,20 +30,24 @@ VampArtifactConfig, ) +_vamp_import_error: ImportError | None +_vamp_module: VampModuleProtocol | None +_VAMP_OFFICIAL_ROBOT_MODULES: dict[str, VampRobotModuleProtocol] + try: - import vamp as _imported_vamp - import vamp.baxter as _vamp_baxter - import vamp.fetch as _vamp_fetch - import vamp.panda as _vamp_panda - import vamp.sphere as _vamp_sphere - import vamp.ur5 as _vamp_ur5 + import vamp as _imported_vamp # type: ignore[import-not-found] + import vamp.baxter as _vamp_baxter # type: ignore[import-not-found] + import vamp.fetch as _vamp_fetch # type: ignore[import-not-found] + import vamp.panda as _vamp_panda # type: ignore[import-not-found] + import vamp.sphere as _vamp_sphere # type: ignore[import-not-found] + import vamp.ur5 as _vamp_ur5 # type: ignore[import-not-found] except ImportError as exc: - _vamp_import_error: ImportError | None = exc + _vamp_import_error = exc _vamp_module = None - _VAMP_OFFICIAL_ROBOT_MODULES: dict[str, VampRobotModuleProtocol] = {} + _VAMP_OFFICIAL_ROBOT_MODULES = {} else: _vamp_import_error = None - _vamp_module: VampModuleProtocol | None = cast("VampModuleProtocol", _imported_vamp) + _vamp_module = cast("VampModuleProtocol", _imported_vamp) _VAMP_OFFICIAL_ROBOT_MODULES = { "baxter": cast("VampRobotModuleProtocol", _vamp_baxter), "fetch": cast("VampRobotModuleProtocol", _vamp_fetch), From d29eb755347a845f313b6d1954a604cfeffe885e Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 18 Jun 2026 23:56:09 -0700 Subject: [PATCH 18/20] fix(manipulation): simplify VAMP optional imports --- dimos/manipulation/planning/vamp/loader.py | 62 ++++---- dimos/manipulation/planning/vamp/protocols.py | 146 ------------------ .../planning/vamp/test_vamp_backend.py | 21 +-- dimos/manipulation/planning/vamp/utils.py | 42 ----- .../manipulation/planning/world/vamp_world.py | 44 +++--- 5 files changed, 69 insertions(+), 246 deletions(-) delete mode 100644 dimos/manipulation/planning/vamp/protocols.py delete mode 100644 dimos/manipulation/planning/vamp/utils.py diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py index 4907249df7..aa3d0efc3d 100644 --- a/dimos/manipulation/planning/vamp/loader.py +++ b/dimos/manipulation/planning/vamp/loader.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Lazy loaders for VAMP robot artifacts.""" +"""Direct optional imports for VAMP robot artifacts.""" from __future__ import annotations @@ -20,61 +20,61 @@ import importlib.util from pathlib import Path import sys +from types import ModuleType from typing import cast from dimos.manipulation.planning.vamp.errors import VampDependencyError -from dimos.manipulation.planning.vamp.protocols import VampModuleProtocol, VampRobotModuleProtocol from dimos.manipulation.planning.world.config import ( CustomVampArtifactConfig, OfficialVampArtifactConfig, VampArtifactConfig, ) -_vamp_import_error: ImportError | None -_vamp_module: VampModuleProtocol | None -_VAMP_OFFICIAL_ROBOT_MODULES: dict[str, VampRobotModuleProtocol] +vamp: ModuleType | None +_VAMP_IMPORT_ERROR: ImportError | None +_VAMP_OFFICIAL_ROBOT_MODULES: dict[str, ModuleType] try: - import vamp as _imported_vamp # type: ignore[import-not-found] + import vamp as _vamp_package # type: ignore[import-not-found] import vamp.baxter as _vamp_baxter # type: ignore[import-not-found] import vamp.fetch as _vamp_fetch # type: ignore[import-not-found] import vamp.panda as _vamp_panda # type: ignore[import-not-found] import vamp.sphere as _vamp_sphere # type: ignore[import-not-found] import vamp.ur5 as _vamp_ur5 # type: ignore[import-not-found] except ImportError as exc: - _vamp_import_error = exc - _vamp_module = None + vamp = None + _VAMP_IMPORT_ERROR = exc _VAMP_OFFICIAL_ROBOT_MODULES = {} else: - _vamp_import_error = None - _vamp_module = cast("VampModuleProtocol", _imported_vamp) + vamp = cast("ModuleType", _vamp_package) + _VAMP_IMPORT_ERROR = None _VAMP_OFFICIAL_ROBOT_MODULES = { - "baxter": cast("VampRobotModuleProtocol", _vamp_baxter), - "fetch": cast("VampRobotModuleProtocol", _vamp_fetch), - "panda": cast("VampRobotModuleProtocol", _vamp_panda), - "sphere": cast("VampRobotModuleProtocol", _vamp_sphere), - "ur5": cast("VampRobotModuleProtocol", _vamp_ur5), + "baxter": cast("ModuleType", _vamp_baxter), + "fetch": cast("ModuleType", _vamp_fetch), + "panda": cast("ModuleType", _vamp_panda), + "sphere": cast("ModuleType", _vamp_sphere), + "ur5": cast("ModuleType", _vamp_ur5), } -def load_vamp_robot_module( - artifact: VampArtifactConfig, -) -> tuple[VampModuleProtocol, VampRobotModuleProtocol]: - """Load the VAMP package and configured robot module.""" - if _vamp_module is None: - raise VampDependencyError() from _vamp_import_error - vamp_module = _vamp_module +def require_vamp() -> ModuleType: + """Return the imported VAMP package or raise with install guidance.""" + if vamp is None: + raise VampDependencyError() from _VAMP_IMPORT_ERROR + return vamp + + +def load_vamp_robot_module(artifact: VampArtifactConfig) -> ModuleType: + """Load the configured VAMP robot module.""" + require_vamp() if isinstance(artifact, OfficialVampArtifactConfig): - return vamp_module, _load_official_robot_module(vamp_module, artifact.robot) + return _load_official_robot_module(artifact.robot) if isinstance(artifact, CustomVampArtifactConfig): - return vamp_module, _load_custom_robot_module(artifact.path) + return _load_custom_robot_module(artifact.path) raise TypeError(f"Unsupported VAMP artifact config: {type(artifact).__name__}") -def _load_official_robot_module( - vamp_module: VampModuleProtocol, robot: str -) -> VampRobotModuleProtocol: - del vamp_module +def _load_official_robot_module(robot: str) -> ModuleType: try: return _VAMP_OFFICIAL_ROBOT_MODULES[robot] except KeyError as exc: @@ -83,7 +83,7 @@ def _load_official_robot_module( ) from exc -def _load_custom_robot_module(path: Path) -> VampRobotModuleProtocol: +def _load_custom_robot_module(path: Path) -> ModuleType: artifact_path = path.expanduser().resolve() if not artifact_path.exists(): raise FileNotFoundError(f"VAMP custom artifact path does not exist: {artifact_path}") @@ -92,7 +92,7 @@ def _load_custom_robot_module(path: Path) -> VampRobotModuleProtocol: parent = str(artifact_path.parent) if parent not in sys.path: sys.path.insert(0, parent) - return cast("VampRobotModuleProtocol", importlib.import_module(artifact_path.name)) + return importlib.import_module(artifact_path.name) module_name = artifact_path.stem spec = importlib.util.spec_from_file_location(module_name, artifact_path) @@ -101,4 +101,4 @@ def _load_custom_robot_module(path: Path) -> VampRobotModuleProtocol: module = importlib.util.module_from_spec(spec) sys.modules[module_name] = module spec.loader.exec_module(module) - return cast("VampRobotModuleProtocol", module) + return module diff --git a/dimos/manipulation/planning/vamp/protocols.py b/dimos/manipulation/planning/vamp/protocols.py deleted file mode 100644 index e35fda7824..0000000000 --- a/dimos/manipulation/planning/vamp/protocols.py +++ /dev/null @@ -1,146 +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. - -"""Protocols for the optional VAMP Python bindings.""" - -from __future__ import annotations - -from collections.abc import Callable, Sequence -from typing import Protocol - -import numpy as np -from numpy.typing import NDArray - - -class VampPathProtocol(Protocol): - """Path value returned by VAMP bindings.""" - - def numpy(self) -> NDArray[np.float64]: - """Return path waypoints as an array.""" - ... - - -VampPathSource = VampPathProtocol | Sequence[Sequence[float]] | NDArray[np.float64] - - -class VampSphereProtocol(Protocol): - """Sphere primitive handle accepted by VAMP environments.""" - - -class VampCuboidProtocol(Protocol): - """Cuboid primitive handle accepted by VAMP environments.""" - - -class VampCylinderProtocol(Protocol): - """Cylinder primitive handle accepted by VAMP environments.""" - - -class VampPlannerSettingsProtocol(Protocol): - """Planner settings handle returned by VAMP.""" - - -class VampSimplifySettingsProtocol(Protocol): - """Simplification settings handle returned by VAMP.""" - - -class VampSamplerProtocol(Protocol): - """Sampler handle returned by robot-specific VAMP modules.""" - - -class VampPlanningResultProtocol(Protocol): - """Planning or simplification result returned by VAMP bindings.""" - - solved: bool - path: VampPathSource - iterations: int - - -class VampEnvironmentProtocol(Protocol): - """VAMP collision environment.""" - - def add_sphere(self, sphere: VampSphereProtocol) -> None: ... - - def add_cuboid(self, cuboid: VampCuboidProtocol) -> None: ... - - def add_capsule(self, capsule: VampCylinderProtocol) -> None: ... - - -VampPlannerFunction = Callable[ - [ - Sequence[float], - Sequence[float], - VampEnvironmentProtocol, - VampPlannerSettingsProtocol, - VampSamplerProtocol, - ], - VampPlanningResultProtocol, -] - - -class VampRobotModuleProtocol(Protocol): - """Robot-specific VAMP module such as ``vamp.panda``.""" - - __name__: str - - def halton(self) -> VampSamplerProtocol: ... - - def validate( - self, - configuration: Sequence[float], - environment: VampEnvironmentProtocol, - check_bounds: bool, - ) -> bool: ... - - def validate_motion( - self, - configuration_in: Sequence[float], - configuration_out: Sequence[float], - environment: VampEnvironmentProtocol, - check_bounds: bool, - ) -> bool: ... - - def eefk(self, configuration: Sequence[float]) -> NDArray[np.float64]: ... - - def simplify( - self, - path: VampPathSource, - environment: VampEnvironmentProtocol, - settings: VampSimplifySettingsProtocol, - sampler: VampSamplerProtocol, - ) -> VampPlanningResultProtocol: ... - - -class VampModuleProtocol(Protocol): - """Top-level VAMP module.""" - - def Environment(self) -> VampEnvironmentProtocol: ... - - def Sphere(self, center: Sequence[float], radius: float) -> VampSphereProtocol: ... - - def Cuboid( - self, center: Sequence[float], euler_xyz: Sequence[float], half_extents: Sequence[float] - ) -> VampCuboidProtocol: ... - - def Cylinder( - self, center: Sequence[float], euler_xyz: Sequence[float], radius: float, length: float - ) -> VampCylinderProtocol: ... - - def configure_robot_and_planner_with_kwargs( - self, robot_name: str, planner_name: str, max_iterations: int - ) -> tuple[ - VampRobotModuleProtocol, - VampPlannerFunction, - VampPlannerSettingsProtocol, - VampSimplifySettingsProtocol, - ]: ... diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index 4bc47c645a..2d5bbd55f4 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -235,7 +235,8 @@ def fake_vamp_modules(mocker) -> tuple[FakeVampModule, FakeRobotModule]: robot_module = FakeRobotModule() vamp_module = FakeVampModule(robot_module) mocker.patch.dict(sys.modules, {"vamp": vamp_module, "vamp.panda": robot_module}) - mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", vamp_module) + mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", vamp_module) + mocker.patch("dimos.manipulation.planning.vamp.loader._VAMP_IMPORT_ERROR", None) mocker.patch( "dimos.manipulation.planning.vamp.loader._VAMP_OFFICIAL_ROBOT_MODULES", {"panda": robot_module}, @@ -268,7 +269,11 @@ def finalized_vamp_world() -> VampWorld: def test_vamp_dependency_error_has_install_guidance(mocker) -> None: """Selecting VAMP without the optional package raises an actionable error.""" - mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", None) + mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", None) + mocker.patch( + "dimos.manipulation.planning.vamp.loader._VAMP_IMPORT_ERROR", + ImportError("No module named 'vamp'"), + ) with pytest.raises(VampDependencyError, match="vamp-planner"): load_vamp_robot_module(OfficialVampArtifactConfig(robot="panda")) @@ -276,7 +281,7 @@ def test_vamp_dependency_error_has_install_guidance(mocker) -> None: def test_rrt_planner_creation_works_when_vamp_unavailable(mocker) -> None: """Default planner creation still works when the optional VAMP package is unavailable.""" - mocker.patch("dimos.manipulation.planning.vamp.loader._vamp_module", None) + mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", None) planner = create_planner(config=RRTConnectPlannerConfig()) @@ -291,11 +296,10 @@ def test_vamp_config_rejects_invalid_algorithm() -> None: def test_official_vamp_artifact_loading_uses_installed_robot_module(fake_vamp_modules) -> None: """Official artifact mode loads robot modules exposed by the VAMP package.""" - vamp_module, robot_module = fake_vamp_modules + _, robot_module = fake_vamp_modules - loaded_vamp, loaded_robot = load_vamp_robot_module(OfficialVampArtifactConfig(robot="panda")) + loaded_robot = load_vamp_robot_module(OfficialVampArtifactConfig(robot="panda")) - assert loaded_vamp is vamp_module assert loaded_robot is robot_module @@ -303,13 +307,12 @@ def test_custom_vamp_artifact_loading_uses_explicit_module_path( fake_vamp_modules, tmp_path ) -> None: """Custom artifact mode imports a user-prepared local Python robot module.""" - vamp_module, _ = fake_vamp_modules + assert fake_vamp_modules is not None artifact_path = tmp_path / "custom_panda.py" artifact_path.write_text("ROBOT_NAME = 'custom_panda'\n", encoding="utf-8") - loaded_vamp, loaded_robot = load_vamp_robot_module(CustomVampArtifactConfig(path=artifact_path)) + loaded_robot = load_vamp_robot_module(CustomVampArtifactConfig(path=artifact_path)) - assert loaded_vamp is vamp_module assert isinstance(loaded_robot, ModuleType) assert loaded_robot.ROBOT_NAME == "custom_panda" diff --git a/dimos/manipulation/planning/vamp/utils.py b/dimos/manipulation/planning/vamp/utils.py deleted file mode 100644 index dde0303ce8..0000000000 --- a/dimos/manipulation/planning/vamp/utils.py +++ /dev/null @@ -1,42 +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. - -"""Utilities for adapting VAMP binding values to DimOS models.""" - -from __future__ import annotations - -from typing import TypeGuard - -import numpy as np -from numpy.typing import NDArray - -from dimos.manipulation.planning.vamp.protocols import VampPathProtocol, VampPathSource -from dimos.msgs.sensor_msgs.JointState import JointState - - -def path_to_joint_states(path_source: VampPathSource, joint_names: list[str]) -> list[JointState]: - """Convert VAMP path data or numeric waypoints into joint states.""" - path_array = path_to_array(path_source) - return [JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array] - - -def path_to_array(path_source: VampPathSource) -> NDArray[np.float64]: - """Convert VAMP path data into a float waypoint array.""" - if _has_numpy(path_source): - return np.asarray(path_source.numpy(), dtype=np.float64) - return np.asarray(path_source, dtype=np.float64) - - -def _has_numpy(path_source: VampPathSource) -> TypeGuard[VampPathProtocol]: - return hasattr(path_source, "numpy") diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index 8dc9f824de..1129dd9c9b 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -33,8 +33,7 @@ from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.path_utils import compute_path_length from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError -from dimos.manipulation.planning.vamp.loader import load_vamp_robot_module -from dimos.manipulation.planning.vamp.utils import path_to_joint_states +from dimos.manipulation.planning.vamp.loader import load_vamp_robot_module, require_vamp from dimos.manipulation.planning.world.config import VampWorldConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -65,8 +64,9 @@ class VampWorld(WorldSpec): def __init__(self, config: VampWorldConfig) -> None: self.config = config - self._vamp_module, self._robot_module = load_vamp_robot_module(config.artifact) - self._environment = self._vamp_module.Environment() + vamp = require_vamp() + self._robot_module = load_vamp_robot_module(config.artifact) + self._environment = vamp.Environment() # type: ignore[attr-defined] self._robot_id: WorldRobotID | None = None self._robot_config: RobotModelConfig | None = None self._live_joint_state: JointState | None = None @@ -212,7 +212,7 @@ def check_edge_collision_free( list(end_state.position), self._environment, True, - ) + ) # type: ignore[attr-defined] return bool(result) def get_ee_pose(self, ctx: _VampContext, robot_id: WorldRobotID) -> PoseStamped: @@ -220,7 +220,8 @@ def get_ee_pose(self, ctx: _VampContext, robot_id: WorldRobotID) -> PoseStamped: self._assert_robot_id(robot_id) joint_state = ctx.joint_state transform = np.asarray( - self._robot_module.eefk(list(joint_state.position)), dtype=np.float64 + self._robot_module.eefk(list(joint_state.position)), # type: ignore[attr-defined] + dtype=np.float64, ) pose = matrix_to_pose(transform) return PoseStamped(position=pose.position, orientation=pose.orientation, frame_id="world") @@ -233,7 +234,10 @@ def get_link_pose( if link_name != config.end_effector_link: raise UnsupportedWorldCapabilityError("vamp", f"link pose for '{link_name}'") joint_state = ctx.joint_state - return np.asarray(self._robot_module.eefk(list(joint_state.position)), dtype=np.float64) + return np.asarray( + self._robot_module.eefk(list(joint_state.position)), # type: ignore[attr-defined] + dtype=np.float64, + ) def get_jacobian(self, ctx: _VampContext, robot_id: WorldRobotID) -> NDArray[np.float64]: """VAMP's Python API does not expose a Jacobian.""" @@ -260,13 +264,13 @@ def plan_joint_path( return _failure(PlanningStatus.COLLISION_AT_GOAL, "Goal configuration is invalid") robot_module, planner_func, plan_settings, simplify_settings = ( - self._vamp_module.configure_robot_and_planner_with_kwargs( + require_vamp().configure_robot_and_planner_with_kwargs( # type: ignore[attr-defined] self._robot_name(), planner_config.algorithm, max_iterations=_timeout_to_iteration_budget(timeout), ) ) - sampler = robot_module.halton() + sampler = robot_module.halton() # type: ignore[attr-defined] result = planner_func( list(start.position), list(goal.position), @@ -286,13 +290,15 @@ def plan_joint_path( if planner_config.simplify: simplified = robot_module.simplify( path_source, self._environment, simplify_settings, sampler - ) + ) # type: ignore[attr-defined] if simplified.solved: path_source = simplified.path - path = path_to_joint_states( - path_source, start.name or self.get_robot_config(robot_id).joint_names - ) + path_array = np.asarray(path_source.numpy(), dtype=np.float64) + joint_names = start.name or self.get_robot_config(robot_id).joint_names + path = [ + JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array + ] if planner_config.validate_path and not self._validate_path(robot_id, path): return _failure( PlanningStatus.NO_SOLUTION, @@ -346,10 +352,11 @@ def _validate_state(self, joint_state: JointState, check_bounds: bool) -> bool: self._environment, check_bounds, ) - ) + ) # type: ignore[attr-defined] def _rebuild_environment(self) -> None: - self._environment = self._vamp_module.Environment() + vamp = require_vamp() + self._environment = vamp.Environment() # type: ignore[attr-defined] for obstacle in self._obstacles.values(): self._add_obstacle_to_environment(obstacle) @@ -367,14 +374,15 @@ def _add_obstacle_to_environment(self, obstacle: Obstacle) -> None: .as_euler("xyz") .tolist() ) + vamp = require_vamp() if obstacle.obstacle_type == ObstacleType.SPHERE: - self._environment.add_sphere(self._vamp_module.Sphere(center, obstacle.dimensions[0])) + self._environment.add_sphere(vamp.Sphere(center, obstacle.dimensions[0])) # type: ignore[attr-defined] elif obstacle.obstacle_type == ObstacleType.BOX: half_extents = [dimension / 2.0 for dimension in obstacle.dimensions] - self._environment.add_cuboid(self._vamp_module.Cuboid(center, euler_xyz, half_extents)) + self._environment.add_cuboid(vamp.Cuboid(center, euler_xyz, half_extents)) # type: ignore[attr-defined] elif obstacle.obstacle_type == ObstacleType.CYLINDER: self._environment.add_capsule( - self._vamp_module.Cylinder( + vamp.Cylinder( # type: ignore[attr-defined] center, euler_xyz, obstacle.dimensions[0], From 82ffe9a376527eff93cb1cd55eafbf434d4771e3 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 00:24:57 -0700 Subject: [PATCH 19/20] fix(manipulation): type VAMP direct imports with stubs --- dimos/manipulation/planning/vamp/loader.py | 65 +++++++------- .../planning/vamp/test_vamp_backend.py | 13 ++- .../manipulation/planning/world/vamp_world.py | 35 ++++---- stubs/vamp/__init__.pyi | 84 +++++++++++++++++++ stubs/vamp/baxter.pyi | 5 ++ stubs/vamp/fetch.pyi | 5 ++ stubs/vamp/panda.pyi | 5 ++ stubs/vamp/sphere.pyi | 5 ++ stubs/vamp/ur5.pyi | 5 ++ 9 files changed, 164 insertions(+), 58 deletions(-) create mode 100644 stubs/vamp/__init__.pyi create mode 100644 stubs/vamp/baxter.pyi create mode 100644 stubs/vamp/fetch.pyi create mode 100644 stubs/vamp/panda.pyi create mode 100644 stubs/vamp/sphere.pyi create mode 100644 stubs/vamp/ur5.pyi diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py index aa3d0efc3d..5530c371f6 100644 --- a/dimos/manipulation/planning/vamp/loader.py +++ b/dimos/manipulation/planning/vamp/loader.py @@ -20,7 +20,6 @@ import importlib.util from pathlib import Path import sys -from types import ModuleType from typing import cast from dimos.manipulation.planning.vamp.errors import VampDependencyError @@ -30,41 +29,28 @@ VampArtifactConfig, ) -vamp: ModuleType | None _VAMP_IMPORT_ERROR: ImportError | None -_VAMP_OFFICIAL_ROBOT_MODULES: dict[str, ModuleType] try: - import vamp as _vamp_package # type: ignore[import-not-found] - import vamp.baxter as _vamp_baxter # type: ignore[import-not-found] - import vamp.fetch as _vamp_fetch # type: ignore[import-not-found] - import vamp.panda as _vamp_panda # type: ignore[import-not-found] - import vamp.sphere as _vamp_sphere # type: ignore[import-not-found] - import vamp.ur5 as _vamp_ur5 # type: ignore[import-not-found] + import vamp + import vamp.baxter + import vamp.fetch + import vamp.panda + import vamp.sphere + import vamp.ur5 except ImportError as exc: - vamp = None _VAMP_IMPORT_ERROR = exc - _VAMP_OFFICIAL_ROBOT_MODULES = {} else: - vamp = cast("ModuleType", _vamp_package) _VAMP_IMPORT_ERROR = None - _VAMP_OFFICIAL_ROBOT_MODULES = { - "baxter": cast("ModuleType", _vamp_baxter), - "fetch": cast("ModuleType", _vamp_fetch), - "panda": cast("ModuleType", _vamp_panda), - "sphere": cast("ModuleType", _vamp_sphere), - "ur5": cast("ModuleType", _vamp_ur5), - } - - -def require_vamp() -> ModuleType: - """Return the imported VAMP package or raise with install guidance.""" - if vamp is None: + + +def require_vamp() -> None: + """Raise with install guidance when the optional VAMP package is unavailable.""" + if _VAMP_IMPORT_ERROR is not None: raise VampDependencyError() from _VAMP_IMPORT_ERROR - return vamp -def load_vamp_robot_module(artifact: VampArtifactConfig) -> ModuleType: +def load_vamp_robot_module(artifact: VampArtifactConfig) -> vamp.RobotModule: """Load the configured VAMP robot module.""" require_vamp() if isinstance(artifact, OfficialVampArtifactConfig): @@ -74,16 +60,23 @@ def load_vamp_robot_module(artifact: VampArtifactConfig) -> ModuleType: raise TypeError(f"Unsupported VAMP artifact config: {type(artifact).__name__}") -def _load_official_robot_module(robot: str) -> ModuleType: - try: - return _VAMP_OFFICIAL_ROBOT_MODULES[robot] - except KeyError as exc: - raise ValueError( - f"Installed VAMP package does not expose robot artifact '{robot}'" - ) from exc +def _load_official_robot_module(robot: str) -> vamp.RobotModule: + match robot: + case "baxter": + return cast("vamp.RobotModule", vamp.baxter) + case "fetch": + return cast("vamp.RobotModule", vamp.fetch) + case "panda": + return cast("vamp.RobotModule", vamp.panda) + case "sphere": + return cast("vamp.RobotModule", vamp.sphere) + case "ur5": + return cast("vamp.RobotModule", vamp.ur5) + case _: + raise ValueError(f"Installed VAMP package does not expose robot artifact '{robot}'") -def _load_custom_robot_module(path: Path) -> ModuleType: +def _load_custom_robot_module(path: Path) -> vamp.RobotModule: artifact_path = path.expanduser().resolve() if not artifact_path.exists(): raise FileNotFoundError(f"VAMP custom artifact path does not exist: {artifact_path}") @@ -92,7 +85,7 @@ def _load_custom_robot_module(path: Path) -> ModuleType: parent = str(artifact_path.parent) if parent not in sys.path: sys.path.insert(0, parent) - return importlib.import_module(artifact_path.name) + return cast("vamp.RobotModule", importlib.import_module(artifact_path.name)) module_name = artifact_path.stem spec = importlib.util.spec_from_file_location(module_name, artifact_path) @@ -101,4 +94,4 @@ def _load_custom_robot_module(path: Path) -> ModuleType: module = importlib.util.module_from_spec(spec) sys.modules[module_name] = module spec.loader.exec_module(module) - return module + return cast("vamp.RobotModule", module) diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index 2d5bbd55f4..bf5f8a6a68 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -235,12 +235,9 @@ def fake_vamp_modules(mocker) -> tuple[FakeVampModule, FakeRobotModule]: robot_module = FakeRobotModule() vamp_module = FakeVampModule(robot_module) mocker.patch.dict(sys.modules, {"vamp": vamp_module, "vamp.panda": robot_module}) - mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", vamp_module) + mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", vamp_module, create=True) + mocker.patch("dimos.manipulation.planning.world.vamp_world.vamp", vamp_module, create=True) mocker.patch("dimos.manipulation.planning.vamp.loader._VAMP_IMPORT_ERROR", None) - mocker.patch( - "dimos.manipulation.planning.vamp.loader._VAMP_OFFICIAL_ROBOT_MODULES", - {"panda": robot_module}, - ) return vamp_module, robot_module @@ -269,7 +266,6 @@ def finalized_vamp_world() -> VampWorld: def test_vamp_dependency_error_has_install_guidance(mocker) -> None: """Selecting VAMP without the optional package raises an actionable error.""" - mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", None) mocker.patch( "dimos.manipulation.planning.vamp.loader._VAMP_IMPORT_ERROR", ImportError("No module named 'vamp'"), @@ -281,7 +277,10 @@ def test_vamp_dependency_error_has_install_guidance(mocker) -> None: def test_rrt_planner_creation_works_when_vamp_unavailable(mocker) -> None: """Default planner creation still works when the optional VAMP package is unavailable.""" - mocker.patch("dimos.manipulation.planning.vamp.loader.vamp", None) + mocker.patch( + "dimos.manipulation.planning.vamp.loader._VAMP_IMPORT_ERROR", + ImportError("No module named 'vamp'"), + ) planner = create_planner(config=RRTConnectPlannerConfig()) diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index 1129dd9c9b..102e50ea5b 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -26,6 +26,11 @@ import numpy as np from scipy.spatial.transform import Rotation as R +try: + import vamp +except ImportError: + pass + from dimos.manipulation.planning.planners.config import VampPlannerConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType, PlanningStatus @@ -64,9 +69,9 @@ class VampWorld(WorldSpec): def __init__(self, config: VampWorldConfig) -> None: self.config = config - vamp = require_vamp() + require_vamp() self._robot_module = load_vamp_robot_module(config.artifact) - self._environment = vamp.Environment() # type: ignore[attr-defined] + self._environment = vamp.Environment() self._robot_id: WorldRobotID | None = None self._robot_config: RobotModelConfig | None = None self._live_joint_state: JointState | None = None @@ -212,7 +217,7 @@ def check_edge_collision_free( list(end_state.position), self._environment, True, - ) # type: ignore[attr-defined] + ) return bool(result) def get_ee_pose(self, ctx: _VampContext, robot_id: WorldRobotID) -> PoseStamped: @@ -220,7 +225,7 @@ def get_ee_pose(self, ctx: _VampContext, robot_id: WorldRobotID) -> PoseStamped: self._assert_robot_id(robot_id) joint_state = ctx.joint_state transform = np.asarray( - self._robot_module.eefk(list(joint_state.position)), # type: ignore[attr-defined] + self._robot_module.eefk(list(joint_state.position)), dtype=np.float64, ) pose = matrix_to_pose(transform) @@ -235,7 +240,7 @@ def get_link_pose( raise UnsupportedWorldCapabilityError("vamp", f"link pose for '{link_name}'") joint_state = ctx.joint_state return np.asarray( - self._robot_module.eefk(list(joint_state.position)), # type: ignore[attr-defined] + self._robot_module.eefk(list(joint_state.position)), dtype=np.float64, ) @@ -264,13 +269,13 @@ def plan_joint_path( return _failure(PlanningStatus.COLLISION_AT_GOAL, "Goal configuration is invalid") robot_module, planner_func, plan_settings, simplify_settings = ( - require_vamp().configure_robot_and_planner_with_kwargs( # type: ignore[attr-defined] + vamp.configure_robot_and_planner_with_kwargs( self._robot_name(), planner_config.algorithm, max_iterations=_timeout_to_iteration_budget(timeout), ) ) - sampler = robot_module.halton() # type: ignore[attr-defined] + sampler = robot_module.halton() result = planner_func( list(start.position), list(goal.position), @@ -290,7 +295,7 @@ def plan_joint_path( if planner_config.simplify: simplified = robot_module.simplify( path_source, self._environment, simplify_settings, sampler - ) # type: ignore[attr-defined] + ) if simplified.solved: path_source = simplified.path @@ -352,11 +357,11 @@ def _validate_state(self, joint_state: JointState, check_bounds: bool) -> bool: self._environment, check_bounds, ) - ) # type: ignore[attr-defined] + ) def _rebuild_environment(self) -> None: - vamp = require_vamp() - self._environment = vamp.Environment() # type: ignore[attr-defined] + require_vamp() + self._environment = vamp.Environment() for obstacle in self._obstacles.values(): self._add_obstacle_to_environment(obstacle) @@ -374,15 +379,15 @@ def _add_obstacle_to_environment(self, obstacle: Obstacle) -> None: .as_euler("xyz") .tolist() ) - vamp = require_vamp() + require_vamp() if obstacle.obstacle_type == ObstacleType.SPHERE: - self._environment.add_sphere(vamp.Sphere(center, obstacle.dimensions[0])) # type: ignore[attr-defined] + self._environment.add_sphere(vamp.Sphere(center, obstacle.dimensions[0])) elif obstacle.obstacle_type == ObstacleType.BOX: half_extents = [dimension / 2.0 for dimension in obstacle.dimensions] - self._environment.add_cuboid(vamp.Cuboid(center, euler_xyz, half_extents)) # type: ignore[attr-defined] + self._environment.add_cuboid(vamp.Cuboid(center, euler_xyz, half_extents)) elif obstacle.obstacle_type == ObstacleType.CYLINDER: self._environment.add_capsule( - vamp.Cylinder( # type: ignore[attr-defined] + vamp.Cylinder( center, euler_xyz, obstacle.dimensions[0], diff --git a/stubs/vamp/__init__.pyi b/stubs/vamp/__init__.pyi new file mode 100644 index 0000000000..71ce378efa --- /dev/null +++ b/stubs/vamp/__init__.pyi @@ -0,0 +1,84 @@ +from collections.abc import Callable, Sequence + +import numpy as np +from numpy.typing import NDArray + +from . import baxter as baxter, fetch as fetch, panda as panda, sphere as sphere, ur5 as ur5 + +class Path: + def numpy(self) -> NDArray[np.float64]: ... + +class Environment: + def add_sphere(self, sphere: Sphere) -> None: ... + def add_cuboid(self, cuboid: Cuboid) -> None: ... + def add_capsule(self, capsule: Cylinder) -> None: ... + +class Sphere: + def __init__(self, center: Sequence[float], radius: float) -> None: ... + +class Cuboid: + def __init__( + self, + center: Sequence[float], + euler_xyz: Sequence[float], + half_extents: Sequence[float], + ) -> None: ... + +class Cylinder: + def __init__( + self, + center: Sequence[float], + euler_xyz: Sequence[float], + radius: float, + length: float, + ) -> None: ... + +class PlanningResult: + solved: bool + path: Path + iterations: int + +class PlannerSettings: + pass + +class SimplifySettings: + pass + +class Sampler: + pass + +class RobotModule: + __name__: str + def halton(self) -> Sampler: ... + def validate( + self, + configuration: Sequence[float], + environment: Environment, + check_bounds: bool, + ) -> bool: ... + def validate_motion( + self, + configuration_in: Sequence[float], + configuration_out: Sequence[float], + environment: Environment, + check_bounds: bool, + ) -> bool: ... + def eefk(self, configuration: Sequence[float]) -> NDArray[np.float64]: ... + def simplify( + self, + path: Path, + environment: Environment, + settings: SimplifySettings, + sampler: Sampler, + ) -> PlanningResult: ... + +PlannerFunction = Callable[ + [Sequence[float], Sequence[float], Environment, PlannerSettings, Sampler], + PlanningResult, +] + +def configure_robot_and_planner_with_kwargs( + robot_name: str, + planner_name: str, + max_iterations: int, +) -> tuple[RobotModule, PlannerFunction, PlannerSettings, SimplifySettings]: ... diff --git a/stubs/vamp/baxter.pyi b/stubs/vamp/baxter.pyi new file mode 100644 index 0000000000..c4d7527c1d --- /dev/null +++ b/stubs/vamp/baxter.pyi @@ -0,0 +1,5 @@ +from . import RobotModule + +__name__: str + +def halton() -> RobotModule: ... diff --git a/stubs/vamp/fetch.pyi b/stubs/vamp/fetch.pyi new file mode 100644 index 0000000000..c4d7527c1d --- /dev/null +++ b/stubs/vamp/fetch.pyi @@ -0,0 +1,5 @@ +from . import RobotModule + +__name__: str + +def halton() -> RobotModule: ... diff --git a/stubs/vamp/panda.pyi b/stubs/vamp/panda.pyi new file mode 100644 index 0000000000..c4d7527c1d --- /dev/null +++ b/stubs/vamp/panda.pyi @@ -0,0 +1,5 @@ +from . import RobotModule + +__name__: str + +def halton() -> RobotModule: ... diff --git a/stubs/vamp/sphere.pyi b/stubs/vamp/sphere.pyi new file mode 100644 index 0000000000..c4d7527c1d --- /dev/null +++ b/stubs/vamp/sphere.pyi @@ -0,0 +1,5 @@ +from . import RobotModule + +__name__: str + +def halton() -> RobotModule: ... diff --git a/stubs/vamp/ur5.pyi b/stubs/vamp/ur5.pyi new file mode 100644 index 0000000000..c4d7527c1d --- /dev/null +++ b/stubs/vamp/ur5.pyi @@ -0,0 +1,5 @@ +from . import RobotModule + +__name__: str + +def halton() -> RobotModule: ... From 2fe0b0af4285d8a8d9ad64d1cdec44abc7b1aec1 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 01:35:44 -0700 Subject: [PATCH 20/20] fix(manipulation): address Greptile VAMP review --- dimos/manipulation/planning/factory.py | 6 ++-- dimos/manipulation/planning/vamp/loader.py | 9 ++++-- .../planning/vamp/test_vamp_backend.py | 32 +++++++++++++++++-- .../manipulation/planning/world/vamp_world.py | 8 +++-- 4 files changed, 46 insertions(+), 9 deletions(-) diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index 822695fc9f..7a1b62bff4 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -31,14 +31,12 @@ RRTConnectPlannerConfig, VampPlannerConfig, ) -from dimos.manipulation.planning.planners.vamp_planner import VampPlanner from dimos.manipulation.planning.world.config import ( MANIPULATION_WORLD_CONFIG_ADAPTER, DrakeWorldConfig, ManipulationWorldConfig, VampWorldConfig, ) -from dimos.manipulation.planning.world.vamp_world import VampWorld if TYPE_CHECKING: from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec, WorldSpec @@ -59,6 +57,8 @@ def create_world( return DrakeWorld(enable_viz=enable_viz, **kwargs) if isinstance(config, VampWorldConfig): + from dimos.manipulation.planning.world.vamp_world import VampWorld + return VampWorld(config=config, **kwargs) raise TypeError(f"Unsupported world config: {type(config).__name__}") @@ -110,6 +110,8 @@ def create_planner( **kwargs, ) if isinstance(config, VampPlannerConfig): + from dimos.manipulation.planning.planners.vamp_planner import VampPlanner + return VampPlanner(config=config, **kwargs) raise TypeError(f"Unsupported planner config: {type(config).__name__}") diff --git a/dimos/manipulation/planning/vamp/loader.py b/dimos/manipulation/planning/vamp/loader.py index 5530c371f6..95c07fcaa7 100644 --- a/dimos/manipulation/planning/vamp/loader.py +++ b/dimos/manipulation/planning/vamp/loader.py @@ -83,9 +83,14 @@ def _load_custom_robot_module(path: Path) -> vamp.RobotModule: if artifact_path.is_dir(): parent = str(artifact_path.parent) - if parent not in sys.path: + should_remove_parent = parent not in sys.path + if should_remove_parent: sys.path.insert(0, parent) - return cast("vamp.RobotModule", importlib.import_module(artifact_path.name)) + try: + return cast("vamp.RobotModule", importlib.import_module(artifact_path.name)) + finally: + if should_remove_parent: + sys.path.remove(parent) module_name = artifact_path.stem spec = importlib.util.spec_from_file_location(module_name, artifact_path) diff --git a/dimos/manipulation/planning/vamp/test_vamp_backend.py b/dimos/manipulation/planning/vamp/test_vamp_backend.py index bf5f8a6a68..93e26d92d1 100644 --- a/dimos/manipulation/planning/vamp/test_vamp_backend.py +++ b/dimos/manipulation/planning/vamp/test_vamp_backend.py @@ -316,6 +316,27 @@ def test_custom_vamp_artifact_loading_uses_explicit_module_path( assert loaded_robot.ROBOT_NAME == "custom_panda" +def test_custom_vamp_artifact_directory_import_restores_sys_path( + fake_vamp_modules, tmp_path +) -> None: + """Directory artifact imports do not permanently change module resolution order.""" + assert fake_vamp_modules is not None + artifact_dir = tmp_path / "custom_robot" + artifact_dir.mkdir() + (artifact_dir / "__init__.py").write_text("ROBOT_NAME = 'custom_robot'\n", encoding="utf-8") + parent = str(tmp_path) + assert parent not in sys.path + + try: + loaded_robot = load_vamp_robot_module(CustomVampArtifactConfig(path=artifact_dir)) + + assert isinstance(loaded_robot, ModuleType) + assert loaded_robot.ROBOT_NAME == "custom_robot" + assert parent not in sys.path + finally: + sys.modules.pop("custom_robot", None) + + def test_create_world_and_planner_from_vamp_configs(fake_vamp_modules) -> None: """Factory functions create VAMP world and planner adapters from typed configs.""" world = create_world(config=VampWorldConfig()) @@ -366,13 +387,20 @@ def test_vamp_planner_dispatches_algorithm_simplifies_and_validates(fake_vamp_mo world = finalized_vamp_world() robot_id = world.get_robot_ids()[0] planner = VampPlanner(VampPlannerConfig(algorithm="prm", simplify=True, validate_path=True)) - start = JointState(name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0]) - goal = JointState(name=["joint1", "joint2", "joint3"], position=[1.0, 0.5, 0.25]) + start = JointState( + name=["joint1", "joint2", "joint3", "gripper"], + position=[0.0, 0.0, 0.0, 0.9], + ) + goal = JointState( + name=["joint1", "joint2", "joint3", "gripper"], + position=[1.0, 0.5, 0.25, 0.1], + ) result = planner.plan_joint_path(world, robot_id, start, goal, timeout=0.25) assert result.status == PlanningStatus.SUCCESS assert [point.position for point in result.path] == [[0.0, 0.0, 0.0], [1.0, 0.5, 0.25]] + assert [point.name for point in result.path] == [["joint1", "joint2", "joint3"]] * 2 assert vamp_module.configure_calls == [("panda", "prm", 250)] assert vamp_module.planner_calls == [ ([0.0, 0.0, 0.0], [1.0, 0.5, 0.25], FakeSampler("fake_sampler")) diff --git a/dimos/manipulation/planning/world/vamp_world.py b/dimos/manipulation/planning/world/vamp_world.py index 102e50ea5b..be33a26d44 100644 --- a/dimos/manipulation/planning/world/vamp_world.py +++ b/dimos/manipulation/planning/world/vamp_world.py @@ -267,6 +267,8 @@ def plan_joint_path( return _failure(PlanningStatus.COLLISION_AT_START, "Start configuration is invalid") if not self.check_config_collision_free(robot_id, goal): return _failure(PlanningStatus.COLLISION_AT_GOAL, "Goal configuration is invalid") + start_state = self._joint_state_for_robot_order(robot_id, start) + goal_state = self._joint_state_for_robot_order(robot_id, goal) robot_module, planner_func, plan_settings, simplify_settings = ( vamp.configure_robot_and_planner_with_kwargs( @@ -277,8 +279,8 @@ def plan_joint_path( ) sampler = robot_module.halton() result = planner_func( - list(start.position), - list(goal.position), + list(start_state.position), + list(goal_state.position), self._environment, plan_settings, sampler, @@ -300,7 +302,7 @@ def plan_joint_path( path_source = simplified.path path_array = np.asarray(path_source.numpy(), dtype=np.float64) - joint_names = start.name or self.get_robot_config(robot_id).joint_names + joint_names = start_state.name or self.get_robot_config(robot_id).joint_names path = [ JointState(name=joint_names, position=row.astype(float).tolist()) for row in path_array ]