From fe7a7de5874ffbc4ba290e97aae6a863205dd4f0 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 4 Jun 2026 13:41:32 -0700 Subject: [PATCH 01/27] 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/27] 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 d92a78ba757fe49a07321b69eb03a00ae0876ea4 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 13 Jun 2026 11:13:23 -0700 Subject: [PATCH 03/27] spec: planning group --- CONTEXT.md | 61 ++ .../add-planning-groups/.openspec.yaml | 2 + .../changes/add-planning-groups/design.md | 555 ++++++++++++++++++ .../changes/add-planning-groups/proposal.md | 53 ++ 4 files changed, 671 insertions(+) create mode 100644 CONTEXT.md create mode 100644 openspec/changes/add-planning-groups/.openspec.yaml create mode 100644 openspec/changes/add-planning-groups/design.md create mode 100644 openspec/changes/add-planning-groups/proposal.md diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..6597467a7f --- /dev/null +++ b/CONTEXT.md @@ -0,0 +1,61 @@ +# DimOS Robotics Language + +Shared vocabulary for DimOS robotics concepts. These terms define domain language, not implementation details. + +## Language + +**Planning Group**: +A named selectable serial kinematic chain of robot joints used as the unit of manipulation planning. A planning group is defined by its chain/joints, not by end-effector metadata. +_Avoid_: Move group, movegroup + +**Planning Group Definition**: +The model-level declaration of a planning group before it is bound to a concrete robot in a planning world. +_Avoid_: Runtime group, robot ID + +**End-Effector Association**: +Separate metadata used for pose-targeted operations. For a planning group defined by a chain, the end-effector link is the chain tip. For a planning group defined only by joints, there is no end-effector link. +_Avoid_: Planning group definition + +**Resolved Planning Group**: +A planning group after model-level declarations have been bound to a concrete robot, namespace, and planning world. +_Avoid_: Planning group config, robot ID + +**Planning Group Selection**: +The set of one or more planning groups chosen for a planning request. +_Avoid_: Composite group + +**Auxiliary Planning Group**: +A planning group selected to participate in a specific planning request without receiving a direct end-effector pose constraint in that request. A planning group may be auxiliary in one request and directly targeted in another. +_Avoid_: Joint-only group, intrinsic auxiliary group + +**Coordinated Planning Problem**: +A planning request over one or more selected planning groups that is solved as one combined joint-space problem with one synchronized result. +_Avoid_: Batch planning, independent planning + +**Planning Group ID**: +An API-level identifier for a planning group, always namespaced as `{robot_name}/{group_name}`. +_Avoid_: Bare group name, robot ID + +**Planning Group Descriptor**: +A read-only snapshot returned by query APIs that describes an available planning group and may be used ergonomically as a planning group selector. +_Avoid_: Live planning group handle, resolved planning group + +**Joint State**: +A resolved-joint-name-keyed robot state that can represent any set of joints and is not inherently coupled to a robot, planning group, or planning group selection. +_Avoid_: Planning-group-scoped state + +**Robot Model Joint Names**: +The objective set of controllable joints exposed by a robot coordinator for state and command. This usually aligns with the model's actuated joints, but is not itself a planning group. +_Avoid_: Implicit planning group + +**Local Model Joint Name**: +A joint name as it appears inside a robot model or SRDF before the model is bound to a concrete robot in a planning world. +_Avoid_: Runtime joint name, coordinator joint name + +**Resolved Joint Name**: +A world-level joint name exposed above the model parsing layer, always namespaced as `{robot_name}/{local_joint_name}` so it is stable and unique within a planning world. +_Avoid_: Bare joint name, local joint name + +**Robot Placement**: +The placement of a robot model within the planning world. Robot placement belongs in the robot model description rather than in a separate planning configuration transform. +_Avoid_: Planning base pose, config placement transform diff --git a/openspec/changes/add-planning-groups/.openspec.yaml b/openspec/changes/add-planning-groups/.openspec.yaml new file mode 100644 index 0000000000..494ca38369 --- /dev/null +++ b/openspec/changes/add-planning-groups/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-06-13 diff --git a/openspec/changes/add-planning-groups/design.md b/openspec/changes/add-planning-groups/design.md new file mode 100644 index 0000000000..3f24a3069b --- /dev/null +++ b/openspec/changes/add-planning-groups/design.md @@ -0,0 +1,555 @@ +# Planning Groups Design + +## 1. Summary + +This change makes **Planning Group** a first-class manipulation planning concept in DimOS. + +Today, manipulation planning is centered on robot identity: planner and IK interfaces select a `WorldRobotID`, while `RobotModelConfig` carries one `joint_names`, one `base_link`, and one `end_effector_link`. That shape works for a single serial arm, but it conflates robot/model identity with the kinematic subset being planned. + +The new design separates those concerns: + +- Robot identity describes the hardware/model instance. +- Planning group identity describes the selectable kinematic planning unit. +- SRDF `` declarations are the primary source of planning groups. +- Existing single-chain robots without SRDF can continue through conservative fallback generation of `{robot_name}/manipulator`. +- Planning and IK APIs select planning groups, not robot IDs. +- Pose planning supports request-scoped auxiliary groups, such as torso/waist joints that contribute free DOFs without direct pose constraints. +- Generated plans store only selected group IDs and one synchronized resolved-joint path. +- Preview and execution project from that generated plan lazily. + +The design also standardizes public joint naming above model parsing: all public joint states and paths use resolved joint names of the form `{robot_name}/{local_joint_name}`. + +## 2. Motivation + +Current manipulation planning treats a robot instance as the planning unit. `PlannerSpec.plan_joint_path(...)`, `KinematicsSpec.solve(...)`, and several `WorldSpec` methods all center on `WorldRobotID`. `ManipulationModule` also stores planned paths and trajectories by `RobotName`. + +That makes several important cases awkward: + +- Arm-only planning versus torso+arm planning. +- Coordinated dual-arm planning. +- Future multi-robot coordinated planning. +- Robots with multiple selectable serial chains. +- Explicit distinction between controllable joints and planning groups. + +The current `RobotModelConfig` fields also hide planning-group semantics. A single `end_effector_link` and `base_link` imply one planning chain per robot config. A single `joint_names` list currently acts like a hidden planning group, while also serving as the controllable/coordinator joint set. + +Finally, local URDF joint names are ambiguous once multiple robots or arms with repeated names participate in one plan. Public state/path APIs need stable resolved names so a path can represent coordinated multi-group or multi-robot motion without relying on external robot scoping. + +## 3. Goals and Non-Goals + +### Goals + +- Make planning groups first-class planning units. +- Use SRDF `` declarations as the primary source of planning group definitions. +- Provide conservative fallback generation for current single-chain arms without SRDF. +- Require explicit planning group selection for planning and IK. +- Support coordinated planning over one or more selected planning groups. +- Support pose planning with request-scoped auxiliary groups that contribute free DOFs. +- Expose stable resolved joint names above model/SRDF parsing. +- Add group-aware IK and planner interfaces. +- Replace robot-scoped end-effector FK/Jacobian queries with group-scoped APIs. +- Store minimal generated plan artifacts and project lazily for preview/execution. +- Keep existing trajectory controllers unaware of planning groups. + +### Non-Goals + +- Full MoveIt SRDF support. +- First-class composite or nested planning groups. +- Mixed pose+joint target planning in one request. +- Atomic multi-task trajectory batch dispatch. +- Rollback-on-rejection for multi-task trajectory dispatch. +- Planning config placement transforms such as `base_pose`. +- Silent compatibility mode that treats old `joint_names` as an implicit planning group without validation. +- Making controllers, coordinator tasks, or hardware drivers planning-group-aware. + +## 4. Domain Model + +Root `CONTEXT.md` contains the canonical glossary for this change. The core design shape is: + +```text +PlanningGroupDefinition + model-level declaration from SRDF or fallback generation + ↓ bind to concrete robot/world +ResolvedPlanningGroup + runtime/world-bound group with resolved joints and link frames + ↓ selected by request +PlanningGroupSelection + one or more non-overlapping resolved groups + ↓ IK/planner solve +GeneratedPlan + selected group IDs + synchronized resolved-joint path +``` + +Important terms: + +- **Planning Group**: named selectable serial kinematic chain of robot joints used as the manipulation planning unit. +- **Planning Group Definition**: model-level declaration before binding to a concrete robot/world. +- **Resolved Planning Group**: runtime/world-bound group with concrete robot identity, namespace, resolved joints, and frame data. +- **Planning Group Selection**: one or more planning groups chosen for a planning request. +- **Auxiliary Planning Group**: group selected in a specific pose-planning request without receiving a direct pose constraint in that request. +- **Planning Group ID**: public identifier, always `{robot_name}/{group_name}`. +- **Planning Group Descriptor**: immutable query snapshot describing an available planning group; not a live handle. +- **Local Model Joint Name**: name inside URDF/SRDF, such as `joint1`. +- **Resolved Joint Name**: public world-level name, always `{robot_name}/{local_joint_name}`. + +Identifier layering: + +```text +robot_name Stable robot/model instance name. +WorldRobotID Internal runtime world handle only. +PlanningGroupID {robot_name}/{group_name} +Local joint name joint1 +Resolved joint name {robot_name}/{local_joint_name} +Coordinator name Hardware/control boundary name; default identity with resolved name. +``` + +`WorldRobotID` must not appear in public state, path, generated plan, or planning group selection APIs. + +## 5. Planning Group Sources + +Planning group definitions are discovered in this precedence order: + +1. Explicit `srdf_path` on robot/model config. +2. Conservative SRDF auto-discovery with visible warning. +3. Fallback single-chain generation. +4. Error if no supported SRDF groups exist and fallback cannot infer exactly one unambiguous serial chain. + +Supported SRDF forms for this change: + +```xml + + + +``` + +```xml + + + + + +``` + +Unsupported forms are skipped with warnings: + +- `` groups. +- Nested `` references. +- Mixed link/joint/chain forms. +- Branching, disconnected, unordered, or otherwise non-serial groups. +- SRDF `` metadata. + +If a caller later selects a skipped group, resolution fails as unknown or unsupported. + +## 6. Fallback Group Generation + +When no SRDF is available, DimOS may generate exactly one planning group: + +```text +group name: manipulator +group ID: {robot_name}/manipulator +``` + +Fallback rules: + +- Use `RobotModelConfig.joint_names` as the candidate controllable set. +- Validate that candidate joints form one unambiguous serial chain in the parsed model. +- Allow prismatic joints inside the serial chain. +- Exclude only terminal/tip prismatic joints when they are likely finger/gripper joints. +- Set fallback pose target tip to the last controlled chain link. +- Error for ambiguous, branching, disconnected, or multi-chain models; such robots require SRDF. + +This preserves current single serial arm behavior without pretending every robot's `joint_names` list is a valid planning group. + +## 7. End-Effector Semantics + +A planning group is defined by chain/joints, not by SRDF `` metadata. This change ignores SRDF `` entirely. + +Rules: + +- Chain-defined group: pose target frame is the chain `tip_link`. +- Explicit joint-list group: may have a pose target frame only if validation proves it is one serial chain with a unique tip. +- Group with no valid tip: may participate in joint planning or as an auxiliary group, but cannot be directly pose-targeted. + +Pose-targeted APIs validate only targeted groups. Auxiliary groups do not need to be pose-ineligible; auxiliary status is request-scoped. A group may have a tip and still be auxiliary in a particular request. + +## 8. Public Planning APIs + +Representative API shape for pose targets: + +```python +plan_to_poses( + pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], + *, + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), +) -> GeneratedPlan +``` + +Meaning: + +- `pose_targets` are selected groups with end-effector pose constraints in this request. +- `auxiliary_groups` are selected groups with no pose constraint in this request. +- Effective selection is `pose_targets.keys() ∪ auxiliary_groups`. +- Auxiliary groups are free DOFs for IK and planning. +- Mixed pose+joint targets are not supported in this change. + +Example: + +```python +plan_to_poses( + pose_targets={"robot/arm": target_pose}, + auxiliary_groups=["robot/torso"], +) +``` + +This plans one coordinated problem over arm and torso joints. The arm tip must reach `target_pose`; torso joints are free to move as needed. + +Representative API shape for joint targets: + +```python +plan_to_joint_targets( + joint_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, JointState], +) -> GeneratedPlan +``` + +Rules: + +- Joint targets are keyed by planning group at the API boundary. +- Each group's joint target keys must exactly match that group's selected resolved joints. +- Internally the request lowers to one ordered combined resolved-joint start/goal problem. + +Planning APIs may accept either a Planning Group ID string or a Planning Group Descriptor. Descriptors are ergonomic immutable snapshots. APIs normalize descriptors by extracting their ID and re-resolving current runtime group data. + +## 9. Spec Interface Changes + +This section refers to DimOS Python `Spec` Protocols, not OpenSpec behavior specs. + +### WorldSpec + +`WorldSpec` owns planning group listing and resolution: + +```python +list_planning_groups() -> Sequence[PlanningGroupDescriptor] +resolve_planning_groups(group_ids: Sequence[PlanningGroupID]) -> Sequence[ResolvedPlanningGroup] +``` + +Resolution responsibilities: + +- Validate IDs are known. +- Bind model-level definitions to concrete robot/world data. +- Convert local model joint names to resolved joint names. +- Detect overlapping resolved joints across selected groups and fail. +- Return enough resolved data for IK/planner/backend internals to map back to local names and model instances. + +Group-scoped query APIs replace robot-scoped end-effector APIs: + +```python +get_group_pose(ctx, group_id) -> PoseStamped +get_group_jacobian(ctx, group_id) -> ... +``` + +These are valid only for groups with a pose target frame. `get_link_pose(ctx, robot_id, link_name)` remains as a lower-level query. + +### KinematicsSpec + +IK must solve over the full effective planning group selection: + +```python +solve_pose_targets( + world: WorldSpec, + pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], + *, + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + seed: JointState | None = None, + tolerances: PoseTolerance | None = None, + check_collision: bool = True, + max_attempts: int = 1, +) -> IKResult +``` + +IK constraints apply only to `pose_targets`. Auxiliary group joints are decision variables with no direct pose constraints. `IKResult.solution` contains exactly the selected resolved joints, not full robot/world state. + +### PlannerSpec + +Planner APIs operate over selected groups / resolved selected joints rather than a single `robot_id`. + +For joint planning: + +- Start and goal `JointState` keys must exactly equal selected resolved joints. +- Missing keys, extra keys, or partial goals fail early. + +For pose planning: + +- IK returns a selected-joint goal. +- The planner solves one combined joint-space problem from selected-joint start to selected-joint goal. + +Backends may return or raise `UNSUPPORTED` for backend limitations, including cross-robot coordinated planning. The public interface permits multi-robot selections. + +## 10. State, Naming, and Exactness Rules + +Above the model/SRDF parsing layer, all joint states use resolved joint names: + +```text +{robot_name}/{local_joint_name} +``` + +Examples: + +```text +left/joint1 +right/joint1 +a750/joint3 +``` + +Local names remain inside URDF/SRDF parsing and backend internals. Backends may strip the robot namespace and use `(local_joint_name, model_instance)` internally. + +Exactness rules: + +- Joint-space start keys must exactly equal selected resolved joints. +- Joint-space goal keys must exactly equal selected resolved joints. +- No missing selected joints. +- No extra joints. +- No partial targets. +- `IKResult.solution.keys()` equals selected resolved joints. +- `PlanningResult.path[i].keys()` equals selected resolved joints for every waypoint. + +These rules prevent silently planning for the wrong group or ignoring caller-supplied joints. + +## 11. Generated Plan Model + +`GeneratedPlan` is the canonical planning artifact: + +```python +GeneratedPlan: + group_ids: tuple[PlanningGroupID, ...] + path: list[JointState] + status: PlanningStatus + planning_time: float | None + path_length: float | None + iterations: int | None + message: str +``` + +`GeneratedPlan.path[i]` contains exactly the selected resolved joints for every waypoint. + +The plan does not store: + +- Per-robot paths. +- Per-task trajectories. +- Preview samples. +- Live world/planner handles. +- Controller-specific execution plans. + +`ManipulationModule` may keep `_last_plan: GeneratedPlan | None` as convenience state, but the returned plan object is canonical. + +## 12. Preview and Execution Projection + +Preview and execution project lazily from `GeneratedPlan`. + +### Preview + +`preview_plan(plan)`: + +- Uses the selected resolved-joint path. +- Projects into visualization/world monitor data as needed. +- Does not require execution-specific trajectory precomputation. + +### Execution + +`execute_plan(plan)`: + +1. Group resolved joint names by robot namespace/coordinator task. +2. Project the combined path into one `JointTrajectory` per affected trajectory task. +3. Order trajectory positions according to the task's configured joint order. +4. Convert resolved joint names to coordinator joint names if a boundary mapping exists. +5. Invoke each trajectory task. + +The current coordinator/JTC architecture already treats task invocation as asynchronous dispatch. JTCs run concurrently in the coordinator tick loop. This change does not add atomic batch dispatch, rollback-on-rejection, or a new execution batch abstraction. + +Planning groups do not enter the controller layer. Controllers consume joint-name-keyed trajectories. + +## 13. Migration Plan + +Configuration changes: + +- Add `srdf_path` to `RobotConfig`. +- Add `srdf_path` to `RobotModelConfig`. +- Store parsed planning group definitions on `RobotModelConfig`. +- Keep `RobotConfig.joint_names` and `RobotModelConfig.joint_names`, but define them as the controllable/coordinator joint set, not a planning group. + +Fields removed or deprecated under the new design: + +- `RobotConfig.base_link` +- `RobotConfig.base_pose` +- `RobotModelConfig.base_link` +- `RobotModelConfig.base_pose` +- `RobotModelConfig.end_effector_link` + +Implementation rollout: + +1. Add planning group data model and SRDF/fallback extraction. +2. Add deterministic local/resolved joint-name helpers. +3. Update `WorldSpec` and Drake world to list and resolve planning groups. +4. Add group-scoped pose/Jacobian APIs. +5. Update `KinematicsSpec` and IK implementation to solve selected pose targets plus auxiliary free groups. +6. Update `PlannerSpec` and planner implementation to operate over selected resolved joints. +7. Replace `ManipulationModule` robot-keyed planned path/trajectory caches with `GeneratedPlan` and optional `_last_plan`. +8. Update preview and execution projection. +9. Update manipulation skills/wrappers and documentation. +10. Update existing robot configs; rely on fallback for current single serial arms unless SRDF is added. + +Generated registry updates are not expected unless implementation changes blueprint names or adds/removes blueprint module-level variables. If that happens, run: + +```bash +pytest dimos/robot/test_all_blueprints_generation.py +``` + +## 14. Validation and Errors + +Validation should fail clearly for: + +- Unknown planning group ID. +- Unsupported selected SRDF group form. +- Fallback cannot infer one serial chain. +- Selected planning groups overlap in resolved joints. +- Pose-targeted group has no valid tip/pose target frame. +- Joint target keys do not exactly match selected group joints. +- Start/goal joint states contain missing or extra selected joints. +- Backend does not support the requested coordinated planning problem. +- Cross-robot coordinated planning requested on a backend that cannot support it. + +Warnings should be emitted for: + +- SRDF auto-discovery. +- Unsupported SRDF groups skipped during parsing. + +Manual QA should cover: + +- Single-arm no-SRDF fallback planning. +- SRDF chain group planning. +- SRDF joint-list group planning. +- Arm+torso pose planning where torso is auxiliary/free. +- Multi-group result shape and no-overlap validation. +- Multi-task execution projection with distinct joint namespaces. + +## 15. Alternatives Considered + +- **Composite groups as first-class objects**: rejected. Selecting multiple groups per request is enough and avoids duplicate group modeling. +- **Bare group names**: rejected. Planning Group IDs are always namespaced as `{robot_name}/{group_name}`. +- **Robot-scoped planning API**: rejected. Robot identity and planning group selection are separate concerns. +- **Full SRDF support**: deferred. This change supports only serial chain and ordered joint-list groups. +- **Parsing SRDF ``**: rejected for this change. Group pose target frame comes from chain/joint validation, not end-effector metadata. +- **Implicit default planning group**: rejected. Planning group selection is required. +- **Treating old `joint_names` as a compatibility planning group**: rejected unless it validates through conservative fallback. +- **`include_groups` with implicit semantics**: rejected in favor of request-scoped `auxiliary_groups`. +- **Enforcing auxiliary groups to be joint-only/no-EEF**: rejected. Auxiliary status belongs to the request, not the group definition. +- **Mixed pose+joint constraints now**: deferred. +- **Precomputed execution plans**: rejected. Generated plans stay minimal; downstream projections happen lazily. +- **Atomic multi-task trajectory dispatch**: deferred. Existing task invocation is acceptable for now. + +## 16. Rollout / Implementation Phases + +Suggested implementation phases: + +1. **Data model and parsing** + - Add planning group definition/descriptor/resolved-group types. + - Add SRDF path fields. + - Implement supported SRDF group parsing. + - Implement fallback generation. + +2. **World resolution** + - Add `WorldSpec.list_planning_groups()` and `resolve_planning_groups(...)`. + - Update Drake world to bind group definitions to model instances and resolved names. + - Add overlap validation. + +3. **Group-scoped kinematics queries** + - Add `get_group_pose(...)` and `get_group_jacobian(...)`. + - Remove/deprecate robot-scoped end-effector queries. + +4. **IK and planner interfaces** + - Update IK to accept pose targets plus auxiliary groups. + - Update planner to operate over selected resolved joints. + - Enforce exact start/goal key rules. + +5. **Generated plan flow** + - Add `GeneratedPlan`. + - Replace robot-keyed planned path/trajectory caches. + - Store only optional `_last_plan` convenience state. + +6. **Preview and execution projection** + - Project generated plans to visualization as needed. + - Project generated plans to per-task `JointTrajectory` at execution time. + +7. **Robot config migration and docs** + - Update existing robot configs. + - Add docs for SRDF support, fallback generation, APIs, and naming rules. + - Update manipulation skills/wrappers. + +## 17. Safety / Simulation / Replay + +This is primarily a planning API and modeling change. It should not change low-level trajectory controller semantics. + +Hardware-facing assumptions: + +- Execution still sends `JointTrajectory` messages to existing coordinator trajectory tasks. +- Affected trajectory tasks control disjoint resolved/coordinator joint sets. +- Multi-task dispatch is fast and non-blocking. +- Trajectory tasks execute concurrently in the coordinator tick loop. +- No new hardware-safety behavior, rollback, or atomic all-or-nothing dispatch is introduced. + +Simulation and replay should mirror hardware behavior because group resolution and generated plan projection happen above hardware drivers. Existing single-arm simulation/replay stacks should continue through fallback if they form one unambiguous serial chain. + +## 18. Risks / Trade-offs + +- **API breakage:** Existing callers plan by robot name or robot ID. Mitigation: provide migration docs and temporary wrapper conveniences where appropriate. +- **Partial SRDF support confusion:** Users may expect full MoveIt SRDF semantics. Mitigation: warn on skipped groups and document supported forms clearly. +- **Fallback misclassification:** Terminal prismatic stripping may accidentally exclude a real controllable prismatic axis. Mitigation: fallback only applies to unambiguous single chains; SRDF is required for precise modeling. +- **Joint naming migration cost:** Resolved names require updates across planner results, state monitors, and execution projection. Mitigation: deterministic helper conversion and strict layering. +- **Backend capability mismatch:** Some planners may not support multi-robot coordinated planning. Mitigation: allow `UNSUPPORTED` while preserving interface semantics. +- **Dispatch skew for multiple trajectory tasks:** Sequential task invocation can create tiny start-time differences. Mitigation: accepted for now; future coordinator batch dispatch can address this if needed. + +## 19. Open Questions + +- Exact error/status enum names for unsupported groups, no target frame, overlapping groups, and backend-unsupported problems. +- Exact temporary compatibility wrappers, if any, for existing manipulation skill APIs. +- Whether to write a short ADR for removing planning config placement fields in favor of URDF/model placement. +- Whether future coordinator-level `execute_batch(...)` is needed for tighter multi-task synchronization. + +## Appendix: Design Q&A Summary + +- **Term:** Use Planning Group; avoid Move Group/movegroup. +- **Ownership:** Define groups at model/config level; resolve to runtime/world-bound groups. +- **Composites:** Do not create composite groups now; select multiple groups per request. +- **Multi-group meaning:** One coordinated joint-space problem and one synchronized result. +- **End effectors:** Groups are defined by chain/joints, not SRDF `` metadata. +- **Group declaration:** Support chain or ordered joint list; validate as serial chain. +- **Default selection:** Planning group selection is required; no implicit default selection. +- **IDs:** Planning Group ID is always `{robot_name}/{group_name}`. +- **Descriptors:** Query APIs return immutable snapshots, not live handles. +- **Selectors:** APIs may accept group ID strings or descriptors; no dedicated selector type. +- **Joint goals:** Joint target APIs are keyed by planning group at API boundary, then lowered to one ordered resolved-joint problem. +- **Resolution:** `WorldSpec` owns group listing/resolution; planner delegates resolution to world. +- **Joint state exactness:** Joint-space start/goal keys must exactly equal selected resolved joints. +- **Source of truth:** SRDF first, conservative fallback only when no SRDF is present. +- **Fallback name:** Generated group is `manipulator`. +- **Fallback source:** Use `RobotModelConfig.joint_names` as candidate controllable joints. +- **Prismatic joints:** Middle prismatic joints are allowed; terminal/tip prismatic finger joints may be excluded. +- **SRDF scope:** Parse `` only; ignore ``. +- **Unsupported SRDF:** Skip unsupported group forms with warnings. +- **SRDF discovery:** Explicit path, then warning auto-discovery, then fallback, then error. +- **Config fields:** Add `srdf_path`; remove planning-level `base_link`, `end_effector_link`, and `base_pose` in the new design. +- **Robot placement:** Placement belongs in URDF/model, not separate planning config transforms. +- **FK/Jacobian:** Replace robot-scoped end-effector APIs with group-scoped APIs. +- **Cross-robot planning:** Interface allows it; backends may report unsupported. +- **Overlaps:** Selected groups must never overlap in resolved joints. +- **Result shape:** `PlanningResult.path` remains combined and synchronized. +- **Stored plan:** Store selected group IDs and path only; project lazily for preview/execution. +- **Resolved naming:** Above parsing, use `{robot_name}/{local_joint_name}` everywhere. +- **Coordinator mapping:** Coordinator names are a control-boundary concern; default identity with resolved names. +- **Auxiliary groups:** Auxiliary means selected without pose constraint in this request only. +- **Auxiliary DOFs:** Auxiliary groups are free DOFs for pose planning. +- **Mixed targets:** No mixed pose+joint target API in this change. +- **IK:** IK solves over pose-targeted groups plus auxiliary groups. +- **IK result:** `IKResult.solution` contains exactly selected resolved joints. +- **Planning result:** `PlanningResult.path` contains exactly selected resolved joints. +- **Execution:** Split path by trajectory task and send to existing JTCs; no new batch/rollback semantics. +- **Generated plan:** Returned plan object is canonical; module last-plan storage is convenience. diff --git a/openspec/changes/add-planning-groups/proposal.md b/openspec/changes/add-planning-groups/proposal.md new file mode 100644 index 0000000000..ce14a0ee70 --- /dev/null +++ b/openspec/changes/add-planning-groups/proposal.md @@ -0,0 +1,53 @@ +## Why + +DimOS manipulation planning is currently robot-centric: planner and kinematics interfaces select a `robot_id`, while `RobotModelConfig` carries a single `joint_names`, `base_link`, and `end_effector_link` shape. That works for a single serial arm, but it hides the actual planning unit and makes torso+arm, dual-arm, and coordinated multi-group planning awkward or ambiguous. + +Planning groups should be first-class. Robot identity should describe the hardware/model instance, while planning group selection should describe which kinematic chains participate in IK and motion planning. This change also makes joint naming unambiguous above the model parsing layer by using stable resolved joint names. + +## What Changes + +- Add first-class planning group definitions sourced primarily from SRDF `` entries. +- Add conservative fallback generation of one `manipulator` planning group for unambiguous single-chain robots without SRDF. +- **BREAKING**: Planning and IK APIs move from robot-ID selection to explicit planning group selection. +- **BREAKING**: Joint states and generated paths above model parsing use resolved joint names of the form `{robot_name}/{local_joint_name}`. +- Add pose planning over pose-targeted planning groups plus request-scoped auxiliary planning groups that contribute free DOFs. +- Add coordinated multi-group planning semantics: one selected joint set, one synchronized path, and no overlapping selected joints. +- Replace robot-scoped end-effector FK/Jacobian queries with group-scoped queries. +- Introduce a minimal generated plan artifact that stores selected group IDs and a combined resolved-joint path, projecting lazily for preview and execution. +- Remove planning configuration concepts that duplicate robot model structure, including robot-level planning `base_link`, `end_effector_link`, and `base_pose` fields in the new design. + +## Affected DimOS Surfaces + +- Modules/streams: + - Manipulation planning module plan/preview/execute flow. + - Planning `WorldSpec`, `KinematicsSpec`, and `PlannerSpec` interfaces. + - Drake planning world implementation and world monitor/preview integration. + - Robot model/config parsing and model-to-planning config conversion. +- Blueprints/CLI: + - Existing manipulation blueprints should continue for single serial arms through fallback group generation. + - Ambiguous, branching, or multi-arm robots require SRDF rather than silent compatibility behavior. +- Skills/MCP: + - Manipulation skills that call pose or joint planning must select planning groups explicitly or use wrapper defaults supplied by the skill layer. +- Hardware/simulation/replay: + - Execution still sends projected trajectories to existing trajectory controller tasks. + - Multi-task execution dispatches per-task trajectories; trajectory controllers own runtime concurrency. + - No new hardware-safety behavior or atomic multi-task batch dispatch is introduced. +- Docs/generated registries: + - User/developer docs for manipulation planning APIs, SRDF support, fallback generation, and joint naming need updates. + - No generated blueprint registry changes are expected unless robot config/blueprint names change during implementation. + +## Capabilities + +### New Capabilities + +- `manipulation-planning-groups`: Planning group discovery, selection, coordinated planning, IK target semantics, generated plans, and preview/execution projection. + +### Modified Capabilities + +- None. No existing OpenSpec capability specs are present in this repository checkout. + +## Impact + +This is a public manipulation planning API redesign. Existing code that plans by robot name or assumes bare local joint names will need migration to explicit planning group IDs and resolved joint names. Existing single-arm robots without SRDF should keep working through generated `{robot_name}/manipulator` groups if their configured controllable joints form one unambiguous serial chain. + +The implementation needs tests for SRDF parsing, fallback generation, group resolution, resolved joint naming, IK with auxiliary groups, exact joint-target validation, multi-group planning result shape, and lazy preview/execution projection. Documentation should emphasize the distinction between robot identity, planning group identity, local model joint names, resolved joint names, and coordinator joint names. From 1730a5e7473d46a28dce9162984cb6100b3a618d Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 17 Jun 2026 15:59:41 -0700 Subject: [PATCH 04/27] feat: add manipulation planning groups --- .../test_manipulation_planning_groups.py | 219 +++++++ dimos/manipulation/manipulation_module.py | 565 +++++++++++++++++- .../planning/kinematics/jacobian_ik.py | 104 +++- .../kinematics/test_jacobian_ik_selection.py | 114 ++++ .../planning/monitor/world_monitor.py | 33 +- dimos/manipulation/planning/names.py | 88 +++ .../planning/planners/rrt_planner.py | 192 +++++- .../planners/test_rrt_planner_selection.py | 184 ++++++ .../manipulation/planning/planning_groups.py | 365 +++++++++++ dimos/manipulation/planning/spec/config.py | 21 +- dimos/manipulation/planning/spec/enums.py | 1 + dimos/manipulation/planning/spec/models.py | 93 ++- dimos/manipulation/planning/spec/protocols.py | 57 +- .../planning/test_planning_groups.py | 224 +++++++ .../planning/world/drake_world.py | 277 ++++++++- .../world/test_drake_world_planning_groups.py | 207 +++++++ dimos/manipulation/test_manipulation_unit.py | 187 ++++++ dimos/robot/config.py | 29 +- .../manipulation/adding_a_custom_arm.md | 25 +- .../manipulation/planning_groups.md | 121 ++++ docs/capabilities/manipulation/readme.md | 8 + openspec/changes/add-planning-groups/docs.md | 45 ++ .../manipulation-planning-groups/spec.md | 216 +++++++ openspec/changes/add-planning-groups/tasks.md | 92 +++ pyproject.toml | 1 + 25 files changed, 3385 insertions(+), 83 deletions(-) create mode 100644 dimos/e2e_tests/test_manipulation_planning_groups.py create mode 100644 dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py create mode 100644 dimos/manipulation/planning/names.py create mode 100644 dimos/manipulation/planning/planners/test_rrt_planner_selection.py create mode 100644 dimos/manipulation/planning/planning_groups.py create mode 100644 dimos/manipulation/planning/test_planning_groups.py create mode 100644 dimos/manipulation/planning/world/test_drake_world_planning_groups.py create mode 100644 docs/capabilities/manipulation/planning_groups.md create mode 100644 openspec/changes/add-planning-groups/docs.md create mode 100644 openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md create mode 100644 openspec/changes/add-planning-groups/tasks.md diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py new file mode 100644 index 0000000000..70ae660b1a --- /dev/null +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -0,0 +1,219 @@ +# 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. + +"""Large E2E tests for manipulation planning groups with a coordinator. + +These tests launch a real ManipulationModule + ControlCoordinator blueprint and +exercise the public planning RPCs over LCM, matching the self-hosted large-test +style used by the navigation stack. +""" + +from __future__ import annotations + +from collections.abc import Callable +import importlib.util +import time +from typing import Any + +import pytest + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.rpc_client import RPCClient +from dimos.e2e_tests.dimos_cli_call import DimosCliCall +from dimos.e2e_tests.lcm_spy import LcmSpy +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState + +pytestmark = [pytest.mark.self_hosted_large] + +JOINT_STATE_TOPIC = "/coordinator/joint_state#sensor_msgs.JointState" +BLUEPRINT = "openarm-mock-planner-coordinator" + + +def _drake_available() -> bool: + return importlib.util.find_spec("pydrake") is not None + + +def _wait_for_robot_info( + client: RPCClient, + robot_name: str, + *, + timeout: float = 120.0, +) -> dict[str, Any]: + deadline = time.time() + timeout + last_error: BaseException | None = None + while time.time() < deadline: + try: + info = client.get_robot_info(robot_name) + if info and info.get("planning_groups"): + return info + except BaseException as exc: + last_error = exc + time.sleep(0.5) + raise TimeoutError(f"Timed out waiting for {robot_name!r} robot info") from last_error + + +def _wait_for_trajectory_completion( + client: RPCClient, + robot_name: str, + *, + timeout: float = 10.0, +) -> None: + deadline = time.time() + timeout + last_status: dict[str, Any] | None = None + while time.time() < deadline: + last_status = client.get_trajectory_status(robot_name) + if last_status is not None and last_status.get("state") == TrajectoryState.COMPLETED: + return + time.sleep(0.1) + raise TimeoutError(f"{robot_name!r} trajectory did not complete; last={last_status}") + + +def _wait_for_manipulation_state( + client: RPCClient, + state_name: str, + *, + timeout: float = 10.0, +) -> None: + deadline = time.time() + timeout + last_state: str | None = None + while time.time() < deadline: + last_state = client.get_state() + if last_state == state_name: + return + time.sleep(0.1) + raise TimeoutError(f"ManipulationModule did not reach {state_name}; last={last_state}") + + +def _wait_for_current_joints( + client: RPCClient, + robot_names: tuple[str, ...], + *, + timeout: float = 10.0, +) -> None: + deadline = time.time() + timeout + missing = robot_names + while time.time() < deadline: + missing = tuple( + robot_name + for robot_name in robot_names + if client.get_current_joints(robot_name) is None + ) + if not missing: + return + time.sleep(0.1) + raise TimeoutError(f"Timed out waiting for current joints from {missing}") + + +def _prepare_for_planning(client: RPCClient, robot_names: tuple[str, ...]) -> None: + client.reset() + _wait_for_manipulation_state(client, "IDLE") + _wait_for_current_joints(client, robot_names) + # Robot info and joint-state topics can become available just before the + # manipulation module finishes finalizing world monitors. Require a stable + # ready state after joint state is flowing to avoid command-readiness flakes. + time.sleep(0.25) + _wait_for_manipulation_state(client, "IDLE") + + +def _planning_group_id(info: dict[str, Any]) -> str: + groups = info["planning_groups"] + assert len(groups) == 1 + group_id = groups[0]["id"] + assert isinstance(group_id, str) + return group_id + + +def _offset_target(client: RPCClient, robot_name: str, delta: float) -> JointState: + current = client.get_current_joints(robot_name) + assert current is not None + return JointState(position=[position + delta for position in current]) + + +def _start_openarm_mock_planner( + start_blueprint: Callable[..., DimosCliCall], lcm_spy: LcmSpy +) -> None: + lcm_spy.save_topic(JOINT_STATE_TOPIC) + start_blueprint(BLUEPRINT) + lcm_spy.wait_for_saved_topic(JOINT_STATE_TOPIC, timeout=120.0) + + +@pytest.mark.skipif(not _drake_available(), reason="Drake not installed") +def test_single_arm_plans_and_executes_through_control_coordinator( + lcm_spy: LcmSpy, + start_blueprint: Callable[..., DimosCliCall], +) -> None: + """Plan with one arm and execute through its trajectory task.""" + _start_openarm_mock_planner(start_blueprint, lcm_spy) + + client = RPCClient(None, ManipulationModule) + coordinator_client = RPCClient(None, ControlCoordinator) + try: + left_info = _wait_for_robot_info(client, "left_arm") + left_id = _planning_group_id(left_info) + + tasks = coordinator_client.list_tasks() + assert left_info["coordinator_task_name"] in tasks + + _prepare_for_planning(client, ("left_arm",)) + + planned = client.plan_to_joint_targets({left_id: _offset_target(client, "left_arm", 0.02)}) + assert planned, client.get_error() + assert client.has_planned_path() + assert client.execute_plan(robot_name="left_arm") + + _wait_for_trajectory_completion(client, "left_arm") + finally: + coordinator_client.stop_rpc_client() + client.stop_rpc_client() + + +@pytest.mark.skipif(not _drake_available(), reason="Drake not installed") +def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( + lcm_spy: LcmSpy, + start_blueprint: Callable[..., DimosCliCall], +) -> None: + """Plan one generated plan over both arms and dispatch both JTC tasks.""" + _start_openarm_mock_planner(start_blueprint, lcm_spy) + + client = RPCClient(None, ManipulationModule) + coordinator_client = RPCClient(None, ControlCoordinator) + try: + left_info = _wait_for_robot_info(client, "left_arm") + right_info = _wait_for_robot_info(client, "right_arm") + left_id = _planning_group_id(left_info) + right_id = _planning_group_id(right_info) + + tasks = coordinator_client.list_tasks() + assert left_info["coordinator_task_name"] in tasks + assert right_info["coordinator_task_name"] in tasks + + _prepare_for_planning(client, ("left_arm", "right_arm")) + + planned = client.plan_to_joint_targets( + { + left_id: _offset_target(client, "left_arm", 0.02), + right_id: _offset_target(client, "right_arm", -0.02), + } + ) + assert planned, client.get_error() + assert client.has_planned_path() + assert client.execute_plan() + + _wait_for_trajectory_completion(client, "left_arm") + _wait_for_trajectory_completion(client, "right_arm") + finally: + coordinator_client.stop_rpc_client() + client.stop_rpc_client() diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c5e2cafc84..70d6096a30 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -24,6 +24,7 @@ from __future__ import annotations +from collections.abc import Mapping, Sequence from enum import Enum import threading import time @@ -40,9 +41,19 @@ from dimos.core.stream import In from dimos.manipulation.planning.factory import create_kinematics, create_planner from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor +from dimos.manipulation.planning.names import to_resolved_joint_name from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType -from dimos.manipulation.planning.spec.models import JointPath, Obstacle, RobotName, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + GeneratedPlan, + JointPath, + Obstacle, + PlanningGroupDescriptor, + PlanningGroupID, + PlanningResult, + RobotName, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( JointTrajectoryGenerator, @@ -131,6 +142,7 @@ def __init__(self, **kwargs: Any) -> None: # Stored path for plan/preview/execute workflow (per robot) self._planned_paths: PlannedPaths = {} self._planned_trajectories: PlannedTrajectories = {} + self._last_plan: GeneratedPlan | None = None # Coordinator integration (lazy initialized) self._coordinator_client: RPCClient | None = None @@ -310,10 +322,20 @@ def _tf_publish_loop(self) -> None: break transforms: list[Transform] = [] for robot_id, config, _ in self._robots.values(): - # Publish world → EE - ee_pose = self._world_monitor.get_ee_pose(robot_id) - if ee_pose is not None: - ee_tf = Transform.from_pose(config.end_effector_link, ee_pose) + # Publish world → primary planning-group target frame. + # Fall back to deprecated robot-scoped EE only for legacy configs. + target_frame = config.end_effector_link + pose_group_id = self._primary_pose_group_id_for_robot(config.name) + if pose_group_id is not None: + pose_group = self._world_monitor.world.resolve_planning_groups( + (pose_group_id,) + )[0] + target_frame = pose_group.tip_link + ee_pose = self._world_monitor.get_group_pose(pose_group_id) + else: + ee_pose = self._world_monitor.get_ee_pose(robot_id) + if ee_pose is not None and target_frame is not None: + ee_tf = Transform.from_pose(target_frame, ee_pose) ee_tf.frame_id = "world" transforms.append(ee_tf) @@ -437,6 +459,247 @@ def _fail(self, msg: str) -> bool: self._error_message = msg return False + def _default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return wrapper-level default group for legacy single-group RPCs.""" + assert self._world_monitor is not None + group_ids = [ + group.id + for group in self._world_monitor.world.list_planning_groups() + if group.robot_name == robot_name + ] + if len(group_ids) != 1: + logger.error( + "Robot '%s' has %d planning groups; select a planning group explicitly", + robot_name, + len(group_ids), + ) + return None + return group_ids[0] + + def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the first pose-targetable group for robot-scoped compatibility paths.""" + assert self._world_monitor is not None + for group in self._world_monitor.world.list_planning_groups(): + if group.robot_name == robot_name and group.has_pose_target: + return group.id + return None + + def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: + """Collect current state for exactly the selected resolved joints.""" + assert self._world_monitor is not None + resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) + names: list[str] = [] + positions: list[float] = [] + current_by_robot: dict[WorldRobotID, dict[str, float]] = {} + + for group in resolved_groups: + if group.robot_id not in current_by_robot: + current = self._world_monitor.get_current_joint_state(group.robot_id) + if current is None: + logger.error("No joint state for robot '%s'", group.robot_name) + return None + current_by_robot[group.robot_id] = dict( + zip(current.name, current.position, strict=False) + ) + + robot_state = current_by_robot[group.robot_id] + for resolved_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=True + ): + if resolved_name in robot_state: + position = robot_state[resolved_name] + elif local_name in robot_state: + position = robot_state[local_name] + else: + logger.error("Current state missing selected joint '%s'", resolved_name) + return None + names.append(resolved_name) + positions.append(position) + + return JointState(name=names, position=positions) + + def _normalize_group_selector( + self, selector: PlanningGroupID | PlanningGroupDescriptor + ) -> PlanningGroupID: + """Normalize a planning group selector to its stable public ID.""" + if isinstance(selector, PlanningGroupDescriptor): + return selector.id + return selector + + def _normalize_joint_target( + self, group_id: PlanningGroupID, target: JointState + ) -> JointState | None: + """Normalize a group joint target to resolved joint names in group order.""" + assert self._world_monitor is not None + group = self._world_monitor.world.resolve_planning_groups((group_id,))[0] + if not target.name: + if len(target.position) != len(group.joint_names): + logger.error("Target for '%s' has wrong joint count", group_id) + return None + return JointState(name=list(group.joint_names), position=list(target.position)) + + positions_by_name = dict(zip(target.name, target.position, strict=False)) + resolved_positions: list[float] = [] + for resolved_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=False + ): + if resolved_name in positions_by_name: + resolved_positions.append(positions_by_name[resolved_name]) + elif local_name in positions_by_name: + resolved_positions.append(positions_by_name[local_name]) + else: + logger.error("Target for '%s' is missing joint '%s'", group_id, resolved_name) + return None + extra = set(target.name) - set(group.joint_names) - set(group.local_joint_names) + if extra: + logger.error("Target for '%s' has extra joints: %s", group_id, sorted(extra)) + return None + return JointState(name=list(group.joint_names), position=resolved_positions) + + def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotName) -> JointPath: + """Project combined plan path to one robot in configured local joint order. + + Generated plans only contain selected resolved joints. Trajectory tasks may + still be configured for the robot's full controllable joint set, so + non-selected joints are held at their current positions during projection. + """ + robot_id, config, _ = self._robots[robot_name] + resolved_joint_names = [ + to_resolved_joint_name(robot_name, joint) for joint in config.joint_names + ] + current_by_name: dict[str, float] = {} + if self._world_monitor is not None: + current = self._world_monitor.get_current_joint_state(robot_id) + if current is not None: + current_by_name = dict(zip(current.name, current.position, strict=False)) + projected: JointPath = [] + for waypoint in plan.path: + position_by_name = dict(zip(waypoint.name, waypoint.position, strict=False)) + positions: list[float] = [] + for local_name, resolved_name in zip( + config.joint_names, resolved_joint_names, strict=False + ): + if resolved_name in position_by_name: + positions.append(position_by_name[resolved_name]) + elif resolved_name in current_by_name: + positions.append(current_by_name[resolved_name]) + elif local_name in current_by_name: + positions.append(current_by_name[local_name]) + else: + logger.error( + "Cannot project plan for '%s': missing joint '%s'", + robot_name, + resolved_name, + ) + return [] + projected.append( + JointState( + name=list(config.joint_names), + position=positions, + ) + ) + return projected + + def _trajectory_for_robot_plan( + self, plan: GeneratedPlan, robot_name: RobotName + ) -> JointTrajectory | None: + """Generate a task-ordered trajectory for one affected robot lazily.""" + projected_path = self._project_plan_path_for_robot(plan, robot_name) + if len(projected_path) < 2: + logger.error("Plan projection for '%s' has fewer than two waypoints", robot_name) + return None + _, config, traj_gen = self._robots[robot_name] + trajectory = traj_gen.generate([list(state.position) for state in projected_path]) + return JointTrajectory( + joint_names=list(config.joint_names), + points=trajectory.points, + timestamp=trajectory.timestamp, + ) + + def _affected_robot_names(self, plan: GeneratedPlan) -> list[RobotName]: + """Get stable robot names affected by a generated plan.""" + assert self._world_monitor is not None + resolved_groups = self._world_monitor.world.resolve_planning_groups(plan.group_ids) + names: list[RobotName] = [] + for group in resolved_groups: + if group.robot_name not in names: + names.append(group.robot_name) + return names + + def _store_generated_plan( + self, group_ids: tuple[PlanningGroupID, ...], result: PlanningResult + ) -> None: + """Store canonical generated plan and compatibility per-robot projections.""" + self._last_plan = GeneratedPlan( + group_ids=group_ids, + path=result.path, + status=result.status, + planning_time=result.planning_time, + path_length=result.path_length, + iterations=result.iterations, + message=result.message, + ) + self._planned_paths.clear() + self._planned_trajectories.clear() + for robot_name in self._affected_robot_names(self._last_plan): + projected_path = self._project_plan_path_for_robot(self._last_plan, robot_name) + if projected_path: + self._planned_paths[robot_name] = projected_path + trajectory = self._trajectory_for_robot_plan(self._last_plan, robot_name) + if trajectory is not None: + self._planned_trajectories[robot_name] = trajectory + + def _plan_selected_path( + self, group_ids: tuple[PlanningGroupID, ...], start: JointState, goal: JointState + ) -> bool: + """Plan over an explicit planning group selection and store the result.""" + assert self._world_monitor and self._planner + result = self._planner.plan_selected_joint_path( + world=self._world_monitor.world, + group_ids=group_ids, + start=start, + goal=goal, + timeout=self.config.planning_timeout, + ) + if not result.is_success(): + return self._fail(f"Planning failed: {result.status.name}") + + logger.info("Path: %d waypoints", len(result.path)) + self._store_generated_plan(group_ids, result) + self._state = ManipulationState.COMPLETED + return True + + def _interpolate_preview_path( + self, + planned_path: JointPath, + trajectory: JointTrajectory | None, + animation_duration: float, + target_fps: float, + ) -> JointPath: + """Densify a planned path for visualization using a timed trajectory.""" + interpolated = list(planned_path) + if trajectory is None or target_fps <= 0 or animation_duration <= 0: + return interpolated + + times = np.array([point.time_from_start for point in trajectory.points], dtype=np.float64) + positions = np.array([point.positions for point in trajectory.points], dtype=np.float64) + if len(times) <= 1 or positions.ndim != 2 or times[-1] <= times[0]: + return interpolated + + frame_count = int(np.ceil(animation_duration * target_fps)) + 1 + sample_times = np.linspace(times[0], times[-1], frame_count) + joint_names = trajectory.joint_names or planned_path[0].name + sampled_positions = np.column_stack( + [ + np.interp(sample_times, times, positions[:, joint]) + for joint in range(positions.shape[1]) + ] + ) + return [ + JointState(name=joint_names, position=position.tolist()) + for position in sampled_positions + ] + def _dismiss_preview(self, robot_id: WorldRobotID) -> None: """Hide the preview ghost if the world supports it.""" if self._world_monitor is None: @@ -470,6 +733,23 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: orientation=pose.orientation, ) + group_id = self._default_group_id_for_robot(robot_name) + if group_id is not None: + ik = self._kinematics.solve_pose_targets( + world=self._world_monitor.world, + pose_targets={group_id: target_pose}, + seed=current, + check_collision=True, + ) + if not ik.is_success() or ik.joint_state is None: + return self._fail(f"IK failed: {ik.status.name}") + + start = self._selected_joint_state((group_id,)) + if start is None: + return self._fail("No joint state") + logger.info(f"IK solved, error: {ik.position_error:.4f}m") + return self._plan_selected_path((group_id,), start, ik.joint_state) + ik = self._kinematics.solve( world=self._world_monitor.world, robot_id=robot_id, @@ -483,6 +763,54 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: logger.info(f"IK solved, error: {ik.position_error:.4f}m") return self._plan_path_only(robot_name, robot_id, ik.joint_state) + @rpc + def plan_to_poses( + self, + pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, Pose], + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + ) -> bool: + """Plan to one or more group pose targets with optional auxiliary groups.""" + if self._world_monitor is None or self._kinematics is None: + return False + if not pose_targets: + return self._fail("At least one pose target is required") + with self._lock: + if self._state not in (ManipulationState.IDLE, ManipulationState.COMPLETED): + logger.warning(f"Cannot plan: state is {self._state.name}") + return False + self._state = ManipulationState.PLANNING + + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + stamped_targets = { + self._normalize_group_selector(group): PoseStamped( + frame_id="world", + position=pose.position, + orientation=pose.orientation, + ) + for group, pose in pose_targets.items() + } + auxiliary_ids = tuple(self._normalize_group_selector(group) for group in auxiliary_groups) + group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) + + try: + start = self._selected_joint_state(group_ids) + except Exception as exc: + return self._fail(f"Failed to resolve planning groups: {exc}") + if start is None: + return self._fail("No joint state") + + ik = self._kinematics.solve_pose_targets( + world=self._world_monitor.world, + pose_targets=stamped_targets, + auxiliary_groups=auxiliary_ids, + seed=start, + check_collision=True, + ) + if not ik.is_success() or ik.joint_state is None: + return self._fail(f"IK failed: {ik.status.name}") + return self._plan_selected_path(group_ids, start, ik.joint_state) + @rpc def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: """Plan motion to joint config. Use preview_path() then execute(). @@ -495,8 +823,53 @@ def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None return False robot_name, robot_id = r logger.info(f"Planning to joints for {robot_name}: {[f'{j:.3f}' for j in joints.position]}") + group_id = self._default_group_id_for_robot(robot_name) + if group_id is not None: + goal = self._normalize_joint_target(group_id, joints) + if goal is None: + return self._fail("Invalid joint target") + start = self._selected_joint_state((group_id,)) + if start is None: + return self._fail("No joint state") + return self._plan_selected_path((group_id,), start, goal) return self._plan_path_only(robot_name, robot_id, joints) + @rpc + def plan_to_joint_targets( + self, joint_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, JointState] + ) -> bool: + """Plan to joint targets keyed by planning group.""" + if self._world_monitor is None or self._planner is None: + return False + if not joint_targets: + return self._fail("At least one joint target is required") + with self._lock: + if self._state not in (ManipulationState.IDLE, ManipulationState.COMPLETED): + logger.warning(f"Cannot plan: state is {self._state.name}") + return False + self._state = ManipulationState.PLANNING + + group_ids = tuple(self._normalize_group_selector(group) for group in joint_targets) + try: + start = self._selected_joint_state(group_ids) + except Exception as exc: + return self._fail(f"Failed to resolve planning groups: {exc}") + if start is None: + return self._fail("No joint state") + + goal_names: list[str] = [] + goal_positions: list[float] = [] + for group, target in joint_targets.items(): + group_id = self._normalize_group_selector(group) + normalized = self._normalize_joint_target(group_id, target) + if normalized is None: + return self._fail(f"Invalid joint target for '{group_id}'") + goal_names.extend(normalized.name) + goal_positions.extend(normalized.position) + + goal = JointState(name=goal_names, position=goal_positions) + return self._plan_selected_path(group_ids, start, goal) + def _plan_path_only( self, robot_name: RobotName, robot_id: WorldRobotID, goal: JointState ) -> bool: @@ -537,6 +910,46 @@ def _plan_path_only( self._state = ManipulationState.COMPLETED return True + @rpc + def preview_plan( + self, + plan: GeneratedPlan | None = None, + duration: float | None = None, + robot_name: RobotName | None = None, + target_fps: float = 30.0, + ) -> bool: + """Preview a generated plan, defaulting to `_last_plan` when omitted.""" + if self._world_monitor is None: + return False + plan = plan or getattr(self, "_last_plan", None) + if plan is None or not plan.path: + logger.warning("No generated plan to preview") + return False + + robot_names = [robot_name] if robot_name is not None else self._affected_robot_names(plan) + previewed = False + for name in robot_names: + robot = self._get_robot(name) + if robot is None: + return False + resolved_name, robot_id, _, _ = robot + planned_path = self._project_plan_path_for_robot(plan, resolved_name) + if not planned_path: + logger.warning(f"No planned path to preview for {resolved_name}") + return False + trajectory = self._trajectory_for_robot_plan(plan, resolved_name) + animation_duration = ( + duration + if duration is not None + else (trajectory.duration if trajectory is not None else 3.0) + ) + interpolated = self._interpolate_preview_path( + planned_path, trajectory, animation_duration, target_fps + ) + self._world_monitor.animate_path(robot_id, interpolated, animation_duration) + previewed = True + return previewed + @rpc def preview_path( self, @@ -551,6 +964,10 @@ def preview_path( robot_name: Robot to preview (required if multiple robots configured) target_fps: Nominal preview update rate. Set <= 0 to use planned waypoints directly. """ + last_plan = getattr(self, "_last_plan", None) + if last_plan is not None and last_plan.path: + return self.preview_plan(last_plan, duration, robot_name, target_fps) + if self._world_monitor is None: return False @@ -571,26 +988,9 @@ def preview_path( trajectory = self._planned_trajectories.get(robot_name) animation_duration = duration - interpolated = list(planned_path) - if trajectory is not None and target_fps > 0 and animation_duration > 0: - times = np.array( - [point.time_from_start for point in trajectory.points], dtype=np.float64 - ) - positions = np.array([point.positions for point in trajectory.points], dtype=np.float64) - if len(times) > 1 and positions.ndim == 2 and times[-1] > times[0]: - frame_count = int(np.ceil(animation_duration * target_fps)) + 1 - sample_times = np.linspace(times[0], times[-1], frame_count) - joint_names = trajectory.joint_names or planned_path[0].name - sampled_positions = np.column_stack( - [ - np.interp(sample_times, times, positions[:, joint]) - for joint in range(positions.shape[1]) - ] - ) - interpolated = [ - JointState(name=joint_names, position=position.tolist()) - for position in sampled_positions - ] + interpolated = self._interpolate_preview_path( + planned_path, trajectory, animation_duration, target_fps + ) self._world_monitor.animate_path(robot_id, interpolated, animation_duration) return True @@ -601,11 +1001,14 @@ def has_planned_path(self) -> bool: Returns: True if a path is planned and ready """ + last_plan = getattr(self, "_last_plan", None) + if last_plan is not None: + return bool(last_plan.path) + robot = self._get_robot() if robot is None: return False robot_name, _, _, _ = robot - path = self._planned_paths.get(robot_name) return path is not None and len(path) > 0 @@ -627,13 +1030,9 @@ def clear_planned_path(self) -> bool: Returns: True if cleared """ - robot = self._get_robot() - if robot is None: - return False - robot_name, _, _, _ = robot - - self._planned_paths.pop(robot_name, None) - self._planned_trajectories.pop(robot_name, None) + self._last_plan = None + self._planned_paths.clear() + self._planned_trajectories.clear() return True @rpc @@ -660,13 +1059,33 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> dict[str, Any] return None robot_name, robot_id, config, _ = robot + planning_groups = ( + [ + { + "id": group.id, + "name": group.group_name, + "joint_names": list(group.joint_names), + "local_joint_names": list(group.local_joint_names), + "base_link": group.base_link, + "tip_link": group.tip_link, + "source": group.source, + "has_pose_target": group.has_pose_target, + } + for group in self._world_monitor.world.list_planning_groups() + if group.robot_name == robot_name + ] + if self._world_monitor is not None + else [] + ) return { "name": config.name, "world_robot_id": robot_id, "joint_names": config.joint_names, + "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, + "deprecated_fields": ["end_effector_link", "base_link"], "max_velocity": config.max_velocity, "max_acceleration": config.max_acceleration, "has_joint_name_mapping": bool(config.joint_name_mapping), @@ -778,6 +1197,10 @@ def _translate_trajectory_to_coordinator( @rpc def execute(self, robot_name: RobotName | None = None) -> bool: """Execute planned trajectory via ControlCoordinator.""" + last_plan = getattr(self, "_last_plan", None) + if last_plan is not None and last_plan.path: + return self.execute_plan(last_plan, robot_name) + if (robot := self._get_robot(robot_name)) is None: return False robot_name, _, config, _ = robot @@ -808,9 +1231,73 @@ def execute(self, robot_name: RobotName | None = None) -> bool: else: return self._fail("Coordinator rejected trajectory") + @rpc + def execute_plan( + self, plan: GeneratedPlan | None = None, robot_name: RobotName | None = None + ) -> bool: + """Project and execute a generated plan through affected trajectory tasks.""" + plan = plan or getattr(self, "_last_plan", None) + if plan is None or not plan.path: + logger.warning("No generated plan") + return False + if (client := self._get_coordinator_client()) is None: + logger.error("No coordinator client") + return False + + try: + affected = self._affected_robot_names(plan) + except Exception as exc: + return self._fail(f"Failed to resolve generated plan: {exc}") + robot_names = [robot_name] if robot_name is not None else affected + + dispatches: list[tuple[RobotName, RobotModelConfig, JointTrajectory]] = [] + for name in robot_names: + if name not in affected: + logger.error("Generated plan does not affect robot '%s'", name) + return False + robot = self._get_robot(name) + if robot is None: + return False + resolved_name, _, config, _ = robot + if not config.coordinator_task_name: + logger.error(f"No coordinator_task_name for '{resolved_name}'") + return False + trajectory = self._trajectory_for_robot_plan(plan, resolved_name) + if trajectory is None: + return False + dispatches.append((resolved_name, config, trajectory)) + + self._state = ManipulationState.EXECUTING + for name, config, trajectory in dispatches: + self._planned_trajectories[name] = trajectory + translated = self._translate_trajectory_to_coordinator(trajectory, config) + logger.info( + "Executing: task='%s', %d pts, %.2fs", + config.coordinator_task_name, + len(translated.points), + translated.duration, + ) + result = client.task_invoke( + config.coordinator_task_name, "execute", {"trajectory": translated} + ) + if not result: + return self._fail("Coordinator rejected trajectory") + + logger.info("Trajectory accepted") + self._state = ManipulationState.COMPLETED + return True + @rpc def get_trajectory_status(self, robot_name: RobotName | None = None) -> dict[str, Any] | None: """Get trajectory execution status via coordinator task_invoke.""" + last_plan = getattr(self, "_last_plan", None) + if robot_name is None and last_plan is not None and last_plan.path: + statuses = { + name: self.get_trajectory_status(name) + for name in self._affected_robot_names(last_plan) + } + return {"robots": statuses} + if (robot := self._get_robot(robot_name)) is None: return None _, _, config, _ = robot @@ -968,6 +1455,18 @@ def _wait_for_trajectory_completion( Returns: True if trajectory completed successfully """ + last_plan = getattr(self, "_last_plan", None) + if robot_name is None and last_plan is not None and last_plan.path: + try: + robot_names = self._affected_robot_names(last_plan) + except Exception as exc: + logger.warning("Failed to resolve generated plan while waiting: %s", exc) + return False + return all( + self._wait_for_trajectory_completion(name, timeout, poll_interval) + for name in robot_names + ) + robot = self._get_robot(robot_name) if robot is None: return True diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 7727b6fa0f..7c810d6302 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -24,12 +24,18 @@ from __future__ import annotations +from collections.abc import Mapping, Sequence from typing import TYPE_CHECKING import numpy as np from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + IKResult, + PlanningGroupDescriptor, + PlanningGroupID, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import ( check_singularity, @@ -185,6 +191,85 @@ def solve( f"IK failed after {max_attempts} attempts", ) + def solve_pose_targets( + self, + world: WorldSpec, + pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + """Solve pose targets keyed by planning group with request-scoped auxiliaries. + + This backend currently supports one directly pose-targeted robot at a time. Auxiliary + groups are included in the selected result shape when they belong to the same resolved + planning problem; existing robot-scoped IK provides their seed/current values as free + degrees of freedom. + """ + if not pose_targets: + return _create_failure_result( + IKStatus.NO_SOLUTION, "At least one pose target is required" + ) + + pose_group_ids = tuple(_selector_id(group) for group in pose_targets.keys()) + auxiliary_group_ids = tuple(_selector_id(group) for group in auxiliary_groups) + selected_group_ids = pose_group_ids + auxiliary_group_ids + try: + resolved_groups = world.resolve_planning_groups(selected_group_ids) + except (KeyError, ValueError) as exc: + return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) + + if len(pose_group_ids) != 1: + return _create_failure_result( + IKStatus.NO_SOLUTION, + "JacobianIK supports exactly one pose target per request", + ) + + target_group = next(group for group in resolved_groups if group.id == pose_group_ids[0]) + if not target_group.has_pose_target: + return _create_failure_result( + IKStatus.NO_SOLUTION, + f"Planning group '{target_group.id}' has no pose target frame", + ) + + robot_ids = {group.robot_id for group in resolved_groups} + if len(robot_ids) != 1: + return _create_failure_result( + IKStatus.NO_SOLUTION, + "JacobianIK does not support cross-robot pose IK", + ) + + full_seed = seed + if full_seed is None: + with world.scratch_context() as ctx: + full_seed = world.get_joint_state(ctx, target_group.robot_id) + + target_pose = pose_targets[next(iter(pose_targets.keys()))] + result = self.solve( + world=world, + robot_id=target_group.robot_id, + target_pose=target_pose, + seed=full_seed, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + check_collision=check_collision, + max_attempts=max_attempts, + ) + if not result.is_success() or result.joint_state is None: + return result + + selected_joint_names: list[str] = [] + for group in resolved_groups: + selected_joint_names.extend(group.joint_names) + try: + result.joint_state = _filter_joint_state(result.joint_state, selected_joint_names) + except ValueError as exc: + return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) + return result + def solve_iterative( self, world: WorldSpec, @@ -421,6 +506,23 @@ def _create_success_result( ) +def _selector_id(selector: PlanningGroupID | PlanningGroupDescriptor) -> PlanningGroupID: + if isinstance(selector, PlanningGroupDescriptor): + return selector.id + return selector + + +def _filter_joint_state(joint_state: JointState, joint_names: list[str]) -> JointState: + positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=False)) + missing = [joint_name for joint_name in joint_names if joint_name not in positions_by_name] + if missing: + raise ValueError(f"IK result is missing selected joints: {missing}") + return JointState( + name=joint_names, + position=[positions_by_name[joint_name] for joint_name in joint_names], + ) + + def _create_failure_result( status: IKStatus, message: str, diff --git a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py new file mode 100644 index 0000000000..a013af34ac --- /dev/null +++ b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py @@ -0,0 +1,114 @@ +# 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 Jacobian IK selected planning group result contracts.""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import cast + +from dimos.manipulation.planning.kinematics.jacobian_ik import JacobianIK +from dimos.manipulation.planning.spec.enums import IKStatus +from dimos.manipulation.planning.spec.models import IKResult, ResolvedPlanningGroup +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _pose() -> PoseStamped: + return PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + + +def _joint_state(names: list[str], positions: list[float]) -> JointState: + return JointState({"name": names, "position": positions}) + + +def _group( + group_id: str, joint_names: tuple[str, ...], tip_link: str | None = "tool0" +) -> ResolvedPlanningGroup: + return ResolvedPlanningGroup( + id=group_id, + robot_id="robot_1", + robot_name="arm", + group_name=group_id.split("/", maxsplit=1)[1], + joint_names=joint_names, + local_joint_names=tuple(name.split("/", maxsplit=1)[1] for name in joint_names), + base_link="base_link", + tip_link=tip_link, + ) + + +class _IKWorld: + def __init__(self, groups: Mapping[str, ResolvedPlanningGroup]) -> None: + self._groups = groups + + def resolve_planning_groups( + self, group_ids: tuple[str, ...] + ) -> tuple[ResolvedPlanningGroup, ...]: + return tuple(self._groups[group_id] for group_id in group_ids) + + +class _SuccessfulIK(JacobianIK): + def solve( + self, + world: WorldSpec, + robot_id: str, + target_pose: PoseStamped, + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + return IKResult( + status=IKStatus.SUCCESS, + joint_state=_joint_state( + ["arm/joint1", "arm/joint2", "arm/gripper", "arm/unrelated"], + [0.1, 0.2, 0.3, 0.4], + ), + ) + + +def test_solve_pose_targets_filters_result_to_target_and_auxiliary_joints() -> None: + world = _IKWorld( + { + "arm/arm": _group("arm/arm", ("arm/joint1", "arm/joint2")), + "arm/gripper": _group("arm/gripper", ("arm/gripper",), tip_link=None), + } + ) + + result = _SuccessfulIK().solve_pose_targets( + world=cast("WorldSpec", world), + pose_targets={"arm/arm": _pose()}, + auxiliary_groups=["arm/gripper"], + seed=_joint_state(["arm/joint1", "arm/joint2", "arm/gripper"], [0.0, 0.0, 0.0]), + ) + + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["arm/joint1", "arm/joint2", "arm/gripper"] + assert result.joint_state.position == [0.1, 0.2, 0.3] + + +def test_solve_pose_targets_rejects_group_without_pose_target_frame() -> None: + world = _IKWorld({"arm/gripper": _group("arm/gripper", ("arm/gripper",), tip_link=None)}) + + result = JacobianIK().solve_pose_targets( + world=cast("WorldSpec", world), + pose_targets={"arm/gripper": _pose()}, + ) + + assert result.status == IKStatus.NO_SOLUTION + assert "no pose target frame" in result.message diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 0bcb032b3a..92e067b4b6 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -40,6 +40,7 @@ CollisionObjectMessage, JointPath, Obstacle, + PlanningGroupID, WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import WorldSpec @@ -357,7 +358,10 @@ def get_min_distance(self, robot_id: WorldRobotID) -> float: def get_ee_pose( self, robot_id: WorldRobotID, joint_state: JointState | None = None ) -> PoseStamped: - """Get end-effector pose. Uses current state if joint_state is None.""" + """Get end-effector pose. Uses current state if joint_state is None. + + Deprecated: use get_group_pose() with an explicit planning group ID. + """ with self._world.scratch_context() as ctx: # If no state provided, fetch current from state monitor if joint_state is None: @@ -368,6 +372,19 @@ def get_ee_pose( return self._world.get_ee_pose(ctx, robot_id) + def get_group_pose( + self, group_id: PlanningGroupID, joint_state: JointState | None = None + ) -> PoseStamped: + """Get planning group target-frame pose using current state by default.""" + group = self._world.resolve_planning_groups((group_id,))[0] + with self._world.scratch_context() as ctx: + if joint_state is None: + joint_state = self.get_current_joint_state(group.robot_id) + if joint_state is not None: + self._world.set_joint_state(ctx, group.robot_id, joint_state) + + return self._world.get_group_pose(ctx, group_id) + def get_link_pose( self, robot_id: WorldRobotID, link_name: str, joint_state: JointState | None = None ) -> PoseStamped | None: @@ -401,11 +418,23 @@ def get_link_pose( ) def get_jacobian(self, robot_id: WorldRobotID, joint_state: JointState) -> NDArray[np.float64]: - """Get 6xN Jacobian matrix.""" + """Get robot-scoped 6xN Jacobian matrix. + + Deprecated: use get_group_jacobian() with an explicit planning group ID. + """ with self._world.scratch_context() as ctx: self._world.set_joint_state(ctx, robot_id, joint_state) return self._world.get_jacobian(ctx, robot_id) + def get_group_jacobian( + self, group_id: PlanningGroupID, joint_state: JointState + ) -> NDArray[np.float64]: + """Get planning group target-frame 6xN Jacobian matrix.""" + group = self._world.resolve_planning_groups((group_id,))[0] + with self._world.scratch_context() as ctx: + self._world.set_joint_state(ctx, group.robot_id, joint_state) + return self._world.get_group_jacobian(ctx, group_id) + # Lifecycle def finalize(self) -> None: diff --git a/dimos/manipulation/planning/names.py b/dimos/manipulation/planning/names.py new file mode 100644 index 0000000000..1b105eec56 --- /dev/null +++ b/dimos/manipulation/planning/names.py @@ -0,0 +1,88 @@ +# 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. + +"""Name helpers for planning/model/coordinator boundary layers.""" + +from __future__ import annotations + +from dimos.manipulation.planning.spec.models import ( + LocalModelJointName, + PlanningGroupID, + ResolvedJointName, + RobotName, +) + + +def to_planning_group_id(robot_name: RobotName, group_name: str) -> PlanningGroupID: + """Build a public planning group ID.""" + if not robot_name or "/" in robot_name: + raise ValueError(f"Invalid robot name for planning group ID: {robot_name!r}") + if not group_name or "/" in group_name: + raise ValueError(f"Invalid planning group name: {group_name!r}") + return f"{robot_name}/{group_name}" + + +def split_planning_group_id(group_id: PlanningGroupID) -> tuple[RobotName, str]: + """Split and validate a planning group ID.""" + parts = group_id.split("/", maxsplit=1) + if len(parts) != 2 or not parts[0] or not parts[1] or "/" in parts[1]: + raise ValueError( + f"Invalid planning group ID {group_id!r}; expected '{{robot_name}}/{{group_name}}'" + ) + return parts[0], parts[1] + + +def to_resolved_joint_name( + robot_name: RobotName, + local_joint_name: LocalModelJointName, +) -> ResolvedJointName: + """Convert a local model joint name to a public resolved joint name.""" + if not robot_name or "/" in robot_name: + raise ValueError(f"Invalid robot name for resolved joint name: {robot_name!r}") + if not local_joint_name or "/" in local_joint_name: + raise ValueError(f"Invalid local joint name: {local_joint_name!r}") + return f"{robot_name}/{local_joint_name}" + + +def to_resolved_joint_names( + robot_name: RobotName, + local_joint_names: list[LocalModelJointName] | tuple[LocalModelJointName, ...], +) -> list[ResolvedJointName]: + """Convert local model joint names to public resolved joint names.""" + return [to_resolved_joint_name(robot_name, name) for name in local_joint_names] + + +def strip_resolved_joint_name( + robot_name: RobotName, + resolved_joint_name: ResolvedJointName, +) -> LocalModelJointName: + """Validate and strip a resolved joint name for backend internals.""" + prefix = f"{robot_name}/" + if not resolved_joint_name.startswith(prefix): + raise ValueError( + f"Resolved joint name {resolved_joint_name!r} does not belong to robot {robot_name!r}" + ) + local_name = resolved_joint_name[len(prefix) :] + if not local_name or "/" in local_name: + raise ValueError(f"Invalid resolved joint name: {resolved_joint_name!r}") + return local_name + + +__all__ = [ + "split_planning_group_id", + "strip_resolved_joint_name", + "to_planning_group_id", + "to_resolved_joint_name", + "to_resolved_joint_names", +] diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 3ca19eb099..9e41a2e9ff 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -27,7 +27,13 @@ import numpy as np from dimos.manipulation.planning.spec.enums import PlanningStatus -from dimos.manipulation.planning.spec.models import JointPath, PlanningResult, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + JointPath, + PlanningGroupID, + PlanningResult, + ResolvedPlanningGroup, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.path_utils import compute_path_length from dimos.msgs.sensor_msgs.JointState import JointState @@ -98,6 +104,13 @@ def plan_joint_path( if error is not None: return error + if world.check_edge_collision_free(robot_id, start, goal, self._collision_step_size): + return _create_success_result( + [start, goal], + time.time() - start_time, + 0, + ) + lower, upper = world.get_joint_limits(robot_id) start_tree = [TreeNode(config=q_start.copy())] goal_tree = [TreeNode(config=q_goal.copy())] @@ -147,6 +160,126 @@ def get_name(self) -> str: """Get planner name.""" return "RRTConnect" + def plan_selected_joint_path( + self, + world: WorldSpec, + group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...], + start: JointState, + goal: JointState, + timeout: float = 10.0, + ) -> PlanningResult: + """Plan a collision-free path for an explicit planning-group selection.""" + try: + resolved_groups = world.resolve_planning_groups(tuple(group_ids)) + except (KeyError, ValueError) as exc: + return _create_failure_result(PlanningStatus.INVALID_GOAL, str(exc)) + + selected_joint_names = [ + joint_name for group in resolved_groups for joint_name in group.joint_names + ] + exact_error = _validate_exact_joint_keys(start, selected_joint_names, "start") + if exact_error is not None: + return exact_error + exact_error = _validate_exact_joint_keys(goal, selected_joint_names, "goal") + if exact_error is not None: + return exact_error + + robot_ids = {group.robot_id for group in resolved_groups} + if len(robot_ids) != 1: + return self._plan_multi_robot_selected_joint_path( + world=world, + resolved_groups=resolved_groups, + start=start, + goal=goal, + timeout=timeout, + ) + + robot_id = next(iter(robot_ids)) + robot_config = world.get_robot_config(robot_id) + full_resolved_joint_names = [ + f"{robot_config.name}/{joint_name}" for joint_name in robot_config.joint_names + ] + if selected_joint_names != full_resolved_joint_names: + return _create_failure_result( + PlanningStatus.UNSUPPORTED, + "RRTConnectPlanner currently requires the selected groups to cover " + "the robot controllable joint set exactly", + ) + + return self.plan_joint_path( + world=world, + robot_id=robot_id, + start=_order_joint_state(start, selected_joint_names), + goal=_order_joint_state(goal, selected_joint_names), + timeout=timeout, + ) + + def _plan_multi_robot_selected_joint_path( + self, + world: WorldSpec, + resolved_groups: tuple[ResolvedPlanningGroup, ...], + start: JointState, + goal: JointState, + timeout: float, + ) -> PlanningResult: + """Plan each selected robot independently and synchronize waypoints. + + This supports coordinated joint targets across multiple robots when the + selected groups cover each affected robot's full controllable joint set. + Cross-robot collision coupling is not optimized by this backend; each + per-robot plan is still checked by the world backend for that robot. + """ + start_time = time.time() + groups_by_robot: dict[WorldRobotID, list[ResolvedPlanningGroup]] = {} + robot_order: list[WorldRobotID] = [] + for group in resolved_groups: + if group.robot_id not in groups_by_robot: + groups_by_robot[group.robot_id] = [] + robot_order.append(group.robot_id) + groups_by_robot[group.robot_id].append(group) + + paths_by_robot: dict[WorldRobotID, JointPath] = {} + total_iterations = 0 + for robot_id in robot_order: + robot_config = world.get_robot_config(robot_id) + robot_joint_names = [ + joint_name + for group in groups_by_robot[robot_id] + for joint_name in group.joint_names + ] + full_resolved_joint_names = [ + f"{robot_config.name}/{joint_name}" for joint_name in robot_config.joint_names + ] + if robot_joint_names != full_resolved_joint_names: + return _create_failure_result( + PlanningStatus.UNSUPPORTED, + "RRTConnectPlanner currently requires selected groups to cover " + "each affected robot's controllable joint set exactly", + ) + + remaining_timeout = max(timeout - (time.time() - start_time), 0.001) + result = self.plan_joint_path( + world=world, + robot_id=robot_id, + start=_order_joint_state(start, robot_joint_names), + goal=_order_joint_state(goal, robot_joint_names), + timeout=remaining_timeout, + ) + total_iterations += result.iterations + if not result.is_success(): + return result + paths_by_robot[robot_id] = result.path + + combined_path = _synchronize_robot_paths(robot_order, paths_by_robot) + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=combined_path, + planning_time=time.time() - start_time, + path_length=compute_path_length(combined_path), + iterations=total_iterations, + message="Multi-robot plan composed from independently planned robot paths", + ) + def _validate_inputs( self, world: WorldSpec, @@ -344,3 +477,60 @@ def _create_failure_result( iterations=iterations, message=message, ) + + +def _synchronize_robot_paths( + robot_order: list[WorldRobotID], paths_by_robot: dict[WorldRobotID, JointPath] +) -> JointPath: + max_waypoints = max(len(path) for path in paths_by_robot.values()) + if max_waypoints == 0: + return [] + + combined: JointPath = [] + for waypoint_index in range(max_waypoints): + names: list[str] = [] + positions: list[float] = [] + for robot_id in robot_order: + path = paths_by_robot[robot_id] + if max_waypoints == 1 or len(path) == 1: + source_index = 0 + else: + source_index = round(waypoint_index * (len(path) - 1) / (max_waypoints - 1)) + waypoint = path[source_index] + names.extend(waypoint.name) + positions.extend(waypoint.position) + combined.append(JointState(name=names, position=positions)) + return combined + + +def _validate_exact_joint_keys( + joint_state: JointState, selected_joint_names: list[str], state_name: str +) -> PlanningResult | None: + actual_names = list(joint_state.name) + expected_names = selected_joint_names + if set(actual_names) != set(expected_names): + missing = [name for name in expected_names if name not in actual_names] + extra = [name for name in actual_names if name not in expected_names] + details: list[str] = [] + if missing: + details.append(f"missing={missing}") + if extra: + details.append(f"extra={extra}") + return _create_failure_result( + PlanningStatus.INVALID_START if state_name == "start" else PlanningStatus.INVALID_GOAL, + f"{state_name} joint names must exactly match selected joints ({', '.join(details)})", + ) + if len(joint_state.position) != len(joint_state.name): + return _create_failure_result( + PlanningStatus.INVALID_START if state_name == "start" else PlanningStatus.INVALID_GOAL, + f"{state_name} joint name and position lengths must match", + ) + return None + + +def _order_joint_state(joint_state: JointState, joint_names: list[str]) -> JointState: + position_by_name = dict(zip(joint_state.name, joint_state.position, strict=False)) + return JointState( + name=joint_names, + position=[position_by_name[name] for name in joint_names], + ) diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py new file mode 100644 index 0000000000..5d11e047d6 --- /dev/null +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -0,0 +1,184 @@ +# 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 selected-joint RRT planning group contracts.""" + +from __future__ import annotations + +from pathlib import Path +from typing import cast + +import numpy as np + +from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import ResolvedPlanningGroup +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _pose() -> PoseStamped: + return PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + + +def _robot_config(name: str, joint_names: list[str]) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("robot.urdf"), + base_pose=_pose(), + joint_names=joint_names, + end_effector_link="tool0", + ) + + +def _joint_state(names: list[str], positions: list[float]) -> JointState: + return JointState({"name": names, "position": positions}) + + +def _group( + group_id: str, + robot_id: str, + robot_name: str, + joint_names: tuple[str, ...], +) -> ResolvedPlanningGroup: + return ResolvedPlanningGroup( + id=group_id, + robot_id=robot_id, + robot_name=robot_name, + group_name=group_id.split("/", maxsplit=1)[1], + joint_names=joint_names, + local_joint_names=tuple(name.split("/", maxsplit=1)[1] for name in joint_names), + base_link="base_link", + tip_link="tool0", + ) + + +class _SelectionWorld: + is_finalized = True + + def __init__( + self, + groups: dict[str, ResolvedPlanningGroup], + robot_configs: dict[str, RobotModelConfig], + ) -> None: + self._groups = groups + self._robot_configs = robot_configs + + def resolve_planning_groups( + self, group_ids: tuple[str, ...] + ) -> tuple[ResolvedPlanningGroup, ...]: + return tuple(self._groups[group_id] for group_id in group_ids) + + def get_robot_config(self, robot_id: str) -> RobotModelConfig: + return self._robot_configs[robot_id] + + def get_robot_ids(self) -> list[str]: + return list(self._robot_configs) + + def check_config_collision_free(self, robot_id: str, joint_state: JointState) -> bool: + return True + + def get_joint_limits(self, robot_id: str) -> tuple[np.ndarray, np.ndarray]: + joint_count = len(self._robot_configs[robot_id].joint_names) + return -np.ones(joint_count), np.ones(joint_count) + + def check_edge_collision_free( + self, + robot_id: str, + start: JointState, + goal: JointState, + step_size: float, + ) -> bool: + return True + + +def test_plan_selected_joint_path_rejects_missing_and_extra_start_names() -> None: + world = _SelectionWorld( + groups={"arm/arm": _group("arm/arm", "robot_1", "arm", ("arm/joint1", "arm/joint2"))}, + robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, + ) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + ["arm/arm"], + start=_joint_state(["arm/joint1", "arm/extra"], [0.0, 0.0]), + goal=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), + ) + + assert result.status == PlanningStatus.INVALID_START + assert "missing" in result.message + assert "extra" in result.message + + +def test_plan_selected_joint_path_rejects_missing_and_extra_goal_names() -> None: + world = _SelectionWorld( + groups={"arm/arm": _group("arm/arm", "robot_1", "arm", ("arm/joint1", "arm/joint2"))}, + robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, + ) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + ["arm/arm"], + start=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), + goal=_joint_state(["arm/joint1", "arm/extra"], [0.0, 0.0]), + ) + + assert result.status == PlanningStatus.INVALID_GOAL + assert "missing" in result.message + assert "extra" in result.message + + +def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> None: + world = _SelectionWorld( + groups={ + "left/arm": _group("left/arm", "left_robot", "left", ("left/joint1",)), + "right/arm": _group("right/arm", "right_robot", "right", ("right/joint1",)), + }, + robot_configs={ + "left_robot": _robot_config("left", ["joint1"]), + "right_robot": _robot_config("right", ["joint1"]), + }, + ) + joint_state = _joint_state(["left/joint1", "right/joint1"], [0.0, 0.0]) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + ["left/arm", "right/arm"], + start=joint_state, + goal=_joint_state(["left/joint1", "right/joint1"], [0.1, -0.1]), + ) + + assert result.status == PlanningStatus.SUCCESS + assert len(result.path) == 2 + assert result.path[0].name == ["left/joint1", "right/joint1"] + assert result.path[-1].position == [0.1, -0.1] + + +def test_plan_selected_joint_path_rejects_single_robot_subset_selection() -> None: + world = _SelectionWorld( + groups={"arm/wrist": _group("arm/wrist", "robot_1", "arm", ("arm/joint2",))}, + robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, + ) + joint_state = _joint_state(["arm/joint2"], [0.0]) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + ["arm/wrist"], + start=joint_state, + goal=joint_state, + ) + + assert result.status == PlanningStatus.UNSUPPORTED diff --git a/dimos/manipulation/planning/planning_groups.py b/dimos/manipulation/planning/planning_groups.py new file mode 100644 index 0000000000..ed082f42f0 --- /dev/null +++ b/dimos/manipulation/planning/planning_groups.py @@ -0,0 +1,365 @@ +# 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. + +"""Planning group discovery from SRDF or conservative model fallback.""" + +from __future__ import annotations + +import itertools +from pathlib import Path +import warnings +import xml.etree.ElementTree as ET + +from dimos.manipulation.planning.spec.models import PlanningGroupDefinition +from dimos.robot.model_parser import JointDescription, ModelDescription +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +FALLBACK_PLANNING_GROUP_NAME = "manipulator" + + +class PlanningGroupDiscoveryError(ValueError): + """Raised when planning groups cannot be discovered for a model.""" + + +def _warn(message: str) -> None: + logger.warning(message) + warnings.warn(message, UserWarning, stacklevel=2) + + +def discover_planning_group_definitions( + *, + robot_name: str, + model_path: Path, + model: ModelDescription, + controllable_joint_names: list[str], + srdf_path: Path | None = None, +) -> list[PlanningGroupDefinition]: + """Discover planning groups from SRDF or fallback generation. + + Precedence is explicit SRDF path, conservative auto-discovery with warning, + then fallback generation from the controllable joint set. + """ + resolved_srdf_path = _resolve_srdf_path(model_path, srdf_path) + if resolved_srdf_path is not None: + groups = parse_srdf_planning_groups( + resolved_srdf_path, + model=model, + controllable_joint_names=controllable_joint_names, + ) + if groups: + return groups + _warn( + f"No supported planning groups found in SRDF {resolved_srdf_path} " + f"for robot {robot_name}; trying fallback generation" + ) + + return [ + generate_fallback_planning_group( + model=model, + controllable_joint_names=controllable_joint_names, + ) + ] + + +def parse_srdf_planning_groups( + srdf_path: Path, + *, + model: ModelDescription, + controllable_joint_names: list[str], +) -> list[PlanningGroupDefinition]: + """Parse supported SRDF planning group definitions. + + Supported forms are a single ```` + child or an ordered list of ```` children. Other forms, + including SRDF ```` metadata, are ignored for planning group + extraction. + """ + root = ET.parse(srdf_path).getroot() + groups: list[PlanningGroupDefinition] = [] + for group_elem in root.findall("group"): + group_name = group_elem.get("name") + if not group_name: + _warn(f"Skipping SRDF group without a name in {srdf_path}") + continue + + children = [child for child in list(group_elem) if isinstance(child.tag, str)] + chain_children = [child for child in children if child.tag == "chain"] + joint_children = [child for child in children if child.tag == "joint"] + unsupported_children = [child for child in children if child.tag not in {"chain", "joint"}] + + if len(chain_children) == 1 and not joint_children and not unsupported_children: + definition = _parse_chain_group( + group_name, + chain_children[0], + model=model, + controllable_joint_names=controllable_joint_names, + srdf_path=srdf_path, + ) + elif joint_children and len(joint_children) == len(children): + definition = _parse_joint_list_group( + group_name, + joint_children, + model=model, + controllable_joint_names=controllable_joint_names, + srdf_path=srdf_path, + ) + else: + child_tags = [child.tag for child in children] + _warn( + f"Skipping unsupported SRDF planning group {group_name} in " + f"{srdf_path} with child tags {child_tags}" + ) + definition = None + + if definition is not None: + groups.append(definition) + + return groups + + +def generate_fallback_planning_group( + *, + model: ModelDescription, + controllable_joint_names: list[str], +) -> PlanningGroupDefinition: + """Generate one conservative fallback planning group named ``manipulator``.""" + ordered_joints = _validate_and_order_serial_joints(model, controllable_joint_names) + while ordered_joints and ordered_joints[-1].type == "prismatic": + removed = ordered_joints.pop() + _warn( + f"Excluding terminal prismatic joint {removed.name} from " + f"fallback planning group {FALLBACK_PLANNING_GROUP_NAME}" + ) + + if not ordered_joints: + raise PlanningGroupDiscoveryError( + "Fallback planning group generation removed all candidate joints; provide SRDF" + ) + + return PlanningGroupDefinition( + name=FALLBACK_PLANNING_GROUP_NAME, + joint_names=tuple(joint.name for joint in ordered_joints), + base_link=ordered_joints[0].parent_link, + tip_link=ordered_joints[-1].child_link, + source="fallback", + ) + + +def _resolve_srdf_path(model_path: Path, srdf_path: Path | None) -> Path | None: + if srdf_path is not None: + if srdf_path.exists(): + return srdf_path + raise FileNotFoundError(f"SRDF file not found: {srdf_path}") + + for candidate in _srdf_auto_discovery_candidates(model_path): + if candidate.exists(): + _warn(f"Auto-discovered SRDF at {candidate}") + return candidate + return None + + +def _srdf_auto_discovery_candidates(model_path: Path) -> list[Path]: + candidates: list[Path] = [] + name = model_path.name + if name.endswith(".urdf.xacro"): + candidates.append(model_path.with_name(name.removesuffix(".urdf.xacro") + ".srdf")) + elif model_path.suffix: + candidates.append(model_path.with_suffix(".srdf")) + candidates.append(model_path.parent / "config" / "robot.srdf") + candidates.append(model_path.parent.parent / "config" / "robot.srdf") + return list(dict.fromkeys(candidates)) + + +def _parse_chain_group( + group_name: str, + chain_elem: ET.Element, + *, + model: ModelDescription, + controllable_joint_names: list[str], + srdf_path: Path, +) -> PlanningGroupDefinition | None: + base_link = chain_elem.get("base_link") + tip_link = chain_elem.get("tip_link") + if not base_link or not tip_link: + _warn( + f"Skipping SRDF chain group {group_name} in {srdf_path} because " + "base_link or tip_link is missing" + ) + return None + + try: + ordered_joints = _ordered_joints_between_links(model, base_link, tip_link) + controlled_joints = [j for j in ordered_joints if j.type != "fixed"] + _validate_controllable(group_name, controlled_joints, controllable_joint_names) + except PlanningGroupDiscoveryError as exc: + _warn(f"Skipping SRDF chain group {group_name} in {srdf_path}: {exc}") + return None + + return PlanningGroupDefinition( + name=group_name, + joint_names=tuple(joint.name for joint in controlled_joints), + base_link=base_link, + tip_link=tip_link, + source="srdf", + ) + + +def _parse_joint_list_group( + group_name: str, + joint_children: list[ET.Element], + *, + model: ModelDescription, + controllable_joint_names: list[str], + srdf_path: Path, +) -> PlanningGroupDefinition | None: + joint_names = [child.get("name", "") for child in joint_children] + if any(not name for name in joint_names): + _warn(f"Skipping SRDF joint-list group {group_name} in {srdf_path} with empty joint name") + return None + try: + ordered_joints = _validate_ordered_serial_joints(model, joint_names) + _validate_controllable(group_name, ordered_joints, controllable_joint_names) + except PlanningGroupDiscoveryError as exc: + _warn(f"Skipping SRDF joint-list group {group_name} in {srdf_path}: {exc}") + return None + + return PlanningGroupDefinition( + name=group_name, + joint_names=tuple(joint.name for joint in ordered_joints), + base_link=ordered_joints[0].parent_link, + tip_link=ordered_joints[-1].child_link, + source="srdf", + ) + + +def _ordered_joints_between_links( + model: ModelDescription, + base_link: str, + tip_link: str, +) -> list[JointDescription]: + joints_by_parent: dict[str, list[JointDescription]] = {} + for joint in model.joints: + joints_by_parent.setdefault(joint.parent_link, []).append(joint) + + ordered_joints: list[JointDescription] = [] + current_link = base_link + visited_links = {base_link} + while current_link != tip_link: + children = joints_by_parent.get(current_link, []) + if len(children) != 1: + raise PlanningGroupDiscoveryError( + f"chain from {base_link} to {tip_link} is branching or disconnected at {current_link}" + ) + joint = children[0] + ordered_joints.append(joint) + current_link = joint.child_link + if current_link in visited_links: + raise PlanningGroupDiscoveryError("chain contains a cycle") + visited_links.add(current_link) + + return ordered_joints + + +def _validate_ordered_serial_joints( + model: ModelDescription, + joint_names: list[str], +) -> list[JointDescription]: + ordered_joints: list[JointDescription] = [] + for joint_name in joint_names: + joint = model.get_joint(joint_name) + if joint is None: + raise PlanningGroupDiscoveryError(f"joint {joint_name} does not exist in model") + if joint.type == "fixed": + raise PlanningGroupDiscoveryError(f"joint {joint_name} is fixed") + ordered_joints.append(joint) + + if not ordered_joints: + raise PlanningGroupDiscoveryError("planning group contains no joints") + + for previous, current in itertools.pairwise(ordered_joints): + if previous.child_link != current.parent_link: + raise PlanningGroupDiscoveryError( + f"joints {previous.name} and {current.name} are not adjacent in a serial chain" + ) + return ordered_joints + + +def _validate_and_order_serial_joints( + model: ModelDescription, + joint_names: list[str], +) -> list[JointDescription]: + if not joint_names: + raise PlanningGroupDiscoveryError("fallback requires at least one controllable joint") + + joints: list[JointDescription] = [] + for joint_name in joint_names: + joint = model.get_joint(joint_name) + if joint is None: + raise PlanningGroupDiscoveryError(f"joint {joint_name} does not exist in model") + if joint.type == "fixed": + raise PlanningGroupDiscoveryError(f"joint {joint_name} is fixed") + joints.append(joint) + + by_parent = {joint.parent_link: joint for joint in joints} + by_child = {joint.child_link: joint for joint in joints} + if len(by_parent) != len(joints) or len(by_child) != len(joints): + raise PlanningGroupDiscoveryError("controllable joints branch or merge; provide SRDF") + + starts = [joint for joint in joints if joint.parent_link not in by_child] + ends = [joint for joint in joints if joint.child_link not in by_parent] + if len(starts) != 1 or len(ends) != 1: + raise PlanningGroupDiscoveryError( + "controllable joints are disconnected or cyclic; provide SRDF" + ) + + ordered_joints: list[JointDescription] = [] + current = starts[0] + while True: + ordered_joints.append(current) + next_joint = by_parent.get(current.child_link) + if next_joint is None: + break + current = next_joint + + if len(ordered_joints) != len(joints): + raise PlanningGroupDiscoveryError("controllable joints are disconnected; provide SRDF") + return ordered_joints + + +def _validate_controllable( + group_name: str, + joints: list[JointDescription], + controllable_joint_names: list[str], +) -> None: + if not joints: + raise PlanningGroupDiscoveryError( + f"planning group {group_name} contains no controllable joints" + ) + controllable = set(controllable_joint_names) + missing = [joint.name for joint in joints if joint.name not in controllable] + if missing: + raise PlanningGroupDiscoveryError( + f"planning group {group_name} includes joints outside controllable set: {missing}" + ) + + +__all__ = [ + "FALLBACK_PLANNING_GROUP_NAME", + "PlanningGroupDiscoveryError", + "discover_planning_group_definitions", + "generate_fallback_planning_group", + "parse_srdf_planning_groups", +] diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 74dc3bd69b..b607fe6148 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,6 +21,7 @@ from pydantic import Field from dimos.core.module import ModuleConfig +from dimos.manipulation.planning.spec.models import PlanningGroupDefinition from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -30,10 +31,16 @@ class RobotModelConfig(ModuleConfig): Attributes: name: Human-readable robot name model_path: Path to robot model file (.urdf, .xacro, or .xml/MJCF) - base_pose: Pose of robot base in world frame (position + orientation) - joint_names: Ordered list of controlled joint names (in URDF namespace) - end_effector_link: Name of the end-effector link for FK/IK - base_link: Name of the base link (default: "base_link") + srdf_path: Optional path to SRDF file containing planning group definitions + base_pose: Deprecated planning placement transform retained for + compatibility. Prefer encoding placement in the robot model. + joint_names: Ordered list of controllable/coordinator joints in the + local model namespace. This is not a planning group. + end_effector_link: Deprecated robot-scoped end-effector link retained + for compatibility. Pose targets should use planning group target + frames instead. + base_link: Deprecated robot-scoped base link retained for Drake weld + compatibility. Planning groups own chain base links. package_paths: Dict mapping package names to filesystem Paths joint_limits_lower: Lower joint limits (radians) joint_limits_upper: Upper joint limits (radians) @@ -54,10 +61,12 @@ class RobotModelConfig(ModuleConfig): name: str model_path: Path - base_pose: PoseStamped + srdf_path: Path | None = None + base_pose: PoseStamped = Field(default_factory=PoseStamped) joint_names: list[str] - end_effector_link: str + end_effector_link: str | None = None base_link: str = "base_link" + planning_groups: list[PlanningGroupDefinition] = Field(default_factory=list) package_paths: dict[str, Path] = Field(default_factory=dict) joint_limits_lower: list[float] | None = None joint_limits_upper: list[float] | None = None diff --git a/dimos/manipulation/planning/spec/enums.py b/dimos/manipulation/planning/spec/enums.py index 66a17ee199..e1c7c1a735 100644 --- a/dimos/manipulation/planning/spec/enums.py +++ b/dimos/manipulation/planning/spec/enums.py @@ -47,3 +47,4 @@ class PlanningStatus(Enum): INVALID_GOAL = auto() COLLISION_AT_START = auto() COLLISION_AT_GOAL = auto() + UNSUPPORTED = auto() diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index 37daa331e4..729b5a7ac6 100644 --- a/dimos/manipulation/planning/spec/models.py +++ b/dimos/manipulation/planning/spec/models.py @@ -17,7 +17,7 @@ from __future__ import annotations from dataclasses import dataclass, field -from typing import TYPE_CHECKING, TypeAlias +from typing import TYPE_CHECKING, Literal, TypeAlias from dimos.manipulation.planning.spec.enums import ( IKStatus, @@ -39,6 +39,15 @@ WorldRobotID: TypeAlias = str """Internal Drake world robot ID""" +PlanningGroupID: TypeAlias = str +"""Public planning group ID of the form {robot_name}/{group_name}.""" + +LocalModelJointName: TypeAlias = str +"""Joint name as it appears in URDF/SRDF before world binding.""" + +ResolvedJointName: TypeAlias = str +"""Public joint name of the form {robot_name}/{local_joint_name}.""" + JointPath: TypeAlias = "list[JointState]" """List of joint states forming a path (each waypoint has names + positions)""" @@ -46,6 +55,88 @@ Jacobian: TypeAlias = "NDArray[np.float64]" """6 x n Jacobian matrix (rows: [vx, vy, vz, wx, wy, wz])""" +PlanningGroupSource: TypeAlias = Literal["srdf", "fallback"] + + +@dataclass(frozen=True) +class PlanningGroupDefinition: + """Model-level declaration of a planning group. + + Joint names are local model names. The definition is not bound to a world + robot ID and is safe to store on RobotModelConfig. + """ + + name: str + joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group has a valid pose target frame.""" + return self.tip_link is not None + + +@dataclass(frozen=True) +class PlanningGroupDescriptor: + """Read-only public snapshot for an available planning group.""" + + id: PlanningGroupID + robot_name: RobotName + group_name: str + joint_names: tuple[ResolvedJointName, ...] + local_joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group can be directly pose-targeted.""" + return self.tip_link is not None + + +@dataclass(frozen=True) +class ResolvedPlanningGroup: + """Runtime/world-bound planning group data.""" + + id: PlanningGroupID + robot_id: WorldRobotID + robot_name: RobotName + group_name: str + joint_names: tuple[ResolvedJointName, ...] + local_joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group can be directly pose-targeted.""" + return self.tip_link is not None + + +@dataclass +class GeneratedPlan: + """Canonical generated planning artifact. + + The path uses resolved joint names and contains exactly the selected joints. + Downstream preview/execution projections are computed lazily from this data. + """ + + group_ids: tuple[PlanningGroupID, ...] + path: list[JointState] = field(default_factory=list) + status: PlanningStatus = PlanningStatus.NO_SOLUTION + planning_time: float = 0.0 + path_length: float = 0.0 + iterations: int = 0 + message: str = "" + + def is_success(self) -> bool: + """Check if planning was successful.""" + return self.status == PlanningStatus.SUCCESS + @dataclass class Obstacle: diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index 1131312882..ed20ce57e1 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -33,7 +33,10 @@ IKResult, JointPath, Obstacle, + PlanningGroupDescriptor, + PlanningGroupID, PlanningResult, + ResolvedPlanningGroup, WorldRobotID, ) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -71,6 +74,16 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: """Get robot configuration.""" ... + def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: + """List available planning groups as immutable descriptor snapshots.""" + ... + + def resolve_planning_groups( + self, group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...] + ) -> tuple[ResolvedPlanningGroup, ...]: + """Resolve planning group IDs against current world robot data.""" + ... + def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: # lower limits, upper limits @@ -155,8 +168,19 @@ def check_edge_collision_free( ... # Forward Kinematics (require context) + def get_group_pose(self, ctx: Any, group_id: PlanningGroupID) -> PoseStamped: + """Get pose for a planning group's target frame.""" + ... + + def get_group_jacobian(self, ctx: Any, group_id: PlanningGroupID) -> NDArray[np.float64]: + """Get planning group target-frame Jacobian over the group's selected joints.""" + ... + def get_ee_pose(self, ctx: Any, robot_id: WorldRobotID) -> PoseStamped: - """Get end-effector pose.""" + """Get robot-scoped end-effector pose. + + Deprecated: use get_group_pose() with an explicit planning group ID. + """ ... def get_link_pose( @@ -166,7 +190,10 @@ def get_link_pose( ... def get_jacobian(self, ctx: Any, robot_id: WorldRobotID) -> NDArray[np.float64]: - """Get end-effector Jacobian (6 x n_joints).""" + """Get robot-scoped end-effector Jacobian (6 x n_joints). + + Deprecated: use get_group_jacobian() with an explicit planning group ID. + """ ... @@ -224,6 +251,21 @@ def solve( """Solve IK with optional collision checking.""" ... + def solve_pose_targets( + self, + world: WorldSpec, + pose_targets: dict[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], + auxiliary_groups: list[PlanningGroupID | PlanningGroupDescriptor] + | tuple[PlanningGroupID | PlanningGroupDescriptor, ...] = (), + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + """Solve pose targets over planning groups plus request-scoped auxiliaries.""" + ... + @runtime_checkable class PlannerSpec(Protocol): @@ -249,6 +291,17 @@ def plan_joint_path( """Plan a collision-free joint-space path.""" ... + def plan_selected_joint_path( + self, + world: WorldSpec, + group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...], + start: JointState, + goal: JointState, + timeout: float = 10.0, + ) -> PlanningResult: + """Plan over an explicit planning-group selection.""" + ... + def get_name(self) -> str: """Get planner name.""" ... diff --git a/dimos/manipulation/planning/test_planning_groups.py b/dimos/manipulation/planning/test_planning_groups.py new file mode 100644 index 0000000000..beb6984851 --- /dev/null +++ b/dimos/manipulation/planning/test_planning_groups.py @@ -0,0 +1,224 @@ +# 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 planning group discovery.""" + +from __future__ import annotations + +from pathlib import Path + +import pytest + +from dimos.manipulation.planning.planning_groups import ( + FALLBACK_PLANNING_GROUP_NAME, + PlanningGroupDiscoveryError, + discover_planning_group_definitions, + generate_fallback_planning_group, + parse_srdf_planning_groups, +) +from dimos.robot.model_parser import JointDescription, ModelDescription + + +def _serial_model(*joint_types: str) -> ModelDescription: + joints = [ + JointDescription( + name=f"joint{i + 1}", + type=joint_type, + parent_link=f"link{i}", + child_link=f"link{i + 1}", + ) + for i, joint_type in enumerate(joint_types) + ] + return ModelDescription( + joints=joints, + root_link="link0", + links=[f"link{i}" for i in range(len(joint_types) + 1)], + ) + + +def _branching_model() -> ModelDescription: + return ModelDescription( + joints=[ + JointDescription( + name="left_joint", + type="revolute", + parent_link="base", + child_link="left_link", + ), + JointDescription( + name="right_joint", + type="revolute", + parent_link="base", + child_link="right_link", + ), + ], + root_link="base", + links=["base", "left_link", "right_link"], + ) + + +def _write_srdf(tmp_path: Path, body: str) -> Path: + srdf_path = tmp_path / "robot.srdf" + srdf_path.write_text(f"{body}") + return srdf_path + + +def test_parse_srdf_chain_group(tmp_path: Path) -> None: + model = _serial_model("revolute", "revolute", "revolute") + srdf_path = _write_srdf( + tmp_path, + "", + ) + + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2", "joint3"], + ) + + assert len(groups) == 1 + assert groups[0].name == "arm" + assert groups[0].joint_names == ("joint1", "joint2", "joint3") + assert groups[0].base_link == "link0" + assert groups[0].tip_link == "link3" + assert groups[0].source == "srdf" + + +def test_parse_srdf_ordered_joint_list_group(tmp_path: Path) -> None: + model = _serial_model("revolute", "prismatic", "revolute") + srdf_path = _write_srdf( + tmp_path, + """ + + + + + + """, + ) + + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2", "joint3"], + ) + + assert len(groups) == 1 + assert groups[0].joint_names == ("joint1", "joint2", "joint3") + assert groups[0].base_link == "link0" + assert groups[0].tip_link == "link3" + + +def test_parse_srdf_skips_unsupported_groups_and_ignores_end_effector( + tmp_path: Path, +) -> None: + model = _serial_model("revolute", "revolute") + srdf_path = _write_srdf( + tmp_path, + """ + + + + + """, + ) + + with pytest.warns(UserWarning) as warnings: + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2"], + ) + + assert [group.name for group in groups] == ["arm"] + warning_text = "\n".join(str(warning.message) for warning in warnings) + assert "Skipping unsupported SRDF planning group links" in warning_text + assert "Skipping unsupported SRDF planning group nested" in warning_text + + +def test_fallback_generates_manipulator_for_unambiguous_serial_chain() -> None: + model = _serial_model("revolute", "prismatic", "revolute") + + group = generate_fallback_planning_group( + model=model, + controllable_joint_names=["joint2", "joint1", "joint3"], + ) + + assert group.name == FALLBACK_PLANNING_GROUP_NAME + assert group.joint_names == ("joint1", "joint2", "joint3") + assert group.base_link == "link0" + assert group.tip_link == "link3" + assert group.source == "fallback" + + +def test_fallback_strips_terminal_prismatic_joints() -> None: + model = _serial_model("revolute", "revolute", "prismatic") + + group = generate_fallback_planning_group( + model=model, + controllable_joint_names=["joint1", "joint2", "joint3"], + ) + + assert group.joint_names == ("joint1", "joint2") + assert group.tip_link == "link2" + + +def test_fallback_rejects_branching_model() -> None: + with pytest.raises(PlanningGroupDiscoveryError, match="branch"): + generate_fallback_planning_group( + model=_branching_model(), + controllable_joint_names=["left_joint", "right_joint"], + ) + + +def test_discovery_prefers_explicit_srdf_over_fallback(tmp_path: Path) -> None: + model = _serial_model("revolute", "revolute") + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + srdf_path = _write_srdf( + tmp_path, + "", + ) + + groups = discover_planning_group_definitions( + robot_name="robot", + model_path=model_path, + model=model, + controllable_joint_names=["joint1", "joint2"], + srdf_path=srdf_path, + ) + + assert [group.name for group in groups] == ["srdf_arm"] + + +def test_discovery_auto_discovers_srdf_with_warning( + tmp_path: Path, +) -> None: + model = _serial_model("revolute") + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + _write_srdf( + tmp_path, + "", + ) + + with pytest.warns(UserWarning, match="Auto-discovered SRDF"): + groups = discover_planning_group_definitions( + robot_name="robot", + model_path=model_path, + model=model, + controllable_joint_names=["joint1"], + ) + + assert [group.name for group in groups] == ["auto_arm"] diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 429e442434..7d6181f65e 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,9 +25,23 @@ import numpy as np +from dimos.manipulation.planning.names import ( + split_planning_group_id, + strip_resolved_joint_name, + to_planning_group_id, + to_resolved_joint_name, + to_resolved_joint_names, +) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType -from dimos.manipulation.planning.spec.models import JointPath, Obstacle, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + JointPath, + Obstacle, + PlanningGroupDescriptor, + PlanningGroupID, + ResolvedPlanningGroup, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import VisualizationSpec, WorldSpec from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.utils.logging_config import setup_logger @@ -79,6 +93,22 @@ logger = setup_logger() +def _pose_stamped_from_drake_transform(transform: Any) -> PoseStamped: + """Convert a Drake RigidTransform-like object to a world-frame pose.""" + position = transform.translation() + quaternion = transform.rotation().ToQuaternion() + return PoseStamped( + frame_id="world", + position=[float(position[0]), float(position[1]), float(position[2])], + orientation=[ + float(quaternion.x()), + float(quaternion.y()), + float(quaternion.z()), + float(quaternion.w()), + ], + ) + + @dataclass class _RobotData: """Internal data for tracking a robot in the world.""" @@ -87,8 +117,8 @@ class _RobotData: config: RobotModelConfig model_instance: Any # ModelInstanceIndex joint_indices: list[int] # Indices into plant's position vector - ee_frame: Any # BodyFrame for end-effector - base_frame: Any # BodyFrame for base + ee_frame: Any | None # Deprecated robot-scoped end-effector frame + base_frame: Any # Deprecated robot-scoped base frame preview_model_instance: Any = None # ModelInstanceIndex for preview (yellow) robot preview_joint_indices: list[int] = field(default_factory=list) @@ -197,6 +227,8 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: """Add a robot to the world. Returns robot_id. Same model_path + base_pose reuses the model instance (e.g. two arms in one URDF). + base_pose/base_link/end_effector_link are deprecated compatibility fields; + planning should use planning-group base/tip links. """ if self._finalized: raise RuntimeError("Cannot add robot after world is finalized") @@ -210,9 +242,7 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: self._validate_joints(config, model_instance) - ee_frame = self._plant.GetBodyByName( - config.end_effector_link, model_instance - ).body_frame() + ee_frame = self._legacy_ee_frame(config, model_instance) base_frame = self._plant.GetBodyByName(config.base_link, model_instance).body_frame() # Preview (yellow ghost) — always a separate instance per robot @@ -234,6 +264,12 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: logger.info(f"Added robot '{robot_id}' ({config.name})") return robot_id + def _legacy_ee_frame(self, config: RobotModelConfig, model_instance: Any) -> Any | None: + """Resolve deprecated robot-scoped EE frame, if available.""" + if config.end_effector_link is None: + return None + return self._plant.GetBodyByName(config.end_effector_link, model_instance).body_frame() + def _load_model(self, config: RobotModelConfig) -> Any: """Load robot model (URDF/xacro/MJCF) and return model instance.""" original_path = config.model_path.resolve() @@ -312,6 +348,91 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: raise KeyError(f"Robot '{robot_id}' not found") return self._robots[robot_id].config + def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: + """List available planning groups as immutable descriptor snapshots.""" + descriptors: list[PlanningGroupDescriptor] = [] + for robot_data in self._robots.values(): + config = robot_data.config + for group in config.planning_groups: + descriptors.append( + PlanningGroupDescriptor( + id=to_planning_group_id(config.name, group.name), + robot_name=config.name, + group_name=group.name, + joint_names=tuple(to_resolved_joint_names(config.name, group.joint_names)), + local_joint_names=group.joint_names, + base_link=group.base_link, + tip_link=group.tip_link, + source=group.source, + ) + ) + return tuple(descriptors) + + def resolve_planning_groups( + self, group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...] + ) -> tuple[ResolvedPlanningGroup, ...]: + """Resolve planning group IDs against current world robot data.""" + resolved_groups: list[ResolvedPlanningGroup] = [] + seen_joints: dict[str, PlanningGroupID] = {} + + for group_id in group_ids: + robot_name, group_name = split_planning_group_id(group_id) + robot_data = self._get_robot_data_by_name(robot_name) + group = next( + ( + candidate + for candidate in robot_data.config.planning_groups + if candidate.name == group_name + ), + None, + ) + if group is None: + raise KeyError(f"Unknown planning group ID: {group_id}") + + resolved_joint_names = tuple( + to_resolved_joint_name(robot_name, local_name) for local_name in group.joint_names + ) + for joint_name in resolved_joint_names: + previous_group_id = seen_joints.get(joint_name) + if previous_group_id is not None: + raise ValueError( + "Selected planning groups overlap on resolved joint " + f"{joint_name}: {previous_group_id} and {group_id}" + ) + seen_joints[joint_name] = group_id + + resolved_groups.append( + ResolvedPlanningGroup( + id=group_id, + robot_id=robot_data.robot_id, + robot_name=robot_name, + group_name=group_name, + joint_names=resolved_joint_names, + local_joint_names=group.joint_names, + base_link=group.base_link, + tip_link=group.tip_link, + source=group.source, + ) + ) + + return tuple(resolved_groups) + + def _get_robot_data_by_name(self, robot_name: str) -> _RobotData: + matches = [ + robot_data + for robot_data in self._robots.values() + if robot_data.config.name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found for planning group resolution") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + return matches[0] + + def _resolve_single_planning_group(self, group_id: PlanningGroupID) -> ResolvedPlanningGroup: + resolved_groups = self.resolve_planning_groups((group_id,)) + return resolved_groups[0] + def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: @@ -763,8 +884,7 @@ def sync_from_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) if not self._finalized or self._plant_context is None: return # Silently ignore before finalization - # Extract positions as numpy array for internal use - positions = np.array(joint_state.position, dtype=np.float64) + positions = self._positions_for_robot_state(robot_id, joint_state) with self._lock: self._set_positions_internal(self._plant_context, robot_id, positions) @@ -782,8 +902,7 @@ def set_joint_state( if not self._finalized: raise RuntimeError("World must be finalized first") - # Extract positions as numpy array for internal use - positions = np.array(joint_state.position, dtype=np.float64) + positions = self._positions_for_robot_state(robot_id, joint_state) # Get plant context from diagram context plant_ctx = self._diagram.GetMutableSubsystemContext(self._plant, ctx) @@ -804,6 +923,43 @@ def _set_positions_internal( self._plant.SetPositions(plant_ctx, full_positions) + def _positions_for_robot_state( + self, robot_id: WorldRobotID, joint_state: JointState + ) -> NDArray[np.float64]: + if robot_id not in self._robots: + raise KeyError(f"Robot '{robot_id}' not found") + robot_data = self._robots[robot_id] + local_joint_names = robot_data.config.joint_names + + if not joint_state.name: + if len(joint_state.position) != len(local_joint_names): + raise ValueError( + f"JointState position length {len(joint_state.position)} does not match " + f"robot {robot_data.config.name} joint count {len(local_joint_names)}" + ) + return np.array(joint_state.position, dtype=np.float64) + + state_by_local_name: dict[str, float] = {} + if len(joint_state.name) != len(joint_state.position): + raise ValueError("JointState name and position lengths must match") + + for name, position in zip(joint_state.name, joint_state.position, strict=False): + local_name = self._state_name_to_local(robot_data.config, name) + state_by_local_name[local_name] = float(position) + + missing = [name for name in local_joint_names if name not in state_by_local_name] + if missing: + raise ValueError( + f"JointState for robot {robot_data.config.name} is missing joints: {missing}" + ) + return np.array([state_by_local_name[name] for name in local_joint_names], dtype=np.float64) + + def _state_name_to_local(self, config: RobotModelConfig, joint_name: str) -> str: + try: + return strip_resolved_joint_name(config.name, joint_name) + except ValueError: + return config.get_urdf_joint_name(joint_name) + def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: """Get robot joint state from given context.""" if not self._finalized: @@ -813,11 +969,19 @@ def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: raise KeyError(f"Robot '{robot_id}' not found") robot_data = self._robots[robot_id] + if robot_data.ee_frame is None: + raise ValueError( + f"Robot '{robot_id}' has no robot-scoped end-effector link; " + "use get_group_pose() with an explicit planning group ID" + ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) full_positions = self._plant.GetPositions(plant_ctx) positions = [float(full_positions[idx]) for idx in robot_data.joint_indices] - return JointState(name=robot_data.config.joint_names, position=positions) + return JointState( + name=to_resolved_joint_names(robot_data.config.name, robot_data.config.joint_names), + position=positions, + ) # Collision Checking (context-based) @@ -898,8 +1062,31 @@ def check_edge_collision_free( # Forward Kinematics (context-based) + def get_group_pose(self, ctx: Context, group_id: PlanningGroupID) -> PoseStamped: + """Get pose for a planning group's target frame.""" + if not self._finalized: + raise RuntimeError("World must be finalized first") + + group = self._resolve_single_planning_group(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no pose target frame") + + robot_data = self._robots[group.robot_id] + plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) + + try: + tip_body = self._plant.GetBodyByName(group.tip_link, robot_data.model_instance) + except RuntimeError: + raise KeyError(f"Planning group '{group_id}' target link '{group.tip_link}' not found") + + tip_pose = self._plant.EvalBodyPoseInWorld(plant_ctx, tip_body) + return _pose_stamped_from_drake_transform(tip_pose) + def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: - """Get end-effector pose.""" + """Get robot-scoped end-effector pose. + + Deprecated: use get_group_pose() with an explicit planning group ID. + """ if not self._finalized: raise RuntimeError("World must be finalized first") @@ -907,20 +1094,16 @@ def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: raise KeyError(f"Robot '{robot_id}' not found") robot_data = self._robots[robot_id] + if robot_data.ee_frame is None: + raise ValueError( + f"Robot '{robot_id}' has no robot-scoped end-effector link; " + "use get_group_jacobian() with an explicit planning group ID" + ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) ee_body = robot_data.ee_frame.body() X_WE = self._plant.EvalBodyPoseInWorld(plant_ctx, ee_body) - - # Extract position and quaternion from Drake transform - pos = X_WE.translation() - quat = X_WE.rotation().ToQuaternion() # Drake returns [w, x, y, z] - - return PoseStamped( - frame_id="world", - position=[float(pos[0]), float(pos[1]), float(pos[2])], - orientation=[float(quat.x()), float(quat.y()), float(quat.z()), float(quat.w())], - ) + return _pose_stamped_from_drake_transform(X_WE) def get_link_pose( self, ctx: Context, robot_id: WorldRobotID, link_name: str @@ -946,7 +1129,9 @@ def get_link_pose( return result # type: ignore[no-any-return] def get_jacobian(self, ctx: Context, robot_id: WorldRobotID) -> NDArray[np.float64]: - """Get geometric Jacobian (6 x n_joints). + """Get robot-scoped geometric Jacobian (6 x n_joints). + + Deprecated: use get_group_jacobian() with an explicit planning group ID. Rows: [vx, vy, vz, wx, wy, wz] (linear, then angular) """ @@ -981,6 +1166,52 @@ def get_jacobian(self, ctx: Context, robot_id: WorldRobotID) -> NDArray[np.float return J_reordered + def get_group_jacobian(self, ctx: Context, group_id: PlanningGroupID) -> NDArray[np.float64]: + """Get geometric Jacobian for a planning group's target frame. + + Rows: [vx, vy, vz, wx, wy, wz] (linear, then angular). Columns follow + the resolved planning group's joint order. + """ + if not self._finalized: + raise RuntimeError("World must be finalized first") + + group = self._resolve_single_planning_group(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no pose target frame") + + robot_data = self._robots[group.robot_id] + plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) + + try: + tip_body = self._plant.GetBodyByName(group.tip_link, robot_data.model_instance) + except RuntimeError: + raise KeyError(f"Planning group '{group_id}' target link '{group.tip_link}' not found") + + jacobian_full = self._plant.CalcJacobianSpatialVelocity( + plant_ctx, + JacobianWrtVariable.kQDot, + tip_body.body_frame(), + np.array([0.0, 0.0, 0.0]), # type: ignore[arg-type] + self._plant.world_frame(), + self._plant.world_frame(), + ) + + joint_indices_by_name = dict( + zip(robot_data.config.joint_names, robot_data.joint_indices, strict=False) + ) + jacobian_group = np.zeros((6, len(group.local_joint_names))) + for index, local_joint_name in enumerate(group.local_joint_names): + try: + joint_index = joint_indices_by_name[local_joint_name] + except KeyError: + raise ValueError( + f"Planning group '{group_id}' references non-controllable joint " + f"'{local_joint_name}'" + ) + jacobian_group[:, index] = jacobian_full[:, joint_index] + + return np.vstack([jacobian_group[3:6, :], jacobian_group[0:3, :]]) + # Visualization def get_visualization_url(self) -> str | None: diff --git a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py new file mode 100644 index 0000000000..d4d4f4867e --- /dev/null +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -0,0 +1,207 @@ +# 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 DrakeWorld planning group name/world resolution.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import pytest + +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.models import PlanningGroupDefinition +from dimos.manipulation.planning.world.drake_world import DrakeWorld, _RobotData +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _pose() -> PoseStamped: + return PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + + +def _config( + name: str, + joint_names: list[str], + groups: list[PlanningGroupDefinition], + joint_name_mapping: dict[str, str] | None = None, +) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("robot.urdf"), + base_pose=_pose(), + joint_names=joint_names, + end_effector_link="tool0", + base_link="base_link", + planning_groups=groups, + joint_name_mapping=joint_name_mapping or {}, + ) + + +def _world(*configs: RobotModelConfig) -> DrakeWorld: + world = DrakeWorld.__new__(DrakeWorld) + world._robots = { + f"robot_{index}": _RobotData( + robot_id=f"robot_{index}", + config=config, + model_instance=None, + joint_indices=[], + ee_frame=None, + base_frame=None, + ) + for index, config in enumerate(configs, start=1) + } + return world + + +def _arm_group(*joint_names: str) -> PlanningGroupDefinition: + return PlanningGroupDefinition( + name="arm", + joint_names=joint_names, + base_link="base_link", + tip_link="tool0", + source="srdf", + ) + + +def test_list_planning_groups_returns_stable_ids_and_resolved_joint_names() -> None: + world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) + + groups = world.list_planning_groups() + + assert len(groups) == 1 + assert groups[0].id == "left/arm" + assert groups[0].robot_name == "left" + assert groups[0].group_name == "arm" + assert groups[0].joint_names == ("left/joint1", "left/joint2") + assert groups[0].local_joint_names == ("joint1", "joint2") + + +def test_robot_model_config_allows_planning_groups_without_robot_scoped_ee() -> None: + config = RobotModelConfig( + name="left", + model_path=Path("robot.urdf"), + joint_names=["joint1"], + planning_groups=[_arm_group("joint1")], + ) + world = _world(config) + + groups = world.list_planning_groups() + + assert config.end_effector_link is None + assert groups[0].id == "left/arm" + + +def test_duplicate_local_joint_names_across_robots_are_disambiguated() -> None: + world = _world( + _config("left", ["joint1"], [_arm_group("joint1")]), + _config("right", ["joint1"], [_arm_group("joint1")]), + ) + + groups = world.list_planning_groups() + + assert [group.id for group in groups] == ["left/arm", "right/arm"] + assert [group.joint_names for group in groups] == [("left/joint1",), ("right/joint1",)] + + +def test_resolve_planning_groups_returns_robot_ids_and_joint_names() -> None: + world = _world( + _config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")]), + _config("right", ["joint1", "joint2"], [_arm_group("joint2")]), + ) + + resolved = world.resolve_planning_groups(("left/arm", "right/arm")) + + assert [group.id for group in resolved] == ["left/arm", "right/arm"] + assert [group.robot_id for group in resolved] == ["robot_1", "robot_2"] + assert [group.joint_names for group in resolved] == [ + ("left/joint1", "left/joint2"), + ("right/joint2",), + ] + assert [group.local_joint_names for group in resolved] == [("joint1", "joint2"), ("joint2",)] + + +def test_resolve_planning_groups_unknown_group_raises_key_error() -> None: + world = _world(_config("left", ["joint1"], [_arm_group("joint1")])) + + with pytest.raises(KeyError, match="Unknown planning group ID: left/gripper"): + world.resolve_planning_groups(("left/gripper",)) + + +def test_resolve_planning_groups_overlapping_same_robot_groups_raise_value_error() -> None: + world = _world( + _config( + "left", + ["joint1", "joint2"], + [ + _arm_group("joint1", "joint2"), + PlanningGroupDefinition( + name="wrist", + joint_names=("joint2",), + base_link="link1", + tip_link="tool0", + ), + ], + ) + ) + + with pytest.raises(ValueError, match="overlap.*left/joint2"): + world.resolve_planning_groups(("left/arm", "left/wrist")) + + +def test_positions_for_robot_state_accepts_resolved_joint_names_in_config_order() -> None: + world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) + joint_state = JointState({"name": ["left/joint2", "left/joint1"], "position": [2.0, 1.0]}) + + positions = world._positions_for_robot_state("robot_1", joint_state) + + np.testing.assert_allclose(positions, np.array([1.0, 2.0])) + + +def test_positions_for_robot_state_falls_back_to_coordinator_mapping() -> None: + world = _world( + _config( + "left", + ["urdf_joint1", "urdf_joint2"], + [_arm_group("urdf_joint1", "urdf_joint2")], + joint_name_mapping={"coord_joint1": "urdf_joint1", "coord_joint2": "urdf_joint2"}, + ) + ) + joint_state = JointState({"name": ["coord_joint2", "coord_joint1"], "position": [2.0, 1.0]}) + + positions = world._positions_for_robot_state("robot_1", joint_state) + + np.testing.assert_allclose(positions, np.array([1.0, 2.0])) + + +def test_group_pose_rejects_group_without_target_frame() -> None: + world = _world( + _config( + "left", + ["joint1"], + [ + PlanningGroupDefinition( + name="waist", + joint_names=("joint1",), + base_link="base_link", + tip_link=None, + ) + ], + ) + ) + world._finalized = True + + with pytest.raises(ValueError, match="left/waist.*no pose target frame"): + world.get_group_pose(None, "left/waist") diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 85afa6642c..d8cab7820f 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -28,6 +28,8 @@ ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import GeneratedPlan, ResolvedPlanningGroup from dimos.manipulation.planning.spec.protocols import VisualizationSpec from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -102,6 +104,7 @@ def _make_module(): module._planner = None module._kinematics = None module._coordinator_client = None + module._last_plan = None return module @@ -317,6 +320,65 @@ def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: ) +def _make_robot_config( + name: str, + joints: list[str], + coordinator_joints: list[str], + task_name: str, +) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("/path/to/robot.urdf"), + base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + joint_names=joints, + end_effector_link="ee", + base_link="base", + joint_name_mapping=dict(zip(coordinator_joints, joints, strict=True)), + coordinator_task_name=task_name, + ) + + +def _make_resolved_group( + robot_name: str, group_name: str, joints: list[str] +) -> ResolvedPlanningGroup: + return ResolvedPlanningGroup( + id=f"{robot_name}/{group_name}", + robot_id=f"robot_{robot_name}", + robot_name=robot_name, + group_name=group_name, + joint_names=tuple(f"{robot_name}/{joint}" for joint in joints), + local_joint_names=tuple(joints), + base_link="base", + tip_link="ee", + ) + + +def _make_generated_plan(group_ids: tuple[str, ...], *points: list[float]) -> GeneratedPlan: + return GeneratedPlan( + group_ids=group_ids, + path=[ + JointState( + name=["left/j1", "left/j2", "right/j1"], + position=list(point), + ) + for point in points + ], + status=PlanningStatus.SUCCESS, + ) + + +def _trajectory_generator() -> MagicMock: + generator = MagicMock() + generator.generate.side_effect = lambda positions: JointTrajectory( + joint_names=[], + points=[ + TrajectoryPoint(time_from_start=float(index), positions=list(position)) + for index, position in enumerate(positions) + ], + ) + return generator + + def _make_world_monitor_with_viz(viz: object | None) -> WorldMonitor: world = viz if viz is not None else object() with patch( @@ -566,3 +628,128 @@ def test_preview_path_returns_false_for_missing_inputs(self): module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} module._planned_paths = {"arm": []} assert module.preview_path(robot_name="arm") is False + + +class TestGeneratedPlanProjection: + def test_selected_joint_state_accepts_local_current_state_names(self): + config = _make_robot_config("left", ["j1", "j2"], ["c/j1", "c/j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_resolved_group("left", "arm", ["j1", "j2"]) + ] + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2"], position=[1.0, 2.0] + ) + + selected = module._selected_joint_state(("left/arm",)) + + assert selected is not None + assert selected.name == ["left/j1", "left/j2"] + assert selected.position == [1.0, 2.0] + + def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): + left_config = _make_robot_config( + "left", + ["j1", "j2", "j3"], + ["left_task/j1", "left_task/j2", "left_task/j3"], + "left_task", + ) + right_config = _make_robot_config( + "right", ["j1", "j2"], ["right_task/j1", "right_task/j2"], "right_task" + ) + module = _make_module_with_monitor(left_config, right_config) + left_gen = _trajectory_generator() + right_gen = _trajectory_generator() + module._robots["left"] = ("robot_left", left_config, left_gen) + module._robots["right"] = ("robot_right", right_config, right_gen) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_resolved_group("left", "arm", ["j1", "j2"]), + _make_resolved_group("right", "arm", ["j1"]), + ] + module._world_monitor.get_current_joint_state.side_effect = [ + JointState(name=["left/j1", "left/j2", "left/j3"], position=[0.0, 0.0, 9.0]), + JointState(name=["right/j1", "right/j2"], position=[0.0, 8.0]), + ] + module._coordinator_client = MagicMock() + module._coordinator_client.task_invoke.return_value = True + plan = _make_generated_plan(("left/arm", "right/arm"), [1.0, 2.0, 3.0], [4.0, 5.0, 6.0]) + + assert module.execute_plan(plan) is True + + assert module._coordinator_client.task_invoke.call_count == 2 + left_call, right_call = module._coordinator_client.task_invoke.call_args_list + assert left_call.args[0:2] == ("left_task", "execute") + left_trajectory = left_call.args[2]["trajectory"] + assert left_trajectory.joint_names == ["left_task/j1", "left_task/j2", "left_task/j3"] + assert [point.positions for point in left_trajectory.points] == [ + [1.0, 2.0, 9.0], + [4.0, 5.0, 9.0], + ] + assert right_call.args[0:2] == ("right_task", "execute") + right_trajectory = right_call.args[2]["trajectory"] + assert right_trajectory.joint_names == ["right_task/j1", "right_task/j2"] + assert [point.positions for point in right_trajectory.points] == [[3.0, 8.0], [6.0, 8.0]] + + def test_project_plan_holds_non_selected_joints_from_current_state(self): + config = _make_robot_config("left", ["j1", "j2", "j3"], ["c/j1", "c/j2", "c/j3"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["left/j1", "left/j2", "left/j3"], position=[10.0, 20.0, 30.0] + ) + plan = GeneratedPlan( + group_ids=("left/arm",), + path=[ + JointState(name=["left/j2"], position=[2.0]), + JointState(name=["left/j2"], position=[3.0]), + ], + status=PlanningStatus.SUCCESS, + ) + + projected = module._project_plan_path_for_robot(plan, "left") + + assert [state.name for state in projected] == [["j1", "j2", "j3"], ["j1", "j2", "j3"]] + assert [state.position for state in projected] == [[10.0, 2.0, 30.0], [10.0, 3.0, 30.0]] + + def test_preview_path_with_last_plan_projects_lazily_to_world_monitor(self): + config = _make_robot_config("left", ["j1", "j2"], ["c/j1", "c/j2"], "task") + module = _make_module_with_monitor(config) + module._robots["left"] = ("robot_left", config, _trajectory_generator()) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_resolved_group("left", "arm", ["j1"]) + ] + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["left/j1", "left/j2"], position=[0.0, 7.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("left/arm",), + path=[ + JointState(name=["left/j1"], position=[1.0]), + JointState(name=["left/j1"], position=[2.0]), + ], + status=PlanningStatus.SUCCESS, + ) + + assert module.preview_path(robot_name="left", target_fps=0.0) is True + + module._world_monitor.animate_path.assert_called_once() + robot_id, path, duration = module._world_monitor.animate_path.call_args.args + assert robot_id == "robot_left" + assert duration == 1.0 + assert [state.position for state in path] == [[1.0, 7.0], [2.0, 7.0]] + + def test_has_and_clear_planned_path_use_last_plan(self): + module = _make_module() + module._last_plan = GeneratedPlan( + group_ids=("left/arm",), + path=[JointState(name=["left/j1"], position=[1.0])], + status=PlanningStatus.SUCCESS, + ) + module._planned_paths = {"left": _make_path([1.0])} + module._planned_trajectories = {"left": _make_trajectory((0.0, [1.0]))} + + assert module.has_planned_path() is True + assert module.clear_planned_path() is True + assert module.has_planned_path() is False + assert module._last_plan is None + assert module._planned_paths == {} + assert module._planned_trajectories == {} diff --git a/dimos/robot/config.py b/dimos/robot/config.py index d0922cb842..e701099480 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -28,6 +28,7 @@ from dimos.control.components import HardwareComponent, HardwareType from dimos.control.coordinator import TaskConfig +from dimos.manipulation.planning.planning_groups import discover_planning_group_definitions from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -54,7 +55,7 @@ class RobotConfig(BaseModel): # Required fields name: str model_path: Path | None = None - end_effector_link: str | None = None + end_effector_link: str | None = None # Deprecated for planning; use planning group tip links. # Physical dimensions (meters) height_clearance: float | None = None # max height @@ -75,11 +76,14 @@ class RobotConfig(BaseModel): # Optional overrides (derived from model if not set) joint_names: list[str] | None = None - base_link: str | None = None + base_link: str | None = ( + None # Deprecated planning override; derived from model root when absent. + ) home_joints: list[float] | None = None # Multi-robot / coordinator joint_prefix: str | None = None # defaults to "{name}_" + # Deprecated for planning placement. Prefer encoding placement in URDF/xacro/MJCF. base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # Planning @@ -94,6 +98,7 @@ class RobotConfig(BaseModel): package_paths: dict[str, Path] = Field(default_factory=dict) xacro_args: dict[str, str] = Field(default_factory=dict) auto_convert_meshes: bool = True + srdf_path: Path | None = None # TF publishing tf_extra_links: list[str] = Field(default_factory=list) @@ -193,11 +198,6 @@ def coordinator_task_name(self) -> str: def to_robot_model_config(self) -> RobotModelConfig: """Generate RobotModelConfig for ManipulationModule.""" - if self.end_effector_link is None: - raise ValueError( - f"RobotConfig '{self.name}' has no end_effector_link — " - "cannot generate RobotModelConfig for manipulation." - ) if self.model_path is None: raise ValueError( f"RobotConfig '{self.name}' has no model_path — " @@ -218,14 +218,27 @@ def to_robot_model_config(self) -> RobotModelConfig: self.joint_names if self.joint_names is not None else self.resolved_joint_names ) base_link = self.base_link if self.base_link is not None else self.resolved_base_link + planning_groups = discover_planning_group_definitions( + robot_name=self.name, + model_path=self.model_path, + model=self.model_description, + controllable_joint_names=joint_names, + srdf_path=self.srdf_path, + ) + legacy_end_effector_link = self.end_effector_link or next( + (group.tip_link for group in planning_groups if group.tip_link is not None), + None, + ) return RobotModelConfig( name=self.name, model_path=self.model_path, + srdf_path=self.srdf_path, base_pose=base_pose, joint_names=joint_names, - end_effector_link=self.end_effector_link, + end_effector_link=legacy_end_effector_link, base_link=base_link, + planning_groups=planning_groups, package_paths=self.package_paths, xacro_args=self.xacro_args, collision_exclusion_pairs=exclusions, diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 1e0a14a2a0..3b373e4daf 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -509,6 +509,7 @@ Place your URDF/xacro files under LFS data so they can be resolved via `LfsPath` from dimos.utils.data import LfsPath from dimos.manipulation.manipulation_module import manipulation_module from dimos.manipulation.planning.spec import RobotModelConfig +from dimos.manipulation.planning.spec.models import PlanningGroupDefinition from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -550,10 +551,18 @@ def _make_yourarm_config( return RobotModelConfig( name=name, model_path=_YOURARM_URDF_PATH, - base_pose=_make_base_pose(y=y_offset), joint_names=joint_names, - end_effector_link="link6", # Last link in your URDF's kinematic chain - base_link="base_link", # Root link of your URDF + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(joint_names), + base_link="base_link", + tip_link="link6", + source="fallback", + ) + ], + base_pose=_make_base_pose(y=y_offset), # Deprecated; prefer model placement + base_link="base_link", # Deprecated robot-scoped compatibility package_paths={"yourarm_description": _YOURARM_PACKAGE_PATH}, xacro_args={}, # Xacro arguments if using .xacro files collision_exclusion_pairs=[], # Pairs of links that can touch (e.g., gripper fingers) @@ -587,14 +596,18 @@ yourarm_planner = manipulation_module( | Field | Description | |-------|-------------| | `model_path` | Path to `.urdf` or `.xacro` file | -| `joint_names` | Ordered list of controlled joints (must match URDF) | -| `end_effector_link` | Link to use as the end-effector for IK | -| `base_link` | Root link of the robot model | +| `joint_names` | Ordered controllable/coordinator joint set (must match URDF); not itself a planning group | +| `planning_groups` / `srdf_path` | Explicit planning groups or SRDF source; fallback can generate `{robot_name}/manipulator` for an unambiguous single chain | | `package_paths` | Maps `package://` URIs to filesystem paths (for xacro) | | `joint_name_mapping` | Maps coordinator names (e.g., `"arm_joint1"`) to URDF names (e.g., `"joint1"`) | | `coordinator_task_name` | Must match the `TaskConfig.name` in your coordinator blueprint | | `collision_exclusion_pairs` | List of `(link_a, link_b)` tuples for links that may legitimately touch (e.g., gripper fingers) | +`base_link`, `base_pose`, and `end_effector_link` are deprecated planning-level +fields. New planning code should use SRDF/planning-group chain base/tip links +and encode robot placement in the model. See +[Planning Groups](/docs/capabilities/manipulation/planning_groups.md). + ## Step 5: Register Blueprints The blueprint registry in `dimos/robot/all_blueprints.py` is **auto-generated** by scanning the codebase for blueprint declarations. After adding your blueprints: diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md new file mode 100644 index 0000000000..f5ab9cecd3 --- /dev/null +++ b/docs/capabilities/manipulation/planning_groups.md @@ -0,0 +1,121 @@ +# Manipulation Planning Groups + +Planning groups are named, selectable kinematic chains used by manipulation +planning. They separate the hardware robot identity from the part of the robot +being planned. + +## Concepts + +| Concept | Meaning | +|---------|---------| +| Planning group | A named serial chain of controllable robot joints. | +| Planning group ID | Stable API ID in the form `{robot_name}/{group_name}`. | +| Resolved joint name | World-level joint name in the form `{robot_name}/{local_joint_name}`. | +| Generated plan | Minimal planning artifact containing selected group IDs and one synchronized resolved-joint path. | +| Auxiliary group | A group selected for a pose request without receiving its own pose target. | + +Local URDF/SRDF joint names stay inside model parsing and backend internals. +Public planning states and generated plan paths use resolved joint names so two +robots can safely have the same local joint names. + +## Planning group sources + +DimOS discovers planning groups in this order: + +1. Explicit `srdf_path` on `RobotConfig` / `RobotModelConfig`. +2. Conservative SRDF auto-discovery near the model path, with a visible warning. +3. Fallback generation of one `{robot_name}/manipulator` group if the configured + controllable joints form exactly one unambiguous serial chain. +4. Error if no SRDF exists and fallback cannot infer a single chain. + +Supported SRDF group forms: + +```xml + + + +``` + +```xml + + + + + +``` + +Unsupported SRDF forms are skipped with warnings: link groups, nested group +references, mixed group declarations, branching/non-serial groups, and SRDF +`` metadata. A chain group's `tip_link` is the pose target frame. +An ordered joint-list group may be pose-targeted only when DimOS can validate a +unique serial target frame. + +## Fallback behavior + +When no SRDF is available, fallback uses `RobotModelConfig.joint_names` as the +candidate controllable set. This field is the robot's controllable/coordinator +joint set, not an implicit planning group. + +Fallback succeeds only when those joints form one unambiguous serial chain. It +allows prismatic joints in the middle of the chain and strips only terminal/tip +prismatic joints, which usually represent gripper fingers. The generated group +name is always `manipulator`. + +## Planning APIs + +Planning APIs select groups explicitly. Descriptors returned by +`WorldSpec.list_planning_groups()` can be passed where a group ID is accepted; +the API normalizes them back to IDs and re-resolves current world state. + +```python skip +# Joint-space planning for one group. +manip.plan_to_joint_targets({ + "left_arm/manipulator": JointState( + name=["left_arm/joint1", "left_arm/joint2"], + position=[0.2, -0.1], + ) +}) + +# Pose planning for an arm while a torso/waist group participates as free DOFs. +manip.plan_to_poses( + {"robot/arm": target_pose}, + auxiliary_groups=["robot/torso"], +) + +plan = manip._last_plan +manip.preview_plan(plan) +manip.execute_plan(plan) +``` + +For joint-space planning, start and goal joint keys must exactly match the +selected resolved joints: no missing, extra, or partial joints. + +## Generated plans and execution + +A `GeneratedPlan` stores: + +- selected planning group IDs; +- a single synchronized path of `JointState` waypoints keyed by resolved joint + names; +- status, timing, path length, iteration count, and message metadata. + +Preview and execution project this path lazily. Preview sends projected joint +paths to the world monitor. Execution splits the path by affected coordinator +trajectory task, orders each trajectory by that task's configured joint order, +maps resolved/local names to coordinator names at the boundary, and invokes each +trajectory controller. Controllers remain planning-group agnostic. + +Multi-task dispatch is not atomic in this change: if one trajectory task accepts +and a later task rejects, DimOS reports the rejection but does not roll back the +accepted task. + +## Deprecated planning config fields + +`RobotConfig.base_link`, `RobotConfig.base_pose`, +`RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and +`RobotModelConfig.end_effector_link` remain as compatibility fields for the +current Drake weld and deprecated robot-scoped FK/Jacobian APIs. New planning +logic should use model/SRDF structure and planning group base/tip links instead. + +Robot placement should be encoded in URDF/xacro/MJCF. `joint_names` remains +supported and should describe the controllable/coordinator joint set. diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index c9cc01cd42..26166acbc6 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -114,6 +114,14 @@ visualization backend. [guide is here](/docs/capabilities/manipulation/adding_a_custom_arm.md) +## Planning Groups + +Manipulation planning uses explicit planning group IDs such as +`arm/manipulator` and resolved joint names such as `arm/joint1`. See +[Planning Groups](/docs/capabilities/manipulation/planning_groups.md) for SRDF +support, fallback generation, auxiliary groups, generated plans, and execution +projection. + ## Key Files | File | Description | diff --git a/openspec/changes/add-planning-groups/docs.md b/openspec/changes/add-planning-groups/docs.md new file mode 100644 index 0000000000..1a8ad2b45b --- /dev/null +++ b/openspec/changes/add-planning-groups/docs.md @@ -0,0 +1,45 @@ +## User-Facing Docs + +- Update `docs/usage/` manipulation planning documentation to introduce planning groups as the user-facing planning selection unit. +- Document Planning Group IDs as `{robot_name}/{group_name}` and resolved joint names as `{robot_name}/{local_joint_name}`. +- Document supported SRDF forms: + - `` + - `...` when the joints validate as one serial chain. +- Document unsupported SRDF forms and warning behavior for skipped groups. +- Document fallback behavior for robots without SRDF: one generated `{robot_name}/manipulator` group only when configured controllable joints form one unambiguous serial chain. +- Document public planning API usage: + - pose targets keyed by planning group + - request-scoped `auxiliary_groups` + - joint targets keyed by planning group + - generated plans returned as the canonical artifact +- Document lazy preview/execution flow: generated plan first, then `preview_plan(plan)` and `execute_plan(plan)` project as needed. + +## Contributor Docs + +- Update `docs/development/` manipulation/planning contributor documentation, if present, to explain: + - SRDF/fallback extraction responsibilities + - local versus resolved joint-name layering + - where group resolution belongs + - why controllers should remain planning-group agnostic +- If no dedicated manipulation contributor doc exists, add the contributor notes to the user-facing manipulation planning doc or create a short development note for planning backends. + +## Coding-Agent Docs + +- Update `docs/coding-agents/` or `AGENTS.md` only if implementation introduces new recurring coding-agent guidance. +- Likely guidance: + - do not add robot-scoped planning APIs for new manipulation work + - use explicit Planning Group IDs in examples/tests + - keep local URDF/SRDF names below parsing/backend internals + - use resolved joint names in public paths/states + +## Doc Validation + +- Run doc link validation if available: + - `uv run doclinks` +- For docs containing executable Python snippets, run the relevant markdown execution command if supported: + - `uv run md-babel-py run ` +- Run relevant tests that exercise documented examples or API snippets after implementation. + +## No Docs Needed + +Documentation is needed. This change alters public manipulation planning concepts, API examples, naming conventions, SRDF support expectations, and migration guidance for existing callers. diff --git a/openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md b/openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md new file mode 100644 index 0000000000..603a392b0e --- /dev/null +++ b/openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md @@ -0,0 +1,216 @@ +## ADDED Requirements + +### Requirement: Planning group discovery + +DimOS SHALL expose manipulation planning groups discovered from supported SRDF group declarations or from conservative fallback generation when no SRDF is available. + +#### Scenario: supported SRDF chain group is discovered +- **GIVEN** a robot model configuration references an SRDF containing a `` with one `` +- **WHEN** planning groups are listed for that robot +- **THEN** the chain group is available as a planning group +- **AND** its public Planning Group ID is `{robot_name}/{group_name}` + +#### Scenario: supported SRDF joint-list group is discovered +- **GIVEN** a robot model configuration references an SRDF containing a `` with an ordered list of `` entries +- **AND** those joints validate as one serial chain +- **WHEN** planning groups are listed for that robot +- **THEN** the joint-list group is available as a planning group + +#### Scenario: unsupported SRDF groups are skipped +- **GIVEN** an SRDF contains unsupported group forms such as link groups, nested group references, mixed forms, or non-serial groups +- **WHEN** planning groups are discovered +- **THEN** unsupported groups are skipped with warnings +- **AND** supported groups from the same SRDF remain available + +#### Scenario: fallback group is generated for unambiguous single-chain robot +- **GIVEN** a robot has no SRDF +- **AND** its configured controllable joints form exactly one unambiguous serial chain +- **WHEN** planning groups are listed for that robot +- **THEN** DimOS exposes one generated planning group named `manipulator` +- **AND** the group ID is `{robot_name}/manipulator` + +#### Scenario: ambiguous fallback fails +- **GIVEN** a robot has no SRDF +- **AND** its configured controllable joints are branching, disconnected, ambiguous, or otherwise not one serial chain +- **WHEN** planning groups are discovered +- **THEN** DimOS fails with an error requiring SRDF rather than silently creating an implicit group + +### Requirement: Planning group descriptors + +DimOS SHALL provide read-only planning group descriptors that identify available groups without acting as live runtime handles. + +#### Scenario: descriptor includes stable identity +- **GIVEN** a planning group exists for a robot +- **WHEN** a caller lists planning groups +- **THEN** the returned descriptor includes the Planning Group ID +- **AND** the Planning Group ID is stable and namespaced as `{robot_name}/{group_name}` + +#### Scenario: descriptor can be used as selector +- **GIVEN** a caller receives a planning group descriptor from a query API +- **WHEN** the caller passes that descriptor to a planning API +- **THEN** DimOS selects the group by descriptor ID +- **AND** DimOS re-resolves current runtime group data instead of trusting stale descriptor fields + +### Requirement: Resolved joint naming + +DimOS SHALL expose resolved joint names above the model parsing layer using `{robot_name}/{local_joint_name}`. + +#### Scenario: generated path uses resolved names +- **GIVEN** a robot named `left` has a local model joint named `joint1` +- **WHEN** DimOS returns a planning result path that includes that joint +- **THEN** the path uses `left/joint1` as the joint name +- **AND** the bare local name `joint1` is not exposed in the public path + +#### Scenario: duplicate local names remain unambiguous +- **GIVEN** two robots both contain a local model joint named `joint1` +- **WHEN** a coordinated plan includes both joints +- **THEN** the plan distinguishes them with resolved names such as `left/joint1` and `right/joint1` + +### Requirement: Planning group selection validation + +DimOS SHALL validate that selected planning groups are known and do not overlap in resolved joints. + +#### Scenario: non-overlapping groups are accepted +- **GIVEN** a planning request selects two known planning groups with disjoint resolved joints +- **WHEN** DimOS resolves the planning group selection +- **THEN** the selection is accepted +- **AND** the effective selected joint set is the union of both groups' resolved joints + +#### Scenario: overlapping groups are rejected +- **GIVEN** a planning request selects two planning groups that share at least one resolved joint +- **WHEN** DimOS resolves the planning group selection +- **THEN** the request fails before planning begins +- **AND** the error identifies overlapping selected joints or groups + +#### Scenario: unknown group is rejected +- **GIVEN** a planning request references a Planning Group ID that is not available +- **WHEN** DimOS resolves the planning group selection +- **THEN** the request fails with an unknown planning group error + +### Requirement: Pose planning with auxiliary groups + +DimOS SHALL support pose planning with pose-targeted groups and request-scoped auxiliary groups that contribute free degrees of freedom. + +#### Scenario: auxiliary torso helps arm pose planning +- **GIVEN** a pose planning request targets `robot/arm` +- **AND** the request includes `robot/torso` as an auxiliary group +- **WHEN** DimOS solves IK and planning for the request +- **THEN** the effective planning selection includes both arm and torso joints +- **AND** the arm pose target is enforced +- **AND** torso joints are free decision variables with no direct pose constraint in that request + +#### Scenario: auxiliary status is request-scoped +- **GIVEN** a planning group has a valid pose target frame +- **WHEN** that group is listed in `auxiliary_groups` for a pose planning request +- **THEN** DimOS treats it as unconstrained by pose for that request +- **AND** the same group may be directly pose-targeted in a different request + +#### Scenario: pose-targeted group without target frame is rejected +- **GIVEN** a planning group has no valid pose target frame +- **WHEN** a caller uses that group as a key in pose targets +- **THEN** DimOS rejects the request before planning begins + +### Requirement: Joint target planning exactness + +DimOS SHALL require joint target planning requests to provide exact selected resolved joint keys. + +#### Scenario: exact joint target is accepted +- **GIVEN** a joint target request selects a planning group with resolved joints `robot/joint1` and `robot/joint2` +- **WHEN** the request provides target values for exactly `robot/joint1` and `robot/joint2` +- **THEN** DimOS accepts the target for planning + +#### Scenario: missing joint target is rejected +- **GIVEN** a joint target request selects a planning group with resolved joints `robot/joint1` and `robot/joint2` +- **WHEN** the request provides a target for only `robot/joint1` +- **THEN** DimOS rejects the request as incomplete + +#### Scenario: extra joint target is rejected +- **GIVEN** a joint target request selects a planning group with resolved joints `robot/joint1` and `robot/joint2` +- **WHEN** the request also includes `robot/joint3` +- **THEN** DimOS rejects the request because the target contains joints outside the selected planning groups + +### Requirement: IK result shape + +DimOS SHALL return IK solutions containing exactly the resolved joints selected by the effective planning group selection. + +#### Scenario: IK result excludes unrelated joints +- **GIVEN** a pose request targets `robot/arm` and includes `robot/torso` as auxiliary +- **WHEN** IK succeeds +- **THEN** the IK solution contains arm and torso resolved joints +- **AND** it excludes unrelated gripper, base, or other-arm joints that were not selected + +#### Scenario: IK solves over auxiliary joints +- **GIVEN** a pose request has auxiliary groups +- **WHEN** IK attempts to satisfy pose targets +- **THEN** auxiliary group joints are available as free variables during the solve + +### Requirement: Generated plan artifact + +DimOS SHALL return a generated plan as the canonical artifact for successful planning. + +#### Scenario: generated plan contains selected groups and combined path +- **GIVEN** a planning request succeeds for one or more planning groups +- **WHEN** DimOS returns the generated plan +- **THEN** the plan includes the selected Planning Group IDs +- **AND** the plan includes one synchronized path of resolved-joint-keyed joint states + +#### Scenario: generated plan path contains exactly selected joints +- **GIVEN** a generated plan was produced for a planning group selection +- **WHEN** a caller inspects any waypoint in the path +- **THEN** that waypoint contains exactly the selected resolved joints +- **AND** unrelated joints are excluded + +#### Scenario: generated plan is reusable for downstream calls +- **GIVEN** a caller receives a generated plan +- **WHEN** the caller previews or executes the plan +- **THEN** DimOS uses the generated plan as the source artifact +- **AND** the caller does not need hidden robot-keyed planned path state + +### Requirement: Preview and execution projection + +DimOS SHALL project generated plans lazily for preview and execution without making controllers planning-group-aware. + +#### Scenario: preview projects selected path +- **GIVEN** a generated plan contains a resolved-joint path +- **WHEN** a caller previews the plan +- **THEN** DimOS projects the path into visualization using the selected joints +- **AND** preview does not require a precomputed execution trajectory stored in the plan + +#### Scenario: execution projects per trajectory task +- **GIVEN** a generated plan contains resolved joints for one or more coordinator trajectory tasks +- **WHEN** a caller executes the plan +- **THEN** DimOS projects the combined path into one trajectory per affected task +- **AND** each trajectory is ordered according to that task's configured joint order +- **AND** planning group concepts are not exposed to the trajectory controller + +#### Scenario: multi-task execution is dispatched without batch atomicity +- **GIVEN** a generated plan affects multiple trajectory tasks with disjoint joints +- **WHEN** the plan is executed +- **THEN** DimOS dispatches projected trajectories to the affected tasks +- **AND** runtime concurrency is handled by the coordinator and trajectory tasks +- **AND** DimOS does not require an atomic batch dispatch for this change + +### Requirement: Group-scoped kinematics queries + +DimOS SHALL provide group-scoped pose and Jacobian queries for planning groups with valid pose target frames. + +#### Scenario: group pose query succeeds for chain group +- **GIVEN** a planning group has a valid tip or pose target frame +- **WHEN** a caller requests the group's pose +- **THEN** DimOS returns the pose for that group target frame + +#### Scenario: group pose query fails without target frame +- **GIVEN** a planning group has no valid pose target frame +- **WHEN** a caller requests the group's pose or Jacobian +- **THEN** DimOS fails with a clear error rather than guessing a frame + +### Requirement: Backend unsupported reporting + +DimOS SHALL allow planning backends to reject coordinated planning problems they cannot support. + +#### Scenario: cross-robot planning unsupported by backend +- **GIVEN** a request selects planning groups across multiple robots +- **AND** the active backend cannot solve cross-robot coordinated planning +- **WHEN** planning is requested +- **THEN** DimOS reports the request as unsupported +- **AND** the failure occurs without sending commands to trajectory controllers diff --git a/openspec/changes/add-planning-groups/tasks.md b/openspec/changes/add-planning-groups/tasks.md new file mode 100644 index 0000000000..b706372e11 --- /dev/null +++ b/openspec/changes/add-planning-groups/tasks.md @@ -0,0 +1,92 @@ +## 1. Planning group data model and parsing + +- [x] 1.1 Add planning group model types for definitions, descriptors, resolved groups, and generated plans. +- [x] 1.2 Add `srdf_path` to robot/model configuration and carry it through model-to-planning config conversion. +- [x] 1.3 Implement SRDF parsing for supported `` declarations. +- [x] 1.4 Implement SRDF parsing for ordered `...` declarations that validate as one serial chain. +- [x] 1.5 Emit warnings and skip unsupported SRDF groups, including link groups, nested group references, mixed forms, and non-serial groups. +- [x] 1.6 Ignore SRDF `` metadata for planning group extraction. +- [x] 1.7 Implement conservative SRDF auto-discovery with visible warning after explicit `srdf_path` lookup fails or is absent. +- [x] 1.8 Implement fallback generation of `{robot_name}/manipulator` from `RobotModelConfig.joint_names` when no SRDF is available. +- [x] 1.9 Validate fallback as exactly one unambiguous serial chain, allowing middle prismatic joints and excluding only terminal/tip prismatic finger joints. +- [x] 1.10 Add unit tests for SRDF chain groups, joint-list groups, skipped unsupported groups, and fallback success/failure cases. + +## 2. Naming and group resolution + +- [x] 2.1 Add deterministic helpers for local model joint names to resolved joint names: `{robot_name}/{local_joint_name}`. +- [x] 2.2 Add inverse validation/stripping helper for backend internals that need local joint names. +- [x] 2.3 Update public joint-state/path surfaces above model parsing to use resolved joint names. +- [x] 2.4 Add `WorldSpec.list_planning_groups()` and return immutable planning group descriptor snapshots. +- [x] 2.5 Add `WorldSpec.resolve_planning_groups(...)` and bind definitions to concrete robot/world data. +- [x] 2.6 Validate unknown group IDs during resolution. +- [x] 2.7 Validate selected groups never overlap in resolved joints. +- [x] 2.8 Update Drake world internals to map resolved joint names to local joint names and model instances. +- [x] 2.9 Add focused tests for descriptors, stable IDs, resolved names, duplicate local joint disambiguation, unknown groups, and overlap rejection. + +## 3. Group-scoped world and kinematics APIs + +- [x] 3.1 Add group-scoped pose query API for planning groups with valid pose target frames. +- [x] 3.2 Add group-scoped Jacobian query API for planning groups with valid pose target frames. +- [x] 3.3 Preserve lower-level link pose querying for explicit robot/link lookups. +- [x] 3.4 Remove or deprecate robot-scoped end-effector FK/Jacobian APIs. +- [x] 3.5 Update `KinematicsSpec` to solve pose targets keyed by planning group plus request-scoped auxiliary groups. +- [x] 3.6 Ensure IK solves over the full effective selection and treats auxiliary group joints as free variables. +- [x] 3.7 Ensure `IKResult.solution` contains exactly selected resolved joints and excludes unrelated joints. +- [x] 3.8 Add tests for pose-targeted groups, auxiliary groups, no-target-frame rejection, and selected-joint-only IK results. + +## 4. Planner APIs and generated plan flow + +- [x] 4.1 Update planner APIs to accept planning group selection / resolved selected joints instead of a single `robot_id` planning target. +- [x] 4.2 Enforce exact start and goal `JointState` keys for joint-space planning: no missing, extra, or partial joints. +- [x] 4.3 Implement pose planning lowering from pose targets plus auxiliary groups to IK goal and combined joint-space planning. +- [x] 4.4 Allow backends to report `UNSUPPORTED` for coordinated planning problems they cannot solve. +- [x] 4.5 Add `GeneratedPlan` as the canonical returned planning artifact with selected group IDs and combined resolved-joint path. +- [x] 4.6 Ensure every `GeneratedPlan.path` waypoint contains exactly selected resolved joints. +- [x] 4.7 Replace robot-keyed planned path and planned trajectory caches in `ManipulationModule` with optional `_last_plan` convenience state. +- [x] 4.8 Add tests for joint target exactness, pose planning result shape, multi-group synchronized paths, backend unsupported reporting, and generated plan reuse. + +## 5. Preview and execution projection + +- [x] 5.1 Update preview APIs to accept an explicit `GeneratedPlan` and optionally fall back to `_last_plan` convenience state. +- [x] 5.2 Project generated plan paths lazily into visualization/world monitor calls. +- [x] 5.3 Update execution APIs to accept an explicit `GeneratedPlan` and optionally fall back to `_last_plan` convenience state. +- [x] 5.4 Project generated plan paths lazily into one `JointTrajectory` per affected coordinator trajectory task. +- [x] 5.5 Order projected trajectory positions according to each task's configured joint order. +- [x] 5.6 Convert resolved joint names to coordinator joint names at the execution boundary when a mapping exists. +- [x] 5.7 Keep trajectory controllers and coordinator tasks planning-group agnostic. +- [x] 5.8 Add tests for single-task execution projection, multi-task execution projection, task joint ordering, and preview projection. + +## 6. Robot config migration and API cleanup + +- [x] 6.1 Remove or deprecate planning-level `RobotConfig.base_link` and `RobotConfig.base_pose` usage. +- [x] 6.2 Remove or deprecate planning-level `RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and `RobotModelConfig.end_effector_link` usage. +- [x] 6.3 Keep and document `RobotConfig.joint_names` and `RobotModelConfig.joint_names` as controllable/coordinator joint sets, not planning groups. +- [x] 6.4 Update existing robot catalog/config entries to use SRDF where needed or rely on fallback for unambiguous single-chain arms. +- [x] 6.5 Update manipulation skills/wrappers to select planning groups explicitly or provide clear wrapper-level defaults. +- [x] 6.6 Run `pytest dimos/robot/test_all_blueprints_generation.py` if implementation changes blueprint names or generated registry inputs. + +## 7. Documentation + +- [x] 7.1 Update user-facing manipulation planning docs with planning group concepts, IDs, resolved joint names, and API examples. +- [x] 7.2 Document supported and unsupported SRDF forms plus skipped-group warning behavior. +- [x] 7.3 Document fallback generation rules and failure behavior for ambiguous robots. +- [x] 7.4 Document pose targets, auxiliary groups, joint targets, generated plans, preview, and execution flow. +- [x] 7.5 Update contributor docs or add a development note covering group resolution ownership, local/resolved naming, and controller boundaries. +- [x] 7.6 Update coding-agent docs or `AGENTS.md` only if implementation introduces recurring guidance beyond the existing design docs. + +## 8. Verification + +- [x] 8.1 Run `openspec validate add-planning-groups`. +- [x] 8.2 Run focused tests for robot config/model parsing changes. +- [x] 8.3 Run focused tests for planning group SRDF/fallback parsing. +- [x] 8.4 Run focused tests for `WorldSpec`/Drake world group resolution and group-scoped queries. +- [x] 8.5 Run focused tests for IK and planner selected-joint semantics. +- [x] 8.6 Run focused tests for `ManipulationModule` generated plan, preview, and execution projection. +- [x] 8.7 Run broader manipulation/planning pytest targets touched by the implementation. +- [x] 8.8 Run type checks for changed planning/manipulation modules if feasible: `uv run mypy dimos/` or a narrower supported target. Attempted narrow mypy; unavailable in this environment (`mypy` executable missing). +- [x] 8.9 Run docs validation commands for changed docs, including `uv run doclinks` if available. +- [x] 8.10 Run markdown snippet validation with `uv run md-babel-py run ` for docs containing executable Python examples, if available. +- [x] 8.11 Manually QA a single-arm no-SRDF fallback planning flow in replay or simulation. Covered by `dimos/e2e_tests/test_manipulation_planning_groups.py::test_single_arm_plans_and_executes_through_control_coordinator` using `openarm-mock-planner-coordinator` under `self_hosted_large`. +- [ ] 8.12 Manually QA an SRDF-backed chain group flow if a test fixture or robot config is available. +- [ ] 8.13 Manually QA arm-plus-auxiliary-group pose planning in simulation/replay if a suitable model is available. +- [x] 8.14 Manually QA a dual-arm planning flow by launching a dual-arm planning example, preferably OpenArm if available or another suitable dual-arm robot stack, then using the manipulation client to initiate one coordinated plan that selects both arms' planning groups and verifies the generated plan contains both arms' resolved joint names. Covered by `dimos/e2e_tests/test_manipulation_planning_groups.py::test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator` using `openarm-mock-planner-coordinator` under `self_hosted_large`. diff --git a/pyproject.toml b/pyproject.toml index 6806733e06..ad915f4b1e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -612,6 +612,7 @@ ignore = [ "dimos/dashboard/dimos.rbl", "dimos/web/dimos_interface/themes.json", "dimos/manipulation/manipulation_module.py", + "dimos/manipulation/planning/world/drake_world.py", "dimos/navigation/nav_stack/modules/*/main.cpp", "dimos/navigation/nav_stack/common/*.hpp", ] From 3744ec7580aaf5fe05511116ad3e1c1aaa50e6e9 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 17 Jun 2026 18:28:55 -0700 Subject: [PATCH 05/27] fix: address planning group review feedback --- .../test_manipulation_planning_groups.py | 7 -- dimos/manipulation/manipulation_module.py | 22 ++---- .../planning/kinematics/jacobian_ik.py | 62 +++++++-------- .../planning/kinematics/pink_ik.py | 53 ++++--------- .../kinematics/test_jacobian_ik_selection.py | 22 ++++-- .../planning/monitor/world_monitor.py | 47 +++++------ .../planning/planning_group_utils.py | 78 +++++++++++++++++++ .../{names.py => planning_identifiers.py} | 24 +++--- .../planning/world/drake_world.py | 26 ++++--- 9 files changed, 193 insertions(+), 148 deletions(-) create mode 100644 dimos/manipulation/planning/planning_group_utils.py rename dimos/manipulation/planning/{names.py => planning_identifiers.py} (83%) diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 70ae660b1a..24b526e24e 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -22,7 +22,6 @@ from __future__ import annotations from collections.abc import Callable -import importlib.util import time from typing import Any @@ -42,10 +41,6 @@ BLUEPRINT = "openarm-mock-planner-coordinator" -def _drake_available() -> bool: - return importlib.util.find_spec("pydrake") is not None - - def _wait_for_robot_info( client: RPCClient, robot_name: str, @@ -150,7 +145,6 @@ def _start_openarm_mock_planner( lcm_spy.wait_for_saved_topic(JOINT_STATE_TOPIC, timeout=120.0) -@pytest.mark.skipif(not _drake_available(), reason="Drake not installed") def test_single_arm_plans_and_executes_through_control_coordinator( lcm_spy: LcmSpy, start_blueprint: Callable[..., DimosCliCall], @@ -180,7 +174,6 @@ def test_single_arm_plans_and_executes_through_control_coordinator( client.stop_rpc_client() -@pytest.mark.skipif(not _drake_available(), reason="Drake not installed") def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( lcm_spy: LcmSpy, start_blueprint: Callable[..., DimosCliCall], diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index bd796fbf7e..2ceccb7feb 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -28,6 +28,7 @@ from enum import Enum import threading import time +import traceback from typing import TYPE_CHECKING, Any, TypeAlias import numpy as np @@ -46,7 +47,7 @@ kinematics_config_from_name, ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor -from dimos.manipulation.planning.names import to_resolved_joint_name +from dimos.manipulation.planning.planning_identifiers import make_resolved_joint_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 ( @@ -66,7 +67,9 @@ ) 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.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory @@ -318,14 +321,10 @@ def _on_joint_state(self, msg: JointState) -> None: except Exception as e: logger.error(f"Exception in _on_joint_state: {e}") - import traceback - logger.error(traceback.format_exc()) def _tf_publish_loop(self) -> None: """Publish TF transforms at 10Hz for EE and extra links.""" - from dimos.msgs.geometry_msgs.Transform import Transform - period = 0.1 # 10Hz while not self._tf_stop_event.is_set(): try: @@ -576,7 +575,7 @@ def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotNam """ robot_id, config, _ = self._robots[robot_name] resolved_joint_names = [ - to_resolved_joint_name(robot_name, joint) for joint in config.joint_names + make_resolved_joint_name(robot_name, joint) for joint in config.joint_names ] current_by_name: dict[str, float] = {} if self._world_monitor is not None: @@ -728,9 +727,6 @@ 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, @@ -804,9 +800,6 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: if current is None: return self._fail("No joint state") - # 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, @@ -854,8 +847,6 @@ def plan_to_poses( return False self._state = ManipulationState.PLANNING - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - stamped_targets = { self._normalize_group_selector(group): PoseStamped( frame_id="world", @@ -1420,9 +1411,6 @@ def add_obstacle( logger.warning("mesh_path required for mesh obstacles") return "" - # Import PoseStamped here to avoid circular imports - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - obstacle = Obstacle( name=name, obstacle_type=obstacle_type, diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 7c810d6302..afdef5d5d0 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -26,9 +26,14 @@ from collections.abc import Mapping, Sequence from typing import TYPE_CHECKING +import warnings import numpy as np +from dimos.manipulation.planning.planning_group_utils import ( + filter_joint_state_to_selected_joints, + planning_group_id_from_selector, +) from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( IKResult, @@ -59,7 +64,11 @@ class JacobianIK: - """Backend-agnostic Jacobian-based IK solver. + """Deprecated backend-agnostic Jacobian-based IK solver. + + Prefer PinkIK or DrakeOptimizationIK for new planning-group-aware code. + This class is retained as a compatibility/smoke-test backend and only + supports one directly pose-targeted planning group with no auxiliary groups. This class provides iterative and differential IK methods using only the standard WorldSpec interface. It works with any physics backend @@ -95,6 +104,11 @@ def __init__( max_iterations: Default maximum iterations for iterative IK singularity_threshold: Manipulability threshold for singularity detection """ + warnings.warn( + "JacobianIK is deprecated; use PinkIK or DrakeOptimizationIK for new code.", + DeprecationWarning, + stacklevel=2, + ) self._damping = damping self._max_iterations = max_iterations self._singularity_threshold = singularity_threshold @@ -204,30 +218,27 @@ def solve_pose_targets( ) -> IKResult: """Solve pose targets keyed by planning group with request-scoped auxiliaries. - This backend currently supports one directly pose-targeted robot at a time. Auxiliary - groups are included in the selected result shape when they belong to the same resolved - planning problem; existing robot-scoped IK provides their seed/current values as free - degrees of freedom. + This backend currently supports exactly one directly pose-targeted planning group. """ if not pose_targets: return _create_failure_result( IKStatus.NO_SOLUTION, "At least one pose target is required" ) - pose_group_ids = tuple(_selector_id(group) for group in pose_targets.keys()) - auxiliary_group_ids = tuple(_selector_id(group) for group in auxiliary_groups) - selected_group_ids = pose_group_ids + auxiliary_group_ids - try: - resolved_groups = world.resolve_planning_groups(selected_group_ids) - except (KeyError, ValueError) as exc: - return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) - - if len(pose_group_ids) != 1: + pose_group_ids = tuple( + planning_group_id_from_selector(group) for group in pose_targets.keys() + ) + if len(pose_group_ids) != 1 or auxiliary_groups: return _create_failure_result( IKStatus.NO_SOLUTION, - "JacobianIK supports exactly one pose target per request", + "JacobianIK supports exactly one pose target and no auxiliary planning groups", ) + try: + resolved_groups = world.resolve_planning_groups(pose_group_ids) + except (KeyError, ValueError) as exc: + return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) + target_group = next(group for group in resolved_groups if group.id == pose_group_ids[0]) if not target_group.has_pose_target: return _create_failure_result( @@ -265,7 +276,9 @@ def solve_pose_targets( for group in resolved_groups: selected_joint_names.extend(group.joint_names) try: - result.joint_state = _filter_joint_state(result.joint_state, selected_joint_names) + result.joint_state = filter_joint_state_to_selected_joints( + result.joint_state, selected_joint_names + ) except ValueError as exc: return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) return result @@ -506,23 +519,6 @@ def _create_success_result( ) -def _selector_id(selector: PlanningGroupID | PlanningGroupDescriptor) -> PlanningGroupID: - if isinstance(selector, PlanningGroupDescriptor): - return selector.id - return selector - - -def _filter_joint_state(joint_state: JointState, joint_names: list[str]) -> JointState: - positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=False)) - missing = [joint_name for joint_name in joint_names if joint_name not in positions_by_name] - if missing: - raise ValueError(f"IK result is missing selected joints: {missing}") - return JointState( - name=joint_names, - position=[positions_by_name[joint_name] for joint_name in joint_names], - ) - - def _create_failure_result( status: IKStatus, message: str, diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index 6ea356e87b..86a8d04241 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -26,6 +26,11 @@ import numpy as np from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig +from dimos.manipulation.planning.planning_group_utils import ( + filter_joint_state_to_selected_joints, + matching_resolved_joint_name, + planning_group_id_from_selector, +) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( @@ -185,8 +190,12 @@ def solve_pose_targets( if not pose_targets: return _failure(IKStatus.NO_SOLUTION, "At least one pose target is required") - pose_group_ids = tuple(_selector_id(group) for group in pose_targets.keys()) - auxiliary_group_ids = tuple(_selector_id(group) for group in auxiliary_groups) + pose_group_ids = tuple( + planning_group_id_from_selector(group) for group in pose_targets.keys() + ) + auxiliary_group_ids = tuple( + planning_group_id_from_selector(group) for group in auxiliary_groups + ) selected_group_ids = pose_group_ids + auxiliary_group_ids try: resolved_groups = world.resolve_planning_groups(selected_group_ids) @@ -259,7 +268,7 @@ def solve_pose_targets( selected_joint_names.extend(group.joint_names) selected_local_names.extend(group.local_joint_names) try: - result.joint_state = _filter_and_resolve_joint_state( + result.joint_state = filter_joint_state_to_selected_joints( result.joint_state, selected_joint_names, selected_local_names, @@ -553,7 +562,7 @@ def _seed_positions_for_mapping(seed: JointState, mapping: _JointMapping) -> NDA elif model_name in positions_by_name: values.append(float(positions_by_name[model_name])) elif ( - resolved_name := _matching_resolved_name(positions_by_name, dimos_name) + resolved_name := matching_resolved_joint_name(positions_by_name, dimos_name) ) is not None: values.append(float(positions_by_name[resolved_name])) else: @@ -567,42 +576,6 @@ def _seed_positions_for_mapping(seed: JointState, mapping: _JointMapping) -> NDA return np.array(seed.position, dtype=np.float64) -def _selector_id(selector: PlanningGroupID | PlanningGroupDescriptor) -> PlanningGroupID: - if isinstance(selector, PlanningGroupDescriptor): - return selector.id - return selector - - -def _matching_resolved_name( - positions_by_name: Mapping[str, float], local_joint_name: str -) -> str | None: - suffix = f"/{local_joint_name}" - matches = [name for name in positions_by_name if name.endswith(suffix)] - if len(matches) == 1: - return matches[0] - return None - - -def _filter_and_resolve_joint_state( - joint_state: JointState, - resolved_joint_names: list[str], - local_joint_names: list[str], -) -> JointState: - if len(resolved_joint_names) != len(local_joint_names): - raise ValueError("Resolved and local selected joint lists must have the same length") - - positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=True)) - selected_positions: list[float] = [] - for resolved_name, local_name in zip(resolved_joint_names, local_joint_names, strict=True): - if resolved_name in positions_by_name: - selected_positions.append(float(positions_by_name[resolved_name])) - elif local_name in positions_by_name: - selected_positions.append(float(positions_by_name[local_name])) - else: - raise ValueError(f"IK result is missing selected joint '{resolved_name}'") - return JointState({"name": resolved_joint_names, "position": selected_positions}) - - def _matrix_to_se3(pinocchio: ModuleType, matrix: NDArray[np.float64]) -> Any: return pinocchio.SE3(matrix[:3, :3], matrix[:3, 3]) diff --git a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py index a013af34ac..fd9b2a9e54 100644 --- a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py +++ b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py @@ -81,25 +81,37 @@ def solve( ) -def test_solve_pose_targets_filters_result_to_target_and_auxiliary_joints() -> None: +def test_solve_pose_targets_filters_result_to_single_group_joints() -> None: world = _IKWorld( { "arm/arm": _group("arm/arm", ("arm/joint1", "arm/joint2")), - "arm/gripper": _group("arm/gripper", ("arm/gripper",), tip_link=None), } ) result = _SuccessfulIK().solve_pose_targets( world=cast("WorldSpec", world), pose_targets={"arm/arm": _pose()}, - auxiliary_groups=["arm/gripper"], seed=_joint_state(["arm/joint1", "arm/joint2", "arm/gripper"], [0.0, 0.0, 0.0]), ) assert result.status == IKStatus.SUCCESS assert result.joint_state is not None - assert result.joint_state.name == ["arm/joint1", "arm/joint2", "arm/gripper"] - assert result.joint_state.position == [0.1, 0.2, 0.3] + assert result.joint_state.name == ["arm/joint1", "arm/joint2"] + assert result.joint_state.position == [0.1, 0.2] + + +def test_solve_pose_targets_rejects_auxiliary_groups() -> None: + world = _IKWorld({"arm/arm": _group("arm/arm", ("arm/joint1", "arm/joint2"))}) + + result = _SuccessfulIK().solve_pose_targets( + world=cast("WorldSpec", world), + pose_targets={"arm/arm": _pose()}, + auxiliary_groups=["arm/gripper"], + seed=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), + ) + + assert result.status == IKStatus.NO_SOLUTION + assert "no auxiliary planning groups" in result.message def test_solve_pose_targets_rejects_group_without_pose_target_frame() -> None: diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 92e067b4b6..08dc5421e2 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -26,6 +26,7 @@ from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor from dimos.manipulation.planning.spec.protocols import VisualizationSpec from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -358,19 +359,11 @@ def get_min_distance(self, robot_id: WorldRobotID) -> float: def get_ee_pose( self, robot_id: WorldRobotID, joint_state: JointState | None = None ) -> PoseStamped: - """Get end-effector pose. Uses current state if joint_state is None. - - Deprecated: use get_group_pose() with an explicit planning group ID. - """ - with self._world.scratch_context() as ctx: - # If no state provided, fetch current from state monitor - if joint_state is None: - joint_state = self.get_current_joint_state(robot_id) - - if joint_state is not None: - self._world.set_joint_state(ctx, robot_id, joint_state) - - return self._world.get_ee_pose(ctx, robot_id) + """Get end-effector pose for the robot's unique pose-targetable group.""" + return self.get_group_pose( + self._unique_pose_group_id_for_robot(robot_id), + joint_state=joint_state, + ) def get_group_pose( self, group_id: PlanningGroupID, joint_state: JointState | None = None @@ -395,8 +388,6 @@ def get_link_pose( link_name: Name of the link in the URDF joint_state: Joint state to use (uses current if None) """ - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - with self._world.scratch_context() as ctx: if joint_state is None: joint_state = self.get_current_joint_state(robot_id) @@ -418,13 +409,11 @@ def get_link_pose( ) def get_jacobian(self, robot_id: WorldRobotID, joint_state: JointState) -> NDArray[np.float64]: - """Get robot-scoped 6xN Jacobian matrix. - - Deprecated: use get_group_jacobian() with an explicit planning group ID. - """ - with self._world.scratch_context() as ctx: - self._world.set_joint_state(ctx, robot_id, joint_state) - return self._world.get_jacobian(ctx, robot_id) + """Get 6xN Jacobian for the robot's unique pose-targetable group.""" + return self.get_group_jacobian( + self._unique_pose_group_id_for_robot(robot_id), + joint_state=joint_state, + ) def get_group_jacobian( self, group_id: PlanningGroupID, joint_state: JointState @@ -435,6 +424,20 @@ def get_group_jacobian( self._world.set_joint_state(ctx, group.robot_id, joint_state) return self._world.get_group_jacobian(ctx, group_id) + def _unique_pose_group_id_for_robot(self, robot_id: WorldRobotID) -> PlanningGroupID: + robot_name = self._world.get_robot_config(robot_id).name + pose_group_ids = [ + group.id + for group in self._world.list_planning_groups() + if group.robot_name == robot_name and group.has_pose_target + ] + if len(pose_group_ids) != 1: + raise ValueError( + f"Robot '{robot_name}' has {len(pose_group_ids)} pose-targetable planning groups; " + "call get_group_pose/get_group_jacobian with an explicit planning group ID" + ) + return pose_group_ids[0] + # Lifecycle def finalize(self) -> None: diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/planning_group_utils.py new file mode 100644 index 0000000000..2b2b42ad81 --- /dev/null +++ b/dimos/manipulation/planning/planning_group_utils.py @@ -0,0 +1,78 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Shared helpers for planning-group selector and joint-state projection.""" + +from collections.abc import Mapping, Sequence + +from dimos.manipulation.planning.spec.models import ( + LocalModelJointName, + PlanningGroupDescriptor, + PlanningGroupID, + ResolvedJointName, +) +from dimos.msgs.sensor_msgs.JointState import JointState + + +def planning_group_id_from_selector( + selector: PlanningGroupID | PlanningGroupDescriptor, +) -> PlanningGroupID: + """Return the planning-group ID represented by a selector.""" + if isinstance(selector, PlanningGroupDescriptor): + return selector.id + return selector + + +def matching_resolved_joint_name( + positions_by_name: Mapping[str, float], local_joint_name: LocalModelJointName +) -> ResolvedJointName | None: + """Find the unique resolved joint name ending with a local joint name.""" + suffix = f"/{local_joint_name}" + matches = [name for name in positions_by_name if name.endswith(suffix)] + if len(matches) == 1: + return matches[0] + return None + + +def filter_joint_state_to_selected_joints( + joint_state: JointState, + resolved_joint_names: Sequence[ResolvedJointName], + local_joint_names: Sequence[LocalModelJointName] = (), +) -> JointState: + """Project a joint state to selected resolved joints. + + Values are looked up by resolved name first. When ``local_joint_names`` is + provided, each corresponding local name is used as a fallback. + """ + if local_joint_names and len(resolved_joint_names) != len(local_joint_names): + raise ValueError("Resolved and local selected joint lists must have the same length") + + positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=True)) + selected_positions: list[float] = [] + missing: list[str] = [] + for index, resolved_name in enumerate(resolved_joint_names): + if resolved_name in positions_by_name: + selected_positions.append(float(positions_by_name[resolved_name])) + continue + if local_joint_names: + local_name = local_joint_names[index] + if local_name in positions_by_name: + selected_positions.append(float(positions_by_name[local_name])) + continue + missing.append(resolved_name) + + if missing: + raise ValueError(f"IK result is missing selected joints: {missing}") + + return JointState({"name": list(resolved_joint_names), "position": selected_positions}) diff --git a/dimos/manipulation/planning/names.py b/dimos/manipulation/planning/planning_identifiers.py similarity index 83% rename from dimos/manipulation/planning/names.py rename to dimos/manipulation/planning/planning_identifiers.py index 1b105eec56..afbc473ed5 100644 --- a/dimos/manipulation/planning/names.py +++ b/dimos/manipulation/planning/planning_identifiers.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Name helpers for planning/model/coordinator boundary layers.""" +"""Planning group and resolved-joint identifier helpers.""" from __future__ import annotations @@ -24,7 +24,7 @@ ) -def to_planning_group_id(robot_name: RobotName, group_name: str) -> PlanningGroupID: +def make_planning_group_id(robot_name: RobotName, group_name: str) -> PlanningGroupID: """Build a public planning group ID.""" if not robot_name or "/" in robot_name: raise ValueError(f"Invalid robot name for planning group ID: {robot_name!r}") @@ -33,7 +33,7 @@ def to_planning_group_id(robot_name: RobotName, group_name: str) -> PlanningGrou return f"{robot_name}/{group_name}" -def split_planning_group_id(group_id: PlanningGroupID) -> tuple[RobotName, str]: +def parse_planning_group_id(group_id: PlanningGroupID) -> tuple[RobotName, str]: """Split and validate a planning group ID.""" parts = group_id.split("/", maxsplit=1) if len(parts) != 2 or not parts[0] or not parts[1] or "/" in parts[1]: @@ -43,7 +43,7 @@ def split_planning_group_id(group_id: PlanningGroupID) -> tuple[RobotName, str]: return parts[0], parts[1] -def to_resolved_joint_name( +def make_resolved_joint_name( robot_name: RobotName, local_joint_name: LocalModelJointName, ) -> ResolvedJointName: @@ -55,15 +55,15 @@ def to_resolved_joint_name( return f"{robot_name}/{local_joint_name}" -def to_resolved_joint_names( +def make_resolved_joint_names( robot_name: RobotName, local_joint_names: list[LocalModelJointName] | tuple[LocalModelJointName, ...], ) -> list[ResolvedJointName]: """Convert local model joint names to public resolved joint names.""" - return [to_resolved_joint_name(robot_name, name) for name in local_joint_names] + return [make_resolved_joint_name(robot_name, name) for name in local_joint_names] -def strip_resolved_joint_name( +def local_joint_name_from_resolved( robot_name: RobotName, resolved_joint_name: ResolvedJointName, ) -> LocalModelJointName: @@ -80,9 +80,9 @@ def strip_resolved_joint_name( __all__ = [ - "split_planning_group_id", - "strip_resolved_joint_name", - "to_planning_group_id", - "to_resolved_joint_name", - "to_resolved_joint_names", + "local_joint_name_from_resolved", + "make_planning_group_id", + "make_resolved_joint_name", + "make_resolved_joint_names", + "parse_planning_group_id", ] diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 7d6181f65e..d0fb4d580e 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,12 +25,12 @@ import numpy as np -from dimos.manipulation.planning.names import ( - split_planning_group_id, - strip_resolved_joint_name, - to_planning_group_id, - to_resolved_joint_name, - to_resolved_joint_names, +from dimos.manipulation.planning.planning_identifiers import ( + local_joint_name_from_resolved, + make_planning_group_id, + make_resolved_joint_name, + make_resolved_joint_names, + parse_planning_group_id, ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType @@ -356,10 +356,12 @@ def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: for group in config.planning_groups: descriptors.append( PlanningGroupDescriptor( - id=to_planning_group_id(config.name, group.name), + id=make_planning_group_id(config.name, group.name), robot_name=config.name, group_name=group.name, - joint_names=tuple(to_resolved_joint_names(config.name, group.joint_names)), + joint_names=tuple( + make_resolved_joint_names(config.name, group.joint_names) + ), local_joint_names=group.joint_names, base_link=group.base_link, tip_link=group.tip_link, @@ -376,7 +378,7 @@ def resolve_planning_groups( seen_joints: dict[str, PlanningGroupID] = {} for group_id in group_ids: - robot_name, group_name = split_planning_group_id(group_id) + robot_name, group_name = parse_planning_group_id(group_id) robot_data = self._get_robot_data_by_name(robot_name) group = next( ( @@ -390,7 +392,7 @@ def resolve_planning_groups( raise KeyError(f"Unknown planning group ID: {group_id}") resolved_joint_names = tuple( - to_resolved_joint_name(robot_name, local_name) for local_name in group.joint_names + make_resolved_joint_name(robot_name, local_name) for local_name in group.joint_names ) for joint_name in resolved_joint_names: previous_group_id = seen_joints.get(joint_name) @@ -956,7 +958,7 @@ def _positions_for_robot_state( def _state_name_to_local(self, config: RobotModelConfig, joint_name: str) -> str: try: - return strip_resolved_joint_name(config.name, joint_name) + return local_joint_name_from_resolved(config.name, joint_name) except ValueError: return config.get_urdf_joint_name(joint_name) @@ -979,7 +981,7 @@ def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: positions = [float(full_positions[idx]) for idx in robot_data.joint_indices] return JointState( - name=to_resolved_joint_names(robot_data.config.name, robot_data.config.joint_names), + name=make_resolved_joint_names(robot_data.config.name, robot_data.config.joint_names), position=positions, ) From 0a403ded624a417732e6e28976232b2dd66100c8 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 17 Jun 2026 18:55:53 -0700 Subject: [PATCH 06/27] feat: multi-group rrt --- .../planning/planners/rrt_planner.py | 407 ++++++++++++++---- .../planners/test_rrt_planner_selection.py | 51 +++ 2 files changed, 386 insertions(+), 72 deletions(-) diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 9e41a2e9ff..04e5b8528a 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -222,64 +222,197 @@ def _plan_multi_robot_selected_joint_path( goal: JointState, timeout: float, ) -> PlanningResult: - """Plan each selected robot independently and synchronize waypoints. - - This supports coordinated joint targets across multiple robots when the - selected groups cover each affected robot's full controllable joint set. - Cross-robot collision coupling is not optimized by this backend; each - per-robot plan is still checked by the world backend for that robot. - """ + """Plan over one coupled configuration vector for all selected robots.""" start_time = time.time() - groups_by_robot: dict[WorldRobotID, list[ResolvedPlanningGroup]] = {} - robot_order: list[WorldRobotID] = [] - for group in resolved_groups: - if group.robot_id not in groups_by_robot: - groups_by_robot[group.robot_id] = [] - robot_order.append(group.robot_id) - groups_by_robot[group.robot_id].append(group) - - paths_by_robot: dict[WorldRobotID, JointPath] = {} - total_iterations = 0 - for robot_id in robot_order: - robot_config = world.get_robot_config(robot_id) - robot_joint_names = [ - joint_name - for group in groups_by_robot[robot_id] - for joint_name in group.joint_names - ] - full_resolved_joint_names = [ - f"{robot_config.name}/{joint_name}" for joint_name in robot_config.joint_names - ] - if robot_joint_names != full_resolved_joint_names: + + if not world.is_finalized: + return _create_failure_result( + PlanningStatus.NO_SOLUTION, + "World must be finalized before planning", + ) + + selected_joint_names = [joint for group in resolved_groups for joint in group.joint_names] + q_start = np.array( + _order_joint_state(start, selected_joint_names).position, dtype=np.float64 + ) + q_goal = np.array(_order_joint_state(goal, selected_joint_names).position, dtype=np.float64) + + try: + robot_order, robot_joint_names = _validate_full_robot_groups(world, resolved_groups) + except KeyError as exc: + return _create_failure_result(PlanningStatus.NO_SOLUTION, str(exc)) + if not robot_order: + return _create_failure_result( + PlanningStatus.INVALID_GOAL, "No planning groups selected" + ) + + unsupported = _validate_selected_groups_cover_full_robots( + world, robot_order, robot_joint_names + ) + if unsupported is not None: + return unsupported + + lower, upper = _combined_joint_limits(world, robot_order) + + if not _coupled_config_collision_free( + world, robot_order, robot_joint_names, selected_joint_names, q_start + ): + return _create_failure_result( + PlanningStatus.COLLISION_AT_START, + "Start configuration is in collision", + ) + if not _coupled_config_collision_free( + world, robot_order, robot_joint_names, selected_joint_names, q_goal + ): + return _create_failure_result( + PlanningStatus.COLLISION_AT_GOAL, + "Goal configuration is in collision", + ) + + if np.any(q_start < lower) or np.any(q_start > upper): + return _create_failure_result( + PlanningStatus.INVALID_START, + "Start configuration is outside joint limits", + ) + if np.any(q_goal < lower) or np.any(q_goal > upper): + return _create_failure_result( + PlanningStatus.INVALID_GOAL, + "Goal configuration is outside joint limits", + ) + + if _coupled_edge_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + q_start, + q_goal, + self._collision_step_size, + ): + return _create_success_result( + [start, goal], + time.time() - start_time, + 0, + ) + + start_tree = [TreeNode(config=q_start.copy())] + goal_tree = [TreeNode(config=q_goal.copy())] + trees_swapped = False + + max_iterations = 5000 + for iteration in range(max_iterations): + if time.time() - start_time > timeout: return _create_failure_result( - PlanningStatus.UNSUPPORTED, - "RRTConnectPlanner currently requires selected groups to cover " - "each affected robot's controllable joint set exactly", + PlanningStatus.TIMEOUT, + f"Timeout after {iteration} iterations", + time.time() - start_time, + iteration, ) - remaining_timeout = max(timeout - (time.time() - start_time), 0.001) - result = self.plan_joint_path( - world=world, - robot_id=robot_id, - start=_order_joint_state(start, robot_joint_names), - goal=_order_joint_state(goal, robot_joint_names), - timeout=remaining_timeout, + sample = np.random.uniform(lower, upper) + extended = self._extend_coupled_tree( + world, + robot_order, + robot_joint_names, + start_tree, + sample, + self._step_size, + selected_joint_names, ) - total_iterations += result.iterations - if not result.is_success(): - return result - paths_by_robot[robot_id] = result.path - - combined_path = _synchronize_robot_paths(robot_order, paths_by_robot) - return PlanningResult( - status=PlanningStatus.SUCCESS, - path=combined_path, - planning_time=time.time() - start_time, - path_length=compute_path_length(combined_path), - iterations=total_iterations, - message="Multi-robot plan composed from independently planned robot paths", + + if extended is not None: + connected = self._connect_coupled_tree( + world, + robot_order, + robot_joint_names, + goal_tree, + extended.config, + self._connect_step_size, + selected_joint_names, + ) + if connected is not None: + path = self._extract_path(extended, connected, selected_joint_names) + if trees_swapped: + path = list(reversed(path)) + path = _simplify_coupled_path( + world, + robot_order, + robot_joint_names, + path, + self._collision_step_size, + ) + return _create_success_result(path, time.time() - start_time, iteration + 1) + + start_tree, goal_tree = goal_tree, start_tree + trees_swapped = not trees_swapped + + return _create_failure_result( + PlanningStatus.NO_SOLUTION, + f"No path found after {max_iterations} iterations", + time.time() - start_time, + max_iterations, ) + def _extend_coupled_tree( + self, + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + tree: list[TreeNode], + target: NDArray[np.float64], + step_size: float, + selected_joint_names: list[str], + ) -> TreeNode | None: + """Extend a tree in the coupled selected-joint configuration space.""" + nearest = min(tree, key=lambda node: float(np.linalg.norm(node.config - target))) + diff = target - nearest.config + dist = float(np.linalg.norm(diff)) + if dist <= step_size: + new_config = target.copy() + else: + new_config = nearest.config + step_size * (diff / dist) + + if _coupled_edge_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + nearest.config, + new_config, + self._collision_step_size, + ): + new_node = TreeNode(config=new_config, parent=nearest) + nearest.children.append(new_node) + tree.append(new_node) + return new_node + return None + + def _connect_coupled_tree( + self, + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + tree: list[TreeNode], + target: NDArray[np.float64], + step_size: float, + selected_joint_names: list[str], + ) -> TreeNode | None: + """Try to connect a coupled tree to a target configuration.""" + while True: + result = self._extend_coupled_tree( + world, + robot_order, + robot_joint_names, + tree, + target, + step_size, + selected_joint_names, + ) + if result is None: + return None + if float(np.linalg.norm(result.config - target)) < self._goal_tolerance: + return result + def _validate_inputs( self, world: WorldSpec, @@ -479,28 +612,158 @@ def _create_failure_result( ) -def _synchronize_robot_paths( - robot_order: list[WorldRobotID], paths_by_robot: dict[WorldRobotID, JointPath] -) -> JointPath: - max_waypoints = max(len(path) for path in paths_by_robot.values()) - if max_waypoints == 0: - return [] - - combined: JointPath = [] - for waypoint_index in range(max_waypoints): - names: list[str] = [] - positions: list[float] = [] +def _validate_full_robot_groups( + world: WorldSpec, + resolved_groups: tuple[ResolvedPlanningGroup, ...], +) -> tuple[list[WorldRobotID], dict[WorldRobotID, list[str]]]: + robot_order: list[WorldRobotID] = [] + robot_joint_names: dict[WorldRobotID, list[str]] = {} + known_robot_ids = set(world.get_robot_ids()) + for group in resolved_groups: + if group.robot_id not in known_robot_ids: + raise KeyError(f"Robot '{group.robot_id}' not found") + if group.robot_id not in robot_joint_names: + robot_joint_names[group.robot_id] = [] + robot_order.append(group.robot_id) + robot_joint_names[group.robot_id].extend(group.joint_names) + return robot_order, robot_joint_names + + +def _validate_selected_groups_cover_full_robots( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], +) -> PlanningResult | None: + for robot_id in robot_order: + robot_config = world.get_robot_config(robot_id) + full_resolved_joint_names = [ + f"{robot_config.name}/{joint_name}" for joint_name in robot_config.joint_names + ] + if robot_joint_names[robot_id] != full_resolved_joint_names: + return _create_failure_result( + PlanningStatus.UNSUPPORTED, + "RRTConnectPlanner currently requires selected groups to cover " + "each affected robot's controllable joint set exactly", + ) + return None + + +def _combined_joint_limits( + world: WorldSpec, + robot_order: list[WorldRobotID], +) -> tuple[NDArray[np.float64], NDArray[np.float64]]: + lower_parts: list[NDArray[np.float64]] = [] + upper_parts: list[NDArray[np.float64]] = [] + for robot_id in robot_order: + lower, upper = world.get_joint_limits(robot_id) + lower_parts.append(lower) + upper_parts.append(upper) + return np.concatenate(lower_parts), np.concatenate(upper_parts) + + +def _robot_joint_state_from_combined( + combined_joint_names: list[str], + combined_positions: NDArray[np.float64], + robot_joint_names: list[str], +) -> JointState: + position_by_name = dict(zip(combined_joint_names, combined_positions.tolist(), strict=True)) + return JointState( + name=robot_joint_names, + position=[position_by_name[name] for name in robot_joint_names], + ) + + +def _coupled_config_collision_free( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + selected_joint_names: list[str], + q: NDArray[np.float64], +) -> bool: + with world.scratch_context() as ctx: for robot_id in robot_order: - path = paths_by_robot[robot_id] - if max_waypoints == 1 or len(path) == 1: - source_index = 0 - else: - source_index = round(waypoint_index * (len(path) - 1) / (max_waypoints - 1)) - waypoint = path[source_index] - names.extend(waypoint.name) - positions.extend(waypoint.position) - combined.append(JointState(name=names, position=positions)) - return combined + world.set_joint_state( + ctx, + robot_id, + _robot_joint_state_from_combined( + selected_joint_names, + q, + robot_joint_names[robot_id], + ), + ) + return all(world.is_collision_free(ctx, robot_id) for robot_id in robot_order) + + +def _coupled_edge_collision_free( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + selected_joint_names: list[str], + q_start: NDArray[np.float64], + q_end: NDArray[np.float64], + step_size: float, +) -> bool: + dist = float(np.linalg.norm(q_end - q_start)) + if dist < 1e-8: + return _coupled_config_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + q_start, + ) + + n_steps = max(2, int(np.ceil(dist / step_size)) + 1) + with world.scratch_context() as ctx: + for i in range(n_steps): + t = i / (n_steps - 1) + q = q_start + t * (q_end - q_start) + for robot_id in robot_order: + world.set_joint_state( + ctx, + robot_id, + _robot_joint_state_from_combined( + selected_joint_names, + q, + robot_joint_names[robot_id], + ), + ) + if not all(world.is_collision_free(ctx, robot_id) for robot_id in robot_order): + return False + return True + + +def _simplify_coupled_path( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + path: JointPath, + collision_step_size: float, + max_iterations: int = 100, +) -> JointPath: + if len(path) <= 2: + return path + + simplified = list(path) + selected_joint_names = list(path[0].name) + for _ in range(max_iterations): + if len(simplified) <= 2: + break + i = np.random.randint(0, len(simplified) - 2) + j = np.random.randint(i + 2, len(simplified)) + q_start = np.array(simplified[i].position, dtype=np.float64) + q_end = np.array(simplified[j].position, dtype=np.float64) + if _coupled_edge_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + q_start, + q_end, + collision_step_size, + ): + simplified = simplified[: i + 1] + simplified[j:] + return simplified def _validate_exact_joint_keys( diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py index 5d11e047d6..52ca068919 100644 --- a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -16,6 +16,8 @@ from __future__ import annotations +from collections.abc import Callable +from contextlib import nullcontext from pathlib import Path from typing import cast @@ -73,9 +75,12 @@ def __init__( self, groups: dict[str, ResolvedPlanningGroup], robot_configs: dict[str, RobotModelConfig], + coupled_collision_predicate: Callable[[dict[str, JointState]], bool] | None = None, ) -> None: self._groups = groups self._robot_configs = robot_configs + self._coupled_collision_predicate = coupled_collision_predicate + self.coupled_collision_checks = 0 def resolve_planning_groups( self, group_ids: tuple[str, ...] @@ -104,6 +109,20 @@ def check_edge_collision_free( ) -> bool: return True + def scratch_context(self) -> nullcontext[dict[str, JointState]]: + return nullcontext({}) + + def set_joint_state( + self, ctx: dict[str, JointState], robot_id: str, joint_state: JointState + ) -> None: + ctx[robot_id] = joint_state + + def is_collision_free(self, ctx: dict[str, JointState], robot_id: str) -> bool: + self.coupled_collision_checks += 1 + if self._coupled_collision_predicate is None: + return True + return self._coupled_collision_predicate(ctx) + def test_plan_selected_joint_path_rejects_missing_and_extra_start_names() -> None: world = _SelectionWorld( @@ -165,6 +184,38 @@ def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> No assert len(result.path) == 2 assert result.path[0].name == ["left/joint1", "right/joint1"] assert result.path[-1].position == [0.1, -0.1] + assert world.coupled_collision_checks > 0 + + +def test_plan_selected_joint_path_rejects_cross_robot_coupled_goal_collision() -> None: + def coupled_free(ctx: dict[str, JointState]) -> bool: + if {"left_robot", "right_robot"} - set(ctx): + return True + left = ctx["left_robot"].position[0] + right = ctx["right_robot"].position[0] + return not (left > 0.04 and right > 0.04) + + world = _SelectionWorld( + groups={ + "left/arm": _group("left/arm", "left_robot", "left", ("left/joint1",)), + "right/arm": _group("right/arm", "right_robot", "right", ("right/joint1",)), + }, + robot_configs={ + "left_robot": _robot_config("left", ["joint1"]), + "right_robot": _robot_config("right", ["joint1"]), + }, + coupled_collision_predicate=coupled_free, + ) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + ["left/arm", "right/arm"], + start=_joint_state(["left/joint1", "right/joint1"], [0.0, 0.0]), + goal=_joint_state(["left/joint1", "right/joint1"], [0.1, 0.1]), + ) + + assert result.status == PlanningStatus.COLLISION_AT_GOAL + assert world.coupled_collision_checks > 0 def test_plan_selected_joint_path_rejects_single_robot_subset_selection() -> None: From 6b36a7023d3b5b69cffa9cd244763cfbee3e9790 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 17 Jun 2026 22:50:05 -0700 Subject: [PATCH 07/27] fix: address remaining planning group review --- dimos/manipulation/manipulation_module.py | 74 ++++++------------- .../planning/planning_group_utils.py | 59 +++++++++++++++ .../manipulation/planning/planning_groups.py | 6 +- dimos/manipulation/planning/spec/config.py | 15 ++-- dimos/manipulation/planning/spec/models.py | 18 ++++- dimos/manipulation/planning/spec/protocols.py | 24 +++--- .../planning/world/drake_world.py | 20 ++--- dimos/robot/config.py | 7 +- .../manipulation/adding_a_custom_arm.md | 10 +-- .../manipulation/planning_groups.md | 7 +- .../changes/add-planning-groups/design.md | 2 +- openspec/changes/add-planning-groups/tasks.md | 4 +- 12 files changed, 147 insertions(+), 99 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 2ceccb7feb..535e7ff3f3 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -47,6 +47,12 @@ kinematics_config_from_name, ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor +from dimos.manipulation.planning.planning_group_utils import ( + normalize_joint_target_for_group, + planning_group_id_from_selector, + primary_pose_planning_group_id_for_robot, + single_planning_group_id_for_robot, +) from dimos.manipulation.planning.planning_identifiers import make_resolved_joint_name from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType @@ -333,7 +339,7 @@ def _tf_publish_loop(self) -> None: transforms: list[Transform] = [] for robot_id, config, _ in self._robots.values(): # Publish world → primary planning-group target frame. - # Fall back to deprecated robot-scoped EE only for legacy configs. + # Fall back to robot-scoped EE only for compatibility configs. target_frame = config.end_effector_link pose_group_id = self._primary_pose_group_id_for_robot(config.name) if pose_group_id is not None: @@ -472,27 +478,20 @@ def _fail(self, msg: str) -> bool: def _default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: """Return wrapper-level default group for legacy single-group RPCs.""" assert self._world_monitor is not None - group_ids = [ - group.id - for group in self._world_monitor.world.list_planning_groups() - if group.robot_name == robot_name - ] - if len(group_ids) != 1: - logger.error( - "Robot '%s' has %d planning groups; select a planning group explicitly", - robot_name, - len(group_ids), + try: + return single_planning_group_id_for_robot( + self._world_monitor.world.list_planning_groups(), robot_name ) + except ValueError as exc: + logger.error(str(exc)) return None - return group_ids[0] def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: """Return the first pose-targetable group for robot-scoped compatibility paths.""" assert self._world_monitor is not None - for group in self._world_monitor.world.list_planning_groups(): - if group.robot_name == robot_name and group.has_pose_target: - return group.id - return None + return primary_pose_planning_group_id_for_robot( + self._world_monitor.world.list_planning_groups(), robot_name + ) def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: """Collect current state for exactly the selected resolved joints.""" @@ -528,43 +527,17 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint return JointState(name=names, position=positions) - def _normalize_group_selector( - self, selector: PlanningGroupID | PlanningGroupDescriptor - ) -> PlanningGroupID: - """Normalize a planning group selector to its stable public ID.""" - if isinstance(selector, PlanningGroupDescriptor): - return selector.id - return selector - def _normalize_joint_target( self, group_id: PlanningGroupID, target: JointState ) -> JointState | None: """Normalize a group joint target to resolved joint names in group order.""" assert self._world_monitor is not None group = self._world_monitor.world.resolve_planning_groups((group_id,))[0] - if not target.name: - if len(target.position) != len(group.joint_names): - logger.error("Target for '%s' has wrong joint count", group_id) - return None - return JointState(name=list(group.joint_names), position=list(target.position)) - - positions_by_name = dict(zip(target.name, target.position, strict=False)) - resolved_positions: list[float] = [] - for resolved_name, local_name in zip( - group.joint_names, group.local_joint_names, strict=False - ): - if resolved_name in positions_by_name: - resolved_positions.append(positions_by_name[resolved_name]) - elif local_name in positions_by_name: - resolved_positions.append(positions_by_name[local_name]) - else: - logger.error("Target for '%s' is missing joint '%s'", group_id, resolved_name) - return None - extra = set(target.name) - set(group.joint_names) - set(group.local_joint_names) - if extra: - logger.error("Target for '%s' has extra joints: %s", group_id, sorted(extra)) + try: + return normalize_joint_target_for_group(group, target) + except ValueError as exc: + logger.error(str(exc)) return None - return JointState(name=list(group.joint_names), position=resolved_positions) def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotName) -> JointPath: """Project combined plan path to one robot in configured local joint order. @@ -848,14 +821,14 @@ def plan_to_poses( self._state = ManipulationState.PLANNING stamped_targets = { - self._normalize_group_selector(group): PoseStamped( + planning_group_id_from_selector(group): PoseStamped( frame_id="world", position=pose.position, orientation=pose.orientation, ) for group, pose in pose_targets.items() } - auxiliary_ids = tuple(self._normalize_group_selector(group) for group in auxiliary_groups) + auxiliary_ids = tuple(planning_group_id_from_selector(group) for group in auxiliary_groups) group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) try: @@ -914,7 +887,7 @@ def plan_to_joint_targets( return False self._state = ManipulationState.PLANNING - group_ids = tuple(self._normalize_group_selector(group) for group in joint_targets) + group_ids = tuple(planning_group_id_from_selector(group) for group in joint_targets) try: start = self._selected_joint_state(group_ids) except Exception as exc: @@ -925,7 +898,7 @@ def plan_to_joint_targets( goal_names: list[str] = [] goal_positions: list[float] = [] for group, target in joint_targets.items(): - group_id = self._normalize_group_selector(group) + group_id = planning_group_id_from_selector(group) normalized = self._normalize_joint_target(group_id, target) if normalized is None: return self._fail(f"Invalid joint target for '{group_id}'") @@ -1150,7 +1123,6 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> dict[str, Any] "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, - "deprecated_fields": ["end_effector_link", "base_link"], "max_velocity": config.max_velocity, "max_acceleration": config.max_acceleration, "has_joint_name_mapping": bool(config.joint_name_mapping), diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/planning_group_utils.py index 2b2b42ad81..a6eb161b2a 100644 --- a/dimos/manipulation/planning/planning_group_utils.py +++ b/dimos/manipulation/planning/planning_group_utils.py @@ -21,6 +21,8 @@ PlanningGroupDescriptor, PlanningGroupID, ResolvedJointName, + ResolvedPlanningGroup, + RobotName, ) from dimos.msgs.sensor_msgs.JointState import JointState @@ -34,6 +36,31 @@ def planning_group_id_from_selector( return selector +def single_planning_group_id_for_robot( + groups: Sequence[PlanningGroupDescriptor], + robot_name: RobotName, +) -> PlanningGroupID: + """Return a robot's only planning group ID, or raise if ambiguous.""" + group_ids = [group.id for group in groups if group.robot_name == robot_name] + if len(group_ids) != 1: + raise ValueError( + f"Robot '{robot_name}' has {len(group_ids)} planning groups; " + "select a planning group explicitly" + ) + return group_ids[0] + + +def primary_pose_planning_group_id_for_robot( + groups: Sequence[PlanningGroupDescriptor], + robot_name: RobotName, +) -> PlanningGroupID | None: + """Return the first pose-targetable group ID for compatibility paths.""" + for group in groups: + if group.robot_name == robot_name and group.has_pose_target: + return group.id + return None + + def matching_resolved_joint_name( positions_by_name: Mapping[str, float], local_joint_name: LocalModelJointName ) -> ResolvedJointName | None: @@ -76,3 +103,35 @@ def filter_joint_state_to_selected_joints( raise ValueError(f"IK result is missing selected joints: {missing}") return JointState({"name": list(resolved_joint_names), "position": selected_positions}) + + +def normalize_joint_target_for_group( + group: ResolvedPlanningGroup, + target: JointState, +) -> JointState: + """Normalize a group joint target to resolved joint names in group order.""" + if not target.name: + if len(target.position) != len(group.joint_names): + raise ValueError( + f"Target for '{group.id}' has {len(target.position)} positions, " + f"expected {len(group.joint_names)}" + ) + return JointState(name=list(group.joint_names), position=list(target.position)) + + positions_by_name = dict(zip(target.name, target.position, strict=False)) + resolved_positions: list[float] = [] + missing: list[str] = [] + for resolved_name, local_name in zip(group.joint_names, group.local_joint_names, strict=False): + if resolved_name in positions_by_name: + resolved_positions.append(positions_by_name[resolved_name]) + elif local_name in positions_by_name: + resolved_positions.append(positions_by_name[local_name]) + else: + missing.append(resolved_name) + if missing: + raise ValueError(f"Target for '{group.id}' is missing joints: {missing}") + + extra = set(target.name) - set(group.joint_names) - set(group.local_joint_names) + if extra: + raise ValueError(f"Target for '{group.id}' has extra joints: {sorted(extra)}") + return JointState(name=list(group.joint_names), position=resolved_positions) diff --git a/dimos/manipulation/planning/planning_groups.py b/dimos/manipulation/planning/planning_groups.py index ed082f42f0..2b8c362512 100644 --- a/dimos/manipulation/planning/planning_groups.py +++ b/dimos/manipulation/planning/planning_groups.py @@ -80,12 +80,14 @@ def parse_srdf_planning_groups( model: ModelDescription, controllable_joint_names: list[str], ) -> list[PlanningGroupDefinition]: - """Parse supported SRDF planning group definitions. + """Extract supported SRDF planning group definitions. Supported forms are a single ```` child or an ordered list of ```` children. Other forms, including SRDF ```` metadata, are ignored for planning group - extraction. + extraction. This is intentionally a minimal SRDF group extractor rather + than a full SRDF parser; adopting a ROS/MoveIt parser such as srdfdom would + add substantial dependency overhead for this narrow subset. """ root = ET.parse(srdf_path).getroot() groups: list[PlanningGroupDefinition] = [] diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index b607fe6148..b4c98d2275 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -32,15 +32,16 @@ class RobotModelConfig(ModuleConfig): name: Human-readable robot name model_path: Path to robot model file (.urdf, .xacro, or .xml/MJCF) srdf_path: Optional path to SRDF file containing planning group definitions - base_pose: Deprecated planning placement transform retained for - compatibility. Prefer encoding placement in the robot model. + base_pose: Compatibility placement transform used by current Drake + world loading/welding. Prefer encoding new placement in the robot + model when possible. joint_names: Ordered list of controllable/coordinator joints in the local model namespace. This is not a planning group. - end_effector_link: Deprecated robot-scoped end-effector link retained - for compatibility. Pose targets should use planning group target - frames instead. - base_link: Deprecated robot-scoped base link retained for Drake weld - compatibility. Planning groups own chain base links. + end_effector_link: Compatibility robot-scoped end-effector link used by + legacy helpers. New pose-targeted planning should use planning + group target frames instead. + base_link: Compatibility robot-scoped base link used by current Drake + weld/placement behavior. Planning groups own chain base links. package_paths: Dict mapping package names to filesystem Paths joint_limits_lower: Lower joint limits (radians) joint_limits_upper: Upper joint limits (radians) diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index 729b5a7ac6..8bd91070ce 100644 --- a/dimos/manipulation/planning/spec/models.py +++ b/dimos/manipulation/planning/spec/models.py @@ -63,7 +63,9 @@ class PlanningGroupDefinition: """Model-level declaration of a planning group. Joint names are local model names. The definition is not bound to a world - robot ID and is safe to store on RobotModelConfig. + robot ID and is safe to store on RobotModelConfig. Definitions are parsed + from SRDF or conservative fallback generation before any robot instance is + added to a planning world. """ name: str @@ -80,7 +82,12 @@ def has_pose_target(self) -> bool: @dataclass(frozen=True) class PlanningGroupDescriptor: - """Read-only public snapshot for an available planning group.""" + """Read-only public snapshot for an available planning group. + + Descriptors are returned by query APIs. They expose stable public IDs and + resolved joint names for callers, but intentionally do not expose backend + runtime IDs or mutable world state. + """ id: PlanningGroupID robot_name: RobotName @@ -99,7 +106,12 @@ def has_pose_target(self) -> bool: @dataclass(frozen=True) class ResolvedPlanningGroup: - """Runtime/world-bound planning group data.""" + """Runtime/world-bound planning group data. + + Resolved groups are created from descriptors/IDs for a specific planning + world. They include the internal WorldRobotID and are the form consumed by + planners, IK backends, and group-scoped FK/Jacobian calls. + """ id: PlanningGroupID robot_id: WorldRobotID diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index ed20ce57e1..d36b5ab0c4 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -75,13 +75,23 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: ... def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: - """List available planning groups as immutable descriptor snapshots.""" + """List planning groups for robots currently added to this world. + + SRDF/fallback parsing creates model-level definitions before world + binding. This query returns world-level descriptor snapshots with stable + public IDs and resolved joint names. + """ ... def resolve_planning_groups( self, group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...] ) -> tuple[ResolvedPlanningGroup, ...]: - """Resolve planning group IDs against current world robot data.""" + """Resolve group IDs against this world's runtime robot bindings. + + Resolution is world-bound: it looks up the robots added to this world, + attaches WorldRobotID, validates selected groups, and returns data that + backend planners/IK can use with world contexts. + """ ... def get_joint_limits( @@ -177,10 +187,7 @@ def get_group_jacobian(self, ctx: Any, group_id: PlanningGroupID) -> NDArray[np. ... def get_ee_pose(self, ctx: Any, robot_id: WorldRobotID) -> PoseStamped: - """Get robot-scoped end-effector pose. - - Deprecated: use get_group_pose() with an explicit planning group ID. - """ + """Get pose for a robot's unique pose-targetable planning group.""" ... def get_link_pose( @@ -190,10 +197,7 @@ def get_link_pose( ... def get_jacobian(self, ctx: Any, robot_id: WorldRobotID) -> NDArray[np.float64]: - """Get robot-scoped end-effector Jacobian (6 x n_joints). - - Deprecated: use get_group_jacobian() with an explicit planning group ID. - """ + """Get Jacobian for a robot's unique pose-targetable planning group.""" ... diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index d0fb4d580e..32d1d8fcf6 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -117,8 +117,8 @@ class _RobotData: config: RobotModelConfig model_instance: Any # ModelInstanceIndex joint_indices: list[int] # Indices into plant's position vector - ee_frame: Any | None # Deprecated robot-scoped end-effector frame - base_frame: Any # Deprecated robot-scoped base frame + ee_frame: Any | None # Compatibility robot-scoped end-effector frame + base_frame: Any # Compatibility robot-scoped base frame preview_model_instance: Any = None # ModelInstanceIndex for preview (yellow) robot preview_joint_indices: list[int] = field(default_factory=list) @@ -227,8 +227,9 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: """Add a robot to the world. Returns robot_id. Same model_path + base_pose reuses the model instance (e.g. two arms in one URDF). - base_pose/base_link/end_effector_link are deprecated compatibility fields; - planning should use planning-group base/tip links. + base_pose/base_link/end_effector_link remain compatibility fields for + placement and robot-scoped helpers; group-aware planning should use + planning group base/tip links. """ if self._finalized: raise RuntimeError("Cannot add robot after world is finalized") @@ -265,7 +266,7 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: return robot_id def _legacy_ee_frame(self, config: RobotModelConfig, model_instance: Any) -> Any | None: - """Resolve deprecated robot-scoped EE frame, if available.""" + """Resolve compatibility robot-scoped EE frame, if available.""" if config.end_effector_link is None: return None return self._plant.GetBodyByName(config.end_effector_link, model_instance).body_frame() @@ -1085,10 +1086,7 @@ def get_group_pose(self, ctx: Context, group_id: PlanningGroupID) -> PoseStamped return _pose_stamped_from_drake_transform(tip_pose) def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: - """Get robot-scoped end-effector pose. - - Deprecated: use get_group_pose() with an explicit planning group ID. - """ + """Get pose for a robot's compatibility end-effector frame.""" if not self._finalized: raise RuntimeError("World must be finalized first") @@ -1131,9 +1129,7 @@ def get_link_pose( return result # type: ignore[no-any-return] def get_jacobian(self, ctx: Context, robot_id: WorldRobotID) -> NDArray[np.float64]: - """Get robot-scoped geometric Jacobian (6 x n_joints). - - Deprecated: use get_group_jacobian() with an explicit planning group ID. + """Get robot-scoped geometric Jacobian for the compatibility EE frame. Rows: [vx, vy, vz, wx, wy, wz] (linear, then angular) """ diff --git a/dimos/robot/config.py b/dimos/robot/config.py index e701099480..8dabdf24e1 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -55,7 +55,8 @@ class RobotConfig(BaseModel): # Required fields name: str model_path: Path | None = None - end_effector_link: str | None = None # Deprecated for planning; use planning group tip links. + # Compatibility robot-scoped target frame; new planning uses group tip links. + end_effector_link: str | None = None # Physical dimensions (meters) height_clearance: float | None = None # max height @@ -77,13 +78,13 @@ class RobotConfig(BaseModel): # Optional overrides (derived from model if not set) joint_names: list[str] | None = None base_link: str | None = ( - None # Deprecated planning override; derived from model root when absent. + None # Compatibility planning override; derived from model root when absent. ) home_joints: list[float] | None = None # Multi-robot / coordinator joint_prefix: str | None = None # defaults to "{name}_" - # Deprecated for planning placement. Prefer encoding placement in URDF/xacro/MJCF. + # Compatibility planning placement. Prefer encoding placement in URDF/xacro/MJCF. base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # Planning diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 3b373e4daf..a90dff7a80 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -561,8 +561,8 @@ def _make_yourarm_config( source="fallback", ) ], - base_pose=_make_base_pose(y=y_offset), # Deprecated; prefer model placement - base_link="base_link", # Deprecated robot-scoped compatibility + base_pose=_make_base_pose(y=y_offset), # Compatibility; prefer model placement + base_link="base_link", # Compatibility robot-scoped base package_paths={"yourarm_description": _YOURARM_PACKAGE_PATH}, xacro_args={}, # Xacro arguments if using .xacro files collision_exclusion_pairs=[], # Pairs of links that can touch (e.g., gripper fingers) @@ -603,9 +603,9 @@ yourarm_planner = manipulation_module( | `coordinator_task_name` | Must match the `TaskConfig.name` in your coordinator blueprint | | `collision_exclusion_pairs` | List of `(link_a, link_b)` tuples for links that may legitimately touch (e.g., gripper fingers) | -`base_link`, `base_pose`, and `end_effector_link` are deprecated planning-level -fields. New planning code should use SRDF/planning-group chain base/tip links -and encode robot placement in the model. See +`base_link`, `base_pose`, and `end_effector_link` are compatibility fields used +by current placement and robot-scoped helper paths. New planning code should use +SRDF/planning-group chain base/tip links and encode robot placement in the model. See [Planning Groups](/docs/capabilities/manipulation/planning_groups.md). ## Step 5: Register Blueprints diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md index f5ab9cecd3..80dc9e6e0f 100644 --- a/docs/capabilities/manipulation/planning_groups.md +++ b/docs/capabilities/manipulation/planning_groups.md @@ -109,13 +109,14 @@ Multi-task dispatch is not atomic in this change: if one trajectory task accepts and a later task rejects, DimOS reports the rejection but does not roll back the accepted task. -## Deprecated planning config fields +## Compatibility planning config fields `RobotConfig.base_link`, `RobotConfig.base_pose`, `RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and `RobotModelConfig.end_effector_link` remain as compatibility fields for the -current Drake weld and deprecated robot-scoped FK/Jacobian APIs. New planning -logic should use model/SRDF structure and planning group base/tip links instead. +current Drake weld/placement behavior and robot-scoped compatibility helpers. +New planning logic should use model/SRDF structure and planning group base/tip +links instead. Robot placement should be encoded in URDF/xacro/MJCF. `joint_names` remains supported and should describe the controllable/coordinator joint set. diff --git a/openspec/changes/add-planning-groups/design.md b/openspec/changes/add-planning-groups/design.md index 3f24a3069b..cb1fa7e8fb 100644 --- a/openspec/changes/add-planning-groups/design.md +++ b/openspec/changes/add-planning-groups/design.md @@ -536,7 +536,7 @@ Simulation and replay should mirror hardware behavior because group resolution a - **SRDF scope:** Parse `` only; ignore ``. - **Unsupported SRDF:** Skip unsupported group forms with warnings. - **SRDF discovery:** Explicit path, then warning auto-discovery, then fallback, then error. -- **Config fields:** Add `srdf_path`; remove planning-level `base_link`, `end_effector_link`, and `base_pose` in the new design. +- **Config fields:** Add `srdf_path`; keep `base_link`, `end_effector_link`, and `base_pose` as explicit compatibility fields while new planning code uses planning-group base/tip links and model-owned placement. - **Robot placement:** Placement belongs in URDF/model, not separate planning config transforms. - **FK/Jacobian:** Replace robot-scoped end-effector APIs with group-scoped APIs. - **Cross-robot planning:** Interface allows it; backends may report unsupported. diff --git a/openspec/changes/add-planning-groups/tasks.md b/openspec/changes/add-planning-groups/tasks.md index b706372e11..e8523fc4bd 100644 --- a/openspec/changes/add-planning-groups/tasks.md +++ b/openspec/changes/add-planning-groups/tasks.md @@ -58,8 +58,8 @@ ## 6. Robot config migration and API cleanup -- [x] 6.1 Remove or deprecate planning-level `RobotConfig.base_link` and `RobotConfig.base_pose` usage. -- [x] 6.2 Remove or deprecate planning-level `RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and `RobotModelConfig.end_effector_link` usage. +- [x] 6.1 Reframe planning-level `RobotConfig.base_link` and `RobotConfig.base_pose` usage as active compatibility behavior. +- [x] 6.2 Reframe planning-level `RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and `RobotModelConfig.end_effector_link` usage as active compatibility behavior. - [x] 6.3 Keep and document `RobotConfig.joint_names` and `RobotModelConfig.joint_names` as controllable/coordinator joint sets, not planning groups. - [x] 6.4 Update existing robot catalog/config entries to use SRDF where needed or rely on fallback for unambiguous single-chain arms. - [x] 6.5 Update manipulation skills/wrappers to select planning groups explicitly or provide clear wrapper-level defaults. From 31731b1ac871d8ff0857ec5544ec360c72e6a4ab Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 17 Jun 2026 23:16:52 -0700 Subject: [PATCH 08/27] feat: dual xarm example --- dimos/manipulation/blueprints.py | 49 ++++++++++++++++++++++++++++++ dimos/robot/all_blueprints.py | 1 + dimos/robot/test_all_blueprints.py | 1 + 3 files changed, 51 insertions(+) diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 14409bab7d..b8be3b5216 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -25,6 +25,10 @@ # 3. Interactive RPC client (plan, preview, execute from Python): dimos run xarm7-planner-coordinator python -i -m dimos.manipulation.planning.examples.manipulation_client + + # 4. Dual-arm xArm7 mock planner + coordinator demo: + dimos run dual-xarm7-planner-coordinator + python -i -m dimos.manipulation.planning.examples.manipulation_client """ import math @@ -121,6 +125,50 @@ ) +# Dual XArm7 mock planner + coordinator for bimanual planning demos. +# Usage: dimos run dual-xarm7-planner-coordinator +_left_xarm7_cfg = _catalog_xarm7( + name="left_arm", + adapter_type="mock", + add_gripper=False, + y_offset=0.5, +) +_right_xarm7_cfg = _catalog_xarm7( + name="right_arm", + adapter_type="mock", + add_gripper=False, + y_offset=-0.5, +) + +dual_xarm7_planner_coordinator = autoconnect( + ManipulationModule.blueprint( + robots=[ + _left_xarm7_cfg.to_robot_model_config(), + _right_xarm7_cfg.to_robot_model_config(), + ], + planning_timeout=10.0, + enable_viz=True, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + _left_xarm7_cfg.to_hardware_component(), + _right_xarm7_cfg.to_hardware_component(), + ], + tasks=[ + _left_xarm7_cfg.to_task_config(), + _right_xarm7_cfg.to_task_config(), + ], + ), +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } +) + + # XArm7 planner + LLM agent for testing base ManipulationModule skills # No perception — uses the base module's planning + gripper skills only. # Usage: dimos run coordinator-mock, then dimos run xarm7-planner-coordinator-agent @@ -339,6 +387,7 @@ __all__ = [ "dual_xarm6_planner", + "dual_xarm7_planner_coordinator", "xarm6_planner_only", "xarm7_planner_coordinator", "xarm7_planner_coordinator_agent", diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 24d90dd7fe..7d6dd776f6 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -58,6 +58,7 @@ "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner", + "dual-xarm7-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm7_planner_coordinator", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints:keyboard_teleop_a750", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm", "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm_mock", diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index cdf72e9b6b..ac900e67ea 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -50,6 +50,7 @@ "coordinator-xarm6", "coordinator-xarm7", "dual-xarm6-planner", + "dual-xarm7-planner-coordinator", "teleop-hosted-go2", "teleop-hosted-xarm7", "teleop-quest-dual", From fe3957fbb3a074f13a67d7ce5748087f1b671aa2 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 00:50:15 -0700 Subject: [PATCH 09/27] refactor: simplify manipulator joint naming --- CONTEXT.md | 34 ++++- dimos/manipulation/manipulation_module.py | 137 ++++++++++-------- dimos/manipulation/planning/README.md | 4 +- .../planning/kinematics/pink_ik.py | 10 +- .../planning/monitor/robot_state_monitor.py | 49 ++----- .../planning/monitor/world_monitor.py | 6 - .../planning/planners/rrt_planner.py | 25 ++-- .../planners/test_rrt_planner_selection.py | 1 + .../planning/planning_group_utils.py | 48 +++--- .../planning/planning_identifiers.py | 90 ++++++++---- dimos/manipulation/planning/spec/config.py | 32 ++-- dimos/manipulation/planning/spec/models.py | 10 +- dimos/manipulation/planning/spec/protocols.py | 2 +- .../planning/world/drake_world.py | 32 ++-- .../world/test_drake_world_planning_groups.py | 26 +--- .../manipulation/test_manipulation_module.py | 38 ++--- dimos/manipulation/test_manipulation_unit.py | 116 +++++---------- dimos/robot/catalog/openarm.py | 4 - dimos/robot/config.py | 47 ++---- dimos/robot/manipulators/xarm/blueprints.py | 4 +- .../manipulation/adding_a_custom_arm.md | 14 +- .../manipulation/planning_groups.md | 33 +++-- docs/capabilities/manipulation/readme.md | 2 +- 23 files changed, 344 insertions(+), 420 deletions(-) diff --git a/CONTEXT.md b/CONTEXT.md index 6597467a7f..a5af10041e 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -4,6 +4,18 @@ Shared vocabulary for DimOS robotics concepts. These terms define domain languag ## Language +**Robot Name**: +A stable planning-domain identity for a concrete robot/model instance, used in public planning group and flat joint-name scoping. +_Avoid_: World robot ID, hardware ID, namespace + +**World Robot ID**: +An internal planning-world handle for a robot after it has been added to a backend world. +_Avoid_: Robot name, hardware ID, joint namespace + +**Hardware ID**: +A control-layer routing identity for a hardware component. For manipulator robots, it normally matches the robot name at the coordinator boundary. +_Avoid_: Robot name when discussing planning semantics, world robot ID + **Planning Group**: A named selectable serial kinematic chain of robot joints used as the unit of manipulation planning. A planning group is defined by its chain/joints, not by end-effector metadata. _Avoid_: Move group, movegroup @@ -17,7 +29,7 @@ Separate metadata used for pose-targeted operations. For a planning group define _Avoid_: Planning group definition **Resolved Planning Group**: -A planning group after model-level declarations have been bound to a concrete robot, namespace, and planning world. +A planning group after model-level declarations have been bound to a concrete robot name and planning world. _Avoid_: Planning group config, robot ID **Planning Group Selection**: @@ -33,7 +45,7 @@ A planning request over one or more selected planning groups that is solved as o _Avoid_: Batch planning, independent planning **Planning Group ID**: -An API-level identifier for a planning group, always namespaced as `{robot_name}/{group_name}`. +An API-level identifier for a planning group, always written as `{robot_name}/{group_name}`. `/` is reserved as the delimiter and is not part of either component. _Avoid_: Bare group name, robot ID **Planning Group Descriptor**: @@ -41,20 +53,28 @@ A read-only snapshot returned by query APIs that describes an available planning _Avoid_: Live planning group handle, resolved planning group **Joint State**: -A resolved-joint-name-keyed robot state that can represent any set of joints and is not inherently coupled to a robot, planning group, or planning group selection. +A joint-name-keyed robot state that can represent any set of joints and is not inherently coupled to a robot, planning group, planning group selection, or joint-name scope. At flat multi-robot or coordinator boundaries, joint names are required and are global joint names. Robot identity and local-vs-global meaning are provided by the API boundary or containing type, not by extra fields on the generic joint state. _Avoid_: Planning-group-scoped state **Robot Model Joint Names**: -The objective set of controllable joints exposed by a robot coordinator for state and command. This usually aligns with the model's actuated joints, but is not itself a planning group. +The ordered controllable joints of a robot model in the model's local namespace. This usually aligns with the model's actuated joints, but is not itself a planning group. _Avoid_: Implicit planning group **Local Model Joint Name**: A joint name as it appears inside a robot model or SRDF before the model is bound to a concrete robot in a planning world. _Avoid_: Runtime joint name, coordinator joint name -**Resolved Joint Name**: -A world-level joint name exposed above the model parsing layer, always namespaced as `{robot_name}/{local_joint_name}` so it is stable and unique within a planning world. -_Avoid_: Bare joint name, local joint name +**Robot-Scoped Joint State**: +A single-robot joint state whose robot identity is explicit outside the generic joint state. Robot-scoped APIs may accept unnamed ordered joint vectors in robot model joint order; when joint names are present, they are local model joint names because the robot identity is already explicit. +_Avoid_: Namespaced local joint state, prefixed joint state + +**Generated Plan**: +A flat planning result that may contain one or more robots. Joint states in a generated plan require names and use global joint names so the plan remains unambiguous across robot boundaries. +_Avoid_: Robot-scoped joint plan, local joint plan + +**Global Joint Name**: +A boundary-level joint name that mechanically combines a robot name and local model joint name as `{robot_name}/{local_joint_name}` so it is stable and unique in flat joint-state representations, even when the local model joint name is already descriptive. `/` is reserved as the delimiter and is not part of either component. +_Avoid_: Resolved joint name, coordinator joint name, bare joint name, local joint name **Robot Placement**: The placement of a robot model within the planning world. Robot placement belongs in the robot model description rather than in a separate planning configuration transform. diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 535e7ff3f3..588834996c 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -53,7 +53,12 @@ primary_pose_planning_group_id_for_robot, single_planning_group_id_for_robot, ) -from dimos.manipulation.planning.planning_identifiers import make_resolved_joint_name +from dimos.manipulation.planning.planning_identifiers import ( + assert_global_joint_names, + assert_local_joint_names, + make_global_joint_name, + make_global_joint_names, +) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType from dimos.manipulation.planning.spec.models import ( @@ -281,27 +286,33 @@ def _get_robot( def _on_joint_state(self, msg: JointState) -> None: """Callback when joint state received from driver. - Splits the aggregated JointState by robot using each robot's - coordinator joint names, then routes to the correct monitor. + Splits the aggregated global JointState by robot, then routes local + robot-scoped states to the correct monitor. """ try: if self._world_monitor is None: return + if not msg.name: + raise ValueError("Aggregate joint states must include global joint names") + assert_global_joint_names(msg.name) + # Build name → index map once for the whole message name_to_idx = {name: i for i, name in enumerate(msg.name)} for robot_name, (robot_id, config, _) in self._robots.items(): - coord_names = config.get_coordinator_joint_names() - indices = [name_to_idx.get(cn) for cn in coord_names] + global_names = make_global_joint_names(robot_name, config.joint_names) + indices = [name_to_idx.get(global_name) for global_name in global_names] if any(idx is None for idx in indices): missing = [ - cn for cn, idx in zip(coord_names, indices, strict=False) if idx is None + name + for name, idx in zip(global_names, indices, strict=False) + if idx is None ] logger.warning(f"Skipping '{robot_name}': missing joints {missing}") continue - # Build per-robot sub-message (coordinator namespace) + # Build per-robot sub-message (local model namespace) sub_positions = [msg.position[idx] for idx in indices] # type: ignore[index] sub_velocities = ( [msg.velocity[idx] for idx in indices] # type: ignore[index] @@ -309,7 +320,7 @@ def _on_joint_state(self, msg: JointState) -> None: else [] ) sub_msg = JointState( - name=list(coord_names), + name=list(config.joint_names), position=sub_positions, velocity=sub_velocities, ) @@ -494,7 +505,7 @@ def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGro ) def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: - """Collect current state for exactly the selected resolved joints.""" + """Collect current state for exactly the selected global joints.""" assert self._world_monitor is not None resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) names: list[str] = [] @@ -530,7 +541,7 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint def _normalize_joint_target( self, group_id: PlanningGroupID, target: JointState ) -> JointState | None: - """Normalize a group joint target to resolved joint names in group order.""" + """Normalize a group joint target to global joint names in group order.""" assert self._world_monitor is not None group = self._world_monitor.world.resolve_planning_groups((group_id,))[0] try: @@ -542,13 +553,13 @@ def _normalize_joint_target( def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotName) -> JointPath: """Project combined plan path to one robot in configured local joint order. - Generated plans only contain selected resolved joints. Trajectory tasks may + Generated plans only contain selected global joints. Trajectory tasks may still be configured for the robot's full controllable joint set, so non-selected joints are held at their current positions during projection. """ robot_id, config, _ = self._robots[robot_name] - resolved_joint_names = [ - make_resolved_joint_name(robot_name, joint) for joint in config.joint_names + global_joint_names = [ + make_global_joint_name(robot_name, joint) for joint in config.joint_names ] current_by_name: dict[str, float] = {} if self._world_monitor is not None: @@ -559,20 +570,20 @@ def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotNam for waypoint in plan.path: position_by_name = dict(zip(waypoint.name, waypoint.position, strict=False)) positions: list[float] = [] - for local_name, resolved_name in zip( - config.joint_names, resolved_joint_names, strict=False + for local_name, global_name in zip( + config.joint_names, global_joint_names, strict=False ): - if resolved_name in position_by_name: - positions.append(position_by_name[resolved_name]) - elif resolved_name in current_by_name: - positions.append(current_by_name[resolved_name]) + if global_name in position_by_name: + positions.append(position_by_name[global_name]) + elif global_name in current_by_name: + positions.append(current_by_name[global_name]) elif local_name in current_by_name: positions.append(current_by_name[local_name]) else: logger.error( "Cannot project plan for '%s': missing joint '%s'", robot_name, - resolved_name, + global_name, ) return [] projected.append( @@ -594,7 +605,7 @@ def _trajectory_for_robot_plan( _, config, traj_gen = self._robots[robot_name] trajectory = traj_gen.generate([list(state.position) for state in projected_path]) return JointTrajectory( - joint_names=list(config.joint_names), + joint_names=make_global_joint_names(robot_name, config.joint_names), points=trajectory.points, timestamp=trajectory.timestamp, ) @@ -1125,7 +1136,6 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> dict[str, Any] "base_link": config.base_link, "max_velocity": config.max_velocity, "max_acceleration": config.max_acceleration, - "has_joint_name_mapping": bool(config.joint_name_mapping), "coordinator_task_name": config.coordinator_task_name, "home_joints": config.home_joints, "pre_grasp_offset": config.pre_grasp_offset, @@ -1157,13 +1167,46 @@ def set_init_joints(self, joint_state: JointState, robot_name: RobotName | None robot = self._get_robot(robot_name) if robot is None: return False - self._init_joints[robot[0]] = joint_state + robot_name_resolved, _, config, _ = robot + try: + normalized = self._local_robot_joint_state(config, joint_state) + except ValueError as exc: + logger.error(str(exc)) + return False + self._init_joints[robot_name_resolved] = normalized logger.info( - f"Init joints set for '{robot[0]}': " - f"[{', '.join(f'{j:.3f}' for j in joint_state.position)}]" + f"Init joints set for '{robot_name_resolved}': " + f"[{', '.join(f'{j:.3f}' for j in normalized.position)}]" ) return True + def _local_robot_joint_state( + self, config: RobotModelConfig, joint_state: JointState + ) -> JointState: + """Normalize a robot-scoped joint state to local model joint order.""" + if not joint_state.name: + if len(joint_state.position) != len(config.joint_names): + raise ValueError( + f"JointState has {len(joint_state.position)} positions, " + f"expected {len(config.joint_names)} for robot '{config.name}'" + ) + return JointState(name=list(config.joint_names), position=list(joint_state.position)) + + assert_local_joint_names(joint_state.name) + positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=False)) + missing = [name for name in config.joint_names if name not in positions_by_name] + if missing: + raise ValueError(f"JointState for robot '{config.name}' is missing joints: {missing}") + extra = set(joint_state.name) - set(config.joint_names) + if extra: + raise ValueError( + f"JointState for robot '{config.name}' has extra joints: {sorted(extra)}" + ) + return JointState( + name=list(config.joint_names), + position=[positions_by_name[name] for name in config.joint_names], + ) + @rpc def set_init_joints_to_current(self, robot_name: RobotName | None = None) -> bool: """Set init joints to the current joint positions. @@ -1201,36 +1244,6 @@ def _get_coordinator_client(self) -> RPCClient | None: self._coordinator_client = RPCClient(None, ControlCoordinator) return self._coordinator_client - def _translate_trajectory_to_coordinator( - self, - trajectory: JointTrajectory, - robot_config: RobotModelConfig, - ) -> JointTrajectory: - """Translate trajectory joint names from URDF to coordinator namespace. - - Args: - trajectory: Trajectory with URDF joint names - robot_config: Robot config with joint name mapping - - Returns: - Trajectory with coordinator joint names - """ - if not robot_config.joint_name_mapping: - return trajectory # No translation needed - - # Translate joint names - coordinator_names = [ - robot_config.get_coordinator_joint_name(j) for j in trajectory.joint_names - ] - - # Create new trajectory with translated names - # Note: duration is computed automatically from points in JointTrajectory.__init__ - return JointTrajectory( - joint_names=coordinator_names, - points=trajectory.points, - timestamp=trajectory.timestamp, - ) - @rpc def execute(self, robot_name: RobotName | None = None) -> bool: """Execute planned trajectory via ControlCoordinator.""" @@ -1252,15 +1265,12 @@ def execute(self, robot_name: RobotName | None = None) -> bool: logger.error("No coordinator client") return False - translated = self._translate_trajectory_to_coordinator(traj, config) logger.info( - f"Executing: task='{config.coordinator_task_name}', {len(translated.points)} pts, {translated.duration:.2f}s" + f"Executing: task='{config.coordinator_task_name}', {len(traj.points)} pts, {traj.duration:.2f}s" ) self._state = ManipulationState.EXECUTING - result = client.task_invoke( - config.coordinator_task_name, "execute", {"trajectory": translated} - ) + result = client.task_invoke(config.coordinator_task_name, "execute", {"trajectory": traj}) if result: logger.info("Trajectory accepted") self._state = ManipulationState.COMPLETED @@ -1307,15 +1317,14 @@ def execute_plan( self._state = ManipulationState.EXECUTING for name, config, trajectory in dispatches: self._planned_trajectories[name] = trajectory - translated = self._translate_trajectory_to_coordinator(trajectory, config) logger.info( "Executing: task='%s', %d pts, %.2fs", config.coordinator_task_name, - len(translated.points), - translated.duration, + len(trajectory.points), + trajectory.duration, ) result = client.task_invoke( - config.coordinator_task_name, "execute", {"trajectory": translated} + config.coordinator_task_name, "execute", {"trajectory": trajectory} ) if not result: return self._fail("Coordinator rejected trajectory") diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index 6e5a53eb57..a1712411d7 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -70,7 +70,6 @@ config = RobotModelConfig( joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"], end_effector_link="link7", base_link="link_base", - joint_name_mapping={"arm_joint1": "joint1", ...}, # coordinator <-> URDF coordinator_task_name="traj_arm", ) @@ -93,12 +92,11 @@ module.execute() # Sends to coordinator | `name` | Robot identifier | | `model_path` | Path to URDF/XACRO file | | `base_pose` | PoseStamped for robot base in world frame | -| `joint_names` | Joint names in URDF | +| `joint_names` | Ordered controllable local model joint names | | `end_effector_link` | EE link name | | `base_link` | Base link name | | `max_velocity` | Max joint velocity (rad/s) | | `max_acceleration` | Max acceleration (rad/s²) | -| `joint_name_mapping` | Coordinator → URDF name mapping | | `coordinator_task_name` | Task name for execution RPC | | `package_paths` | ROS package paths for meshes | | `xacro_args` | Xacro arguments (e.g., `{"dof": "7"}`) | diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index 86a8d04241..e7a411ffdf 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -28,7 +28,7 @@ from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.planning_group_utils import ( filter_joint_state_to_selected_joints, - matching_resolved_joint_name, + matching_global_joint_name, planning_group_id_from_selector, ) from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -186,7 +186,7 @@ def solve_pose_targets( check_collision: bool = True, max_attempts: int = 10, ) -> IKResult: - """Solve a planning-group pose target and return only selected resolved joints.""" + """Solve a planning-group pose target and return only selected global joints.""" if not pose_targets: return _failure(IKStatus.NO_SOLUTION, "At least one pose target is required") @@ -502,7 +502,7 @@ def _build_joint_mapping(model: Any, config: RobotModelConfig) -> _JointMapping: model_joint_names: list[str] = [] for dimos_name in config.joint_names: - model_joint_name = config.get_urdf_joint_name(dimos_name) + model_joint_name = dimos_name joint_id = _get_joint_id(model, model_joint_name) joint = model.joints[joint_id] nq = int(getattr(joint, "nq", 1)) @@ -562,9 +562,9 @@ def _seed_positions_for_mapping(seed: JointState, mapping: _JointMapping) -> NDA elif model_name in positions_by_name: values.append(float(positions_by_name[model_name])) elif ( - resolved_name := matching_resolved_joint_name(positions_by_name, dimos_name) + global_name := matching_global_joint_name(positions_by_name, dimos_name) ) is not None: - values.append(float(positions_by_name[resolved_name])) + values.append(float(positions_by_name[global_name])) else: raise ValueError(f"Seed is missing joint '{dimos_name}' (URDF name '{model_name}')") return np.array(values, dtype=np.float64) diff --git a/dimos/manipulation/planning/monitor/robot_state_monitor.py b/dimos/manipulation/planning/monitor/robot_state_monitor.py index 49c6b56366..39492a80ac 100644 --- a/dimos/manipulation/planning/monitor/robot_state_monitor.py +++ b/dimos/manipulation/planning/monitor/robot_state_monitor.py @@ -69,7 +69,6 @@ def __init__( lock: threading.RLock, robot_id: str, joint_names: list[str], - joint_name_mapping: dict[str, str] | None = None, timeout: float = 1.0, ) -> None: """Create a world state monitor. @@ -78,10 +77,7 @@ def __init__( world: WorldSpec instance to sync state to lock: Shared lock for thread-safe access robot_id: ID of the robot to monitor - joint_names: Ordered list of joint names for this robot (URDF names) - joint_name_mapping: Maps coordinator joint names to URDF joint names. - Example: {"left/joint1": "joint1"} means messages with "left/joint1" - will be mapped to URDF "joint1". If None, names must match exactly. + joint_names: Ordered list of local model joint names for this robot timeout: Timeout for waiting for initial state (seconds) """ self._world = world @@ -90,11 +86,6 @@ def __init__( self._joint_names = joint_names self._timeout = timeout - # Joint name mapping: coordinator name -> URDF name - self._joint_name_mapping = joint_name_mapping or {} - # Build reverse mapping: URDF name -> coordinator name - self._reverse_mapping = {v: k for k, v in self._joint_name_mapping.items()} - # Latest state self._latest_positions: NDArray[np.float64] | None = None self._latest_velocities: NDArray[np.float64] | None = None @@ -190,30 +181,19 @@ def on_joint_state(self, msg: JointState) -> None: def _extract_positions(self, msg: JointState) -> NDArray[np.float64] | None: """Extract positions for our joints from JointState message. - Handles joint name translation from coordinator namespace to URDF namespace. - If joint_name_mapping is set, message names are looked up via the reverse mapping. - Args: - msg: JointState message (may use coordinator joint names) + msg: Robot-scoped JointState message with local model joint names Returns: Array of joint positions or None if any joint is missing """ - # Build name->index map from message (coordinator names) name_to_idx = {name: i for i, name in enumerate(msg.name)} positions = [] - for urdf_joint_name in self._joint_names: - # Try direct match first (when no mapping or names already match) - if urdf_joint_name in name_to_idx: - idx = name_to_idx[urdf_joint_name] - else: - # Try reverse mapping: URDF name -> coordinator name -> msg index - orch_name = self._reverse_mapping.get(urdf_joint_name) - if orch_name is None or orch_name not in name_to_idx: - return None # Missing joint - idx = name_to_idx[orch_name] - + for local_joint_name in self._joint_names: + idx = name_to_idx.get(local_joint_name) + if idx is None: + return None if idx >= len(msg.position): return None # Position not available positions.append(msg.position[idx]) @@ -223,7 +203,7 @@ def _extract_positions(self, msg: JointState) -> NDArray[np.float64] | None: def _extract_velocities(self, msg: JointState) -> NDArray[np.float64] | None: """Extract velocities for our joints. - Uses same name translation as _extract_positions. + Uses the same local-name lookup as _extract_positions. """ if not msg.velocity or len(msg.velocity) == 0: return None @@ -231,17 +211,10 @@ def _extract_velocities(self, msg: JointState) -> NDArray[np.float64] | None: name_to_idx = {name: i for i, name in enumerate(msg.name)} velocities = [] - for urdf_joint_name in self._joint_names: - # Try direct match first - if urdf_joint_name in name_to_idx: - idx = name_to_idx[urdf_joint_name] - else: - # Try reverse mapping - orch_name = self._reverse_mapping.get(urdf_joint_name) - if orch_name is None or orch_name not in name_to_idx: - return None - idx = name_to_idx[orch_name] - + for local_joint_name in self._joint_names: + idx = name_to_idx.get(local_joint_name) + if idx is None: + return None if idx >= len(msg.velocity): return None velocities.append(msg.velocity[idx]) diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 08dc5421e2..cb565b2b3f 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -123,7 +123,6 @@ def start_state_monitor( self, robot_id: WorldRobotID, joint_names: list[str] | None = None, - joint_name_mapping: dict[str, str] | None = None, ) -> None: """Start monitoring joint states. Uses config defaults if args are None.""" with self._lock: @@ -141,16 +140,11 @@ def start_state_monitor( else: joint_names = config.joint_names - # Get joint name mapping from config if not provided - if joint_name_mapping is None and config.joint_name_mapping: - joint_name_mapping = config.joint_name_mapping - monitor = RobotStateMonitor( world=self._world, lock=self._lock, robot_id=robot_id, joint_names=joint_names, - joint_name_mapping=joint_name_mapping, ) monitor.start() self._state_monitors[robot_id] = monitor diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 04e5b8528a..641dc8d62c 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -26,6 +26,10 @@ import numpy as np +from dimos.manipulation.planning.planning_identifiers import ( + local_joint_name_from_global, + make_global_joint_names, +) from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import ( JointPath, @@ -196,10 +200,10 @@ def plan_selected_joint_path( robot_id = next(iter(robot_ids)) robot_config = world.get_robot_config(robot_id) - full_resolved_joint_names = [ - f"{robot_config.name}/{joint_name}" for joint_name in robot_config.joint_names - ] - if selected_joint_names != full_resolved_joint_names: + full_global_joint_names = make_global_joint_names( + robot_config.name, robot_config.joint_names + ) + if selected_joint_names != full_global_joint_names: return _create_failure_result( PlanningStatus.UNSUPPORTED, "RRTConnectPlanner currently requires the selected groups to cover " @@ -636,10 +640,10 @@ def _validate_selected_groups_cover_full_robots( ) -> PlanningResult | None: for robot_id in robot_order: robot_config = world.get_robot_config(robot_id) - full_resolved_joint_names = [ - f"{robot_config.name}/{joint_name}" for joint_name in robot_config.joint_names - ] - if robot_joint_names[robot_id] != full_resolved_joint_names: + full_global_joint_names = make_global_joint_names( + robot_config.name, robot_config.joint_names + ) + if robot_joint_names[robot_id] != full_global_joint_names: return _create_failure_result( PlanningStatus.UNSUPPORTED, "RRTConnectPlanner currently requires selected groups to cover " @@ -664,11 +668,12 @@ def _combined_joint_limits( def _robot_joint_state_from_combined( combined_joint_names: list[str], combined_positions: NDArray[np.float64], + robot_name: str, robot_joint_names: list[str], ) -> JointState: position_by_name = dict(zip(combined_joint_names, combined_positions.tolist(), strict=True)) return JointState( - name=robot_joint_names, + name=[local_joint_name_from_global(robot_name, name) for name in robot_joint_names], position=[position_by_name[name] for name in robot_joint_names], ) @@ -688,6 +693,7 @@ def _coupled_config_collision_free( _robot_joint_state_from_combined( selected_joint_names, q, + world.get_robot_config(robot_id).name, robot_joint_names[robot_id], ), ) @@ -725,6 +731,7 @@ def _coupled_edge_collision_free( _robot_joint_state_from_combined( selected_joint_names, q, + world.get_robot_config(robot_id).name, robot_joint_names[robot_id], ), ) diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py index 52ca068919..5262916580 100644 --- a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -115,6 +115,7 @@ def scratch_context(self) -> nullcontext[dict[str, JointState]]: def set_joint_state( self, ctx: dict[str, JointState], robot_id: str, joint_state: JointState ) -> None: + assert joint_state.name == self._robot_configs[robot_id].joint_names ctx[robot_id] = joint_state def is_collision_free(self, ctx: dict[str, JointState], robot_id: str) -> bool: diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/planning_group_utils.py index a6eb161b2a..33cdf020b7 100644 --- a/dimos/manipulation/planning/planning_group_utils.py +++ b/dimos/manipulation/planning/planning_group_utils.py @@ -16,11 +16,12 @@ from collections.abc import Mapping, Sequence +from dimos.manipulation.planning.planning_identifiers import assert_local_joint_names from dimos.manipulation.planning.spec.models import ( + GlobalJointName, LocalModelJointName, PlanningGroupDescriptor, PlanningGroupID, - ResolvedJointName, ResolvedPlanningGroup, RobotName, ) @@ -61,10 +62,10 @@ def primary_pose_planning_group_id_for_robot( return None -def matching_resolved_joint_name( +def matching_global_joint_name( positions_by_name: Mapping[str, float], local_joint_name: LocalModelJointName -) -> ResolvedJointName | None: - """Find the unique resolved joint name ending with a local joint name.""" +) -> GlobalJointName | None: + """Find the unique global joint name ending with a local joint name.""" suffix = f"/{local_joint_name}" matches = [name for name in positions_by_name if name.endswith(suffix)] if len(matches) == 1: @@ -74,42 +75,42 @@ def matching_resolved_joint_name( def filter_joint_state_to_selected_joints( joint_state: JointState, - resolved_joint_names: Sequence[ResolvedJointName], + global_joint_names: Sequence[GlobalJointName], local_joint_names: Sequence[LocalModelJointName] = (), ) -> JointState: - """Project a joint state to selected resolved joints. + """Project a joint state to selected global joints. - Values are looked up by resolved name first. When ``local_joint_names`` is + Values are looked up by global name first. When ``local_joint_names`` is provided, each corresponding local name is used as a fallback. """ - if local_joint_names and len(resolved_joint_names) != len(local_joint_names): - raise ValueError("Resolved and local selected joint lists must have the same length") + if local_joint_names and len(global_joint_names) != len(local_joint_names): + raise ValueError("Global and local selected joint lists must have the same length") positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=True)) selected_positions: list[float] = [] missing: list[str] = [] - for index, resolved_name in enumerate(resolved_joint_names): - if resolved_name in positions_by_name: - selected_positions.append(float(positions_by_name[resolved_name])) + for index, global_name in enumerate(global_joint_names): + if global_name in positions_by_name: + selected_positions.append(float(positions_by_name[global_name])) continue if local_joint_names: local_name = local_joint_names[index] if local_name in positions_by_name: selected_positions.append(float(positions_by_name[local_name])) continue - missing.append(resolved_name) + missing.append(global_name) if missing: raise ValueError(f"IK result is missing selected joints: {missing}") - return JointState({"name": list(resolved_joint_names), "position": selected_positions}) + return JointState({"name": list(global_joint_names), "position": selected_positions}) def normalize_joint_target_for_group( group: ResolvedPlanningGroup, target: JointState, ) -> JointState: - """Normalize a group joint target to resolved joint names in group order.""" + """Normalize a robot-scoped group target to global joint names in group order.""" if not target.name: if len(target.position) != len(group.joint_names): raise ValueError( @@ -118,20 +119,19 @@ def normalize_joint_target_for_group( ) return JointState(name=list(group.joint_names), position=list(target.position)) + assert_local_joint_names(target.name) positions_by_name = dict(zip(target.name, target.position, strict=False)) - resolved_positions: list[float] = [] + global_positions: list[float] = [] missing: list[str] = [] - for resolved_name, local_name in zip(group.joint_names, group.local_joint_names, strict=False): - if resolved_name in positions_by_name: - resolved_positions.append(positions_by_name[resolved_name]) - elif local_name in positions_by_name: - resolved_positions.append(positions_by_name[local_name]) + for local_name in group.local_joint_names: + if local_name in positions_by_name: + global_positions.append(positions_by_name[local_name]) else: - missing.append(resolved_name) + missing.append(local_name) if missing: raise ValueError(f"Target for '{group.id}' is missing joints: {missing}") - extra = set(target.name) - set(group.joint_names) - set(group.local_joint_names) + extra = set(target.name) - set(group.local_joint_names) if extra: raise ValueError(f"Target for '{group.id}' has extra joints: {sorted(extra)}") - return JointState(name=list(group.joint_names), position=resolved_positions) + return JointState(name=list(group.joint_names), position=global_positions) diff --git a/dimos/manipulation/planning/planning_identifiers.py b/dimos/manipulation/planning/planning_identifiers.py index afbc473ed5..2f7c479e5e 100644 --- a/dimos/manipulation/planning/planning_identifiers.py +++ b/dimos/manipulation/planning/planning_identifiers.py @@ -12,22 +12,41 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Planning group and resolved-joint identifier helpers.""" +"""Planning and joint identifier helpers.""" from __future__ import annotations +from collections.abc import Sequence + from dimos.manipulation.planning.spec.models import ( + GlobalJointName, LocalModelJointName, PlanningGroupID, - ResolvedJointName, RobotName, ) +def assert_valid_robot_name(robot_name: RobotName) -> None: + """Validate a robot name for delimiter-based public IDs.""" + if not robot_name or "/" in robot_name: + raise ValueError(f"Invalid robot name: {robot_name!r}") + + +def assert_valid_local_joint_name(local_joint_name: LocalModelJointName) -> None: + """Validate a local model joint name for delimiter-based global joint names.""" + if not local_joint_name or "/" in local_joint_name: + raise ValueError(f"Invalid local joint name: {local_joint_name!r}") + + +def assert_local_joint_names(names: Sequence[LocalModelJointName]) -> None: + """Validate that names are local model joint names, not global joint names.""" + for name in names: + assert_valid_local_joint_name(name) + + def make_planning_group_id(robot_name: RobotName, group_name: str) -> PlanningGroupID: """Build a public planning group ID.""" - if not robot_name or "/" in robot_name: - raise ValueError(f"Invalid robot name for planning group ID: {robot_name!r}") + assert_valid_robot_name(robot_name) if not group_name or "/" in group_name: raise ValueError(f"Invalid planning group name: {group_name!r}") return f"{robot_name}/{group_name}" @@ -43,46 +62,65 @@ def parse_planning_group_id(group_id: PlanningGroupID) -> tuple[RobotName, str]: return parts[0], parts[1] -def make_resolved_joint_name( +def make_global_joint_name( robot_name: RobotName, local_joint_name: LocalModelJointName, -) -> ResolvedJointName: - """Convert a local model joint name to a public resolved joint name.""" - if not robot_name or "/" in robot_name: - raise ValueError(f"Invalid robot name for resolved joint name: {robot_name!r}") - if not local_joint_name or "/" in local_joint_name: - raise ValueError(f"Invalid local joint name: {local_joint_name!r}") +) -> GlobalJointName: + """Convert a local model joint name to a public global joint name.""" + assert_valid_robot_name(robot_name) + assert_valid_local_joint_name(local_joint_name) return f"{robot_name}/{local_joint_name}" -def make_resolved_joint_names( +def make_global_joint_names( robot_name: RobotName, local_joint_names: list[LocalModelJointName] | tuple[LocalModelJointName, ...], -) -> list[ResolvedJointName]: - """Convert local model joint names to public resolved joint names.""" - return [make_resolved_joint_name(robot_name, name) for name in local_joint_names] +) -> list[GlobalJointName]: + """Convert local model joint names to public global joint names.""" + return [make_global_joint_name(robot_name, name) for name in local_joint_names] + + +def is_global_joint_name(name: str) -> bool: + """Return whether name has the exact global joint-name shape.""" + parts = name.split("/") + return len(parts) == 2 and bool(parts[0]) and bool(parts[1]) + + +def assert_global_joint_names(names: Sequence[GlobalJointName]) -> None: + """Validate that names are global joint names.""" + invalid = [name for name in names if not is_global_joint_name(name)] + if invalid: + raise ValueError(f"Expected global joint names; got invalid names: {invalid}") -def local_joint_name_from_resolved( +def local_joint_name_from_global( robot_name: RobotName, - resolved_joint_name: ResolvedJointName, + global_joint_name: GlobalJointName, ) -> LocalModelJointName: - """Validate and strip a resolved joint name for backend internals.""" + """Validate and strip a global joint name for backend internals.""" + assert_valid_robot_name(robot_name) prefix = f"{robot_name}/" - if not resolved_joint_name.startswith(prefix): + if not global_joint_name.startswith(prefix): raise ValueError( - f"Resolved joint name {resolved_joint_name!r} does not belong to robot {robot_name!r}" + f"Global joint name {global_joint_name!r} does not belong to robot {robot_name!r}" ) - local_name = resolved_joint_name[len(prefix) :] - if not local_name or "/" in local_name: - raise ValueError(f"Invalid resolved joint name: {resolved_joint_name!r}") + local_name = global_joint_name[len(prefix) :] + try: + assert_valid_local_joint_name(local_name) + except ValueError as exc: + raise ValueError(f"Invalid global joint name: {global_joint_name!r}") from exc return local_name __all__ = [ - "local_joint_name_from_resolved", + "assert_global_joint_names", + "assert_local_joint_names", + "assert_valid_local_joint_name", + "assert_valid_robot_name", + "is_global_joint_name", + "local_joint_name_from_global", + "make_global_joint_name", + "make_global_joint_names", "make_planning_group_id", - "make_resolved_joint_name", - "make_resolved_joint_names", "parse_planning_group_id", ] diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index b4c98d2275..1365e1c2bc 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,6 +21,10 @@ from pydantic import Field from dimos.core.module import ModuleConfig +from dimos.manipulation.planning.planning_identifiers import ( + assert_local_joint_names, + assert_valid_robot_name, +) from dimos.manipulation.planning.spec.models import PlanningGroupDefinition from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -35,8 +39,8 @@ class RobotModelConfig(ModuleConfig): base_pose: Compatibility placement transform used by current Drake world loading/welding. Prefer encoding new placement in the robot model when possible. - joint_names: Ordered list of controllable/coordinator joints in the - local model namespace. This is not a planning group. + joint_names: Ordered list of controllable joints in the local model + namespace. This is not a planning group. end_effector_link: Compatibility robot-scoped end-effector link used by legacy helpers. New pose-targeted planning should use planning group target frames instead. @@ -53,9 +57,6 @@ class RobotModelConfig(ModuleConfig): links may legitimately overlap (e.g., mimic joints). max_velocity: Maximum joint velocity for trajectory generation (rad/s) max_acceleration: Maximum joint acceleration for trajectory generation (rad/s^2) - joint_name_mapping: Maps coordinator joint names to URDF joint names. - Example: {"left/joint1": "joint1"} means coordinator's "left/joint1" - corresponds to URDF's "joint1". If empty, names are assumed to match. coordinator_task_name: Task name for executing trajectories via coordinator RPC. If set, trajectories can be executed via execute_trajectory() RPC. """ @@ -79,7 +80,6 @@ class RobotModelConfig(ModuleConfig): max_velocity: float = 1.0 max_acceleration: float = 2.0 # Coordinator integration - joint_name_mapping: dict[str, str] = Field(default_factory=dict) coordinator_task_name: str | None = None gripper_hardware_id: str | None = None # TF publishing for extra links (e.g., camera mount) @@ -89,19 +89,7 @@ class RobotModelConfig(ModuleConfig): # Pre-grasp offset distance in meters (along approach direction) pre_grasp_offset: float = 0.10 - def get_urdf_joint_name(self, coordinator_name: str) -> str: - """Translate coordinator joint name to URDF joint name.""" - return self.joint_name_mapping.get(coordinator_name, coordinator_name) - - def get_coordinator_joint_name(self, urdf_name: str) -> str: - """Translate URDF joint name to coordinator joint name.""" - for coord_name, u_name in self.joint_name_mapping.items(): - if u_name == urdf_name: - return coord_name - return urdf_name - - def get_coordinator_joint_names(self) -> list[str]: - """Get joint names in coordinator namespace.""" - if not self.joint_name_mapping: - return self.joint_names - return [self.get_coordinator_joint_name(j) for j in self.joint_names] + def model_post_init(self, __context: object) -> None: + """Validate delimiter-based naming constraints.""" + assert_valid_robot_name(self.name) + assert_local_joint_names(self.joint_names) diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index 8bd91070ce..d7a226d325 100644 --- a/dimos/manipulation/planning/spec/models.py +++ b/dimos/manipulation/planning/spec/models.py @@ -45,7 +45,7 @@ LocalModelJointName: TypeAlias = str """Joint name as it appears in URDF/SRDF before world binding.""" -ResolvedJointName: TypeAlias = str +GlobalJointName: TypeAlias = str """Public joint name of the form {robot_name}/{local_joint_name}.""" JointPath: TypeAlias = "list[JointState]" @@ -85,14 +85,14 @@ class PlanningGroupDescriptor: """Read-only public snapshot for an available planning group. Descriptors are returned by query APIs. They expose stable public IDs and - resolved joint names for callers, but intentionally do not expose backend + global joint names for callers, but intentionally do not expose backend runtime IDs or mutable world state. """ id: PlanningGroupID robot_name: RobotName group_name: str - joint_names: tuple[ResolvedJointName, ...] + joint_names: tuple[GlobalJointName, ...] local_joint_names: tuple[LocalModelJointName, ...] base_link: str tip_link: str | None = None @@ -117,7 +117,7 @@ class ResolvedPlanningGroup: robot_id: WorldRobotID robot_name: RobotName group_name: str - joint_names: tuple[ResolvedJointName, ...] + joint_names: tuple[GlobalJointName, ...] local_joint_names: tuple[LocalModelJointName, ...] base_link: str tip_link: str | None = None @@ -133,7 +133,7 @@ def has_pose_target(self) -> bool: class GeneratedPlan: """Canonical generated planning artifact. - The path uses resolved joint names and contains exactly the selected joints. + The path uses global joint names and contains exactly the selected joints. Downstream preview/execution projections are computed lazily from this data. """ diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index d36b5ab0c4..851bc5a2e7 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -79,7 +79,7 @@ def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: SRDF/fallback parsing creates model-level definitions before world binding. This query returns world-level descriptor snapshots with stable - public IDs and resolved joint names. + public IDs and global joint names. """ ... diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 32d1d8fcf6..666672b048 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -26,10 +26,10 @@ import numpy as np from dimos.manipulation.planning.planning_identifiers import ( - local_joint_name_from_resolved, + assert_local_joint_names, + make_global_joint_name, + make_global_joint_names, make_planning_group_id, - make_resolved_joint_name, - make_resolved_joint_names, parse_planning_group_id, ) from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -360,9 +360,7 @@ def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: id=make_planning_group_id(config.name, group.name), robot_name=config.name, group_name=group.name, - joint_names=tuple( - make_resolved_joint_names(config.name, group.joint_names) - ), + joint_names=tuple(make_global_joint_names(config.name, group.joint_names)), local_joint_names=group.joint_names, base_link=group.base_link, tip_link=group.tip_link, @@ -392,14 +390,14 @@ def resolve_planning_groups( if group is None: raise KeyError(f"Unknown planning group ID: {group_id}") - resolved_joint_names = tuple( - make_resolved_joint_name(robot_name, local_name) for local_name in group.joint_names + global_joint_names = tuple( + make_global_joint_name(robot_name, local_name) for local_name in group.joint_names ) - for joint_name in resolved_joint_names: + for joint_name in global_joint_names: previous_group_id = seen_joints.get(joint_name) if previous_group_id is not None: raise ValueError( - "Selected planning groups overlap on resolved joint " + "Selected planning groups overlap on global joint " f"{joint_name}: {previous_group_id} and {group_id}" ) seen_joints[joint_name] = group_id @@ -410,7 +408,7 @@ def resolve_planning_groups( robot_id=robot_data.robot_id, robot_name=robot_name, group_name=group_name, - joint_names=resolved_joint_names, + joint_names=global_joint_names, local_joint_names=group.joint_names, base_link=group.base_link, tip_link=group.tip_link, @@ -946,9 +944,9 @@ def _positions_for_robot_state( if len(joint_state.name) != len(joint_state.position): raise ValueError("JointState name and position lengths must match") + assert_local_joint_names(joint_state.name) for name, position in zip(joint_state.name, joint_state.position, strict=False): - local_name = self._state_name_to_local(robot_data.config, name) - state_by_local_name[local_name] = float(position) + state_by_local_name[name] = float(position) missing = [name for name in local_joint_names if name not in state_by_local_name] if missing: @@ -957,12 +955,6 @@ def _positions_for_robot_state( ) return np.array([state_by_local_name[name] for name in local_joint_names], dtype=np.float64) - def _state_name_to_local(self, config: RobotModelConfig, joint_name: str) -> str: - try: - return local_joint_name_from_resolved(config.name, joint_name) - except ValueError: - return config.get_urdf_joint_name(joint_name) - def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: """Get robot joint state from given context.""" if not self._finalized: @@ -982,7 +974,7 @@ def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: positions = [float(full_positions[idx]) for idx in robot_data.joint_indices] return JointState( - name=make_resolved_joint_names(robot_data.config.name, robot_data.config.joint_names), + name=make_global_joint_names(robot_data.config.name, robot_data.config.joint_names), position=positions, ) diff --git a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py index d4d4f4867e..adc04aaae6 100644 --- a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -36,7 +36,6 @@ def _config( name: str, joint_names: list[str], groups: list[PlanningGroupDefinition], - joint_name_mapping: dict[str, str] | None = None, ) -> RobotModelConfig: return RobotModelConfig( name=name, @@ -46,7 +45,6 @@ def _config( end_effector_link="tool0", base_link="base_link", planning_groups=groups, - joint_name_mapping=joint_name_mapping or {}, ) @@ -76,7 +74,7 @@ def _arm_group(*joint_names: str) -> PlanningGroupDefinition: ) -def test_list_planning_groups_returns_stable_ids_and_resolved_joint_names() -> None: +def test_list_planning_groups_returns_stable_ids_and_global_joint_names() -> None: world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) groups = world.list_planning_groups() @@ -161,29 +159,21 @@ def test_resolve_planning_groups_overlapping_same_robot_groups_raise_value_error world.resolve_planning_groups(("left/arm", "left/wrist")) -def test_positions_for_robot_state_accepts_resolved_joint_names_in_config_order() -> None: +def test_positions_for_robot_state_accepts_local_joint_names_in_config_order() -> None: world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) - joint_state = JointState({"name": ["left/joint2", "left/joint1"], "position": [2.0, 1.0]}) + joint_state = JointState({"name": ["joint2", "joint1"], "position": [2.0, 1.0]}) positions = world._positions_for_robot_state("robot_1", joint_state) np.testing.assert_allclose(positions, np.array([1.0, 2.0])) -def test_positions_for_robot_state_falls_back_to_coordinator_mapping() -> None: - world = _world( - _config( - "left", - ["urdf_joint1", "urdf_joint2"], - [_arm_group("urdf_joint1", "urdf_joint2")], - joint_name_mapping={"coord_joint1": "urdf_joint1", "coord_joint2": "urdf_joint2"}, - ) - ) - joint_state = JointState({"name": ["coord_joint2", "coord_joint1"], "position": [2.0, 1.0]}) - - positions = world._positions_for_robot_state("robot_1", joint_state) +def test_positions_for_robot_state_rejects_global_joint_names() -> None: + world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) + joint_state = JointState({"name": ["left/joint2", "left/joint1"], "position": [2.0, 1.0]}) - np.testing.assert_allclose(positions, np.array([1.0, 2.0])) + with pytest.raises(ValueError, match="Invalid local joint name: 'left/joint2'"): + world._positions_for_robot_state("robot_1", joint_state) def test_group_pose_rejects_group_without_target_frame() -> None: diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index 06970258b7..bc33aaefa3 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -69,15 +69,6 @@ def _get_xarm7_config() -> RobotModelConfig: auto_convert_meshes=True, max_velocity=1.0, max_acceleration=2.0, - joint_name_mapping={ - "arm/joint1": "joint1", - "arm/joint2": "joint2", - "arm/joint3": "joint3", - "arm/joint4": "joint4", - "arm/joint5": "joint5", - "arm/joint6": "joint6", - "arm/joint7": "joint7", - }, coordinator_task_name="traj_arm", ) @@ -92,13 +83,13 @@ def joint_state_zeros(): """Create a JointState message with zeros for XArm7.""" return JointState( name=[ - "arm/joint1", - "arm/joint2", - "arm/joint3", - "arm/joint4", - "arm/joint5", - "arm/joint6", - "arm/joint7", + "test_arm/joint1", + "test_arm/joint2", + "test_arm/joint3", + "test_arm/joint4", + "test_arm/joint5", + "test_arm/joint6", + "test_arm/joint7", ], position=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], velocity=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], @@ -191,7 +182,6 @@ def test_robot_info(self, module): assert len(info["joint_names"]) == 7 assert info["end_effector_link"] == "link7" assert info["coordinator_task_name"] == "traj_arm" - assert info["has_joint_name_mapping"] is True def test_ee_pose(self, module, joint_state_zeros): """Test getting end-effector pose.""" @@ -204,20 +194,15 @@ def test_ee_pose(self, module, joint_state_zeros): assert hasattr(pose, "y") assert hasattr(pose, "z") - def test_trajectory_name_translation(self, module, joint_state_zeros): - """Test that trajectory joint names are translated for coordinator.""" + def test_planned_trajectory_uses_global_joint_names(self, module, joint_state_zeros): + """Test that planned trajectory joint names are global for coordinator.""" module._on_joint_state(joint_state_zeros) success = module.plan_to_joints(JointState(position=[0.05] * 7)) assert success is True traj = module._planned_trajectories["test_arm"] - robot_config = module._robots["test_arm"][1] - - translated = module._translate_trajectory_to_coordinator(traj, robot_config) - - for name in translated.joint_names: - assert name.startswith("arm_") # Should have arm_ prefix + assert traj.joint_names == [f"test_arm/joint{i}" for i in range(1, 8)] @pytest.mark.skipif(not _drake_available(), reason="Drake not installed") @@ -251,8 +236,7 @@ def test_execute_with_mock_coordinator(self, module, joint_state_zeros): assert method_name == "execute" trajectory = kwargs["trajectory"] assert len(trajectory.points) > 1 - # Joint names should be translated - assert all(n.startswith("arm_") for n in trajectory.joint_names) + assert trajectory.joint_names == [f"test_arm/joint{i}" for i in range(1, 8)] def test_execute_rejected_by_coordinator(self, module, joint_state_zeros): """Test handling of coordinator rejection.""" diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index d01d711cb6..a1f279d8be 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -63,8 +63,8 @@ def robot_config(): @pytest.fixture -def robot_config_with_mapping(): - """Create a robot config with joint name mapping (dual-arm scenario).""" +def left_robot_config(): + """Create a robot config for a scoped left arm.""" return RobotModelConfig( name="left_arm", model_path=Path("/path/to/robot.urdf"), @@ -72,11 +72,6 @@ def robot_config_with_mapping(): joint_names=["joint1", "joint2", "joint3"], end_effector_link="link_tcp", base_link="link_base", - joint_name_mapping={ - "left/joint1": "joint1", - "left/joint2": "joint2", - "left/joint3": "joint3", - }, coordinator_task_name="traj_left", ) @@ -347,27 +342,6 @@ def test_solve_ik_rpc_uses_explicit_seed(self, robot_config): module._world_monitor.get_current_joint_state.assert_not_called() -class TestJointNameTranslation: - """Test trajectory joint name translation for coordinator.""" - - def test_no_mapping_returns_original(self, robot_config, simple_trajectory): - """Without mapping, trajectory is returned unchanged.""" - module = _make_module() - - result = module._translate_trajectory_to_coordinator(simple_trajectory, robot_config) - assert result is simple_trajectory # Same object - - def test_mapping_translates_names(self, robot_config_with_mapping, simple_trajectory): - """With mapping, joint names are translated.""" - module = _make_module() - - result = module._translate_trajectory_to_coordinator( - simple_trajectory, robot_config_with_mapping - ) - assert result.joint_names == ["left/joint1", "left/joint2", "left/joint3"] - assert len(result.points) == 2 # Points preserved - - class TestExecute: """Test coordinator execution.""" @@ -398,7 +372,11 @@ def test_execute_success(self, robot_config, simple_trajectory): """Successful execute calls coordinator via task_invoke.""" module = _make_module() module._robots = {"test_arm": ("id", robot_config, MagicMock())} - module._planned_trajectories = {"test_arm": simple_trajectory} + global_trajectory = JointTrajectory( + joint_names=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + points=simple_trajectory.points, + ) + module._planned_trajectories = {"test_arm": global_trajectory} mock_client = MagicMock() mock_client.task_invoke.return_value = True @@ -407,7 +385,7 @@ def test_execute_success(self, robot_config, simple_trajectory): assert module.execute() is True assert module._state == ManipulationState.COMPLETED mock_client.task_invoke.assert_called_once_with( - "traj_arm", "execute", {"trajectory": simple_trajectory} + "traj_arm", "execute", {"trajectory": global_trajectory} ) def test_execute_rejected(self, robot_config, simple_trajectory): @@ -424,22 +402,6 @@ def test_execute_rejected(self, robot_config, simple_trajectory): assert module._state == ManipulationState.FAULT -class TestRobotModelConfigMapping: - """Test RobotModelConfig joint name mapping helpers.""" - - def test_bidirectional_mapping(self, robot_config_with_mapping): - """Test URDF <-> coordinator name translation.""" - config = robot_config_with_mapping - - # Coordinator -> URDF - assert config.get_urdf_joint_name("left/joint1") == "joint1" - assert config.get_urdf_joint_name("unknown") == "unknown" - - # URDF -> Coordinator - assert config.get_coordinator_joint_name("joint1") == "left/joint1" - assert config.get_coordinator_joint_name("unknown") == "unknown" - - def _make_module_with_monitor(*configs: RobotModelConfig) -> ManipulationModule: """Create a ManipulationModule with a mocked world monitor and robots configured.""" module = _make_module() @@ -473,7 +435,6 @@ def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: def _make_robot_config( name: str, joints: list[str], - coordinator_joints: list[str], task_name: str, ) -> RobotModelConfig: return RobotModelConfig( @@ -483,12 +444,11 @@ def _make_robot_config( joint_names=joints, end_effector_link="ee", base_link="base", - joint_name_mapping=dict(zip(coordinator_joints, joints, strict=True)), coordinator_task_name=task_name, ) -def _make_resolved_group( +def _make_global_group( robot_name: str, group_name: str, joints: list[str] ) -> ResolvedPlanningGroup: return ResolvedPlanningGroup( @@ -541,12 +501,12 @@ def _make_world_monitor_with_viz(viz: object | None) -> WorldMonitor: class TestOnJointState: """Test _on_joint_state routing, splitting, and init capture.""" - def test_routes_positions_to_monitor(self, robot_config_with_mapping): + def test_routes_positions_to_monitor(self, left_robot_config): """Joint positions from aggregated message are routed to the correct monitor.""" - module = _make_module_with_monitor(robot_config_with_mapping) + module = _make_module_with_monitor(left_robot_config) msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.1, 0.2, 0.3], velocity=[1.0, 2.0, 3.0], ) @@ -556,13 +516,14 @@ def test_routes_positions_to_monitor(self, robot_config_with_mapping): module._world_monitor.on_joint_state.assert_called_once() call_args = module._world_monitor.on_joint_state.call_args sub_msg = call_args[0][0] + assert sub_msg.name == ["joint1", "joint2", "joint3"] assert sub_msg.position == [0.1, 0.2, 0.3] assert sub_msg.velocity == [1.0, 2.0, 3.0] assert call_args[1]["robot_id"] == "robot_left_arm" - def test_skips_robot_with_missing_joints(self, robot_config_with_mapping): + def test_skips_robot_with_missing_joints(self, left_robot_config): """Robots whose joints are absent from the message are skipped.""" - module = _make_module_with_monitor(robot_config_with_mapping) + module = _make_module_with_monitor(left_robot_config) # Message has none of left_arm's joints msg = JointState( @@ -573,12 +534,12 @@ def test_skips_robot_with_missing_joints(self, robot_config_with_mapping): module._world_monitor.on_joint_state.assert_not_called() - def test_captures_init_joints_on_first_call(self, robot_config_with_mapping): + def test_captures_init_joints_on_first_call(self, left_robot_config): """First joint state is stored as init joints; subsequent calls don't overwrite.""" - module = _make_module_with_monitor(robot_config_with_mapping) + module = _make_module_with_monitor(left_robot_config) first_msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.1, 0.2, 0.3], ) module._on_joint_state(first_msg) @@ -587,7 +548,7 @@ def test_captures_init_joints_on_first_call(self, robot_config_with_mapping): # Second call should NOT overwrite second_msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.9, 0.8, 0.7], ) module._on_joint_state(second_msg) @@ -602,7 +563,6 @@ def test_multi_robot_splits_correctly(self): joint_names=["j1", "j2"], end_effector_link="ee", base_link="base", - joint_name_mapping={"left/j1": "j1", "left/j2": "j2"}, coordinator_task_name="traj_left", ) right_config = RobotModelConfig( @@ -612,7 +572,6 @@ def test_multi_robot_splits_correctly(self): joint_names=["j1", "j2"], end_effector_link="ee", base_link="base", - joint_name_mapping={"right/j1": "j1", "right/j2": "j2"}, coordinator_task_name="traj_right", ) module = _make_module_with_monitor(left_config, right_config) @@ -636,15 +595,15 @@ def test_multi_robot_splits_correctly(self): assert calls["robot_left"].velocity == [0.1, 0.2] assert calls["robot_right"].velocity == [0.3, 0.4] - def test_no_monitor_returns_early(self, robot_config_with_mapping): + def test_no_monitor_returns_early(self, left_robot_config): """When world_monitor is None, _on_joint_state returns without error.""" module = _make_module() - module._robots = {"left_arm": ("id", robot_config_with_mapping, MagicMock())} + module._robots = {"left_arm": ("id", left_robot_config, MagicMock())} module._world_monitor = None # Should not raise msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.1, 0.2, 0.3], ) module._on_joint_state(msg) @@ -782,10 +741,10 @@ def test_preview_path_returns_false_for_missing_inputs(self): class TestGeneratedPlanProjection: def test_selected_joint_state_accepts_local_current_state_names(self): - config = _make_robot_config("left", ["j1", "j2"], ["c/j1", "c/j2"], "task") + config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_resolved_group("left", "arm", ["j1", "j2"]) + _make_global_group("left", "arm", ["j1", "j2"]) ] module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1", "j2"], position=[1.0, 2.0] @@ -801,24 +760,21 @@ def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): left_config = _make_robot_config( "left", ["j1", "j2", "j3"], - ["left_task/j1", "left_task/j2", "left_task/j3"], "left_task", ) - right_config = _make_robot_config( - "right", ["j1", "j2"], ["right_task/j1", "right_task/j2"], "right_task" - ) + right_config = _make_robot_config("right", ["j1", "j2"], "right_task") module = _make_module_with_monitor(left_config, right_config) left_gen = _trajectory_generator() right_gen = _trajectory_generator() module._robots["left"] = ("robot_left", left_config, left_gen) module._robots["right"] = ("robot_right", right_config, right_gen) module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_resolved_group("left", "arm", ["j1", "j2"]), - _make_resolved_group("right", "arm", ["j1"]), + _make_global_group("left", "arm", ["j1", "j2"]), + _make_global_group("right", "arm", ["j1"]), ] module._world_monitor.get_current_joint_state.side_effect = [ - JointState(name=["left/j1", "left/j2", "left/j3"], position=[0.0, 0.0, 9.0]), - JointState(name=["right/j1", "right/j2"], position=[0.0, 8.0]), + JointState(name=["j1", "j2", "j3"], position=[0.0, 0.0, 9.0]), + JointState(name=["j1", "j2"], position=[0.0, 8.0]), ] module._coordinator_client = MagicMock() module._coordinator_client.task_invoke.return_value = True @@ -830,21 +786,21 @@ def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): left_call, right_call = module._coordinator_client.task_invoke.call_args_list assert left_call.args[0:2] == ("left_task", "execute") left_trajectory = left_call.args[2]["trajectory"] - assert left_trajectory.joint_names == ["left_task/j1", "left_task/j2", "left_task/j3"] + assert left_trajectory.joint_names == ["left/j1", "left/j2", "left/j3"] assert [point.positions for point in left_trajectory.points] == [ [1.0, 2.0, 9.0], [4.0, 5.0, 9.0], ] assert right_call.args[0:2] == ("right_task", "execute") right_trajectory = right_call.args[2]["trajectory"] - assert right_trajectory.joint_names == ["right_task/j1", "right_task/j2"] + assert right_trajectory.joint_names == ["right/j1", "right/j2"] assert [point.positions for point in right_trajectory.points] == [[3.0, 8.0], [6.0, 8.0]] def test_project_plan_holds_non_selected_joints_from_current_state(self): - config = _make_robot_config("left", ["j1", "j2", "j3"], ["c/j1", "c/j2", "c/j3"], "task") + config = _make_robot_config("left", ["j1", "j2", "j3"], "task") module = _make_module_with_monitor(config) module._world_monitor.get_current_joint_state.return_value = JointState( - name=["left/j1", "left/j2", "left/j3"], position=[10.0, 20.0, 30.0] + name=["j1", "j2", "j3"], position=[10.0, 20.0, 30.0] ) plan = GeneratedPlan( group_ids=("left/arm",), @@ -861,14 +817,14 @@ def test_project_plan_holds_non_selected_joints_from_current_state(self): assert [state.position for state in projected] == [[10.0, 2.0, 30.0], [10.0, 3.0, 30.0]] def test_preview_path_with_last_plan_projects_lazily_to_world_monitor(self): - config = _make_robot_config("left", ["j1", "j2"], ["c/j1", "c/j2"], "task") + config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) module._robots["left"] = ("robot_left", config, _trajectory_generator()) module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_resolved_group("left", "arm", ["j1"]) + _make_global_group("left", "arm", ["j1"]) ] module._world_monitor.get_current_joint_state.return_value = JointState( - name=["left/j1", "left/j2"], position=[0.0, 7.0] + name=["j1", "j2"], position=[0.0, 7.0] ) module._last_plan = GeneratedPlan( group_ids=("left/arm",), diff --git a/dimos/robot/catalog/openarm.py b/dimos/robot/catalog/openarm.py index b6e1238cf2..fb57e013b5 100644 --- a/dimos/robot/catalog/openarm.py +++ b/dimos/robot/catalog/openarm.py @@ -65,9 +65,6 @@ def openarm_arm( "adapter_type": adapter_type, "address": address, "joint_names": joint_names, - # URDF already prefixes joints with "left_"/"right_" in bimanual mode, - # so suppress RobotConfig's automatic "{name}_" prefix. - "joint_prefix": "", "base_link": "openarm_body_link0", "home_joints": [0.0] * 7, "base_pose": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], @@ -104,7 +101,6 @@ def openarm_single( "adapter_type": adapter_type, "address": address, "joint_names": [f"openarm_left_joint{i}" for i in range(1, 8)], - "joint_prefix": "", "base_link": "openarm_body_link0", "home_joints": [0.0] * 7, "package_paths": {"openarm_description": _OPENARM_PKG}, diff --git a/dimos/robot/config.py b/dimos/robot/config.py index 8dabdf24e1..829b7d646d 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -29,6 +29,7 @@ from dimos.control.components import HardwareComponent, HardwareType from dimos.control.coordinator import TaskConfig from dimos.manipulation.planning.planning_groups import discover_planning_group_definitions +from dimos.manipulation.planning.planning_identifiers import make_global_joint_names from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -82,8 +83,6 @@ class RobotConfig(BaseModel): ) home_joints: list[float] | None = None - # Multi-robot / coordinator - joint_prefix: str | None = None # defaults to "{name}_" # Compatibility planning placement. Prefer encoding placement in URDF/xacro/MJCF. base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) @@ -113,11 +112,6 @@ class RobotConfig(BaseModel): _parsed: ModelDescription | None = PrivateAttr(default=None) - def _ensure_prefix(self) -> None: - """Ensure joint_prefix is set (no model parsing needed).""" - if self.joint_prefix is None: - self.joint_prefix = f"{self.name}/" - def _ensure_parsed(self) -> ModelDescription: """Parse model lazily on first access.""" if self._parsed is None: @@ -127,7 +121,6 @@ def _ensure_parsed(self) -> ModelDescription: "joint/link info is unavailable. Set model_path to a URDF/MJCF." ) self._parsed = parse_model(self.model_path, self.package_paths, self.xacro_args) - self._ensure_prefix() if self.joint_names is None: self.joint_names = self._parsed.actuated_joint_names if self.base_link is None: @@ -139,7 +132,7 @@ def _ensure_parsed(self) -> ModelDescription: def _compute_default_home(self) -> list[float]: assert self._parsed is not None home = [] - for joint_name in self.resolved_joint_names: + for joint_name in self.local_joint_names: joint = self._parsed.get_joint(joint_name) if ( joint is not None @@ -158,11 +151,15 @@ def model_description(self) -> ModelDescription: return self._ensure_parsed() @property - def resolved_joint_names(self) -> list[str]: + def local_joint_names(self) -> list[str]: self._ensure_parsed() assert self.joint_names is not None return self.joint_names + @property + def global_joint_names(self) -> list[str]: + return make_global_joint_names(self.name, self.local_joint_names) + @property def resolved_base_link(self) -> str: self._ensure_parsed() @@ -173,23 +170,7 @@ def resolved_base_link(self) -> str: def dof(self) -> int: if self.joint_names is not None: return len(self.joint_names) - return len(self.resolved_joint_names) - - @property - def coordinator_joint_names(self) -> list[str]: - self._ensure_prefix() - names = self.joint_names if self.joint_names is not None else self.resolved_joint_names - if not self.joint_prefix: - return list(names) - return [f"{self.joint_prefix}{j}" for j in names] - - @property - def joint_name_mapping(self) -> dict[str, str]: - self._ensure_prefix() - names = self.joint_names if self.joint_names is not None else self.resolved_joint_names - if not self.joint_prefix: - return {} - return {f"{self.joint_prefix}{j}": j for j in names} + return len(self.local_joint_names) @property def coordinator_task_name(self) -> str: @@ -215,9 +196,7 @@ def to_robot_model_config(self) -> RobotModelConfig: exclusions.extend(self.gripper.collision_exclusions) # Use direct fields when available to avoid triggering model parsing at import time - joint_names = ( - self.joint_names if self.joint_names is not None else self.resolved_joint_names - ) + joint_names = self.joint_names if self.joint_names is not None else self.local_joint_names base_link = self.base_link if self.base_link is not None else self.resolved_base_link planning_groups = discover_planning_group_definitions( robot_name=self.name, @@ -246,7 +225,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_name_mapping=self.joint_name_mapping, coordinator_task_name=self.coordinator_task_name, gripper_hardware_id=self.name if self.gripper else None, tf_extra_links=self.tf_extra_links, @@ -256,10 +234,9 @@ def to_robot_model_config(self) -> RobotModelConfig: def to_hardware_component(self) -> HardwareComponent: """Generate HardwareComponent for ControlCoordinator.""" - self._ensure_prefix() gripper_joints: list[str] = [] if self.gripper and self.gripper.joints: - gripper_joints = [f"{self.joint_prefix}{j}" for j in self.gripper.joints] + gripper_joints = make_global_joint_names(self.name, self.gripper.joints) adapter_kwargs = dict(self.adapter_kwargs) if self.home_joints is not None: @@ -268,7 +245,7 @@ def to_hardware_component(self) -> HardwareComponent: return HardwareComponent( hardware_id=self.name, hardware_type=HardwareType.MANIPULATOR, - joints=self.coordinator_joint_names, + joints=self.global_joint_names, adapter_type=self.adapter_type, address=self.address, auto_enable=self.auto_enable, @@ -300,7 +277,7 @@ def to_task_config( return TaskConfig( name=task_name if task_name is not None else self.coordinator_task_name, type=task_type if task_type is not None else self.task_type, - joint_names=self.coordinator_joint_names, + joint_names=self.global_joint_names, priority=priority if priority is not None else self.task_priority, auto_start=auto_start, params=params, diff --git a/dimos/robot/manipulators/xarm/blueprints.py b/dimos/robot/manipulators/xarm/blueprints.py index 6091f83030..09cd5de94b 100644 --- a/dimos/robot/manipulators/xarm/blueprints.py +++ b/dimos/robot/manipulators/xarm/blueprints.py @@ -56,7 +56,7 @@ KeyboardTeleopModule.blueprint( model_path=XARM6_FK_MODEL, ee_joint_id=_xarm6_cfg.dof, - joint_names=_xarm6_cfg.coordinator_joint_names, + joint_names=_xarm6_cfg.global_joint_names, ), ControlCoordinator.blueprint( tick_rate=100.0, @@ -90,7 +90,7 @@ KeyboardTeleopModule.blueprint( model_path=XARM7_FK_MODEL, ee_joint_id=_xarm7_cfg.dof, - joint_names=_xarm7_cfg.coordinator_joint_names, + joint_names=_xarm7_cfg.global_joint_names, ), ControlCoordinator.blueprint( tick_rate=100.0, diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index a90dff7a80..b7d204fd18 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -533,7 +533,6 @@ def _make_base_pose(x=0.0, y=0.0, z=0.0) -> PoseStamped: def _make_yourarm_config( name: str = "arm", y_offset: float = 0.0, - joint_prefix: str = "", coordinator_task: str | None = None, ) -> RobotModelConfig: """Create YourArm robot config for planning. @@ -541,12 +540,10 @@ def _make_yourarm_config( Args: name: Robot name in the Drake planning world. y_offset: Y-axis offset for multi-arm setups. - joint_prefix: Prefix for joint name mapping to coordinator namespace. coordinator_task: Coordinator task name for trajectory execution via RPC. """ # These must match the joint names in your URDF joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] - joint_mapping = {f"{joint_prefix}{j}": j for j in joint_names} if joint_prefix else {} return RobotModelConfig( name=name, @@ -569,7 +566,6 @@ def _make_yourarm_config( auto_convert_meshes=True, # Convert DAE/STL meshes for Drake max_velocity=1.0, # Max velocity scaling factor max_acceleration=2.0, # Max acceleration scaling factor - joint_name_mapping=joint_mapping, coordinator_task_name=coordinator_task, ) ``` @@ -581,7 +577,7 @@ Add this to your `dimos/robot/yourarm/blueprints.py` alongside the coordinator b ```python skip yourarm_planner = manipulation_module( - robots=[_make_yourarm_config("arm", joint_prefix="arm_", coordinator_task="traj_arm")], + robots=[_make_yourarm_config("arm", coordinator_task="traj_arm")], planning_timeout=10.0, enable_viz=True, ).transports( @@ -596,13 +592,17 @@ yourarm_planner = manipulation_module( | Field | Description | |-------|-------------| | `model_path` | Path to `.urdf` or `.xacro` file | -| `joint_names` | Ordered controllable/coordinator joint set (must match URDF); not itself a planning group | +| `joint_names` | Ordered controllable local model joint set (must match URDF); not itself a planning group | | `planning_groups` / `srdf_path` | Explicit planning groups or SRDF source; fallback can generate `{robot_name}/manipulator` for an unambiguous single chain | | `package_paths` | Maps `package://` URIs to filesystem paths (for xacro) | -| `joint_name_mapping` | Maps coordinator names (e.g., `"arm_joint1"`) to URDF names (e.g., `"joint1"`) | | `coordinator_task_name` | Must match the `TaskConfig.name` in your coordinator blueprint | | `collision_exclusion_pairs` | List of `(link_a, link_b)` tuples for links that may legitimately touch (e.g., gripper fingers) | +Coordinator-facing joint states and trajectories use global joint names derived +mechanically as `{robot_name}/{local_joint_name}` (for example, `arm/joint1`). +Keep hardware-native name translation inside the hardware adapter; manipulation +planning config uses local model joint names. + `base_link`, `base_pose`, and `end_effector_link` are compatibility fields used by current placement and robot-scoped helper paths. New planning code should use SRDF/planning-group chain base/tip links and encode robot placement in the model. See diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md index 80dc9e6e0f..24343b5bc1 100644 --- a/docs/capabilities/manipulation/planning_groups.md +++ b/docs/capabilities/manipulation/planning_groups.md @@ -10,13 +10,13 @@ being planned. |---------|---------| | Planning group | A named serial chain of controllable robot joints. | | Planning group ID | Stable API ID in the form `{robot_name}/{group_name}`. | -| Resolved joint name | World-level joint name in the form `{robot_name}/{local_joint_name}`. | -| Generated plan | Minimal planning artifact containing selected group IDs and one synchronized resolved-joint path. | +| Global joint name | Boundary-level joint name in the form `{robot_name}/{local_joint_name}`. | +| Generated plan | Minimal planning artifact containing selected group IDs and one synchronized global-joint path. | | Auxiliary group | A group selected for a pose request without receiving its own pose target. | -Local URDF/SRDF joint names stay inside model parsing and backend internals. -Public planning states and generated plan paths use resolved joint names so two -robots can safely have the same local joint names. +Local URDF/SRDF joint names stay inside robot-scoped APIs, model parsing, and +backend internals. Flat planning states and generated plan paths require global +joint names so two robots can safely have the same local joint names. ## Planning group sources @@ -53,8 +53,8 @@ unique serial target frame. ## Fallback behavior When no SRDF is available, fallback uses `RobotModelConfig.joint_names` as the -candidate controllable set. This field is the robot's controllable/coordinator -joint set, not an implicit planning group. +candidate controllable set. This field is the robot's ordered local model joint +set, not an implicit planning group. Fallback succeeds only when those joints form one unambiguous serial chain. It allows prismatic joints in the middle of the chain and strips only terminal/tip @@ -71,7 +71,7 @@ the API normalizes them back to IDs and re-resolves current world state. # Joint-space planning for one group. manip.plan_to_joint_targets({ "left_arm/manipulator": JointState( - name=["left_arm/joint1", "left_arm/joint2"], + name=["joint1", "joint2"], position=[0.2, -0.1], ) }) @@ -87,23 +87,24 @@ manip.preview_plan(plan) manip.execute_plan(plan) ``` -For joint-space planning, start and goal joint keys must exactly match the -selected resolved joints: no missing, extra, or partial joints. +For robot-scoped joint-space planning, unnamed vectors are interpreted in robot +model joint order. If names are provided, they must be local model joint names: +no global names, missing joints, extra joints, or partial joint sets. ## Generated plans and execution A `GeneratedPlan` stores: - selected planning group IDs; -- a single synchronized path of `JointState` waypoints keyed by resolved joint +- a single synchronized path of `JointState` waypoints keyed by global joint names; - status, timing, path length, iteration count, and message metadata. Preview and execution project this path lazily. Preview sends projected joint -paths to the world monitor. Execution splits the path by affected coordinator -trajectory task, orders each trajectory by that task's configured joint order, -maps resolved/local names to coordinator names at the boundary, and invokes each -trajectory controller. Controllers remain planning-group agnostic. +paths to the world monitor. Execution splits the path by affected trajectory +task, orders each trajectory by the robot's configured local joint order, writes +global joint names at the coordinator boundary, and invokes each trajectory +controller. Controllers remain planning-group agnostic. Multi-task dispatch is not atomic in this change: if one trajectory task accepts and a later task rejects, DimOS reports the rejection but does not roll back the @@ -119,4 +120,4 @@ New planning logic should use model/SRDF structure and planning group base/tip links instead. Robot placement should be encoded in URDF/xacro/MJCF. `joint_names` remains -supported and should describe the controllable/coordinator joint set. +supported and should describe the ordered controllable local model joint set. diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index 5dc0cbee8e..e51251a1aa 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -135,7 +135,7 @@ visualization backend. ## Planning Groups Manipulation planning uses explicit planning group IDs such as -`arm/manipulator` and resolved joint names such as `arm/joint1`. See +`arm/manipulator` and global joint names such as `arm/joint1`. See [Planning Groups](/docs/capabilities/manipulation/planning_groups.md) for SRDF support, fallback generation, auxiliary groups, generated plans, and execution projection. From 702ad744adf020ba698b6be7168ecfb3d433f77a Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 12:33:44 -0700 Subject: [PATCH 10/27] refactor: tighten manipulation joint namespaces --- dimos/manipulation/manipulation_module.py | 171 +++++++++++++----- .../planning/planning_group_utils.py | 46 ++++- .../planning/test_planning_group_utils.py | 64 +++++++ dimos/manipulation/test_manipulation_unit.py | 51 ++++++ 4 files changed, 280 insertions(+), 52 deletions(-) create mode 100644 dimos/manipulation/planning/test_planning_group_utils.py diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 588834996c..47ea0fc0f0 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -56,6 +56,7 @@ from dimos.manipulation.planning.planning_identifiers import ( assert_global_joint_names, assert_local_joint_names, + is_global_joint_name, make_global_joint_name, make_global_joint_names, ) @@ -504,13 +505,65 @@ def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGro self._world_monitor.world.list_planning_groups(), robot_name ) + def _current_positions_by_name( + self, robot_name: RobotName, current: JointState + ) -> tuple[dict[str, float], bool] | None: + """Index a robot current state and report whether its keys are global. + + World-monitor current state is a single-robot backend boundary. It may + be local (the normal backend form) or global during compatibility + migrations, but it must not mix namespaces. + """ + if len(current.name) != len(current.position): + logger.error( + "Current state for '%s' has %d names but %d positions", + robot_name, + len(current.name), + len(current.position), + ) + return None + if not current.name: + logger.error("Current state for '%s' has no joint names", robot_name) + return None + + global_flags = [is_global_joint_name(name) for name in current.name] + if any(global_flags) and not all(global_flags): + logger.error( + "Current state for '%s' mixes global and local joint names: %s", + robot_name, + list(current.name), + ) + return None + + current_is_global = all(global_flags) + try: + if current_is_global: + assert_global_joint_names(current.name) + wrong_robot = [ + name for name in current.name if not name.startswith(f"{robot_name}/") + ] + if wrong_robot: + logger.error( + "Current state for '%s' contains joints from another robot: %s", + robot_name, + wrong_robot, + ) + return None + else: + assert_local_joint_names(current.name) + except ValueError as exc: + logger.error("Invalid current state for '%s': %s", robot_name, exc) + return None + + return dict(zip(current.name, current.position, strict=True)), current_is_global + def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: """Collect current state for exactly the selected global joints.""" assert self._world_monitor is not None resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) names: list[str] = [] positions: list[float] = [] - current_by_robot: dict[WorldRobotID, dict[str, float]] = {} + current_by_robot: dict[WorldRobotID, tuple[dict[str, float], bool]] = {} for group in resolved_groups: if group.robot_id not in current_by_robot: @@ -518,26 +571,48 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint if current is None: logger.error("No joint state for robot '%s'", group.robot_name) return None - current_by_robot[group.robot_id] = dict( - zip(current.name, current.position, strict=False) - ) + indexed_current = self._current_positions_by_name(group.robot_name, current) + if indexed_current is None: + return None + current_by_robot[group.robot_id] = indexed_current - robot_state = current_by_robot[group.robot_id] + robot_state, current_is_global = current_by_robot[group.robot_id] for resolved_name, local_name in zip( group.joint_names, group.local_joint_names, strict=True ): - if resolved_name in robot_state: - position = robot_state[resolved_name] - elif local_name in robot_state: - position = robot_state[local_name] - else: + lookup_name = resolved_name if current_is_global else local_name + if lookup_name not in robot_state: logger.error("Current state missing selected joint '%s'", resolved_name) return None + position = robot_state[lookup_name] names.append(resolved_name) positions.append(position) return JointState(name=names, position=positions) + def _validate_generated_plan_path( + self, group_ids: tuple[PlanningGroupID, ...], path: JointPath + ) -> None: + """Validate canonical generated plans use selected global names in group order.""" + assert self._world_monitor is not None + resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) + expected_names = [ + joint_name for group in resolved_groups for joint_name in group.joint_names + ] + assert_global_joint_names(expected_names) + for index, waypoint in enumerate(path): + if len(waypoint.name) != len(waypoint.position): + raise ValueError( + f"Waypoint {index} has {len(waypoint.name)} names but " + f"{len(waypoint.position)} positions" + ) + assert_global_joint_names(waypoint.name) + if list(waypoint.name) != expected_names: + raise ValueError( + f"Waypoint {index} joint names {list(waypoint.name)} do not match " + f"selected planning joints {expected_names}" + ) + def _normalize_joint_target( self, group_id: PlanningGroupID, target: JointState ) -> JointState | None: @@ -565,21 +640,41 @@ def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotNam if self._world_monitor is not None: current = self._world_monitor.get_current_joint_state(robot_id) if current is not None: - current_by_name = dict(zip(current.name, current.position, strict=False)) + indexed_current = self._current_positions_by_name(robot_name, current) + if indexed_current is None: + return [] + current_by_name, current_is_global = indexed_current + else: + current_is_global = False + else: + current_is_global = False projected: JointPath = [] for waypoint in plan.path: - position_by_name = dict(zip(waypoint.name, waypoint.position, strict=False)) + if len(waypoint.name) != len(waypoint.position): + logger.error( + "Cannot project plan for '%s': waypoint has %d names but %d positions", + robot_name, + len(waypoint.name), + len(waypoint.position), + ) + return [] + try: + assert_global_joint_names(waypoint.name) + except ValueError as exc: + logger.error("Cannot project plan for '%s': %s", robot_name, exc) + return [] + position_by_name = dict(zip(waypoint.name, waypoint.position, strict=True)) positions: list[float] = [] for local_name, global_name in zip( config.joint_names, global_joint_names, strict=False ): if global_name in position_by_name: positions.append(position_by_name[global_name]) - elif global_name in current_by_name: - positions.append(current_by_name[global_name]) - elif local_name in current_by_name: - positions.append(current_by_name[local_name]) else: + current_lookup_name = global_name if current_is_global else local_name + if current_lookup_name in current_by_name: + positions.append(current_by_name[current_lookup_name]) + continue logger.error( "Cannot project plan for '%s': missing joint '%s'", robot_name, @@ -657,6 +752,10 @@ def _plan_selected_path( ) if not result.is_success(): return self._fail(f"Planning failed: {result.status.name}") + try: + self._validate_generated_plan_path(group_ids, result.path) + except ValueError as exc: + return self._fail(f"Planner returned invalid global plan: {exc}") logger.info("Path: %d waypoints", len(result.path)) self._store_generated_plan(group_ids, result) @@ -775,44 +874,30 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: pose: Target end-effector pose robot_name: Robot to plan for (required if multiple robots configured) """ - if self._kinematics is None or (r := self._begin_planning(robot_name)) is None: + if self._kinematics is None or self._world_monitor is None: + return False + robot = self._get_robot(robot_name) + if robot is None: + return False + + selected_robot_name, robot_id, _, _ = robot + group_id = self._default_group_id_for_robot(selected_robot_name) + if group_id is not None: + return self.plan_to_poses({group_id: pose}) + + if self._begin_planning(selected_robot_name) is None: return False - robot_name, robot_id = r - assert self._world_monitor # guaranteed by _begin_planning current = self._world_monitor.get_current_joint_state(robot_id) if current is None: return self._fail("No joint state") - target_pose = PoseStamped( - frame_id="world", - position=pose.position, - orientation=pose.orientation, - ) - - group_id = self._default_group_id_for_robot(robot_name) - if group_id is not None: - ik = self._kinematics.solve_pose_targets( - world=self._world_monitor.world, - pose_targets={group_id: target_pose}, - seed=current, - check_collision=True, - ) - if not ik.is_success() or ik.joint_state is None: - return self._fail(f"IK failed: {ik.status.name}") - - start = self._selected_joint_state((group_id,)) - if start is None: - return self._fail("No joint state") - logger.info(f"IK solved, error: {ik.position_error:.4f}m") - return self._plan_selected_path((group_id,), start, ik.joint_state) - 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}") logger.info(f"IK solved, error: {ik.position_error:.4f}m") - return self._plan_path_only(robot_name, robot_id, ik.joint_state) + return self._plan_path_only(selected_robot_name, robot_id, ik.joint_state) @rpc def plan_to_poses( diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/planning_group_utils.py index 33cdf020b7..d3cbfe6a02 100644 --- a/dimos/manipulation/planning/planning_group_utils.py +++ b/dimos/manipulation/planning/planning_group_utils.py @@ -16,7 +16,11 @@ from collections.abc import Mapping, Sequence -from dimos.manipulation.planning.planning_identifiers import assert_local_joint_names +from dimos.manipulation.planning.planning_identifiers import ( + assert_global_joint_names, + assert_local_joint_names, + is_global_joint_name, +) from dimos.manipulation.planning.spec.models import ( GlobalJointName, LocalModelJointName, @@ -110,7 +114,12 @@ def normalize_joint_target_for_group( group: ResolvedPlanningGroup, target: JointState, ) -> JointState: - """Normalize a robot-scoped group target to global joint names in group order.""" + """Normalize a group joint target to global joint names in group order. + + Named targets may use either the public global planning names or the + robot-local model names used by legacy robot-scoped callers, but the two + namespaces must not be mixed in one target. + """ if not target.name: if len(target.position) != len(group.joint_names): raise ValueError( @@ -119,19 +128,38 @@ def normalize_joint_target_for_group( ) return JointState(name=list(group.joint_names), position=list(target.position)) - assert_local_joint_names(target.name) - positions_by_name = dict(zip(target.name, target.position, strict=False)) + if len(target.name) != len(target.position): + raise ValueError( + f"Target for '{group.id}' has {len(target.name)} names but " + f"{len(target.position)} positions" + ) + + target_names = list(target.name) + global_flags = [is_global_joint_name(name) for name in target_names] + if any(global_flags) and not all(global_flags): + raise ValueError( + f"Target for '{group.id}' mixes global and local joint names: {target_names}" + ) + + if all(global_flags): + assert_global_joint_names(target_names) + expected_names = group.joint_names + else: + assert_local_joint_names(target_names) + expected_names = group.local_joint_names + + positions_by_name = dict(zip(target_names, target.position, strict=True)) global_positions: list[float] = [] missing: list[str] = [] - for local_name in group.local_joint_names: - if local_name in positions_by_name: - global_positions.append(positions_by_name[local_name]) + for expected_name in expected_names: + if expected_name in positions_by_name: + global_positions.append(positions_by_name[expected_name]) else: - missing.append(local_name) + missing.append(expected_name) if missing: raise ValueError(f"Target for '{group.id}' is missing joints: {missing}") - extra = set(target.name) - set(group.local_joint_names) + extra = set(target_names) - set(expected_names) if extra: raise ValueError(f"Target for '{group.id}' has extra joints: {sorted(extra)}") return JointState(name=list(group.joint_names), position=global_positions) diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_utils.py new file mode 100644 index 0000000000..fa25dac217 --- /dev/null +++ b/dimos/manipulation/planning/test_planning_group_utils.py @@ -0,0 +1,64 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for planning group joint-name normalization.""" + +from __future__ import annotations + +import pytest + +from dimos.manipulation.planning.planning_group_utils import normalize_joint_target_for_group +from dimos.manipulation.planning.spec.models import ResolvedPlanningGroup +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _make_group() -> ResolvedPlanningGroup: + return ResolvedPlanningGroup( + id="left/arm", + robot_id="robot_left", + robot_name="left", + group_name="arm", + joint_names=("left/j1", "left/j2", "left/j3"), + local_joint_names=("j1", "j2", "j3"), + base_link="base", + tip_link="ee", + ) + + +def test_normalize_joint_target_accepts_named_global_targets_in_group_order() -> None: + group = _make_group() + target = JointState(name=["left/j3", "left/j1", "left/j2"], position=[3.0, 1.0, 2.0]) + + normalized = normalize_joint_target_for_group(group, target) + + assert normalized.name == ["left/j1", "left/j2", "left/j3"] + assert normalized.position == [1.0, 2.0, 3.0] + + +def test_normalize_joint_target_accepts_named_local_targets_in_group_order() -> None: + group = _make_group() + target = JointState(name=["j2", "j3", "j1"], position=[2.0, 3.0, 1.0]) + + normalized = normalize_joint_target_for_group(group, target) + + assert normalized.name == ["left/j1", "left/j2", "left/j3"] + assert normalized.position == [1.0, 2.0, 3.0] + + +def test_normalize_joint_target_rejects_mixed_global_and_local_target_names() -> None: + group = _make_group() + target = JointState(name=["left/j1", "j2", "left/j3"], position=[1.0, 2.0, 3.0]) + + with pytest.raises(ValueError, match="mixes global and local joint names"): + normalize_joint_target_for_group(group, target) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index a1f279d8be..17cf991917 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -756,6 +756,18 @@ def test_selected_joint_state_accepts_local_current_state_names(self): assert selected.name == ["left/j1", "left/j2"] assert selected.position == [1.0, 2.0] + def test_selected_joint_state_rejects_mixed_current_state_names(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("left", "arm", ["j1", "j2"]) + ] + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["left/j1", "j2"], position=[1.0, 2.0] + ) + + assert module._selected_joint_state(("left/arm",)) is None + def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): left_config = _make_robot_config( "left", @@ -816,6 +828,45 @@ def test_project_plan_holds_non_selected_joints_from_current_state(self): assert [state.name for state in projected] == [["j1", "j2", "j3"], ["j1", "j2", "j3"]] assert [state.position for state in projected] == [[10.0, 2.0, 30.0], [10.0, 3.0, 30.0]] + def test_project_plan_rejects_local_waypoint_names(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2"], position=[10.0, 20.0] + ) + plan = GeneratedPlan( + group_ids=("left/arm",), + path=[JointState(name=["j1"], position=[1.0])], + status=PlanningStatus.SUCCESS, + ) + + assert module._project_plan_path_for_robot(plan, "left") == [] + + def test_project_plan_uses_global_names_for_robots_with_shared_local_names(self): + left_config = _make_robot_config("left", ["j1", "j2"], "left_task") + right_config = _make_robot_config("right", ["j1", "j2"], "right_task") + module = _make_module_with_monitor(left_config, right_config) + module._world_monitor.get_current_joint_state.side_effect = [ + JointState(name=["j1", "j2"], position=[10.0, 20.0]), + JointState(name=["j1", "j2"], position=[30.0, 40.0]), + ] + plan = GeneratedPlan( + group_ids=("left/arm", "right/arm"), + path=[ + JointState(name=["left/j1", "right/j1"], position=[1.0, 3.0]), + JointState(name=["left/j1", "right/j1"], position=[2.0, 4.0]), + ], + status=PlanningStatus.SUCCESS, + ) + + left_projected = module._project_plan_path_for_robot(plan, "left") + right_projected = module._project_plan_path_for_robot(plan, "right") + + assert [state.name for state in left_projected] == [["j1", "j2"], ["j1", "j2"]] + assert [state.position for state in left_projected] == [[1.0, 20.0], [2.0, 20.0]] + assert [state.name for state in right_projected] == [["j1", "j2"], ["j1", "j2"]] + assert [state.position for state in right_projected] == [[3.0, 40.0], [4.0, 40.0]] + def test_preview_path_with_last_plan_projects_lazily_to_world_monitor(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) From d370174b80a04a706ccac17a598c896d0c51b7c1 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 14:10:17 -0700 Subject: [PATCH 11/27] refactor: align manipulation planning group APIs --- CONTEXT.md | 8 + dimos/manipulation/manipulation_module.py | 385 ++++-------------- .../planning/monitor/world_monitor.py | 26 +- .../planning/planners/rrt_planner.py | 67 ++- .../planners/test_rrt_planner_selection.py | 35 ++ .../planning/planning_group_utils.py | 4 +- dimos/manipulation/planning/spec/config.py | 11 + dimos/manipulation/planning/spec/protocols.py | 15 +- .../planning/test_planning_group_utils.py | 8 +- .../planning/world/drake_world.py | 86 +++- dimos/manipulation/test_manipulation_unit.py | 220 ++++++---- 11 files changed, 430 insertions(+), 435 deletions(-) diff --git a/CONTEXT.md b/CONTEXT.md index a5af10041e..701648bbed 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -48,6 +48,10 @@ _Avoid_: Batch planning, independent planning An API-level identifier for a planning group, always written as `{robot_name}/{group_name}`. `/` is reserved as the delimiter and is not part of either component. _Avoid_: Bare group name, robot ID +**Default Planning Group**: +The generated fallback planning group used by robot-scoped compatibility APIs when no planning group is specified explicitly. It is not inferred from arbitrary SRDF group uniqueness. +_Avoid_: Unique planning group, primary planning group + **Planning Group Descriptor**: A read-only snapshot returned by query APIs that describes an available planning group and may be used ergonomically as a planning group selector. _Avoid_: Live planning group handle, resolved planning group @@ -72,6 +76,10 @@ _Avoid_: Namespaced local joint state, prefixed joint state A flat planning result that may contain one or more robots. Joint states in a generated plan require names and use global joint names so the plan remains unambiguous across robot boundaries. _Avoid_: Robot-scoped joint plan, local joint plan +**Group-Scoped Preview**: +A visualization request for a generated plan over a planning group or planning group selection. A visualization backend may render the whole robot when partial-group rendering is not practical, but the API scope remains the generated plan's selected planning groups. +_Avoid_: Robot-scoped preview API + **Global Joint Name**: A boundary-level joint name that mechanically combines a robot name and local model joint name as `{robot_name}/{local_joint_name}` so it is stable and unique in flat joint-state representations, even when the local model joint name is already descriptive. `/` is reserved as the delimiter and is not part of either component. _Avoid_: Resolved joint name, coordinator joint name, bare joint name, local joint name diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 47ea0fc0f0..fa98f52f50 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -31,7 +31,6 @@ import traceback from typing import TYPE_CHECKING, Any, TypeAlias -import numpy as np from pydantic import Field from dimos.agents.annotation import skill @@ -48,17 +47,17 @@ ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.planning_group_utils import ( - normalize_joint_target_for_group, + joint_target_to_global_names, planning_group_id_from_selector, primary_pose_planning_group_id_for_robot, - single_planning_group_id_for_robot, ) +from dimos.manipulation.planning.planning_groups import FALLBACK_PLANNING_GROUP_NAME from dimos.manipulation.planning.planning_identifiers import ( assert_global_joint_names, assert_local_joint_names, - is_global_joint_name, make_global_joint_name, make_global_joint_names, + make_planning_group_id, ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType @@ -352,6 +351,9 @@ def _tf_publish_loop(self) -> None: for robot_id, config, _ in self._robots.values(): # Publish world → primary planning-group target frame. # Fall back to robot-scoped EE only for compatibility configs. + # TODO: Publish one TF per pose-targetable group, or expose the + # backend's full robot TF tree, once consumers stop assuming a + # single robot-scoped end-effector frame. target_frame = config.end_effector_link pose_group_id = self._primary_pose_group_id_for_robot(config.name) if pose_group_id is not None: @@ -361,7 +363,11 @@ def _tf_publish_loop(self) -> None: target_frame = pose_group.tip_link ee_pose = self._world_monitor.get_group_pose(pose_group_id) else: - ee_pose = self._world_monitor.get_ee_pose(robot_id) + ee_pose = ( + self._world_monitor.get_link_pose(robot_id, target_frame) + if target_frame is not None + else None + ) if ee_pose is not None and target_frame is not None: ee_tf = Transform.from_pose(target_frame, ee_pose) ee_tf.frame_id = "world" @@ -443,7 +449,13 @@ def get_ee_pose(self, robot_name: RobotName | None = None) -> Pose | None: robot_name: Robot to query (required if multiple robots configured) """ if (robot := self._get_robot(robot_name)) and self._world_monitor: - return self._world_monitor.get_ee_pose(robot[1], joint_state=None) + _, robot_id, config, _ = robot + pose_group_id = self._primary_pose_group_id_for_robot(config.name) + if pose_group_id is not None: + return self._world_monitor.get_group_pose(pose_group_id, joint_state=None) + if config.end_effector_link is None: + return None + return self._world_monitor.get_link_pose(robot_id, config.end_effector_link) return None @rpc @@ -488,15 +500,17 @@ def _fail(self, msg: str) -> bool: return False def _default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: - """Return wrapper-level default group for legacy single-group RPCs.""" + """Return the generated fallback group used by robot-scoped wrappers.""" assert self._world_monitor is not None - try: - return single_planning_group_id_for_robot( - self._world_monitor.world.list_planning_groups(), robot_name - ) - except ValueError as exc: - logger.error(str(exc)) - return None + group_id = make_planning_group_id(robot_name, FALLBACK_PLANNING_GROUP_NAME) + if any(group.id == group_id for group in self._world_monitor.world.list_planning_groups()): + return group_id + logger.error( + "Robot '%s' has no generated default planning group '%s'; use explicit group APIs", + robot_name, + group_id, + ) + return None def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: """Return the first pose-targetable group for robot-scoped compatibility paths.""" @@ -507,13 +521,8 @@ def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGro def _current_positions_by_name( self, robot_name: RobotName, current: JointState - ) -> tuple[dict[str, float], bool] | None: - """Index a robot current state and report whether its keys are global. - - World-monitor current state is a single-robot backend boundary. It may - be local (the normal backend form) or global during compatibility - migrations, but it must not mix namespaces. - """ + ) -> dict[str, float] | None: + """Index a robot current state by local model joint name.""" if len(current.name) != len(current.position): logger.error( "Current state for '%s' has %d names but %d positions", @@ -525,37 +534,13 @@ def _current_positions_by_name( if not current.name: logger.error("Current state for '%s' has no joint names", robot_name) return None - - global_flags = [is_global_joint_name(name) for name in current.name] - if any(global_flags) and not all(global_flags): - logger.error( - "Current state for '%s' mixes global and local joint names: %s", - robot_name, - list(current.name), - ) - return None - - current_is_global = all(global_flags) try: - if current_is_global: - assert_global_joint_names(current.name) - wrong_robot = [ - name for name in current.name if not name.startswith(f"{robot_name}/") - ] - if wrong_robot: - logger.error( - "Current state for '%s' contains joints from another robot: %s", - robot_name, - wrong_robot, - ) - return None - else: - assert_local_joint_names(current.name) + assert_local_joint_names(current.name) except ValueError as exc: logger.error("Invalid current state for '%s': %s", robot_name, exc) return None - return dict(zip(current.name, current.position, strict=True)), current_is_global + return dict(zip(current.name, current.position, strict=True)) def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: """Collect current state for exactly the selected global joints.""" @@ -563,7 +548,7 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) names: list[str] = [] positions: list[float] = [] - current_by_robot: dict[WorldRobotID, tuple[dict[str, float], bool]] = {} + current_by_robot: dict[WorldRobotID, dict[str, float]] = {} for group in resolved_groups: if group.robot_id not in current_by_robot: @@ -576,51 +561,27 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint return None current_by_robot[group.robot_id] = indexed_current - robot_state, current_is_global = current_by_robot[group.robot_id] + robot_state = current_by_robot[group.robot_id] for resolved_name, local_name in zip( group.joint_names, group.local_joint_names, strict=True ): - lookup_name = resolved_name if current_is_global else local_name - if lookup_name not in robot_state: + if local_name not in robot_state: logger.error("Current state missing selected joint '%s'", resolved_name) return None - position = robot_state[lookup_name] + position = robot_state[local_name] names.append(resolved_name) positions.append(position) return JointState(name=names, position=positions) - def _validate_generated_plan_path( - self, group_ids: tuple[PlanningGroupID, ...], path: JointPath - ) -> None: - """Validate canonical generated plans use selected global names in group order.""" - assert self._world_monitor is not None - resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) - expected_names = [ - joint_name for group in resolved_groups for joint_name in group.joint_names - ] - assert_global_joint_names(expected_names) - for index, waypoint in enumerate(path): - if len(waypoint.name) != len(waypoint.position): - raise ValueError( - f"Waypoint {index} has {len(waypoint.name)} names but " - f"{len(waypoint.position)} positions" - ) - assert_global_joint_names(waypoint.name) - if list(waypoint.name) != expected_names: - raise ValueError( - f"Waypoint {index} joint names {list(waypoint.name)} do not match " - f"selected planning joints {expected_names}" - ) - def _normalize_joint_target( self, group_id: PlanningGroupID, target: JointState ) -> JointState | None: - """Normalize a group joint target to global joint names in group order.""" + """Convert a group joint target to global joint names in group order.""" assert self._world_monitor is not None group = self._world_monitor.world.resolve_planning_groups((group_id,))[0] try: - return normalize_joint_target_for_group(group, target) + return joint_target_to_global_names(group, target) except ValueError as exc: logger.error(str(exc)) return None @@ -643,11 +604,7 @@ def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotNam indexed_current = self._current_positions_by_name(robot_name, current) if indexed_current is None: return [] - current_by_name, current_is_global = indexed_current - else: - current_is_global = False - else: - current_is_global = False + current_by_name = indexed_current projected: JointPath = [] for waypoint in plan.path: if len(waypoint.name) != len(waypoint.position): @@ -671,9 +628,8 @@ def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotNam if global_name in position_by_name: positions.append(position_by_name[global_name]) else: - current_lookup_name = global_name if current_is_global else local_name - if current_lookup_name in current_by_name: - positions.append(current_by_name[current_lookup_name]) + if local_name in current_by_name: + positions.append(current_by_name[local_name]) continue logger.error( "Cannot project plan for '%s': missing joint '%s'", @@ -689,16 +645,21 @@ def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotNam ) return projected - def _trajectory_for_robot_plan( + def _project_plan_to_robot_path_for_execution( self, plan: GeneratedPlan, robot_name: RobotName + ) -> JointPath: + """Project a generated plan to one robot's local execution path.""" + return self._project_plan_path_for_robot(plan, robot_name) + + def _trajectory_from_robot_path( + self, robot_name: RobotName, path: JointPath ) -> JointTrajectory | None: - """Generate a task-ordered trajectory for one affected robot lazily.""" - projected_path = self._project_plan_path_for_robot(plan, robot_name) - if len(projected_path) < 2: + """Generate a coordinator-facing trajectory from a local robot path.""" + if len(path) < 2: logger.error("Plan projection for '%s' has fewer than two waypoints", robot_name) return None _, config, traj_gen = self._robots[robot_name] - trajectory = traj_gen.generate([list(state.position) for state in projected_path]) + trajectory = traj_gen.generate([list(state.position) for state in path]) return JointTrajectory( joint_names=make_global_joint_names(robot_name, config.joint_names), points=trajectory.points, @@ -731,10 +692,12 @@ def _store_generated_plan( self._planned_paths.clear() self._planned_trajectories.clear() for robot_name in self._affected_robot_names(self._last_plan): - projected_path = self._project_plan_path_for_robot(self._last_plan, robot_name) + projected_path = self._project_plan_to_robot_path_for_execution( + self._last_plan, robot_name + ) if projected_path: self._planned_paths[robot_name] = projected_path - trajectory = self._trajectory_for_robot_plan(self._last_plan, robot_name) + trajectory = self._trajectory_from_robot_path(robot_name, projected_path) if trajectory is not None: self._planned_trajectories[robot_name] = trajectory @@ -752,52 +715,17 @@ def _plan_selected_path( ) if not result.is_success(): return self._fail(f"Planning failed: {result.status.name}") - try: - self._validate_generated_plan_path(group_ids, result.path) - except ValueError as exc: - return self._fail(f"Planner returned invalid global plan: {exc}") logger.info("Path: %d waypoints", len(result.path)) self._store_generated_plan(group_ids, result) self._state = ManipulationState.COMPLETED return True - def _interpolate_preview_path( - self, - planned_path: JointPath, - trajectory: JointTrajectory | None, - animation_duration: float, - target_fps: float, - ) -> JointPath: - """Densify a planned path for visualization using a timed trajectory.""" - interpolated = list(planned_path) - if trajectory is None or target_fps <= 0 or animation_duration <= 0: - return interpolated - - times = np.array([point.time_from_start for point in trajectory.points], dtype=np.float64) - positions = np.array([point.positions for point in trajectory.points], dtype=np.float64) - if len(times) <= 1 or positions.ndim != 2 or times[-1] <= times[0]: - return interpolated - - frame_count = int(np.ceil(animation_duration * target_fps)) + 1 - sample_times = np.linspace(times[0], times[-1], frame_count) - joint_names = trajectory.joint_names or planned_path[0].name - sampled_positions = np.column_stack( - [ - np.interp(sample_times, times, positions[:, joint]) - for joint in range(positions.shape[1]) - ] - ) - return [ - JointState(name=joint_names, position=position.tolist()) - for position in sampled_positions - ] - - def _dismiss_preview(self, robot_id: WorldRobotID) -> None: + def _dismiss_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: """Hide the preview ghost if the world supports it.""" if self._world_monitor is None: return - self._world_monitor.hide_preview(robot_id) + self._world_monitor.hide_preview(group_ids) self._world_monitor.publish_visualization() def _solve_ik_for_pose( @@ -880,24 +808,11 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: if robot is None: return False - selected_robot_name, robot_id, _, _ = robot + selected_robot_name, _, _, _ = robot group_id = self._default_group_id_for_robot(selected_robot_name) - if group_id is not None: - return self.plan_to_poses({group_id: pose}) - - if self._begin_planning(selected_robot_name) is None: + if group_id is None: return False - - current = self._world_monitor.get_current_joint_state(robot_id) - if current is None: - return self._fail("No joint state") - - 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}") - - logger.info(f"IK solved, error: {ik.position_error:.4f}m") - return self._plan_path_only(selected_robot_name, robot_id, ik.joint_state) + return self.plan_to_poses({group_id: pose}) @rpc def plan_to_poses( @@ -953,20 +868,15 @@ def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None joints: Target joint state (names + positions) robot_name: Robot to plan for (required if multiple robots configured) """ - if (r := self._begin_planning(robot_name)) is None: + robot = self._get_robot(robot_name) + if robot is None: return False - robot_name, robot_id = r + robot_name, _, _, _ = robot logger.info(f"Planning to joints for {robot_name}: {[f'{j:.3f}' for j in joints.position]}") group_id = self._default_group_id_for_robot(robot_name) - if group_id is not None: - goal = self._normalize_joint_target(group_id, joints) - if goal is None: - return self._fail("Invalid joint target") - start = self._selected_joint_state((group_id,)) - if start is None: - return self._fail("No joint state") - return self._plan_selected_path((group_id,), start, goal) - return self._plan_path_only(robot_name, robot_id, joints) + if group_id is None: + return False + return self.plan_to_joint_targets({group_id: joints}) @rpc def plan_to_joint_targets( @@ -1004,85 +914,26 @@ def plan_to_joint_targets( goal = JointState(name=goal_names, position=goal_positions) return self._plan_selected_path(group_ids, start, goal) - def _plan_path_only( - self, robot_name: RobotName, robot_id: WorldRobotID, goal: JointState - ) -> bool: - """Plan path from current position to goal, store result.""" - assert self._world_monitor and self._planner # guaranteed by _begin_planning - self._dismiss_preview(robot_id) - start = self._world_monitor.get_current_joint_state(robot_id) - if start is None: - return self._fail("No joint state") - - # Trim goal to planner DOF (e.g. strip gripper joint from coordinator state) - planner_dof = len(start.position) - if len(goal.position) > planner_dof: - goal = JointState( - name=list(goal.name[:planner_dof]) if goal.name else [], - position=list(goal.position[:planner_dof]), - ) - - result = self._planner.plan_joint_path( - world=self._world_monitor.world, - robot_id=robot_id, - start=start, - goal=goal, - timeout=self.config.planning_timeout, - ) - if not result.is_success(): - return self._fail(f"Planning failed: {result.status.name}") - - logger.info(f"Path: {len(result.path)} waypoints") - self._planned_paths[robot_name] = result.path - - _, _, traj_gen = self._robots[robot_name] - # Convert JointState path to list of position lists for trajectory generator - traj = traj_gen.generate([list(state.position) for state in result.path]) - self._planned_trajectories[robot_name] = traj - logger.info(f"Trajectory: {traj.duration:.3f}s") - - self._state = ManipulationState.COMPLETED - return True - @rpc def preview_plan( self, plan: GeneratedPlan | None = None, duration: float | None = None, robot_name: RobotName | None = None, - target_fps: float = 30.0, ) -> bool: """Preview a generated plan, defaulting to `_last_plan` when omitted.""" if self._world_monitor is None: return False - plan = plan or getattr(self, "_last_plan", None) + plan = plan or self._last_plan if plan is None or not plan.path: logger.warning("No generated plan to preview") return False - - robot_names = [robot_name] if robot_name is not None else self._affected_robot_names(plan) - previewed = False - for name in robot_names: - robot = self._get_robot(name) - if robot is None: - return False - resolved_name, robot_id, _, _ = robot - planned_path = self._project_plan_path_for_robot(plan, resolved_name) - if not planned_path: - logger.warning(f"No planned path to preview for {resolved_name}") - return False - trajectory = self._trajectory_for_robot_plan(plan, resolved_name) - animation_duration = ( - duration - if duration is not None - else (trajectory.duration if trajectory is not None else 3.0) - ) - interpolated = self._interpolate_preview_path( - planned_path, trajectory, animation_duration, target_fps - ) - self._world_monitor.animate_path(robot_id, interpolated, animation_duration) - previewed = True - return previewed + if robot_name is not None and robot_name not in self._affected_robot_names(plan): + logger.error("Generated plan does not affect robot '%s'", robot_name) + return False + animation_duration = duration if duration is not None else 3.0 + self._world_monitor.animate_plan(plan, animation_duration) + return True @rpc def preview_path( @@ -1091,42 +942,15 @@ def preview_path( robot_name: RobotName | None = None, target_fps: float = 30.0, ) -> bool: - """Preview the planned path in the visualizer. + """Preview the last generated plan in the visualizer. Args: - duration: Total animation duration in seconds. Uses trajectory duration if None. - robot_name: Robot to preview (required if multiple robots configured) - target_fps: Nominal preview update rate. Set <= 0 to use planned waypoints directly. + duration: Total animation duration in seconds. + robot_name: Optional compatibility filter; the stored plan must affect it. + target_fps: Deprecated; generated-plan preview timing is backend-owned. """ - last_plan = getattr(self, "_last_plan", None) - if last_plan is not None and last_plan.path: - return self.preview_plan(last_plan, duration, robot_name, target_fps) - - if self._world_monitor is None: - return False - - robot = self._get_robot(robot_name) - if robot is None: - return False - robot_name, robot_id, _, _ = robot - - planned_path = self._planned_paths.get(robot_name) - if planned_path is None or len(planned_path) == 0: - logger.warning(f"No planned path to preview for {robot_name}") - return False - - if duration is None: - trajectory = self._planned_trajectories.get(robot_name) - animation_duration = trajectory.duration if trajectory is not None else 3.0 - else: - trajectory = self._planned_trajectories.get(robot_name) - animation_duration = duration - - interpolated = self._interpolate_preview_path( - planned_path, trajectory, animation_duration, target_fps - ) - self._world_monitor.animate_path(robot_id, interpolated, animation_duration) - return True + _ = target_fps + return self.preview_plan(self._last_plan, duration, robot_name) @rpc def has_planned_path(self) -> bool: @@ -1135,16 +959,7 @@ def has_planned_path(self) -> bool: Returns: True if a path is planned and ready """ - last_plan = getattr(self, "_last_plan", None) - if last_plan is not None: - return bool(last_plan.path) - - robot = self._get_robot() - if robot is None: - return False - robot_name, _, _, _ = robot - path = self._planned_paths.get(robot_name) - return path is not None and len(path) > 0 + return self._last_plan is not None and bool(self._last_plan.path) @rpc def get_visualization_url(self) -> str | None: @@ -1332,43 +1147,14 @@ def _get_coordinator_client(self) -> RPCClient | None: @rpc def execute(self, robot_name: RobotName | None = None) -> bool: """Execute planned trajectory via ControlCoordinator.""" - last_plan = getattr(self, "_last_plan", None) - if last_plan is not None and last_plan.path: - return self.execute_plan(last_plan, robot_name) - - if (robot := self._get_robot(robot_name)) is None: - return False - robot_name, _, config, _ = robot - - if (traj := self._planned_trajectories.get(robot_name)) is None: - logger.warning("No planned trajectory") - return False - if not config.coordinator_task_name: - logger.error(f"No coordinator_task_name for '{robot_name}'") - return False - if (client := self._get_coordinator_client()) is None: - logger.error("No coordinator client") - return False - - logger.info( - f"Executing: task='{config.coordinator_task_name}', {len(traj.points)} pts, {traj.duration:.2f}s" - ) - - self._state = ManipulationState.EXECUTING - result = client.task_invoke(config.coordinator_task_name, "execute", {"trajectory": traj}) - if result: - logger.info("Trajectory accepted") - self._state = ManipulationState.COMPLETED - return True - else: - return self._fail("Coordinator rejected trajectory") + return self.execute_plan(self._last_plan, robot_name) @rpc def execute_plan( self, plan: GeneratedPlan | None = None, robot_name: RobotName | None = None ) -> bool: """Project and execute a generated plan through affected trajectory tasks.""" - plan = plan or getattr(self, "_last_plan", None) + plan = plan or self._last_plan if plan is None or not plan.path: logger.warning("No generated plan") return False @@ -1394,7 +1180,8 @@ def execute_plan( if not config.coordinator_task_name: logger.error(f"No coordinator_task_name for '{resolved_name}'") return False - trajectory = self._trajectory_for_robot_plan(plan, resolved_name) + projected_path = self._project_plan_to_robot_path_for_execution(plan, resolved_name) + trajectory = self._trajectory_from_robot_path(resolved_name, projected_path) if trajectory is None: return False dispatches.append((resolved_name, config, trajectory)) @@ -1421,7 +1208,7 @@ def execute_plan( @rpc def get_trajectory_status(self, robot_name: RobotName | None = None) -> dict[str, Any] | None: """Get trajectory execution status via coordinator task_invoke.""" - last_plan = getattr(self, "_last_plan", None) + last_plan = self._last_plan if robot_name is None and last_plan is not None and last_plan.path: statuses = { name: self.get_trajectory_status(name) @@ -1583,7 +1370,7 @@ def _wait_for_trajectory_completion( Returns: True if trajectory completed successfully """ - last_plan = getattr(self, "_last_plan", None) + last_plan = self._last_plan if robot_name is None and last_plan is not None and last_plan.path: try: robot_names = self._affected_robot_names(last_plan) diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index cb565b2b3f..80e24f2bef 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -31,7 +31,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from collections.abc import Generator + from collections.abc import Generator, Sequence import numpy as np from numpy.typing import NDArray @@ -39,6 +39,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( CollisionObjectMessage, + GeneratedPlan, JointPath, Obstacle, PlanningGroupID, @@ -459,25 +460,20 @@ def publish_visualization(self) -> None: if self._visualization is not None: self._visualization.publish_visualization() - def show_preview(self, robot_id: WorldRobotID) -> None: - """Show the preview representation for a robot if visualization is available.""" + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Show preview representation for planning groups if visualization is available.""" if self._visualization is not None: - self._visualization.show_preview(robot_id) + self._visualization.show_preview(group_ids) - def hide_preview(self, robot_id: WorldRobotID) -> None: - """Hide the preview representation for a robot if visualization is available.""" + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Hide preview representation for planning groups if visualization is available.""" if self._visualization is not None: - self._visualization.hide_preview(robot_id) + self._visualization.hide_preview(group_ids) - def animate_path( - self, - robot_id: WorldRobotID, - path: JointPath, - duration: float = 3.0, - ) -> None: - """Animate a path if visualization is available.""" + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + """Animate a generated plan if visualization is available.""" if self._visualization is not None: - self._visualization.animate_path(robot_id, path, duration) + self._visualization.animate_plan(plan, duration) def start_visualization_thread(self, rate_hz: float = 10.0) -> None: """Start background thread for visualization updates at given rate.""" diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 641dc8d62c..6fb00f75b9 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -210,13 +210,36 @@ def plan_selected_joint_path( "the robot controllable joint set exactly", ) - return self.plan_joint_path( + local_start = _global_joint_state_to_local( + start, + robot_config.name, + list(robot_config.joint_names), + selected_joint_names, + ) + local_goal = _global_joint_state_to_local( + goal, + robot_config.name, + list(robot_config.joint_names), + selected_joint_names, + ) + result = self.plan_joint_path( world=world, robot_id=robot_id, - start=_order_joint_state(start, selected_joint_names), - goal=_order_joint_state(goal, selected_joint_names), + start=local_start, + goal=local_goal, timeout=timeout, ) + if not result.is_success(): + return result + return PlanningResult( + status=result.status, + path=_local_path_to_global(result.path, robot_config.name, selected_joint_names), + planning_time=result.planning_time, + path_length=result.path_length, + iterations=result.iterations, + message=result.message, + timestamps=result.timestamps, + ) def _plan_multi_robot_selected_joint_path( self, @@ -678,6 +701,44 @@ def _robot_joint_state_from_combined( ) +def _global_joint_state_to_local( + joint_state: JointState, + robot_name: str, + robot_joint_names: list[str], + global_joint_names: list[str], +) -> JointState: + position_by_name = dict(zip(joint_state.name, joint_state.position, strict=True)) + local_joint_names = [ + local_joint_name_from_global(robot_name, name) for name in global_joint_names + ] + if local_joint_names != robot_joint_names: + raise ValueError("Global selected joints do not match robot joint order") + return JointState( + name=robot_joint_names, + position=[position_by_name[global_name] for global_name in global_joint_names], + ) + + +def _local_path_to_global( + path: JointPath, + robot_name: str, + global_joint_names: list[str], +) -> JointPath: + local_joint_names = [ + local_joint_name_from_global(robot_name, name) for name in global_joint_names + ] + global_path: JointPath = [] + for waypoint in path: + position_by_name = dict(zip(waypoint.name, waypoint.position, strict=True)) + global_path.append( + JointState( + name=global_joint_names, + position=[position_by_name[local_name] for local_name in local_joint_names], + ) + ) + return global_path + + def _coupled_config_collision_free( world: WorldSpec, robot_order: list[WorldRobotID], diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py index 5262916580..3c91c719dd 100644 --- a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -81,6 +81,8 @@ def __init__( self._robot_configs = robot_configs self._coupled_collision_predicate = coupled_collision_predicate self.coupled_collision_checks = 0 + self.config_collision_names: list[list[str]] = [] + self.edge_collision_names: list[tuple[list[str], list[str]]] = [] def resolve_planning_groups( self, group_ids: tuple[str, ...] @@ -94,6 +96,7 @@ def get_robot_ids(self) -> list[str]: return list(self._robot_configs) def check_config_collision_free(self, robot_id: str, joint_state: JointState) -> bool: + self.config_collision_names.append(list(joint_state.name)) return True def get_joint_limits(self, robot_id: str) -> tuple[np.ndarray, np.ndarray]: @@ -107,6 +110,7 @@ def check_edge_collision_free( goal: JointState, step_size: float, ) -> bool: + self.edge_collision_names.append((list(start.name), list(goal.name))) return True def scratch_context(self) -> nullcontext[dict[str, JointState]]: @@ -188,6 +192,37 @@ def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> No assert world.coupled_collision_checks > 0 +def test_plan_selected_joint_path_converts_single_robot_backend_boundary_to_local() -> None: + world = _SelectionWorld( + groups={ + "arm/manipulator": _group( + "arm/manipulator", + "robot_1", + "arm", + ("arm/joint1", "arm/joint2"), + ) + }, + robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, + ) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + ["arm/manipulator"], + start=_joint_state(["arm/joint2", "arm/joint1"], [0.2, 0.1]), + goal=_joint_state(["arm/joint1", "arm/joint2"], [0.3, 0.4]), + ) + + assert result.status == PlanningStatus.SUCCESS + assert [waypoint.name for waypoint in result.path] == [ + ["arm/joint1", "arm/joint2"], + ["arm/joint1", "arm/joint2"], + ] + assert result.path[0].position == [0.1, 0.2] + assert result.path[-1].position == [0.3, 0.4] + assert world.config_collision_names == [["joint1", "joint2"], ["joint1", "joint2"]] + assert world.edge_collision_names == [(["joint1", "joint2"], ["joint1", "joint2"])] + + def test_plan_selected_joint_path_rejects_cross_robot_coupled_goal_collision() -> None: def coupled_free(ctx: dict[str, JointState]) -> bool: if {"left_robot", "right_robot"} - set(ctx): diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/planning_group_utils.py index d3cbfe6a02..34c1874177 100644 --- a/dimos/manipulation/planning/planning_group_utils.py +++ b/dimos/manipulation/planning/planning_group_utils.py @@ -110,11 +110,11 @@ def filter_joint_state_to_selected_joints( return JointState({"name": list(global_joint_names), "position": selected_positions}) -def normalize_joint_target_for_group( +def joint_target_to_global_names( group: ResolvedPlanningGroup, target: JointState, ) -> JointState: - """Normalize a group joint target to global joint names in group order. + """Convert a group joint target to global joint names in group order. Named targets may use either the public global planning names or the robot-local model names used by legacy robot-scoped callers, but the two diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 1365e1c2bc..18209cad24 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,6 +21,7 @@ from pydantic import Field from dimos.core.module import ModuleConfig +from dimos.manipulation.planning.planning_groups import FALLBACK_PLANNING_GROUP_NAME from dimos.manipulation.planning.planning_identifiers import ( assert_local_joint_names, assert_valid_robot_name, @@ -93,3 +94,13 @@ def model_post_init(self, __context: object) -> None: """Validate delimiter-based naming constraints.""" assert_valid_robot_name(self.name) assert_local_joint_names(self.joint_names) + if not self.planning_groups: + self.planning_groups = [ + PlanningGroupDefinition( + name=FALLBACK_PLANNING_GROUP_NAME, + joint_names=tuple(self.joint_names), + base_link=self.base_link, + tip_link=self.end_effector_link, + source="fallback", + ) + ] diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index 851bc5a2e7..bb761088a4 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -23,6 +23,7 @@ from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable if TYPE_CHECKING: + from collections.abc import Sequence from contextlib import AbstractContextManager import numpy as np @@ -30,8 +31,8 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( + GeneratedPlan, IKResult, - JointPath, Obstacle, PlanningGroupDescriptor, PlanningGroupID, @@ -220,16 +221,16 @@ def publish_visualization(self, ctx: Any | None = None) -> None: """Publish current state to visualization.""" ... - def show_preview(self, robot_id: WorldRobotID) -> None: - """Show the preview representation for a robot.""" + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Show preview representations for the selected planning groups.""" ... - def hide_preview(self, robot_id: WorldRobotID) -> None: - """Hide the preview representation for a robot.""" + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Hide preview representations for the selected planning groups.""" ... - def animate_path(self, robot_id: WorldRobotID, path: JointPath, duration: float = 3.0) -> None: - """Animate a path in visualization.""" + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + """Animate a generated plan in visualization.""" ... def close(self) -> None: diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_utils.py index fa25dac217..826dd2eab7 100644 --- a/dimos/manipulation/planning/test_planning_group_utils.py +++ b/dimos/manipulation/planning/test_planning_group_utils.py @@ -18,7 +18,7 @@ import pytest -from dimos.manipulation.planning.planning_group_utils import normalize_joint_target_for_group +from dimos.manipulation.planning.planning_group_utils import joint_target_to_global_names from dimos.manipulation.planning.spec.models import ResolvedPlanningGroup from dimos.msgs.sensor_msgs.JointState import JointState @@ -40,7 +40,7 @@ def test_normalize_joint_target_accepts_named_global_targets_in_group_order() -> group = _make_group() target = JointState(name=["left/j3", "left/j1", "left/j2"], position=[3.0, 1.0, 2.0]) - normalized = normalize_joint_target_for_group(group, target) + normalized = joint_target_to_global_names(group, target) assert normalized.name == ["left/j1", "left/j2", "left/j3"] assert normalized.position == [1.0, 2.0, 3.0] @@ -50,7 +50,7 @@ def test_normalize_joint_target_accepts_named_local_targets_in_group_order() -> group = _make_group() target = JointState(name=["j2", "j3", "j1"], position=[2.0, 3.0, 1.0]) - normalized = normalize_joint_target_for_group(group, target) + normalized = joint_target_to_global_names(group, target) assert normalized.name == ["left/j1", "left/j2", "left/j3"] assert normalized.position == [1.0, 2.0, 3.0] @@ -61,4 +61,4 @@ def test_normalize_joint_target_rejects_mixed_global_and_local_target_names() -> target = JointState(name=["left/j1", "j2", "left/j3"], position=[1.0, 2.0, 3.0]) with pytest.raises(ValueError, match="mixes global and local joint names"): - normalize_joint_target_for_group(group, target) + joint_target_to_global_names(group, target) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 666672b048..2d39f37e71 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -35,7 +35,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( - JointPath, + GeneratedPlan, Obstacle, PlanningGroupDescriptor, PlanningGroupID, @@ -47,7 +47,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from collections.abc import Generator + from collections.abc import Generator, Sequence from numpy.typing import NDArray @@ -803,7 +803,7 @@ def finalize(self) -> None: self.publish_visualization() # Hide all preview robots initially for robot_id in self._robots: - self.hide_preview(robot_id) + self._hide_preview_robot(robot_id) @property def is_finalized(self) -> bool: @@ -1233,8 +1233,18 @@ def _set_preview_positions( full_positions[idx] = positions[i] self._plant.SetPositions(plant_ctx, full_positions) - def show_preview(self, robot_id: WorldRobotID) -> None: - """Show the preview (yellow ghost) robot in Meshcat.""" + def _preview_robot_ids_for_groups( + self, group_ids: Sequence[PlanningGroupID] + ) -> list[WorldRobotID]: + """Resolve planning groups to stable preview robot IDs.""" + robot_ids: list[WorldRobotID] = [] + for group in self.resolve_planning_groups(tuple(group_ids)): + if group.robot_id not in robot_ids: + robot_ids.append(group.robot_id) + return robot_ids + + def _show_preview_robot(self, robot_id: WorldRobotID) -> None: + """Show one preview (yellow ghost) robot in Meshcat.""" if self._meshcat is None: return robot_data = self._robots.get(robot_id) @@ -1243,8 +1253,13 @@ def show_preview(self, robot_id: WorldRobotID) -> None: model_name = self._plant.GetModelInstanceName(robot_data.preview_model_instance) self._meshcat.SetProperty(f"visualizer/{model_name}", "visible", True) - def hide_preview(self, robot_id: WorldRobotID) -> None: - """Hide the preview (yellow ghost) robot in Meshcat.""" + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Show preview robots affected by planning groups.""" + for robot_id in self._preview_robot_ids_for_groups(group_ids): + self._show_preview_robot(robot_id) + + def _hide_preview_robot(self, robot_id: WorldRobotID) -> None: + """Hide one preview (yellow ghost) robot in Meshcat.""" if self._meshcat is None: return robot_data = self._robots.get(robot_id) @@ -1253,32 +1268,63 @@ def hide_preview(self, robot_id: WorldRobotID) -> None: model_name = self._plant.GetModelInstanceName(robot_data.preview_model_instance) self._meshcat.SetProperty(f"visualizer/{model_name}", "visible", False) - def animate_path( + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Hide preview robots affected by planning groups.""" + for robot_id in self._preview_robot_ids_for_groups(group_ids): + self._hide_preview_robot(robot_id) + + def _preview_positions_for_waypoint( self, robot_id: WorldRobotID, - path: JointPath, - duration: float = 3.0, - ) -> None: - """Animate a path using the preview (yellow ghost) robot. + selected_positions_by_name: dict[str, float], + current_positions: NDArray[np.float64], + ) -> NDArray[np.float64]: + """Build full local preview positions for one robot from selected globals.""" + robot_data = self._robots[robot_id] + positions = current_positions.copy() + for index, local_name in enumerate(robot_data.config.joint_names): + global_name = make_global_joint_name(robot_data.config.name, local_name) + if global_name in selected_positions_by_name: + positions[index] = selected_positions_by_name[global_name] + return positions + + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + """Animate a generated plan using preview (yellow ghost) robots. The preview stays visible after animation completes. """ - if self._meshcat is None or len(path) < 2: + if self._meshcat is None or len(plan.path) < 2: return - robot_data = self._robots.get(robot_id) - if robot_data is None or robot_data.preview_model_instance is None: + robot_ids = [ + robot_id + for robot_id in self._preview_robot_ids_for_groups(plan.group_ids) + if self._robots[robot_id].preview_model_instance is not None + ] + if not robot_ids: return import time - self.show_preview(robot_id) - dt = duration / (len(path) - 1) - for joint_state in path: - positions = np.array(joint_state.position, dtype=np.float64) + self.show_preview(plan.group_ids) + dt = duration / (len(plan.path) - 1) + for joint_state in plan.path: + selected_positions_by_name = dict( + zip(joint_state.name, joint_state.position, strict=True) + ) with self._lock: assert self._plant_context is not None - self._set_preview_positions(self._plant_context, robot_id, positions) + for robot_id in robot_ids: + robot_data = self._robots[robot_id] + current_positions = self._plant.GetPositions( + self._plant_context, robot_data.model_instance + ) + positions = self._preview_positions_for_waypoint( + robot_id, + selected_positions_by_name, + np.array(current_positions, dtype=np.float64), + ) + self._set_preview_positions(self._plant_context, robot_id, positions) self.publish_visualization() time.sleep(dt) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 17cf991917..4ea115a945 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -355,7 +355,6 @@ def test_execute_requires_trajectory(self, robot_config): def test_execute_requires_task_name(self): """Execute fails without coordinator_task_name.""" - module = _make_module() config_no_task = RobotModelConfig( name="arm", model_path=Path("/path"), @@ -363,20 +362,47 @@ def test_execute_requires_task_name(self): joint_names=["j1"], end_effector_link="ee", ) - module._robots = {"arm": ("id", config_no_task, MagicMock())} - module._planned_trajectories = {"arm": MagicMock()} + module = _make_module_with_monitor(config_no_task) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("arm", "manipulator", ["j1"]) + ] + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1"], position=[0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[1.0])], + status=PlanningStatus.SUCCESS, + ) assert module.execute() is False def test_execute_success(self, robot_config, simple_trajectory): """Successful execute calls coordinator via task_invoke.""" - module = _make_module() - module._robots = {"test_arm": ("id", robot_config, MagicMock())} - global_trajectory = JointTrajectory( - joint_names=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], - points=simple_trajectory.points, + module = _make_module_with_monitor(robot_config) + generator = MagicMock() + generator.generate.return_value = simple_trajectory + module._robots = {"test_arm": ("id", robot_config, generator)} + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"]) + ] + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("test_arm/manipulator",), + path=[ + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ), + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.5, 0.5, 0.5], + ), + ], + status=PlanningStatus.SUCCESS, ) - module._planned_trajectories = {"test_arm": global_trajectory} mock_client = MagicMock() mock_client.task_invoke.return_value = True @@ -384,15 +410,42 @@ def test_execute_success(self, robot_config, simple_trajectory): assert module.execute() is True assert module._state == ManipulationState.COMPLETED - mock_client.task_invoke.assert_called_once_with( - "traj_arm", "execute", {"trajectory": global_trajectory} - ) + mock_client.task_invoke.assert_called_once() + assert mock_client.task_invoke.call_args.args[:2] == ("traj_arm", "execute") + trajectory = mock_client.task_invoke.call_args.args[2]["trajectory"] + assert trajectory.joint_names == [ + "test_arm/joint1", + "test_arm/joint2", + "test_arm/joint3", + ] + assert trajectory.points == simple_trajectory.points def test_execute_rejected(self, robot_config, simple_trajectory): """Rejected execution sets FAULT state.""" - module = _make_module() - module._robots = {"test_arm": ("id", robot_config, MagicMock())} - module._planned_trajectories = {"test_arm": simple_trajectory} + module = _make_module_with_monitor(robot_config) + generator = MagicMock() + generator.generate.return_value = simple_trajectory + module._robots = {"test_arm": ("id", robot_config, generator)} + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"]) + ] + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("test_arm/manipulator",), + path=[ + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ), + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.5, 0.5, 0.5], + ), + ], + status=PlanningStatus.SUCCESS, + ) mock_client = MagicMock() mock_client.task_invoke.return_value = False @@ -623,11 +676,21 @@ def test_visualization_routing_and_stop_all_monitors(self): assert monitor.get_visualization_url() == "123" monitor.publish_visualization() - monitor.show_preview("robot") - monitor.hide_preview("robot") - monitor.animate_path("robot", [1, 2, 3], 4.5) + group_ids = ("robot/manipulator",) + plan = GeneratedPlan( + group_ids=group_ids, + path=[JointState(name=["robot/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) + monitor.show_preview(group_ids) + monitor.hide_preview(group_ids) + monitor.animate_plan(plan, 4.5) assert monitor.visualization is viz + viz.show_preview.assert_called_once_with(group_ids) + viz.hide_preview.assert_called_once_with(group_ids) + viz.animate_plan.assert_called_once_with(plan, 4.5) + monitor.stop_all_monitors() viz.close.assert_called_once() @@ -639,9 +702,14 @@ def test_visualization_none_is_noop(self): assert monitor.get_visualization_url() is None monitor.publish_visualization() - monitor.show_preview("robot") - monitor.hide_preview("robot") - monitor.animate_path("robot", [1], 1.0) + plan = GeneratedPlan( + group_ids=("robot/manipulator",), + path=[JointState(name=["robot/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) + monitor.show_preview(("robot/manipulator",)) + monitor.hide_preview(("robot/manipulator",)) + monitor.animate_plan(plan, 1.0) monitor.start_visualization_thread() assert monitor._viz_thread is None @@ -650,93 +718,83 @@ class TestManipulationPreview: def test_dismiss_preview_noop_without_monitor(self): module = _make_module() - module._dismiss_preview("robot_id") + module._dismiss_preview(("arm/manipulator",)) def test_dismiss_preview_routes_to_monitor(self): module = _make_module() module._world_monitor = MagicMock() - module._dismiss_preview("robot_id") + group_ids = ("arm/manipulator",) + module._dismiss_preview(group_ids) - module._world_monitor.hide_preview.assert_called_once_with("robot_id") + module._world_monitor.hide_preview.assert_called_once_with(group_ids) module._world_monitor.publish_visualization.assert_called_once_with() - def test_preview_path_uses_trajectory_duration_and_interpolates(self): + def test_preview_path_delegates_last_plan_with_default_duration(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [2.0])} - module._planned_trajectories = {"arm": _make_trajectory((0.0, [0.0]), (2.0, [2.0]))} + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) - assert module.preview_path(robot_name="arm", target_fps=2.0) is True + assert module.preview_path(target_fps=2.0) is True - module._world_monitor.animate_path.assert_called_once() - robot_id, preview_path, duration = module._world_monitor.animate_path.call_args.args - assert robot_id == "robot_id" - assert duration == 2.0 - assert [state.position for state in preview_path] == [[0.0], [0.5], [1.0], [1.5], [2.0]] + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 3.0) - def test_preview_path_explicit_duration_overrides_and_fps_densifies(self): + def test_preview_path_explicit_duration_overrides_default(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [9.0])} - module._planned_trajectories = {"arm": _make_trajectory((0.0, [0.0]), (9.0, [9.0]))} + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) - assert module.preview_path(duration=1.5, robot_name="arm", target_fps=2.0) is True + assert module.preview_path(duration=1.5, target_fps=2.0) is True - module._world_monitor.animate_path.assert_called_once() - robot_id, preview_path, duration = module._world_monitor.animate_path.call_args.args - assert robot_id == "robot_id" - assert duration == 1.5 - assert [state.position for state in preview_path] == [[0.0], [3.0], [6.0], [9.0]] + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 1.5) - def test_preview_path_missing_trajectory_uses_default_duration(self): + def test_preview_path_respects_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [1.0])} - module._planned_trajectories = {} + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("arm", "manipulator", ["j1"]) + ] + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) - assert module.preview_path(robot_name="arm", target_fps=10.0) is True + assert module.preview_path(robot_name="arm") is True - module._world_monitor.animate_path.assert_called_once_with( - "robot_id", module._planned_paths["arm"], 3.0 - ) + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 3.0) - def test_preview_path_skips_interpolation_for_nonpositive_fps_or_duration(self): + def test_preview_path_rejects_unaffected_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [1.0])} - module._planned_trajectories = {"arm": _make_trajectory((0.0, [0.0]), (2.0, [1.0]))} + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("arm", "manipulator", ["j1"]) + ] + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) - assert module.preview_path(robot_name="arm", target_fps=0.0) is True - assert module.preview_path(duration=0.0, robot_name="arm", target_fps=20.0) is True + assert module.preview_path(robot_name="other") is False - assert ( - module._world_monitor.animate_path.call_args_list[0].args[1] - == module._planned_paths["arm"] - ) - assert ( - module._world_monitor.animate_path.call_args_list[1].args[1] - == module._planned_paths["arm"] - ) + module._world_monitor.animate_plan.assert_not_called() def test_preview_path_returns_false_for_missing_inputs(self): module = _make_module() - module._planned_paths = {"arm": _make_path([0.0], [1.0])} - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - assert module.preview_path(robot_name="arm") is False + assert module.preview_path() is False module._world_monitor = MagicMock() - module._robots = {} - assert module.preview_path(robot_name="arm") is False - - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": []} - assert module.preview_path(robot_name="arm") is False + assert module.preview_path() is False class TestGeneratedPlanProjection: @@ -867,16 +925,12 @@ def test_project_plan_uses_global_names_for_robots_with_shared_local_names(self) assert [state.name for state in right_projected] == [["j1", "j2"], ["j1", "j2"]] assert [state.position for state in right_projected] == [[3.0, 40.0], [4.0, 40.0]] - def test_preview_path_with_last_plan_projects_lazily_to_world_monitor(self): + def test_preview_path_with_last_plan_animates_generated_plan(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) - module._robots["left"] = ("robot_left", config, _trajectory_generator()) module._world_monitor.world.resolve_planning_groups.return_value = [ _make_global_group("left", "arm", ["j1"]) ] - module._world_monitor.get_current_joint_state.return_value = JointState( - name=["j1", "j2"], position=[0.0, 7.0] - ) module._last_plan = GeneratedPlan( group_ids=("left/arm",), path=[ @@ -888,11 +942,7 @@ def test_preview_path_with_last_plan_projects_lazily_to_world_monitor(self): assert module.preview_path(robot_name="left", target_fps=0.0) is True - module._world_monitor.animate_path.assert_called_once() - robot_id, path, duration = module._world_monitor.animate_path.call_args.args - assert robot_id == "robot_left" - assert duration == 1.0 - assert [state.position for state in path] == [[1.0, 7.0], [2.0, 7.0]] + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 3.0) def test_has_and_clear_planned_path_use_last_plan(self): module = _make_module() From b3470bd9a9b3fbd58f1342944fbfd56c1896b18b Mon Sep 17 00:00:00 2001 From: cc <55869557+TomCC7@users.noreply.github.com> Date: Fri, 19 Jun 2026 14:35:55 -0700 Subject: [PATCH 12/27] Apply suggestions from code review Co-authored-by: cc <55869557+TomCC7@users.noreply.github.com> --- dimos/manipulation/manipulation_module.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index a963b3a5d3..c9cb5eef2f 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -293,8 +293,6 @@ def _on_joint_state(self, msg: JointState) -> None: if self._world_monitor is None: return - if not msg.name: - raise ValueError("Aggregate joint states must include global joint names") assert_global_joint_names(msg.name) # Build name → index map once for the whole message From a3b8459912a3370177841ccd3eb3a8d7f0bd1601 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:12:59 -0700 Subject: [PATCH 13/27] docs: note pose group tf follow-up --- dimos/manipulation/planning/planning_group_utils.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/planning_group_utils.py index 34c1874177..3b2db4701c 100644 --- a/dimos/manipulation/planning/planning_group_utils.py +++ b/dimos/manipulation/planning/planning_group_utils.py @@ -60,6 +60,8 @@ def primary_pose_planning_group_id_for_robot( robot_name: RobotName, ) -> PlanningGroupID | None: """Return the first pose-targetable group ID for compatibility paths.""" + # TODO: Replace this compatibility selection with either one TF publication per + # pose-targetable planning group or backend-level whole-robot TF publishing. for group in groups: if group.robot_name == robot_name and group.has_pose_target: return group.id From 1671ddba9eaf599de54c352394dc8c94743e0bbb Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:14:53 -0700 Subject: [PATCH 14/27] refactor: split selected joint state collection --- dimos/manipulation/manipulation_module.py | 28 +++++++++++++---------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c9cb5eef2f..a7c4087498 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -544,21 +544,25 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint """Collect current state for exactly the selected global joints.""" assert self._world_monitor is not None resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) - names: list[str] = [] - positions: list[float] = [] - current_by_robot: dict[WorldRobotID, dict[str, float]] = {} + robot_names_by_id: dict[WorldRobotID, RobotName] = {} for group in resolved_groups: - if group.robot_id not in current_by_robot: - current = self._world_monitor.get_current_joint_state(group.robot_id) - if current is None: - logger.error("No joint state for robot '%s'", group.robot_name) - return None - indexed_current = self._current_positions_by_name(group.robot_name, current) - if indexed_current is None: - return None - current_by_robot[group.robot_id] = indexed_current + robot_names_by_id.setdefault(group.robot_id, group.robot_name) + + current_by_robot: dict[WorldRobotID, dict[str, float]] = {} + for robot_id, robot_name in robot_names_by_id.items(): + current = self._world_monitor.get_current_joint_state(robot_id) + if current is None: + logger.error("No joint state for robot '%s'", robot_name) + return None + indexed_current = self._current_positions_by_name(robot_name, current) + if indexed_current is None: + return None + current_by_robot[robot_id] = indexed_current + names: list[str] = [] + positions: list[float] = [] + for group in resolved_groups: robot_state = current_by_robot[group.robot_id] for resolved_name, local_name in zip( group.joint_names, group.local_joint_names, strict=True From 288fca5891748dea88a1bb8999e8a86b0fa8c035 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:17:10 -0700 Subject: [PATCH 15/27] refactor: begin planning by group selection --- dimos/manipulation/manipulation_module.py | 36 ++++++++------------ dimos/manipulation/test_manipulation_unit.py | 7 ++-- 2 files changed, 19 insertions(+), 24 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index a7c4087498..b6cd6a169b 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -471,24 +471,18 @@ def is_collision_free(self, joints: list[float], robot_name: RobotName | None = return False def _begin_planning( - self, robot_name: RobotName | None = None - ) -> tuple[RobotName, WorldRobotID] | None: - """Check state and begin planning. Returns (robot_name, robot_id) or None. - - Args: - robot_name: Robot to plan for (required if multiple robots configured) - """ + self, group_ids: Sequence[PlanningGroupID] + ) -> tuple[PlanningGroupID, ...] | None: + """Check state and begin planning for the selected planning groups.""" if self._world_monitor is None: logger.error("Planning not initialized") return None - if (robot := self._get_robot(robot_name)) is None: - return None with self._lock: if self._state not in (ManipulationState.IDLE, ManipulationState.COMPLETED): logger.warning(f"Cannot plan: state is {self._state.name}") return None self._state = ManipulationState.PLANNING - return robot[0], robot[1] + return tuple(group_ids) def _fail(self, msg: str) -> bool: """Set FAULT state with error message.""" @@ -827,11 +821,6 @@ def plan_to_poses( return False if not pose_targets: return self._fail("At least one pose target is required") - with self._lock: - if self._state not in (ManipulationState.IDLE, ManipulationState.COMPLETED): - logger.warning(f"Cannot plan: state is {self._state.name}") - return False - self._state = ManipulationState.PLANNING stamped_targets = { planning_group_id_from_selector(group): PoseStamped( @@ -843,6 +832,10 @@ def plan_to_poses( } auxiliary_ids = tuple(planning_group_id_from_selector(group) for group in auxiliary_groups) group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) + planned_group_ids = self._begin_planning(group_ids) + if planned_group_ids is None: + return False + group_ids = planned_group_ids try: start = self._selected_joint_state(group_ids) @@ -889,13 +882,14 @@ def plan_to_joint_targets( return False if not joint_targets: return self._fail("At least one joint target is required") - with self._lock: - if self._state not in (ManipulationState.IDLE, ManipulationState.COMPLETED): - logger.warning(f"Cannot plan: state is {self._state.name}") - return False - self._state = ManipulationState.PLANNING - group_ids = tuple(planning_group_id_from_selector(group) for group in joint_targets) + group_ids = tuple( + dict.fromkeys(planning_group_id_from_selector(group) for group in joint_targets) + ) + planned_group_ids = self._begin_planning(group_ids) + if planned_group_ids is None: + return False + group_ids = planned_group_ids try: start = self._selected_joint_state(group_ids) except Exception as exc: diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 4ea115a945..5d615be1e6 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -155,19 +155,20 @@ def test_begin_planning_state_checks(self, robot_config): module = _make_module() module._world_monitor = MagicMock() module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + group_ids = ("test_arm/manipulator",) # From IDLE - OK module._state = ManipulationState.IDLE - assert module._begin_planning() == ("test_arm", "robot_id") + assert module._begin_planning(group_ids) == group_ids assert module._state == ManipulationState.PLANNING # From COMPLETED - OK module._state = ManipulationState.COMPLETED - assert module._begin_planning() == ("test_arm", "robot_id") + assert module._begin_planning(group_ids) == group_ids # From EXECUTING - Fail module._state = ManipulationState.EXECUTING - assert module._begin_planning() is None + assert module._begin_planning(group_ids) is None class TestRobotSelection: From 96f3fcca02e8599209abc0120e05f2f3d5a2f6c7 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:18:39 -0700 Subject: [PATCH 16/27] refactor: type drake pose transform --- dimos/manipulation/planning/world/drake_world.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 2d39f37e71..19df494c5e 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -93,7 +93,7 @@ logger = setup_logger() -def _pose_stamped_from_drake_transform(transform: Any) -> PoseStamped: +def _pose_stamped_from_drake_transform(transform: RigidTransform) -> PoseStamped: """Convert a Drake RigidTransform-like object to a world-frame pose.""" position = transform.translation() quaternion = transform.rotation().ToQuaternion() From d5788b28c79a7e3a10e03b17f8f33a636d43635c Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:19:51 -0700 Subject: [PATCH 17/27] refactor: name joint target conversion explicitly --- dimos/manipulation/manipulation_module.py | 10 +++++----- .../manipulation/planning/test_planning_group_utils.py | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index b6cd6a169b..13745a6025 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -570,7 +570,7 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint return JointState(name=names, position=positions) - def _normalize_joint_target( + def _joint_target_to_global_names( self, group_id: PlanningGroupID, target: JointState ) -> JointState | None: """Convert a group joint target to global joint names in group order.""" @@ -901,11 +901,11 @@ def plan_to_joint_targets( goal_positions: list[float] = [] for group, target in joint_targets.items(): group_id = planning_group_id_from_selector(group) - normalized = self._normalize_joint_target(group_id, target) - if normalized is None: + target_global = self._joint_target_to_global_names(group_id, target) + if target_global is None: return self._fail(f"Invalid joint target for '{group_id}'") - goal_names.extend(normalized.name) - goal_positions.extend(normalized.position) + goal_names.extend(target_global.name) + goal_positions.extend(target_global.position) goal = JointState(name=goal_names, position=goal_positions) return self._plan_selected_path(group_ids, start, goal) diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_utils.py index 826dd2eab7..dacfcf53a2 100644 --- a/dimos/manipulation/planning/test_planning_group_utils.py +++ b/dimos/manipulation/planning/test_planning_group_utils.py @@ -36,7 +36,7 @@ def _make_group() -> ResolvedPlanningGroup: ) -def test_normalize_joint_target_accepts_named_global_targets_in_group_order() -> None: +def test_joint_target_to_global_names_accepts_named_global_targets_in_group_order() -> None: group = _make_group() target = JointState(name=["left/j3", "left/j1", "left/j2"], position=[3.0, 1.0, 2.0]) @@ -46,7 +46,7 @@ def test_normalize_joint_target_accepts_named_global_targets_in_group_order() -> assert normalized.position == [1.0, 2.0, 3.0] -def test_normalize_joint_target_accepts_named_local_targets_in_group_order() -> None: +def test_joint_target_to_global_names_accepts_named_local_targets_in_group_order() -> None: group = _make_group() target = JointState(name=["j2", "j3", "j1"], position=[2.0, 3.0, 1.0]) @@ -56,7 +56,7 @@ def test_normalize_joint_target_accepts_named_local_targets_in_group_order() -> assert normalized.position == [1.0, 2.0, 3.0] -def test_normalize_joint_target_rejects_mixed_global_and_local_target_names() -> None: +def test_joint_target_to_global_names_rejects_mixed_global_and_local_target_names() -> None: group = _make_group() target = JointState(name=["left/j1", "j2", "left/j3"], position=[1.0, 2.0, 3.0]) From 4fcb7ef6438264600e2d32d7d906fb5bc6c3c4c9 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:27:42 -0700 Subject: [PATCH 18/27] refactor: execute generated plans directly --- dimos/manipulation/manipulation_module.py | 180 ++++++------------ .../manipulation/test_manipulation_module.py | 17 +- dimos/manipulation/test_manipulation_unit.py | 77 +++----- 3 files changed, 89 insertions(+), 185 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 13745a6025..c0932c7fc9 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -55,7 +55,6 @@ from dimos.manipulation.planning.planning_identifiers import ( assert_global_joint_names, assert_local_joint_names, - make_global_joint_name, make_global_joint_names, make_planning_group_id, ) @@ -64,7 +63,6 @@ from dimos.manipulation.planning.spec.models import ( GeneratedPlan, IKResult, - JointPath, Obstacle, PlanningGroupDescriptor, PlanningGroupID, @@ -98,12 +96,6 @@ RobotRegistry: TypeAlias = dict[RobotName, RobotEntry] """Maps robot_name -> RobotEntry""" -PlannedPaths: TypeAlias = dict[RobotName, JointPath] -"""Maps robot_name -> planned joint path""" - -PlannedTrajectories: TypeAlias = dict[RobotName, JointTrajectory] -"""Maps robot_name -> planned trajectory""" - class ManipulationState(Enum): """State machine for manipulation module.""" @@ -161,9 +153,7 @@ def __init__(self, **kwargs: Any) -> None: # Robot registry: maps robot_name -> (world_robot_id, config, trajectory_gen) self._robots: RobotRegistry = {} - # Stored path for plan/preview/execute workflow (per robot) - self._planned_paths: PlannedPaths = {} - self._planned_trajectories: PlannedTrajectories = {} + # Stored generated plan for preview/execute workflow. self._last_plan: GeneratedPlan | None = None # Coordinator integration (lazy initialized) @@ -582,86 +572,6 @@ def _joint_target_to_global_names( logger.error(str(exc)) return None - def _project_plan_path_for_robot(self, plan: GeneratedPlan, robot_name: RobotName) -> JointPath: - """Project combined plan path to one robot in configured local joint order. - - Generated plans only contain selected global joints. Trajectory tasks may - still be configured for the robot's full controllable joint set, so - non-selected joints are held at their current positions during projection. - """ - robot_id, config, _ = self._robots[robot_name] - global_joint_names = [ - make_global_joint_name(robot_name, joint) for joint in config.joint_names - ] - current_by_name: dict[str, float] = {} - if self._world_monitor is not None: - current = self._world_monitor.get_current_joint_state(robot_id) - if current is not None: - indexed_current = self._current_positions_by_name(robot_name, current) - if indexed_current is None: - return [] - current_by_name = indexed_current - projected: JointPath = [] - for waypoint in plan.path: - if len(waypoint.name) != len(waypoint.position): - logger.error( - "Cannot project plan for '%s': waypoint has %d names but %d positions", - robot_name, - len(waypoint.name), - len(waypoint.position), - ) - return [] - try: - assert_global_joint_names(waypoint.name) - except ValueError as exc: - logger.error("Cannot project plan for '%s': %s", robot_name, exc) - return [] - position_by_name = dict(zip(waypoint.name, waypoint.position, strict=True)) - positions: list[float] = [] - for local_name, global_name in zip( - config.joint_names, global_joint_names, strict=False - ): - if global_name in position_by_name: - positions.append(position_by_name[global_name]) - else: - if local_name in current_by_name: - positions.append(current_by_name[local_name]) - continue - logger.error( - "Cannot project plan for '%s': missing joint '%s'", - robot_name, - global_name, - ) - return [] - projected.append( - JointState( - name=list(config.joint_names), - position=positions, - ) - ) - return projected - - def _project_plan_to_robot_path_for_execution( - self, plan: GeneratedPlan, robot_name: RobotName - ) -> JointPath: - """Project a generated plan to one robot's local execution path.""" - return self._project_plan_path_for_robot(plan, robot_name) - - def _trajectory_from_robot_path( - self, robot_name: RobotName, path: JointPath - ) -> JointTrajectory | None: - """Generate a coordinator-facing trajectory from a local robot path.""" - if len(path) < 2: - logger.error("Plan projection for '%s' has fewer than two waypoints", robot_name) - return None - _, config, traj_gen = self._robots[robot_name] - trajectory = traj_gen.generate([list(state.position) for state in path]) - return JointTrajectory( - joint_names=make_global_joint_names(robot_name, config.joint_names), - points=trajectory.points, - timestamp=trajectory.timestamp, - ) - def _affected_robot_names(self, plan: GeneratedPlan) -> list[RobotName]: """Get stable robot names affected by a generated plan.""" assert self._world_monitor is not None @@ -675,7 +585,7 @@ def _affected_robot_names(self, plan: GeneratedPlan) -> list[RobotName]: def _store_generated_plan( self, group_ids: tuple[PlanningGroupID, ...], result: PlanningResult ) -> None: - """Store canonical generated plan and compatibility per-robot projections.""" + """Store the canonical generated plan.""" self._last_plan = GeneratedPlan( group_ids=group_ids, path=result.path, @@ -685,17 +595,6 @@ def _store_generated_plan( iterations=result.iterations, message=result.message, ) - self._planned_paths.clear() - self._planned_trajectories.clear() - for robot_name in self._affected_robot_names(self._last_plan): - projected_path = self._project_plan_to_robot_path_for_execution( - self._last_plan, robot_name - ) - if projected_path: - self._planned_paths[robot_name] = projected_path - trajectory = self._trajectory_from_robot_path(robot_name, projected_path) - if trajectory is not None: - self._planned_trajectories[robot_name] = trajectory def _plan_selected_path( self, group_ids: tuple[PlanningGroupID, ...], start: JointState, goal: JointState @@ -976,8 +875,6 @@ def clear_planned_path(self) -> bool: True if cleared """ self._last_plan = None - self._planned_paths.clear() - self._planned_trajectories.clear() return True @rpc @@ -1163,6 +1060,7 @@ def execute_plan( except Exception as exc: return self._fail(f"Failed to resolve generated plan: {exc}") robot_names = [robot_name] if robot_name is not None else affected + assert self._world_monitor is not None dispatches: list[tuple[RobotName, RobotModelConfig, JointTrajectory]] = [] for name in robot_names: @@ -1172,19 +1070,65 @@ def execute_plan( robot = self._get_robot(name) if robot is None: return False - resolved_name, _, config, _ = robot + resolved_name, robot_id, config, traj_gen = robot if not config.coordinator_task_name: logger.error(f"No coordinator_task_name for '{resolved_name}'") return False - projected_path = self._project_plan_to_robot_path_for_execution(plan, resolved_name) - trajectory = self._trajectory_from_robot_path(resolved_name, projected_path) - if trajectory is None: + + current_by_name: dict[str, float] = {} + current = self._world_monitor.get_current_joint_state(robot_id) + if current is not None: + indexed_current = self._current_positions_by_name(resolved_name, current) + if indexed_current is None: + return False + current_by_name = indexed_current + + global_joint_names = make_global_joint_names(resolved_name, config.joint_names) + local_path: list[JointState] = [] + for waypoint in plan.path: + if len(waypoint.name) != len(waypoint.position): + logger.error( + "Cannot execute plan for '%s': waypoint has %d names but %d positions", + resolved_name, + len(waypoint.name), + len(waypoint.position), + ) + return False + try: + assert_global_joint_names(waypoint.name) + except ValueError as exc: + logger.error("Cannot execute plan for '%s': %s", resolved_name, exc) + return False + selected_positions = dict(zip(waypoint.name, waypoint.position, strict=True)) + positions: list[float] = [] + for local_name, global_name in zip( + config.joint_names, global_joint_names, strict=True + ): + if global_name in selected_positions: + positions.append(selected_positions[global_name]) + elif local_name in current_by_name: + positions.append(current_by_name[local_name]) + else: + logger.error( + "Cannot execute plan for '%s': missing joint '%s'", + resolved_name, + global_name, + ) + return False + local_path.append(JointState(name=list(config.joint_names), position=positions)) + if len(local_path) < 2: + logger.error("Plan projection for '%s' has fewer than two waypoints", resolved_name) return False + local_trajectory = traj_gen.generate([list(state.position) for state in local_path]) + trajectory = JointTrajectory( + joint_names=list(global_joint_names), + points=local_trajectory.points, + timestamp=local_trajectory.timestamp, + ) dispatches.append((resolved_name, config, trajectory)) self._state = ManipulationState.EXECUTING - for name, config, trajectory in dispatches: - self._planned_trajectories[name] = trajectory + for _name, config, trajectory in dispatches: logger.info( "Executing: task='%s', %d pts, %.2fs", config.coordinator_task_name, @@ -1385,11 +1329,7 @@ def _wait_for_trajectory_completion( client = self._get_coordinator_client() if client is None or not config.coordinator_task_name: - # No coordinator — wait for trajectory duration as fallback - traj = self._planned_trajectories.get(rname) - if traj is not None: - logger.info(f"No coordinator status — waiting {traj.duration:.1f}s for trajectory") - time.sleep(traj.duration + 0.5) + logger.info("No coordinator status available for '%s'", rname) return True # Poll task state via task_invoke @@ -1410,13 +1350,7 @@ def _wait_for_trajectory_completion( # task_invoke returned None — task not found, assume done return True except Exception: - # Fallback: wait for trajectory duration - traj = self._planned_trajectories.get(rname) - if traj is not None: - remaining = traj.duration - (time.time() - start) - if remaining > 0: - logger.info(f"Status poll failed — waiting {remaining:.1f}s for trajectory") - time.sleep(remaining + 0.5) + logger.info("Status poll failed for '%s'", rname) return True time.sleep(poll_interval) diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index 9226a9e87c..4e902cb170 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -151,11 +151,8 @@ def test_plan_to_joints(self, module, joint_state_zeros): assert success is True assert module._state == ManipulationState.COMPLETED assert module.has_planned_path() is True - - assert "test_arm" in module._planned_trajectories - traj = module._planned_trajectories["test_arm"] - assert len(traj.points) > 1 - assert traj.duration > 0 + assert module._last_plan is not None + assert len(module._last_plan.path) > 1 def test_add_and_remove_obstacle(self, module, joint_state_zeros): """Test adding and removing obstacles.""" @@ -201,8 +198,14 @@ def test_planned_trajectory_uses_global_joint_names(self, module, joint_state_ze success = module.plan_to_joints(JointState(position=[0.05] * 7)) assert success is True - traj = module._planned_trajectories["test_arm"] - assert traj.joint_names == [f"test_arm/joint{i}" for i in range(1, 8)] + mock_client = MagicMock() + mock_client.task_invoke.return_value = True + module._coordinator_client = mock_client + + assert module.execute() is True + + trajectory = mock_client.task_invoke.call_args.args[2]["trajectory"] + assert trajectory.joint_names == [f"test_arm/joint{i}" for i in range(1, 8)] @pytest.mark.skipif(not _drake_available(), reason="Drake not installed") diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 5d615be1e6..008959a172 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -100,8 +100,6 @@ def _make_module(): 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 @@ -294,7 +292,6 @@ def test_solve_ik_rpc_calls_configured_backend(self, robot_config): assert result is expected assert module._state == ManipulationState.COMPLETED - assert module._planned_paths == {} module._kinematics.solve.assert_called_once() _, kwargs = module._kinematics.solve.call_args assert kwargs["world"] is module._world_monitor.world @@ -350,7 +347,6 @@ def test_execute_requires_trajectory(self, robot_config): """Execute fails without planned trajectory.""" module = _make_module() module._robots = {"test_arm": ("id", robot_config, MagicMock())} - module._planned_trajectories = {} assert module.execute() is False @@ -471,21 +467,6 @@ def _make_joint_state(positions: list[float], name: list[str] | None = None) -> return JointState(name=name or [f"j{i}" for i in range(len(positions))], position=positions) -def _make_path(*points: list[float]) -> list[JointState]: - return [_make_joint_state(list(point)) for point in points] - - -def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: - joint_names = [f"j{i}" for i in range(len(points[0][1]))] if points else [] - return JointTrajectory( - joint_names=joint_names, - points=[ - TrajectoryPoint(time_from_start=time_from_start, positions=positions) - for time_from_start, positions in points - ], - ) - - def _make_robot_config( name: str, joints: list[str], @@ -867,12 +848,19 @@ def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): assert right_trajectory.joint_names == ["right/j1", "right/j2"] assert [point.positions for point in right_trajectory.points] == [[3.0, 8.0], [6.0, 8.0]] - def test_project_plan_holds_non_selected_joints_from_current_state(self): + def test_execute_plan_holds_non_selected_joints_from_current_state(self): config = _make_robot_config("left", ["j1", "j2", "j3"], "task") module = _make_module_with_monitor(config) + generator = _trajectory_generator() + module._robots["left"] = ("robot_left", config, generator) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("left", "arm", ["j2"]) + ] module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1", "j2", "j3"], position=[10.0, 20.0, 30.0] ) + module._coordinator_client = MagicMock() + module._coordinator_client.task_invoke.return_value = True plan = GeneratedPlan( group_ids=("left/arm",), path=[ @@ -882,49 +870,33 @@ def test_project_plan_holds_non_selected_joints_from_current_state(self): status=PlanningStatus.SUCCESS, ) - projected = module._project_plan_path_for_robot(plan, "left") + assert module.execute_plan(plan) is True - assert [state.name for state in projected] == [["j1", "j2", "j3"], ["j1", "j2", "j3"]] - assert [state.position for state in projected] == [[10.0, 2.0, 30.0], [10.0, 3.0, 30.0]] + trajectory = module._coordinator_client.task_invoke.call_args.args[2]["trajectory"] + assert trajectory.joint_names == ["left/j1", "left/j2", "left/j3"] + assert [point.positions for point in trajectory.points] == [ + [10.0, 2.0, 30.0], + [10.0, 3.0, 30.0], + ] - def test_project_plan_rejects_local_waypoint_names(self): + def test_execute_plan_rejects_local_waypoint_names(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) + module._world_monitor.world.resolve_planning_groups.return_value = [ + _make_global_group("left", "arm", ["j1"]) + ] module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1", "j2"], position=[10.0, 20.0] ) + module._coordinator_client = MagicMock() plan = GeneratedPlan( group_ids=("left/arm",), path=[JointState(name=["j1"], position=[1.0])], status=PlanningStatus.SUCCESS, ) - assert module._project_plan_path_for_robot(plan, "left") == [] - - def test_project_plan_uses_global_names_for_robots_with_shared_local_names(self): - left_config = _make_robot_config("left", ["j1", "j2"], "left_task") - right_config = _make_robot_config("right", ["j1", "j2"], "right_task") - module = _make_module_with_monitor(left_config, right_config) - module._world_monitor.get_current_joint_state.side_effect = [ - JointState(name=["j1", "j2"], position=[10.0, 20.0]), - JointState(name=["j1", "j2"], position=[30.0, 40.0]), - ] - plan = GeneratedPlan( - group_ids=("left/arm", "right/arm"), - path=[ - JointState(name=["left/j1", "right/j1"], position=[1.0, 3.0]), - JointState(name=["left/j1", "right/j1"], position=[2.0, 4.0]), - ], - status=PlanningStatus.SUCCESS, - ) - - left_projected = module._project_plan_path_for_robot(plan, "left") - right_projected = module._project_plan_path_for_robot(plan, "right") - - assert [state.name for state in left_projected] == [["j1", "j2"], ["j1", "j2"]] - assert [state.position for state in left_projected] == [[1.0, 20.0], [2.0, 20.0]] - assert [state.name for state in right_projected] == [["j1", "j2"], ["j1", "j2"]] - assert [state.position for state in right_projected] == [[3.0, 40.0], [4.0, 40.0]] + assert module.execute_plan(plan) is False + module._coordinator_client.task_invoke.assert_not_called() def test_preview_path_with_last_plan_animates_generated_plan(self): config = _make_robot_config("left", ["j1", "j2"], "task") @@ -952,12 +924,7 @@ def test_has_and_clear_planned_path_use_last_plan(self): path=[JointState(name=["left/j1"], position=[1.0])], status=PlanningStatus.SUCCESS, ) - module._planned_paths = {"left": _make_path([1.0])} - module._planned_trajectories = {"left": _make_trajectory((0.0, [1.0]))} - assert module.has_planned_path() is True assert module.clear_planned_path() is True assert module.has_planned_path() is False assert module._last_plan is None - assert module._planned_paths == {} - assert module._planned_trajectories == {} From c07d8da55d536043233b07f518b1385fe3e97ffa Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 15:30:01 -0700 Subject: [PATCH 19/27] refactor: remove legacy preview path api --- dimos/manipulation/manipulation_module.py | 25 +++--------------- .../planning/examples/manipulation_client.py | 5 ++-- dimos/manipulation/test_manipulation_unit.py | 26 +++++++++---------- 3 files changed, 19 insertions(+), 37 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c0932c7fc9..30375f7333 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -15,7 +15,7 @@ """Manipulation Module - Motion planning with ControlCoordinator execution. Base module providing core manipulation infrastructure: -- @rpc: Low-level building blocks (plan_to_pose, plan_to_joints, preview_path, execute) +- @rpc: Low-level building blocks (plan_to_pose, plan_to_joints, preview_plan, execute) - @skill (short-horizon): Single-step actions (move_to_pose, open_gripper, go_home, go_init) Subclass PickAndPlaceModule (pick_and_place_module.py) adds perception integration @@ -691,7 +691,7 @@ def solve_ik( @rpc def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: - """Plan motion to pose. Use preview_path() then execute(). + """Plan motion to pose. Use preview_plan() then execute(). Args: pose: Target end-effector pose @@ -756,7 +756,7 @@ def plan_to_poses( @rpc def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: - """Plan motion to joint config. Use preview_path() then execute(). + """Plan motion to joint config. Use preview_plan() then execute(). Args: joints: Target joint state (names + positions) @@ -830,23 +830,6 @@ def preview_plan( self._world_monitor.animate_plan(plan, animation_duration) return True - @rpc - def preview_path( - self, - duration: float | None = None, - robot_name: RobotName | None = None, - target_fps: float = 30.0, - ) -> bool: - """Preview the last generated plan in the visualizer. - - Args: - duration: Total animation duration in seconds. - robot_name: Optional compatibility filter; the stored plan must affect it. - target_fps: Deprecated; generated-plan preview timing is backend-owned. - """ - _ = target_fps - return self.preview_plan(self._last_plan, duration, robot_name) - @rpc def has_planned_path(self) -> bool: """Check if there's a planned path ready. @@ -1385,7 +1368,7 @@ def _preview_execute_wait( preview_duration: Duration to animate the preview in Meshcat (seconds) """ logger.info("Previewing trajectory...") - self.preview_path(preview_duration, robot_name) + self.preview_plan(duration=preview_duration, robot_name=robot_name) logger.info("Executing trajectory...") if not self.execute(robot_name): diff --git a/dimos/manipulation/planning/examples/manipulation_client.py b/dimos/manipulation/planning/examples/manipulation_client.py index 1185f28f21..335d549b85 100644 --- a/dimos/manipulation/planning/examples/manipulation_client.py +++ b/dimos/manipulation/planning/examples/manipulation_client.py @@ -163,10 +163,9 @@ def plan_pose( def preview( duration: float | None = None, robot_name: str | None = None, - target_fps: float = 30.0, ) -> bool: - """Preview planned path in Meshcat.""" - return _client.preview_path(duration, robot_name, target_fps) + """Preview the last generated plan in Meshcat.""" + return _client.preview_plan(None, duration, robot_name) def execute(robot_name: str | None = None) -> bool: diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 008959a172..9ac5d13918 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -712,7 +712,7 @@ def test_dismiss_preview_routes_to_monitor(self): module._world_monitor.hide_preview.assert_called_once_with(group_ids) module._world_monitor.publish_visualization.assert_called_once_with() - def test_preview_path_delegates_last_plan_with_default_duration(self): + def test_preview_plan_uses_last_plan_with_default_duration(self): module = _make_module() module._world_monitor = MagicMock() module._last_plan = GeneratedPlan( @@ -721,11 +721,11 @@ def test_preview_path_delegates_last_plan_with_default_duration(self): status=PlanningStatus.SUCCESS, ) - assert module.preview_path(target_fps=2.0) is True + assert module.preview_plan() is True module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 3.0) - def test_preview_path_explicit_duration_overrides_default(self): + def test_preview_plan_explicit_duration_overrides_default(self): module = _make_module() module._world_monitor = MagicMock() module._last_plan = GeneratedPlan( @@ -734,11 +734,11 @@ def test_preview_path_explicit_duration_overrides_default(self): status=PlanningStatus.SUCCESS, ) - assert module.preview_path(duration=1.5, target_fps=2.0) is True + assert module.preview_plan(duration=1.5) is True module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 1.5) - def test_preview_path_respects_robot_filter(self): + def test_preview_plan_respects_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() module._world_monitor.world.resolve_planning_groups.return_value = [ @@ -750,11 +750,11 @@ def test_preview_path_respects_robot_filter(self): status=PlanningStatus.SUCCESS, ) - assert module.preview_path(robot_name="arm") is True + assert module.preview_plan(robot_name="arm") is True module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 3.0) - def test_preview_path_rejects_unaffected_robot_filter(self): + def test_preview_plan_rejects_unaffected_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() module._world_monitor.world.resolve_planning_groups.return_value = [ @@ -766,17 +766,17 @@ def test_preview_path_rejects_unaffected_robot_filter(self): status=PlanningStatus.SUCCESS, ) - assert module.preview_path(robot_name="other") is False + assert module.preview_plan(robot_name="other") is False module._world_monitor.animate_plan.assert_not_called() - def test_preview_path_returns_false_for_missing_inputs(self): + def test_preview_plan_returns_false_for_missing_inputs(self): module = _make_module() - assert module.preview_path() is False + assert module.preview_plan() is False module._world_monitor = MagicMock() - assert module.preview_path() is False + assert module.preview_plan() is False class TestGeneratedPlanProjection: @@ -898,7 +898,7 @@ def test_execute_plan_rejects_local_waypoint_names(self): assert module.execute_plan(plan) is False module._coordinator_client.task_invoke.assert_not_called() - def test_preview_path_with_last_plan_animates_generated_plan(self): + def test_preview_plan_with_last_plan_animates_generated_plan(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) module._world_monitor.world.resolve_planning_groups.return_value = [ @@ -913,7 +913,7 @@ def test_preview_path_with_last_plan_animates_generated_plan(self): status=PlanningStatus.SUCCESS, ) - assert module.preview_path(robot_name="left", target_fps=0.0) is True + assert module.preview_plan(robot_name="left") is True module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 3.0) From 95548e502e3fe941d3ce6ae447fc37422bb9380d Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 16:59:18 -0700 Subject: [PATCH 20/27] refactor: decouple planning group registry --- CONTEXT.md | 12 +- dimos/manipulation/manipulation_module.py | 79 ++++++------ .../manipulation/planning/groups/__init__.py | 53 +++++++++ .../discovery.py} | 13 +- dimos/manipulation/planning/groups/models.py | 112 ++++++++++++++++++ .../manipulation/planning/groups/registry.py | 103 ++++++++++++++++ .../utils.py} | 41 +------ .../planning/kinematics/jacobian_ik.py | 50 +++++--- .../planning/kinematics/pink_ik.py | 53 +++++---- .../kinematics/test_jacobian_ik_selection.py | 38 ++++-- .../planning/kinematics/test_pink_ik.py | 60 +++++----- .../planning/monitor/world_monitor.py | 36 ++++-- .../planning/planners/rrt_planner.py | 68 +++++++---- .../planners/test_rrt_planner_selection.py | 74 ++++-------- dimos/manipulation/planning/spec/config.py | 6 +- dimos/manipulation/planning/spec/models.py | 75 +----------- dimos/manipulation/planning/spec/protocols.py | 30 +---- .../planning/test_planning_group_utils.py | 8 +- .../planning/test_planning_groups.py | 2 +- .../planning/world/drake_world.py | 96 ++------------- .../world/test_drake_world_planning_groups.py | 92 +++++++------- dimos/manipulation/test_manipulation_unit.py | 111 ++++++++++------- dimos/robot/config.py | 2 +- 23 files changed, 685 insertions(+), 529 deletions(-) create mode 100644 dimos/manipulation/planning/groups/__init__.py rename dimos/manipulation/planning/{planning_groups.py => groups/discovery.py} (97%) create mode 100644 dimos/manipulation/planning/groups/models.py create mode 100644 dimos/manipulation/planning/groups/registry.py rename dimos/manipulation/planning/{planning_group_utils.py => groups/utils.py} (78%) diff --git a/CONTEXT.md b/CONTEXT.md index 701648bbed..52f23f3a7c 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -17,7 +17,7 @@ A control-layer routing identity for a hardware component. For manipulator robot _Avoid_: Robot name when discussing planning semantics, world robot ID **Planning Group**: -A named selectable serial kinematic chain of robot joints used as the unit of manipulation planning. A planning group is defined by its chain/joints, not by end-effector metadata. +A named selectable serial kinematic chain of robot joints used as the unit of manipulation planning. After binding to a robot name, it is identified by a planning group ID and remains independent of backend world robot IDs. _Avoid_: Move group, movegroup **Planning Group Definition**: @@ -28,10 +28,6 @@ _Avoid_: Runtime group, robot ID Separate metadata used for pose-targeted operations. For a planning group defined by a chain, the end-effector link is the chain tip. For a planning group defined only by joints, there is no end-effector link. _Avoid_: Planning group definition -**Resolved Planning Group**: -A planning group after model-level declarations have been bound to a concrete robot name and planning world. -_Avoid_: Planning group config, robot ID - **Planning Group Selection**: The set of one or more planning groups chosen for a planning request. _Avoid_: Composite group @@ -52,9 +48,9 @@ _Avoid_: Bare group name, robot ID The generated fallback planning group used by robot-scoped compatibility APIs when no planning group is specified explicitly. It is not inferred from arbitrary SRDF group uniqueness. _Avoid_: Unique planning group, primary planning group -**Planning Group Descriptor**: -A read-only snapshot returned by query APIs that describes an available planning group and may be used ergonomically as a planning group selector. -_Avoid_: Live planning group handle, resolved planning group +**Robot-Scoped Compatibility API**: +A convenience API that accepts a robot name for common single-robot calls but immediately delegates to planning-group APIs through the robot's default planning group. It does not define a separate planning model, storage model, or groupless execution path. +_Avoid_: Robot-scoped planner, groupless API **Joint State**: A joint-name-keyed robot state that can represent any set of joints and is not inherently coupled to a robot, planning group, planning group selection, or joint-name scope. At flat multi-robot or coordinator boundaries, joint names are required and are global joint names. Robot identity and local-vs-global meaning are provided by the API boundary or containing type, not by extra fields on the generic joint state. diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 30375f7333..f80d9f5c4e 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -40,23 +40,21 @@ 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.groups import ( + PlanningGroup, + joint_target_to_global_names, + planning_group_id_from_selector, +) 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.planning_group_utils import ( - joint_target_to_global_names, - planning_group_id_from_selector, - primary_pose_planning_group_id_for_robot, -) -from dimos.manipulation.planning.planning_groups import FALLBACK_PLANNING_GROUP_NAME from dimos.manipulation.planning.planning_identifiers import ( assert_global_joint_names, assert_local_joint_names, make_global_joint_names, - make_planning_group_id, ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType @@ -64,7 +62,6 @@ GeneratedPlan, IKResult, Obstacle, - PlanningGroupDescriptor, PlanningGroupID, PlanningResult, RobotName, @@ -345,9 +342,7 @@ def _tf_publish_loop(self) -> None: target_frame = config.end_effector_link pose_group_id = self._primary_pose_group_id_for_robot(config.name) if pose_group_id is not None: - pose_group = self._world_monitor.world.resolve_planning_groups( - (pose_group_id,) - )[0] + pose_group = self._world_monitor.planning_groups.get(pose_group_id) target_frame = pose_group.tip_link ee_pose = self._world_monitor.get_group_pose(pose_group_id) else: @@ -484,22 +479,19 @@ def _fail(self, msg: str) -> bool: def _default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: """Return the generated fallback group used by robot-scoped wrappers.""" assert self._world_monitor is not None - group_id = make_planning_group_id(robot_name, FALLBACK_PLANNING_GROUP_NAME) - if any(group.id == group_id for group in self._world_monitor.world.list_planning_groups()): + group_id = self._world_monitor.planning_groups.default_group_id_for_robot(robot_name) + if group_id is not None: return group_id logger.error( - "Robot '%s' has no generated default planning group '%s'; use explicit group APIs", + "Robot '%s' has no generated default planning group; use explicit group APIs", robot_name, - group_id, ) return None def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: """Return the first pose-targetable group for robot-scoped compatibility paths.""" assert self._world_monitor is not None - return primary_pose_planning_group_id_for_robot( - self._world_monitor.world.list_planning_groups(), robot_name - ) + return self._world_monitor.planning_groups.primary_pose_group_id_for_robot(robot_name) def _current_positions_by_name( self, robot_name: RobotName, current: JointState @@ -527,14 +519,18 @@ def _current_positions_by_name( def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: """Collect current state for exactly the selected global joints.""" assert self._world_monitor is not None - resolved_groups = self._world_monitor.world.resolve_planning_groups(group_ids) + selection = self._world_monitor.planning_groups.select(group_ids) - robot_names_by_id: dict[WorldRobotID, RobotName] = {} - for group in resolved_groups: - robot_names_by_id.setdefault(group.robot_id, group.robot_name) + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in selection.robot_names: + try: + robot_ids_by_name[robot_name] = self._robots[robot_name][0] + except KeyError: + logger.error("Robot '%s' is not registered", robot_name) + return None - current_by_robot: dict[WorldRobotID, dict[str, float]] = {} - for robot_id, robot_name in robot_names_by_id.items(): + current_by_robot: dict[RobotName, dict[str, float]] = {} + for robot_name, robot_id in robot_ids_by_name.items(): current = self._world_monitor.get_current_joint_state(robot_id) if current is None: logger.error("No joint state for robot '%s'", robot_name) @@ -542,12 +538,12 @@ def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> Joint indexed_current = self._current_positions_by_name(robot_name, current) if indexed_current is None: return None - current_by_robot[robot_id] = indexed_current + current_by_robot[robot_name] = indexed_current names: list[str] = [] positions: list[float] = [] - for group in resolved_groups: - robot_state = current_by_robot[group.robot_id] + for group in selection.groups: + robot_state = current_by_robot[group.robot_name] for resolved_name, local_name in zip( group.joint_names, group.local_joint_names, strict=True ): @@ -565,7 +561,7 @@ def _joint_target_to_global_names( ) -> JointState | None: """Convert a group joint target to global joint names in group order.""" assert self._world_monitor is not None - group = self._world_monitor.world.resolve_planning_groups((group_id,))[0] + group = self._world_monitor.planning_groups.get(group_id) try: return joint_target_to_global_names(group, target) except ValueError as exc: @@ -575,12 +571,7 @@ def _joint_target_to_global_names( def _affected_robot_names(self, plan: GeneratedPlan) -> list[RobotName]: """Get stable robot names affected by a generated plan.""" assert self._world_monitor is not None - resolved_groups = self._world_monitor.world.resolve_planning_groups(plan.group_ids) - names: list[RobotName] = [] - for group in resolved_groups: - if group.robot_name not in names: - names.append(group.robot_name) - return names + return list(self._world_monitor.planning_groups.select(plan.group_ids).robot_names) def _store_generated_plan( self, group_ids: tuple[PlanningGroupID, ...], result: PlanningResult @@ -603,7 +594,7 @@ def _plan_selected_path( assert self._world_monitor and self._planner result = self._planner.plan_selected_joint_path( world=self._world_monitor.world, - group_ids=group_ids, + selection=self._world_monitor.planning_groups.select(group_ids), start=start, goal=goal, timeout=self.config.planning_timeout, @@ -712,8 +703,8 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: @rpc def plan_to_poses( self, - pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, Pose], - auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + pose_targets: Mapping[PlanningGroupID | PlanningGroup, Pose], + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), ) -> bool: """Plan to one or more group pose targets with optional auxiliary groups.""" if self._world_monitor is None or self._kinematics is None: @@ -745,8 +736,13 @@ def plan_to_poses( ik = self._kinematics.solve_pose_targets( world=self._world_monitor.world, - pose_targets=stamped_targets, - auxiliary_groups=auxiliary_ids, + pose_targets={ + self._world_monitor.planning_groups.get(group_id): pose + for group_id, pose in stamped_targets.items() + }, + auxiliary_groups=tuple( + self._world_monitor.planning_groups.get(group_id) for group_id in auxiliary_ids + ), seed=start, check_collision=True, ) @@ -774,7 +770,7 @@ def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None @rpc def plan_to_joint_targets( - self, joint_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, JointState] + self, joint_targets: Mapping[PlanningGroupID | PlanningGroup, JointState] ) -> bool: """Plan to joint targets keyed by planning group.""" if self._world_monitor is None or self._planner is None: @@ -896,8 +892,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> dict[str, Any] "source": group.source, "has_pose_target": group.has_pose_target, } - for group in self._world_monitor.world.list_planning_groups() - if group.robot_name == robot_name + for group in self._world_monitor.planning_groups.groups_for_robot(robot_name) ] if self._world_monitor is not None else [] diff --git a/dimos/manipulation/planning/groups/__init__.py b/dimos/manipulation/planning/groups/__init__.py new file mode 100644 index 0000000000..ba142e5654 --- /dev/null +++ b/dimos/manipulation/planning/groups/__init__.py @@ -0,0 +1,53 @@ +# 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. + +"""Planning-group domain models, discovery, registry, and helpers.""" + +from dimos.manipulation.planning.groups.discovery import ( + FALLBACK_PLANNING_GROUP_NAME, + PlanningGroupDiscoveryError, + discover_planning_group_definitions, + generate_fallback_planning_group, + parse_srdf_planning_groups, +) +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupDefinition, + PlanningGroupSelection, + PlanningGroupSource, +) +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry +from dimos.manipulation.planning.groups.utils import ( + filter_joint_state_to_selected_joints, + joint_target_to_global_names, + matching_global_joint_name, + planning_group_id_from_selector, +) + +__all__ = [ + "FALLBACK_PLANNING_GROUP_NAME", + "PlanningGroup", + "PlanningGroupDefinition", + "PlanningGroupDiscoveryError", + "PlanningGroupRegistry", + "PlanningGroupSelection", + "PlanningGroupSource", + "discover_planning_group_definitions", + "filter_joint_state_to_selected_joints", + "generate_fallback_planning_group", + "joint_target_to_global_names", + "matching_global_joint_name", + "parse_srdf_planning_groups", + "planning_group_id_from_selector", +] diff --git a/dimos/manipulation/planning/planning_groups.py b/dimos/manipulation/planning/groups/discovery.py similarity index 97% rename from dimos/manipulation/planning/planning_groups.py rename to dimos/manipulation/planning/groups/discovery.py index 2b8c362512..5e9ddfa25d 100644 --- a/dimos/manipulation/planning/planning_groups.py +++ b/dimos/manipulation/planning/groups/discovery.py @@ -21,7 +21,7 @@ import warnings import xml.etree.ElementTree as ET -from dimos.manipulation.planning.spec.models import PlanningGroupDefinition +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.robot.model_parser import JointDescription, ModelDescription from dimos.utils.logging_config import setup_logger @@ -204,7 +204,7 @@ def _parse_chain_group( try: ordered_joints = _ordered_joints_between_links(model, base_link, tip_link) - controlled_joints = [j for j in ordered_joints if j.type != "fixed"] + controlled_joints = [joint for joint in ordered_joints if joint.type != "fixed"] _validate_controllable(group_name, controlled_joints, controllable_joint_names) except PlanningGroupDiscoveryError as exc: _warn(f"Skipping SRDF chain group {group_name} in {srdf_path}: {exc}") @@ -356,12 +356,3 @@ def _validate_controllable( raise PlanningGroupDiscoveryError( f"planning group {group_name} includes joints outside controllable set: {missing}" ) - - -__all__ = [ - "FALLBACK_PLANNING_GROUP_NAME", - "PlanningGroupDiscoveryError", - "discover_planning_group_definitions", - "generate_fallback_planning_group", - "parse_srdf_planning_groups", -] diff --git a/dimos/manipulation/planning/groups/models.py b/dimos/manipulation/planning/groups/models.py new file mode 100644 index 0000000000..c08b1bd4b5 --- /dev/null +++ b/dimos/manipulation/planning/groups/models.py @@ -0,0 +1,112 @@ +# 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. + +"""Backend-independent planning-group domain models.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Literal, TypeAlias + +from dimos.manipulation.planning.spec.models import ( + GlobalJointName, + LocalModelJointName, + PlanningGroupID, + RobotName, +) + +PlanningGroupSource: TypeAlias = Literal["srdf", "fallback"] + + +@dataclass(frozen=True) +class PlanningGroupDefinition: + """Model-level declaration of a planning group. + + Joint names are local model names. The definition is safe to store on + ``RobotModelConfig`` and is not bound to any runtime world robot ID. + """ + + name: str + joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group has a valid pose target frame.""" + return self.tip_link is not None + + +@dataclass(frozen=True) +class PlanningGroup: + """Public backend-independent planning group. + + A planning group exposes stable public IDs and global joint names for + planning APIs. It intentionally does not include backend runtime robot IDs. + """ + + id: PlanningGroupID + robot_name: RobotName + group_name: str + joint_names: tuple[GlobalJointName, ...] + local_joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group can be directly pose-targeted.""" + return self.tip_link is not None + + +@dataclass(frozen=True) +class PlanningGroupSelection: + """Validated ordered selection of planning groups. + + Selection validates ID existence and selected-joint overlap outside any + world backend. Requested group order is preserved. + """ + + groups: tuple[PlanningGroup, ...] + group_ids: tuple[PlanningGroupID, ...] + joint_names: tuple[GlobalJointName, ...] + robot_names: tuple[RobotName, ...] + + @classmethod + def from_groups(cls, groups: tuple[PlanningGroup, ...]) -> PlanningGroupSelection: + """Build a selection, rejecting overlapping selected global joints.""" + seen_joints: dict[GlobalJointName, PlanningGroupID] = {} + joint_names: list[GlobalJointName] = [] + robot_names: list[RobotName] = [] + for group in groups: + if group.robot_name not in robot_names: + robot_names.append(group.robot_name) + for joint_name in group.joint_names: + previous_group_id = seen_joints.get(joint_name) + if previous_group_id is not None: + raise ValueError( + "Selected planning groups overlap on global joint " + f"{joint_name}: {previous_group_id} and {group.id}" + ) + seen_joints[joint_name] = group.id + joint_names.append(joint_name) + + return cls( + groups=groups, + group_ids=tuple(group.id for group in groups), + joint_names=tuple(joint_names), + robot_names=tuple(robot_names), + ) diff --git a/dimos/manipulation/planning/groups/registry.py b/dimos/manipulation/planning/groups/registry.py new file mode 100644 index 0000000000..4006f38a13 --- /dev/null +++ b/dimos/manipulation/planning/groups/registry.py @@ -0,0 +1,103 @@ +# 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. + +"""Backend-independent planning-group registry.""" + +from __future__ import annotations + +from collections.abc import Iterable +from typing import TYPE_CHECKING + +from dimos.manipulation.planning.groups.discovery import FALLBACK_PLANNING_GROUP_NAME +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.planning_identifiers import ( + make_global_joint_names, + make_planning_group_id, +) +from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName + +if TYPE_CHECKING: + from dimos.manipulation.planning.spec.config import RobotModelConfig + + +class PlanningGroupRegistry: + """Registry of public planning groups derived from robot configs.""" + + def __init__(self, robot_configs: Iterable[RobotModelConfig] = ()) -> None: + self._groups: dict[PlanningGroupID, PlanningGroup] = {} + self._groups_by_robot: dict[RobotName, list[PlanningGroup]] = {} + for config in robot_configs: + self.add_robot(config) + + def add_robot(self, config: RobotModelConfig) -> None: + """Register all planning groups declared by one robot config.""" + if config.name in self._groups_by_robot: + raise ValueError(f"Robot '{config.name}' is already registered") + + robot_groups: list[PlanningGroup] = [] + for definition in config.planning_groups: + group_id = make_planning_group_id(config.name, definition.name) + if group_id in self._groups: + raise ValueError(f"Planning group '{group_id}' is already registered") + group = PlanningGroup( + id=group_id, + robot_name=config.name, + group_name=definition.name, + joint_names=tuple(make_global_joint_names(config.name, definition.joint_names)), + local_joint_names=definition.joint_names, + base_link=definition.base_link, + tip_link=definition.tip_link, + source=definition.source, + ) + self._groups[group_id] = group + robot_groups.append(group) + self._groups_by_robot[config.name] = robot_groups + + def list(self) -> tuple[PlanningGroup, ...]: + """List planning groups in robot registration order.""" + groups: list[PlanningGroup] = [] + for robot_groups in self._groups_by_robot.values(): + groups.extend(robot_groups) + return tuple(groups) + + def get(self, group_id: PlanningGroupID) -> PlanningGroup: + """Return one planning group by public ID.""" + try: + return self._groups[group_id] + except KeyError as exc: + raise KeyError(f"Unknown planning group ID: {group_id}") from exc + + def select(self, group_ids: Iterable[PlanningGroupID]) -> PlanningGroupSelection: + """Validate and return an ordered planning-group selection.""" + return PlanningGroupSelection.from_groups( + tuple(self.get(group_id) for group_id in group_ids) + ) + + def groups_for_robot(self, robot_name: RobotName) -> tuple[PlanningGroup, ...]: + """Return planning groups for one robot.""" + return tuple(self._groups_by_robot.get(robot_name, ())) + + def default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the generated fallback group ID for robot-scoped wrappers.""" + group_id = make_planning_group_id(robot_name, FALLBACK_PLANNING_GROUP_NAME) + return group_id if group_id in self._groups else None + + def primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the first pose-targetable group ID for compatibility paths.""" + # TODO: Replace this compatibility selection with either one TF publication per + # pose-targetable planning group or backend-level whole-robot TF publishing. + for group in self.groups_for_robot(robot_name): + if group.has_pose_target: + return group.id + return None diff --git a/dimos/manipulation/planning/planning_group_utils.py b/dimos/manipulation/planning/groups/utils.py similarity index 78% rename from dimos/manipulation/planning/planning_group_utils.py rename to dimos/manipulation/planning/groups/utils.py index 3b2db4701c..5c854ae60f 100644 --- a/dimos/manipulation/planning/planning_group_utils.py +++ b/dimos/manipulation/planning/groups/utils.py @@ -12,10 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Shared helpers for planning-group selector and joint-state projection.""" +"""Shared helpers for planning-group selectors and joint-state projection.""" from collections.abc import Mapping, Sequence +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.planning_identifiers import ( assert_global_joint_names, assert_local_joint_names, @@ -24,50 +25,18 @@ from dimos.manipulation.planning.spec.models import ( GlobalJointName, LocalModelJointName, - PlanningGroupDescriptor, PlanningGroupID, - ResolvedPlanningGroup, - RobotName, ) from dimos.msgs.sensor_msgs.JointState import JointState -def planning_group_id_from_selector( - selector: PlanningGroupID | PlanningGroupDescriptor, -) -> PlanningGroupID: +def planning_group_id_from_selector(selector: PlanningGroupID | PlanningGroup) -> PlanningGroupID: """Return the planning-group ID represented by a selector.""" - if isinstance(selector, PlanningGroupDescriptor): + if isinstance(selector, PlanningGroup): return selector.id return selector -def single_planning_group_id_for_robot( - groups: Sequence[PlanningGroupDescriptor], - robot_name: RobotName, -) -> PlanningGroupID: - """Return a robot's only planning group ID, or raise if ambiguous.""" - group_ids = [group.id for group in groups if group.robot_name == robot_name] - if len(group_ids) != 1: - raise ValueError( - f"Robot '{robot_name}' has {len(group_ids)} planning groups; " - "select a planning group explicitly" - ) - return group_ids[0] - - -def primary_pose_planning_group_id_for_robot( - groups: Sequence[PlanningGroupDescriptor], - robot_name: RobotName, -) -> PlanningGroupID | None: - """Return the first pose-targetable group ID for compatibility paths.""" - # TODO: Replace this compatibility selection with either one TF publication per - # pose-targetable planning group or backend-level whole-robot TF publishing. - for group in groups: - if group.robot_name == robot_name and group.has_pose_target: - return group.id - return None - - def matching_global_joint_name( positions_by_name: Mapping[str, float], local_joint_name: LocalModelJointName ) -> GlobalJointName | None: @@ -113,7 +82,7 @@ def filter_joint_state_to_selected_joints( def joint_target_to_global_names( - group: ResolvedPlanningGroup, + group: PlanningGroup, target: JointState, ) -> JointState: """Convert a group joint target to global joint names in group order. diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index afdef5d5d0..dc77a68d6b 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -30,15 +30,15 @@ import numpy as np -from dimos.manipulation.planning.planning_group_utils import ( +from dimos.manipulation.planning.groups import ( + PlanningGroup, + PlanningGroupSelection, filter_joint_state_to_selected_joints, - planning_group_id_from_selector, ) from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( IKResult, - PlanningGroupDescriptor, - PlanningGroupID, + RobotName, WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import WorldSpec @@ -208,8 +208,8 @@ def solve( def solve_pose_targets( self, world: WorldSpec, - pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], - auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + pose_targets: Mapping[PlanningGroup, PoseStamped], + auxiliary_groups: Sequence[PlanningGroup] = (), seed: JointState | None = None, position_tolerance: float = 0.001, orientation_tolerance: float = 0.01, @@ -225,43 +225,43 @@ def solve_pose_targets( IKStatus.NO_SOLUTION, "At least one pose target is required" ) - pose_group_ids = tuple( - planning_group_id_from_selector(group) for group in pose_targets.keys() - ) - if len(pose_group_ids) != 1 or auxiliary_groups: + pose_groups = tuple(pose_targets.keys()) + if len(pose_groups) != 1 or auxiliary_groups: return _create_failure_result( IKStatus.NO_SOLUTION, "JacobianIK supports exactly one pose target and no auxiliary planning groups", ) try: - resolved_groups = world.resolve_planning_groups(pose_group_ids) + selection = PlanningGroupSelection.from_groups(pose_groups + tuple(auxiliary_groups)) + robot_ids_by_name = _robot_ids_by_name(world, selection.robot_names) except (KeyError, ValueError) as exc: return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) - target_group = next(group for group in resolved_groups if group.id == pose_group_ids[0]) + target_group = pose_groups[0] if not target_group.has_pose_target: return _create_failure_result( IKStatus.NO_SOLUTION, f"Planning group '{target_group.id}' has no pose target frame", ) - robot_ids = {group.robot_id for group in resolved_groups} + robot_ids = {robot_ids_by_name[group.robot_name] for group in selection.groups} if len(robot_ids) != 1: return _create_failure_result( IKStatus.NO_SOLUTION, "JacobianIK does not support cross-robot pose IK", ) + robot_id = robot_ids_by_name[target_group.robot_name] full_seed = seed if full_seed is None: with world.scratch_context() as ctx: - full_seed = world.get_joint_state(ctx, target_group.robot_id) + full_seed = world.get_joint_state(ctx, robot_id) target_pose = pose_targets[next(iter(pose_targets.keys()))] result = self.solve( world=world, - robot_id=target_group.robot_id, + robot_id=robot_id, target_pose=target_pose, seed=full_seed, position_tolerance=position_tolerance, @@ -273,7 +273,7 @@ def solve_pose_targets( return result selected_joint_names: list[str] = [] - for group in resolved_groups: + for group in selection.groups: selected_joint_names.extend(group.joint_names) try: result.joint_state = filter_joint_state_to_selected_joints( @@ -531,3 +531,21 @@ def _create_failure_result( iterations=iterations, message=message, ) + + +def _robot_ids_by_name( + world: WorldSpec, robot_names: tuple[RobotName, ...] +) -> dict[RobotName, WorldRobotID]: + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in robot_names: + matches = [ + robot_id + for robot_id in world.get_robot_ids() + if world.get_robot_config(robot_id).name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + robot_ids_by_name[robot_name] = matches[0] + return robot_ids_by_name diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index e7a411ffdf..bb5dbf62b8 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -25,18 +25,18 @@ import numpy as np -from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig -from dimos.manipulation.planning.planning_group_utils import ( +from dimos.manipulation.planning.groups import ( + PlanningGroup, + PlanningGroupSelection, filter_joint_state_to_selected_joints, matching_global_joint_name, - planning_group_id_from_selector, ) +from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( IKResult, - PlanningGroupDescriptor, - PlanningGroupID, + RobotName, WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import WorldSpec @@ -178,8 +178,8 @@ def solve( def solve_pose_targets( self, world: WorldSpec, - pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], - auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + pose_targets: Mapping[PlanningGroup, PoseStamped], + auxiliary_groups: Sequence[PlanningGroup] = (), seed: JointState | None = None, position_tolerance: float = 0.001, orientation_tolerance: float = 0.01, @@ -190,36 +190,31 @@ def solve_pose_targets( if not pose_targets: return _failure(IKStatus.NO_SOLUTION, "At least one pose target is required") - pose_group_ids = tuple( - planning_group_id_from_selector(group) for group in pose_targets.keys() - ) - auxiliary_group_ids = tuple( - planning_group_id_from_selector(group) for group in auxiliary_groups - ) - selected_group_ids = pose_group_ids + auxiliary_group_ids + pose_groups = tuple(pose_targets.keys()) try: - resolved_groups = world.resolve_planning_groups(selected_group_ids) + selection = PlanningGroupSelection.from_groups(pose_groups + tuple(auxiliary_groups)) + robot_ids_by_name = _robot_ids_by_name(world, selection.robot_names) except (KeyError, ValueError) as exc: return _failure(IKStatus.NO_SOLUTION, str(exc)) - if len(pose_group_ids) != 1: + if len(pose_groups) != 1: return _failure( IKStatus.NO_SOLUTION, "PinkIK supports exactly one pose target per request", ) - target_group = next(group for group in resolved_groups if group.id == pose_group_ids[0]) + target_group = pose_groups[0] if not target_group.has_pose_target or target_group.tip_link is None: return _failure( IKStatus.NO_SOLUTION, f"Planning group '{target_group.id}' has no pose target frame", ) - robot_ids = {group.robot_id for group in resolved_groups} + robot_ids = {robot_ids_by_name[group.robot_name] for group in selection.groups} if len(robot_ids) != 1: return _failure(IKStatus.NO_SOLUTION, "PinkIK does not support cross-robot pose IK") - robot_id = target_group.robot_id + robot_id = robot_ids_by_name[target_group.robot_name] if seed is None: with world.scratch_context() as ctx: seed = world.get_joint_state(ctx, robot_id) @@ -264,7 +259,7 @@ def solve_pose_targets( selected_joint_names: list[str] = [] selected_local_names: list[str] = [] - for group in resolved_groups: + for group in selection.groups: selected_joint_names.extend(group.joint_names) selected_local_names.extend(group.local_joint_names) try: @@ -624,4 +619,22 @@ def _collision_failure(result: IKResult) -> IKResult: ) +def _robot_ids_by_name( + world: WorldSpec, robot_names: tuple[RobotName, ...] +) -> dict[RobotName, WorldRobotID]: + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in robot_names: + matches = [ + robot_id + for robot_id in world.get_robot_ids() + if world.get_robot_config(robot_id).name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + robot_ids_by_name[robot_name] = matches[0] + return robot_ids_by_name + + __all__ = ["PinkIK", "PinkIKConfig", "PinkIKDependencyError"] diff --git a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py index fd9b2a9e54..8eedeb5d4d 100644 --- a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py +++ b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py @@ -17,11 +17,14 @@ from __future__ import annotations from collections.abc import Mapping +from pathlib import Path from typing import cast +from dimos.manipulation.planning.groups import PlanningGroup from dimos.manipulation.planning.kinematics.jacobian_ik import JacobianIK +from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import IKResult, ResolvedPlanningGroup +from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -37,10 +40,9 @@ def _joint_state(names: list[str], positions: list[float]) -> JointState: def _group( group_id: str, joint_names: tuple[str, ...], tip_link: str | None = "tool0" -) -> ResolvedPlanningGroup: - return ResolvedPlanningGroup( +) -> PlanningGroup: + return PlanningGroup( id=group_id, - robot_id="robot_1", robot_name="arm", group_name=group_id.split("/", maxsplit=1)[1], joint_names=joint_names, @@ -51,13 +53,23 @@ def _group( class _IKWorld: - def __init__(self, groups: Mapping[str, ResolvedPlanningGroup]) -> None: + def __init__(self, groups: Mapping[str, PlanningGroup]) -> None: self._groups = groups + self._robot_configs = { + "robot_1": RobotModelConfig( + name="arm", + model_path=Path("robot.urdf"), + base_pose=_pose(), + joint_names=["joint1", "joint2", "gripper"], + end_effector_link="tool0", + ) + } + + def get_robot_ids(self) -> list[str]: + return list(self._robot_configs) - def resolve_planning_groups( - self, group_ids: tuple[str, ...] - ) -> tuple[ResolvedPlanningGroup, ...]: - return tuple(self._groups[group_id] for group_id in group_ids) + def get_robot_config(self, robot_id: str) -> RobotModelConfig: + return self._robot_configs[robot_id] class _SuccessfulIK(JacobianIK): @@ -90,7 +102,7 @@ def test_solve_pose_targets_filters_result_to_single_group_joints() -> None: result = _SuccessfulIK().solve_pose_targets( world=cast("WorldSpec", world), - pose_targets={"arm/arm": _pose()}, + pose_targets={world._groups["arm/arm"]: _pose()}, seed=_joint_state(["arm/joint1", "arm/joint2", "arm/gripper"], [0.0, 0.0, 0.0]), ) @@ -105,8 +117,8 @@ def test_solve_pose_targets_rejects_auxiliary_groups() -> None: result = _SuccessfulIK().solve_pose_targets( world=cast("WorldSpec", world), - pose_targets={"arm/arm": _pose()}, - auxiliary_groups=["arm/gripper"], + pose_targets={world._groups["arm/arm"]: _pose()}, + auxiliary_groups=[_group("arm/gripper", ("arm/gripper",))], seed=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), ) @@ -119,7 +131,7 @@ def test_solve_pose_targets_rejects_group_without_pose_target_frame() -> None: result = JacobianIK().solve_pose_targets( world=cast("WorldSpec", world), - pose_targets={"arm/gripper": _pose()}, + pose_targets={world._groups["arm/gripper"]: _pose()}, ) assert result.status == IKStatus.NO_SOLUTION diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index dd644f8fb5..b855562fdb 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -25,6 +25,7 @@ import pytest from dimos.manipulation.planning.factory import create_kinematics +from dimos.manipulation.planning.groups import PlanningGroup from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.kinematics.pink_ik import ( PinkIK, @@ -37,7 +38,7 @@ ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import IKResult, ResolvedPlanningGroup +from dimos.manipulation.planning.spec.models import IKResult from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -198,6 +199,29 @@ class _FakeWorld: def __init__(self, collision_free: bool = True) -> None: self.config = _robot_config() self.collision_free = collision_free + self.groups = { + "arm/wrist": PlanningGroup( + id="arm/wrist", + robot_name="arm", + group_name="wrist", + joint_names=("arm/joint_a", "arm/joint_b"), + local_joint_names=("joint_a", "joint_b"), + base_link="base", + tip_link="wrist_tool", + ), + "arm/gripper": PlanningGroup( + id="arm/gripper", + robot_name="arm", + group_name="gripper", + joint_names=("arm/joint_c",), + local_joint_names=("joint_c",), + base_link="base", + tip_link=None, + ), + } + + def get_robot_ids(self) -> list[str]: + return ["robot"] def get_robot_config(self, robot_id: str) -> RobotModelConfig: return self.config @@ -217,33 +241,6 @@ def get_joint_limits(self, robot_id: str) -> tuple[np.ndarray, np.ndarray]: def check_config_collision_free(self, robot_id: str, joint_state: JointState) -> bool: return self.collision_free - def resolve_planning_groups( - self, group_ids: tuple[str, ...] - ) -> tuple[ResolvedPlanningGroup, ...]: - groups = { - "arm/wrist": ResolvedPlanningGroup( - id="arm/wrist", - robot_id="robot", - robot_name="arm", - group_name="wrist", - joint_names=("arm/joint_a", "arm/joint_b"), - local_joint_names=("joint_a", "joint_b"), - base_link="base", - tip_link="wrist_tool", - ), - "arm/gripper": ResolvedPlanningGroup( - id="arm/gripper", - robot_id="robot", - robot_name="arm", - group_name="gripper", - joint_names=("arm/joint_c",), - local_joint_names=("joint_c",), - base_link="base", - tip_link=None, - ), - } - return tuple(groups[group_id] for group_id in group_ids) - def test_create_kinematics_pink_missing_dependency_is_actionable( monkeypatch: pytest.MonkeyPatch, @@ -419,15 +416,16 @@ def fake_solve_single(**kwargs: object) -> IKResult: monkeypatch.setattr(ik, "_solve_single", fake_solve_single) + world = _FakeWorld(collision_free=True) result = ik.solve_pose_targets( - world=cast("Any", _FakeWorld(collision_free=True)), + world=cast("Any", world), pose_targets={ - "arm/wrist": PoseStamped( + world.groups["arm/wrist"]: PoseStamped( position=Vector3(0.1, 0.0, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0), ) }, - auxiliary_groups=["arm/gripper"], + auxiliary_groups=[world.groups["arm/gripper"]], seed=JointState( {"name": ["arm/joint_a", "arm/joint_b", "arm/joint_c"], "position": [0.0, 0.0, 0.0]} ), diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 80e24f2bef..e36d5f55fb 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -22,6 +22,7 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.manipulation.planning.factory import create_world +from dimos.manipulation.planning.groups import PlanningGroupRegistry 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 @@ -68,6 +69,8 @@ def __init__( ) self._lock = threading.RLock() self._robot_joints: dict[WorldRobotID, list[str]] = {} + self._robot_ids_by_name: dict[str, WorldRobotID] = {} + self._planning_groups = PlanningGroupRegistry() self._state_monitors: dict[WorldRobotID, RobotStateMonitor] = {} self._obstacle_monitor: WorldObstacleMonitor | None = None self._viz_thread: threading.Thread | None = None @@ -81,9 +84,18 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: with self._lock: robot_id = self._world.add_robot(config) self._robot_joints[robot_id] = config.joint_names + if config.name in self._robot_ids_by_name: + raise ValueError(f"Robot name '{config.name}' is already registered") + self._robot_ids_by_name[config.name] = robot_id + self._planning_groups.add_robot(config) logger.info(f"Added robot '{config.name}' as '{robot_id}'") return robot_id + @property + def planning_groups(self) -> PlanningGroupRegistry: + """Backend-independent planning-group registry for added robots.""" + return self._planning_groups + def get_robot_ids(self) -> list[WorldRobotID]: """Get all robot IDs.""" with self._lock: @@ -364,12 +376,12 @@ def get_group_pose( self, group_id: PlanningGroupID, joint_state: JointState | None = None ) -> PoseStamped: """Get planning group target-frame pose using current state by default.""" - group = self._world.resolve_planning_groups((group_id,))[0] + robot_id = self._robot_id_for_group(group_id) with self._world.scratch_context() as ctx: if joint_state is None: - joint_state = self.get_current_joint_state(group.robot_id) + joint_state = self.get_current_joint_state(robot_id) if joint_state is not None: - self._world.set_joint_state(ctx, group.robot_id, joint_state) + self._world.set_joint_state(ctx, robot_id, joint_state) return self._world.get_group_pose(ctx, group_id) @@ -414,17 +426,18 @@ def get_group_jacobian( self, group_id: PlanningGroupID, joint_state: JointState ) -> NDArray[np.float64]: """Get planning group target-frame 6xN Jacobian matrix.""" - group = self._world.resolve_planning_groups((group_id,))[0] + self._planning_groups.get(group_id) + robot_id = self._robot_id_for_group(group_id) with self._world.scratch_context() as ctx: - self._world.set_joint_state(ctx, group.robot_id, joint_state) + self._world.set_joint_state(ctx, robot_id, joint_state) return self._world.get_group_jacobian(ctx, group_id) def _unique_pose_group_id_for_robot(self, robot_id: WorldRobotID) -> PlanningGroupID: robot_name = self._world.get_robot_config(robot_id).name pose_group_ids = [ group.id - for group in self._world.list_planning_groups() - if group.robot_name == robot_name and group.has_pose_target + for group in self._planning_groups.groups_for_robot(robot_name) + if group.has_pose_target ] if len(pose_group_ids) != 1: raise ValueError( @@ -433,6 +446,15 @@ def _unique_pose_group_id_for_robot(self, robot_id: WorldRobotID) -> PlanningGro ) return pose_group_ids[0] + def _robot_id_for_group(self, group_id: PlanningGroupID) -> WorldRobotID: + group = self._planning_groups.get(group_id) + try: + return self._robot_ids_by_name[group.robot_name] + except KeyError as exc: + raise KeyError( + f"Robot '{group.robot_name}' not found for planning group {group_id}" + ) from exc + # Lifecycle def finalize(self) -> None: diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 6fb00f75b9..28f01f4b78 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -26,6 +26,7 @@ import numpy as np +from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.planning_identifiers import ( local_joint_name_from_global, make_global_joint_names, @@ -33,9 +34,8 @@ from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import ( JointPath, - PlanningGroupID, PlanningResult, - ResolvedPlanningGroup, + RobotName, WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import WorldSpec @@ -167,19 +167,14 @@ def get_name(self) -> str: def plan_selected_joint_path( self, world: WorldSpec, - group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...], + selection: PlanningGroupSelection, start: JointState, goal: JointState, timeout: float = 10.0, ) -> PlanningResult: """Plan a collision-free path for an explicit planning-group selection.""" - try: - resolved_groups = world.resolve_planning_groups(tuple(group_ids)) - except (KeyError, ValueError) as exc: - return _create_failure_result(PlanningStatus.INVALID_GOAL, str(exc)) - selected_joint_names = [ - joint_name for group in resolved_groups for joint_name in group.joint_names + joint_name for group in selection.groups for joint_name in group.joint_names ] exact_error = _validate_exact_joint_keys(start, selected_joint_names, "start") if exact_error is not None: @@ -188,11 +183,17 @@ def plan_selected_joint_path( if exact_error is not None: return exact_error - robot_ids = {group.robot_id for group in resolved_groups} + try: + robot_ids_by_name = _robot_ids_by_name(world, selection.robot_names) + except (KeyError, ValueError) as exc: + return _create_failure_result(PlanningStatus.INVALID_GOAL, str(exc)) + + robot_ids = set(robot_ids_by_name.values()) if len(robot_ids) != 1: return self._plan_multi_robot_selected_joint_path( world=world, - resolved_groups=resolved_groups, + groups=selection.groups, + robot_ids_by_name=robot_ids_by_name, start=start, goal=goal, timeout=timeout, @@ -244,7 +245,8 @@ def plan_selected_joint_path( def _plan_multi_robot_selected_joint_path( self, world: WorldSpec, - resolved_groups: tuple[ResolvedPlanningGroup, ...], + groups: tuple[PlanningGroup, ...], + robot_ids_by_name: dict[RobotName, WorldRobotID], start: JointState, goal: JointState, timeout: float, @@ -258,14 +260,16 @@ def _plan_multi_robot_selected_joint_path( "World must be finalized before planning", ) - selected_joint_names = [joint for group in resolved_groups for joint in group.joint_names] + selected_joint_names = [joint for group in groups for joint in group.joint_names] q_start = np.array( _order_joint_state(start, selected_joint_names).position, dtype=np.float64 ) q_goal = np.array(_order_joint_state(goal, selected_joint_names).position, dtype=np.float64) try: - robot_order, robot_joint_names = _validate_full_robot_groups(world, resolved_groups) + robot_order, robot_joint_names = _validate_full_robot_groups( + world, groups, robot_ids_by_name + ) except KeyError as exc: return _create_failure_result(PlanningStatus.NO_SOLUTION, str(exc)) if not robot_order: @@ -641,21 +645,41 @@ def _create_failure_result( def _validate_full_robot_groups( world: WorldSpec, - resolved_groups: tuple[ResolvedPlanningGroup, ...], + groups: tuple[PlanningGroup, ...], + robot_ids_by_name: dict[RobotName, WorldRobotID], ) -> tuple[list[WorldRobotID], dict[WorldRobotID, list[str]]]: robot_order: list[WorldRobotID] = [] robot_joint_names: dict[WorldRobotID, list[str]] = {} known_robot_ids = set(world.get_robot_ids()) - for group in resolved_groups: - if group.robot_id not in known_robot_ids: - raise KeyError(f"Robot '{group.robot_id}' not found") - if group.robot_id not in robot_joint_names: - robot_joint_names[group.robot_id] = [] - robot_order.append(group.robot_id) - robot_joint_names[group.robot_id].extend(group.joint_names) + for group in groups: + robot_id = robot_ids_by_name[group.robot_name] + if robot_id not in known_robot_ids: + raise KeyError(f"Robot '{robot_id}' not found") + if robot_id not in robot_joint_names: + robot_joint_names[robot_id] = [] + robot_order.append(robot_id) + robot_joint_names[robot_id].extend(group.joint_names) return robot_order, robot_joint_names +def _robot_ids_by_name( + world: WorldSpec, robot_names: tuple[RobotName, ...] +) -> dict[RobotName, WorldRobotID]: + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in robot_names: + matches = [ + robot_id + for robot_id in world.get_robot_ids() + if world.get_robot_config(robot_id).name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + robot_ids_by_name[robot_name] = matches[0] + return robot_ids_by_name + + def _validate_selected_groups_cover_full_robots( world: WorldSpec, robot_order: list[WorldRobotID], diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py index 3c91c719dd..6d4dd20c9e 100644 --- a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -23,10 +23,10 @@ import numpy as np +from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import PlanningStatus -from dimos.manipulation.planning.spec.models import ResolvedPlanningGroup from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -52,13 +52,11 @@ def _joint_state(names: list[str], positions: list[float]) -> JointState: def _group( group_id: str, - robot_id: str, robot_name: str, joint_names: tuple[str, ...], -) -> ResolvedPlanningGroup: - return ResolvedPlanningGroup( +) -> PlanningGroup: + return PlanningGroup( id=group_id, - robot_id=robot_id, robot_name=robot_name, group_name=group_id.split("/", maxsplit=1)[1], joint_names=joint_names, @@ -68,27 +66,24 @@ def _group( ) +def _selection(*groups: PlanningGroup) -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups(tuple(groups)) + + class _SelectionWorld: is_finalized = True def __init__( self, - groups: dict[str, ResolvedPlanningGroup], robot_configs: dict[str, RobotModelConfig], coupled_collision_predicate: Callable[[dict[str, JointState]], bool] | None = None, ) -> None: - self._groups = groups self._robot_configs = robot_configs self._coupled_collision_predicate = coupled_collision_predicate self.coupled_collision_checks = 0 self.config_collision_names: list[list[str]] = [] self.edge_collision_names: list[tuple[list[str], list[str]]] = [] - def resolve_planning_groups( - self, group_ids: tuple[str, ...] - ) -> tuple[ResolvedPlanningGroup, ...]: - return tuple(self._groups[group_id] for group_id in group_ids) - def get_robot_config(self, robot_id: str) -> RobotModelConfig: return self._robot_configs[robot_id] @@ -130,14 +125,12 @@ def is_collision_free(self, ctx: dict[str, JointState], robot_id: str) -> bool: def test_plan_selected_joint_path_rejects_missing_and_extra_start_names() -> None: - world = _SelectionWorld( - groups={"arm/arm": _group("arm/arm", "robot_1", "arm", ("arm/joint1", "arm/joint2"))}, - robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, - ) + group = _group("arm/arm", "arm", ("arm/joint1", "arm/joint2")) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) result = RRTConnectPlanner().plan_selected_joint_path( cast("WorldSpec", world), - ["arm/arm"], + _selection(group), start=_joint_state(["arm/joint1", "arm/extra"], [0.0, 0.0]), goal=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), ) @@ -148,14 +141,12 @@ def test_plan_selected_joint_path_rejects_missing_and_extra_start_names() -> Non def test_plan_selected_joint_path_rejects_missing_and_extra_goal_names() -> None: - world = _SelectionWorld( - groups={"arm/arm": _group("arm/arm", "robot_1", "arm", ("arm/joint1", "arm/joint2"))}, - robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, - ) + group = _group("arm/arm", "arm", ("arm/joint1", "arm/joint2")) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) result = RRTConnectPlanner().plan_selected_joint_path( cast("WorldSpec", world), - ["arm/arm"], + _selection(group), start=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), goal=_joint_state(["arm/joint1", "arm/extra"], [0.0, 0.0]), ) @@ -166,11 +157,9 @@ def test_plan_selected_joint_path_rejects_missing_and_extra_goal_names() -> None def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> None: + left_group = _group("left/arm", "left", ("left/joint1",)) + right_group = _group("right/arm", "right", ("right/joint1",)) world = _SelectionWorld( - groups={ - "left/arm": _group("left/arm", "left_robot", "left", ("left/joint1",)), - "right/arm": _group("right/arm", "right_robot", "right", ("right/joint1",)), - }, robot_configs={ "left_robot": _robot_config("left", ["joint1"]), "right_robot": _robot_config("right", ["joint1"]), @@ -180,7 +169,7 @@ def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> No result = RRTConnectPlanner().plan_selected_joint_path( cast("WorldSpec", world), - ["left/arm", "right/arm"], + _selection(left_group, right_group), start=joint_state, goal=_joint_state(["left/joint1", "right/joint1"], [0.1, -0.1]), ) @@ -193,21 +182,12 @@ def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> No def test_plan_selected_joint_path_converts_single_robot_backend_boundary_to_local() -> None: - world = _SelectionWorld( - groups={ - "arm/manipulator": _group( - "arm/manipulator", - "robot_1", - "arm", - ("arm/joint1", "arm/joint2"), - ) - }, - robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, - ) + group = _group("arm/manipulator", "arm", ("arm/joint1", "arm/joint2")) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) result = RRTConnectPlanner().plan_selected_joint_path( cast("WorldSpec", world), - ["arm/manipulator"], + _selection(group), start=_joint_state(["arm/joint2", "arm/joint1"], [0.2, 0.1]), goal=_joint_state(["arm/joint1", "arm/joint2"], [0.3, 0.4]), ) @@ -231,11 +211,9 @@ def coupled_free(ctx: dict[str, JointState]) -> bool: right = ctx["right_robot"].position[0] return not (left > 0.04 and right > 0.04) + left_group = _group("left/arm", "left", ("left/joint1",)) + right_group = _group("right/arm", "right", ("right/joint1",)) world = _SelectionWorld( - groups={ - "left/arm": _group("left/arm", "left_robot", "left", ("left/joint1",)), - "right/arm": _group("right/arm", "right_robot", "right", ("right/joint1",)), - }, robot_configs={ "left_robot": _robot_config("left", ["joint1"]), "right_robot": _robot_config("right", ["joint1"]), @@ -245,7 +223,7 @@ def coupled_free(ctx: dict[str, JointState]) -> bool: result = RRTConnectPlanner().plan_selected_joint_path( cast("WorldSpec", world), - ["left/arm", "right/arm"], + _selection(left_group, right_group), start=_joint_state(["left/joint1", "right/joint1"], [0.0, 0.0]), goal=_joint_state(["left/joint1", "right/joint1"], [0.1, 0.1]), ) @@ -255,15 +233,13 @@ def coupled_free(ctx: dict[str, JointState]) -> bool: def test_plan_selected_joint_path_rejects_single_robot_subset_selection() -> None: - world = _SelectionWorld( - groups={"arm/wrist": _group("arm/wrist", "robot_1", "arm", ("arm/joint2",))}, - robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}, - ) + group = _group("arm/wrist", "arm", ("arm/joint2",)) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) joint_state = _joint_state(["arm/joint2"], [0.0]) result = RRTConnectPlanner().plan_selected_joint_path( cast("WorldSpec", world), - ["arm/wrist"], + _selection(group), start=joint_state, goal=joint_state, ) diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 18209cad24..32f32f5818 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,12 +21,14 @@ from pydantic import Field from dimos.core.module import ModuleConfig -from dimos.manipulation.planning.planning_groups import FALLBACK_PLANNING_GROUP_NAME +from dimos.manipulation.planning.groups import ( + FALLBACK_PLANNING_GROUP_NAME, + PlanningGroupDefinition, +) from dimos.manipulation.planning.planning_identifiers import ( assert_local_joint_names, assert_valid_robot_name, ) -from dimos.manipulation.planning.spec.models import PlanningGroupDefinition from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index d7a226d325..cafcec37ce 100644 --- a/dimos/manipulation/planning/spec/models.py +++ b/dimos/manipulation/planning/spec/models.py @@ -17,7 +17,7 @@ from __future__ import annotations from dataclasses import dataclass, field -from typing import TYPE_CHECKING, Literal, TypeAlias +from typing import TYPE_CHECKING, TypeAlias from dimos.manipulation.planning.spec.enums import ( IKStatus, @@ -55,79 +55,6 @@ Jacobian: TypeAlias = "NDArray[np.float64]" """6 x n Jacobian matrix (rows: [vx, vy, vz, wx, wy, wz])""" -PlanningGroupSource: TypeAlias = Literal["srdf", "fallback"] - - -@dataclass(frozen=True) -class PlanningGroupDefinition: - """Model-level declaration of a planning group. - - Joint names are local model names. The definition is not bound to a world - robot ID and is safe to store on RobotModelConfig. Definitions are parsed - from SRDF or conservative fallback generation before any robot instance is - added to a planning world. - """ - - name: str - joint_names: tuple[LocalModelJointName, ...] - base_link: str - tip_link: str | None = None - source: PlanningGroupSource = "srdf" - - @property - def has_pose_target(self) -> bool: - """Whether this group has a valid pose target frame.""" - return self.tip_link is not None - - -@dataclass(frozen=True) -class PlanningGroupDescriptor: - """Read-only public snapshot for an available planning group. - - Descriptors are returned by query APIs. They expose stable public IDs and - global joint names for callers, but intentionally do not expose backend - runtime IDs or mutable world state. - """ - - id: PlanningGroupID - robot_name: RobotName - group_name: str - joint_names: tuple[GlobalJointName, ...] - local_joint_names: tuple[LocalModelJointName, ...] - base_link: str - tip_link: str | None = None - source: PlanningGroupSource = "srdf" - - @property - def has_pose_target(self) -> bool: - """Whether this group can be directly pose-targeted.""" - return self.tip_link is not None - - -@dataclass(frozen=True) -class ResolvedPlanningGroup: - """Runtime/world-bound planning group data. - - Resolved groups are created from descriptors/IDs for a specific planning - world. They include the internal WorldRobotID and are the form consumed by - planners, IK backends, and group-scoped FK/Jacobian calls. - """ - - id: PlanningGroupID - robot_id: WorldRobotID - robot_name: RobotName - group_name: str - joint_names: tuple[GlobalJointName, ...] - local_joint_names: tuple[LocalModelJointName, ...] - base_link: str - tip_link: str | None = None - source: PlanningGroupSource = "srdf" - - @property - def has_pose_target(self) -> bool: - """Whether this group can be directly pose-targeted.""" - return self.tip_link is not None - @dataclass class GeneratedPlan: diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index bb761088a4..4df2ccce54 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -29,15 +29,14 @@ import numpy as np from numpy.typing import NDArray + from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( GeneratedPlan, IKResult, Obstacle, - PlanningGroupDescriptor, PlanningGroupID, PlanningResult, - ResolvedPlanningGroup, WorldRobotID, ) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -75,26 +74,6 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: """Get robot configuration.""" ... - def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: - """List planning groups for robots currently added to this world. - - SRDF/fallback parsing creates model-level definitions before world - binding. This query returns world-level descriptor snapshots with stable - public IDs and global joint names. - """ - ... - - def resolve_planning_groups( - self, group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...] - ) -> tuple[ResolvedPlanningGroup, ...]: - """Resolve group IDs against this world's runtime robot bindings. - - Resolution is world-bound: it looks up the robots added to this world, - attaches WorldRobotID, validates selected groups, and returns data that - backend planners/IK can use with world contexts. - """ - ... - def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: # lower limits, upper limits @@ -259,9 +238,8 @@ def solve( def solve_pose_targets( self, world: WorldSpec, - pose_targets: dict[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], - auxiliary_groups: list[PlanningGroupID | PlanningGroupDescriptor] - | tuple[PlanningGroupID | PlanningGroupDescriptor, ...] = (), + pose_targets: dict[PlanningGroup, PoseStamped], + auxiliary_groups: list[PlanningGroup] | tuple[PlanningGroup, ...] = (), seed: JointState | None = None, position_tolerance: float = 0.001, orientation_tolerance: float = 0.01, @@ -299,7 +277,7 @@ def plan_joint_path( def plan_selected_joint_path( self, world: WorldSpec, - group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...], + selection: PlanningGroupSelection, start: JointState, goal: JointState, timeout: float = 10.0, diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_utils.py index dacfcf53a2..1186372ae0 100644 --- a/dimos/manipulation/planning/test_planning_group_utils.py +++ b/dimos/manipulation/planning/test_planning_group_utils.py @@ -18,15 +18,13 @@ import pytest -from dimos.manipulation.planning.planning_group_utils import joint_target_to_global_names -from dimos.manipulation.planning.spec.models import ResolvedPlanningGroup +from dimos.manipulation.planning.groups import PlanningGroup, joint_target_to_global_names from dimos.msgs.sensor_msgs.JointState import JointState -def _make_group() -> ResolvedPlanningGroup: - return ResolvedPlanningGroup( +def _make_group() -> PlanningGroup: + return PlanningGroup( id="left/arm", - robot_id="robot_left", robot_name="left", group_name="arm", joint_names=("left/j1", "left/j2", "left/j3"), diff --git a/dimos/manipulation/planning/test_planning_groups.py b/dimos/manipulation/planning/test_planning_groups.py index beb6984851..303c469e67 100644 --- a/dimos/manipulation/planning/test_planning_groups.py +++ b/dimos/manipulation/planning/test_planning_groups.py @@ -20,7 +20,7 @@ import pytest -from dimos.manipulation.planning.planning_groups import ( +from dimos.manipulation.planning.groups import ( FALLBACK_PLANNING_GROUP_NAME, PlanningGroupDiscoveryError, discover_planning_group_definitions, diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 19df494c5e..a884616b47 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,21 +25,17 @@ import numpy as np +from dimos.manipulation.planning.groups import PlanningGroupRegistry from dimos.manipulation.planning.planning_identifiers import ( assert_local_joint_names, make_global_joint_name, - make_global_joint_names, - make_planning_group_id, - parse_planning_group_id, ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( GeneratedPlan, Obstacle, - PlanningGroupDescriptor, PlanningGroupID, - ResolvedPlanningGroup, WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec, WorldSpec @@ -209,6 +205,7 @@ def __init__(self, time_step: float = 0.0, enable_viz: bool = False) -> None: # Tracking data self._robots: dict[WorldRobotID, _RobotData] = {} + self._planning_groups = PlanningGroupRegistry() self._obstacles: dict[str, _ObstacleData] = {} self._robot_counter = 0 self._obstacle_counter = 0 @@ -261,6 +258,7 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: base_frame=base_frame, preview_model_instance=preview_model_instance, ) + self._planning_groups.add_robot(config) logger.info(f"Added robot '{robot_id}' ({config.name})") return robot_id @@ -349,75 +347,6 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: raise KeyError(f"Robot '{robot_id}' not found") return self._robots[robot_id].config - def list_planning_groups(self) -> tuple[PlanningGroupDescriptor, ...]: - """List available planning groups as immutable descriptor snapshots.""" - descriptors: list[PlanningGroupDescriptor] = [] - for robot_data in self._robots.values(): - config = robot_data.config - for group in config.planning_groups: - descriptors.append( - PlanningGroupDescriptor( - id=make_planning_group_id(config.name, group.name), - robot_name=config.name, - group_name=group.name, - joint_names=tuple(make_global_joint_names(config.name, group.joint_names)), - local_joint_names=group.joint_names, - base_link=group.base_link, - tip_link=group.tip_link, - source=group.source, - ) - ) - return tuple(descriptors) - - def resolve_planning_groups( - self, group_ids: list[PlanningGroupID] | tuple[PlanningGroupID, ...] - ) -> tuple[ResolvedPlanningGroup, ...]: - """Resolve planning group IDs against current world robot data.""" - resolved_groups: list[ResolvedPlanningGroup] = [] - seen_joints: dict[str, PlanningGroupID] = {} - - for group_id in group_ids: - robot_name, group_name = parse_planning_group_id(group_id) - robot_data = self._get_robot_data_by_name(robot_name) - group = next( - ( - candidate - for candidate in robot_data.config.planning_groups - if candidate.name == group_name - ), - None, - ) - if group is None: - raise KeyError(f"Unknown planning group ID: {group_id}") - - global_joint_names = tuple( - make_global_joint_name(robot_name, local_name) for local_name in group.joint_names - ) - for joint_name in global_joint_names: - previous_group_id = seen_joints.get(joint_name) - if previous_group_id is not None: - raise ValueError( - "Selected planning groups overlap on global joint " - f"{joint_name}: {previous_group_id} and {group_id}" - ) - seen_joints[joint_name] = group_id - - resolved_groups.append( - ResolvedPlanningGroup( - id=group_id, - robot_id=robot_data.robot_id, - robot_name=robot_name, - group_name=group_name, - joint_names=global_joint_names, - local_joint_names=group.joint_names, - base_link=group.base_link, - tip_link=group.tip_link, - source=group.source, - ) - ) - - return tuple(resolved_groups) - def _get_robot_data_by_name(self, robot_name: str) -> _RobotData: matches = [ robot_data @@ -430,10 +359,6 @@ def _get_robot_data_by_name(self, robot_name: str) -> _RobotData: raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") return matches[0] - def _resolve_single_planning_group(self, group_id: PlanningGroupID) -> ResolvedPlanningGroup: - resolved_groups = self.resolve_planning_groups((group_id,)) - return resolved_groups[0] - def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: @@ -1062,11 +987,11 @@ def get_group_pose(self, ctx: Context, group_id: PlanningGroupID) -> PoseStamped if not self._finalized: raise RuntimeError("World must be finalized first") - group = self._resolve_single_planning_group(group_id) + group = self._planning_groups.get(group_id) if group.tip_link is None: raise ValueError(f"Planning group '{group_id}' has no pose target frame") - robot_data = self._robots[group.robot_id] + robot_data = self._get_robot_data_by_name(group.robot_name) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) try: @@ -1165,11 +1090,11 @@ def get_group_jacobian(self, ctx: Context, group_id: PlanningGroupID) -> NDArray if not self._finalized: raise RuntimeError("World must be finalized first") - group = self._resolve_single_planning_group(group_id) + group = self._planning_groups.get(group_id) if group.tip_link is None: raise ValueError(f"Planning group '{group_id}' has no pose target frame") - robot_data = self._robots[group.robot_id] + robot_data = self._get_robot_data_by_name(group.robot_name) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) try: @@ -1238,9 +1163,10 @@ def _preview_robot_ids_for_groups( ) -> list[WorldRobotID]: """Resolve planning groups to stable preview robot IDs.""" robot_ids: list[WorldRobotID] = [] - for group in self.resolve_planning_groups(tuple(group_ids)): - if group.robot_id not in robot_ids: - robot_ids.append(group.robot_id) + for group in self._planning_groups.select(tuple(group_ids)).groups: + robot_id = self._get_robot_data_by_name(group.robot_name).robot_id + if robot_id not in robot_ids: + robot_ids.append(robot_id) return robot_ids def _show_preview_robot(self, robot_id: WorldRobotID) -> None: diff --git a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py index adc04aaae6..5a1f8f1ecb 100644 --- a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -21,8 +21,8 @@ import numpy as np import pytest +from dimos.manipulation.planning.groups import PlanningGroupDefinition, PlanningGroupRegistry from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.models import PlanningGroupDefinition from dimos.manipulation.planning.world.drake_world import DrakeWorld, _RobotData from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -61,6 +61,7 @@ def _world(*configs: RobotModelConfig) -> DrakeWorld: ) for index, config in enumerate(configs, start=1) } + world._planning_groups = PlanningGroupRegistry(configs) return world @@ -74,10 +75,11 @@ def _arm_group(*joint_names: str) -> PlanningGroupDefinition: ) -def test_list_planning_groups_returns_stable_ids_and_global_joint_names() -> None: - world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) +def test_planning_group_registry_returns_stable_ids_and_global_joint_names() -> None: + config = _config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")]) + registry = PlanningGroupRegistry([config]) - groups = world.list_planning_groups() + groups = registry.list() assert len(groups) == 1 assert groups[0].id == "left/arm" @@ -94,69 +96,79 @@ def test_robot_model_config_allows_planning_groups_without_robot_scoped_ee() -> joint_names=["joint1"], planning_groups=[_arm_group("joint1")], ) - world = _world(config) + registry = PlanningGroupRegistry([config]) - groups = world.list_planning_groups() + groups = registry.list() assert config.end_effector_link is None assert groups[0].id == "left/arm" def test_duplicate_local_joint_names_across_robots_are_disambiguated() -> None: - world = _world( - _config("left", ["joint1"], [_arm_group("joint1")]), - _config("right", ["joint1"], [_arm_group("joint1")]), + registry = PlanningGroupRegistry( + [ + _config("left", ["joint1"], [_arm_group("joint1")]), + _config("right", ["joint1"], [_arm_group("joint1")]), + ] ) - groups = world.list_planning_groups() + groups = registry.list() assert [group.id for group in groups] == ["left/arm", "right/arm"] assert [group.joint_names for group in groups] == [("left/joint1",), ("right/joint1",)] -def test_resolve_planning_groups_returns_robot_ids_and_joint_names() -> None: - world = _world( - _config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")]), - _config("right", ["joint1", "joint2"], [_arm_group("joint2")]), +def test_planning_group_selection_returns_ordered_global_joint_names() -> None: + registry = PlanningGroupRegistry( + [ + _config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")]), + _config("right", ["joint1", "joint2"], [_arm_group("joint2")]), + ] ) - resolved = world.resolve_planning_groups(("left/arm", "right/arm")) + selection = registry.select(("left/arm", "right/arm")) - assert [group.id for group in resolved] == ["left/arm", "right/arm"] - assert [group.robot_id for group in resolved] == ["robot_1", "robot_2"] - assert [group.joint_names for group in resolved] == [ + assert list(selection.group_ids) == ["left/arm", "right/arm"] + assert list(selection.robot_names) == ["left", "right"] + assert [group.joint_names for group in selection.groups] == [ ("left/joint1", "left/joint2"), ("right/joint2",), ] - assert [group.local_joint_names for group in resolved] == [("joint1", "joint2"), ("joint2",)] + assert list(selection.joint_names) == ["left/joint1", "left/joint2", "right/joint2"] + assert [group.local_joint_names for group in selection.groups] == [ + ("joint1", "joint2"), + ("joint2",), + ] -def test_resolve_planning_groups_unknown_group_raises_key_error() -> None: - world = _world(_config("left", ["joint1"], [_arm_group("joint1")])) +def test_planning_group_registry_unknown_group_raises_key_error() -> None: + registry = PlanningGroupRegistry([_config("left", ["joint1"], [_arm_group("joint1")])]) with pytest.raises(KeyError, match="Unknown planning group ID: left/gripper"): - world.resolve_planning_groups(("left/gripper",)) - - -def test_resolve_planning_groups_overlapping_same_robot_groups_raise_value_error() -> None: - world = _world( - _config( - "left", - ["joint1", "joint2"], - [ - _arm_group("joint1", "joint2"), - PlanningGroupDefinition( - name="wrist", - joint_names=("joint2",), - base_link="link1", - tip_link="tool0", - ), - ], - ) + registry.select(("left/gripper",)) + + +def test_planning_group_selection_overlapping_same_robot_groups_raise_value_error() -> None: + registry = PlanningGroupRegistry( + [ + _config( + "left", + ["joint1", "joint2"], + [ + _arm_group("joint1", "joint2"), + PlanningGroupDefinition( + name="wrist", + joint_names=("joint2",), + base_link="link1", + tip_link="tool0", + ), + ], + ) + ] ) with pytest.raises(ValueError, match="overlap.*left/joint2"): - world.resolve_planning_groups(("left/arm", "left/wrist")) + registry.select(("left/arm", "left/wrist")) def test_positions_for_robot_state_accepts_local_joint_names_in_config_order() -> None: diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 9ac5d13918..ce0202a23f 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -27,6 +27,7 @@ ManipulationModuleConfig, ManipulationState, ) +from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -34,7 +35,6 @@ from dimos.manipulation.planning.spec.models import ( GeneratedPlan, IKResult, - ResolvedPlanningGroup, ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec from dimos.msgs.geometry_msgs.Pose import Pose @@ -360,9 +360,9 @@ def test_execute_requires_task_name(self): end_effector_link="ee", ) module = _make_module_with_monitor(config_no_task) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("arm", "manipulator", ["j1"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("arm", "manipulator", ["j1"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1"], position=[0.0] ) @@ -380,9 +380,9 @@ def test_execute_success(self, robot_config, simple_trajectory): generator = MagicMock() generator.generate.return_value = simple_trajectory module._robots = {"test_arm": ("id", robot_config, generator)} - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] ) @@ -423,9 +423,9 @@ def test_execute_rejected(self, robot_config, simple_trajectory): generator = MagicMock() generator.generate.return_value = simple_trajectory module._robots = {"test_arm": ("id", robot_config, generator)} - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] ) @@ -460,6 +460,12 @@ def _make_module_with_monitor(*configs: RobotModelConfig) -> ManipulationModule: for config in configs: robot_id = f"robot_{config.name}" module._robots[config.name] = (robot_id, config, MagicMock()) + module._world_monitor.planning_groups = _FakePlanningGroups( + [ + _make_global_group(config.name, "manipulator", list(config.joint_names)) + for config in configs + ] + ) return module @@ -483,12 +489,9 @@ def _make_robot_config( ) -def _make_global_group( - robot_name: str, group_name: str, joints: list[str] -) -> ResolvedPlanningGroup: - return ResolvedPlanningGroup( +def _make_global_group(robot_name: str, group_name: str, joints: list[str]) -> PlanningGroup: + return PlanningGroup( id=f"{robot_name}/{group_name}", - robot_id=f"robot_{robot_name}", robot_name=robot_name, group_name=group_name, joint_names=tuple(f"{robot_name}/{joint}" for joint in joints), @@ -498,6 +501,32 @@ def _make_global_group( ) +class _FakePlanningGroups: + def __init__(self, groups: list[PlanningGroup]) -> None: + self._groups = {group.id: group for group in groups} + + def get(self, group_id: str) -> PlanningGroup: + return self._groups[group_id] + + def select(self, group_ids: tuple[str, ...]) -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups( + tuple(self._groups[group_id] for group_id in group_ids) + ) + + def groups_for_robot(self, robot_name: str) -> tuple[PlanningGroup, ...]: + return tuple(group for group in self._groups.values() if group.robot_name == robot_name) + + def default_group_id_for_robot(self, robot_name: str) -> str | None: + group_id = f"{robot_name}/manipulator" + return group_id if group_id in self._groups else None + + def primary_pose_group_id_for_robot(self, robot_name: str) -> str | None: + for group in self.groups_for_robot(robot_name): + if group.has_pose_target: + return group.id + return None + + def _make_generated_plan(group_ids: tuple[str, ...], *points: list[float]) -> GeneratedPlan: return GeneratedPlan( group_ids=group_ids, @@ -741,9 +770,9 @@ def test_preview_plan_explicit_duration_overrides_default(self): def test_preview_plan_respects_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("arm", "manipulator", ["j1"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("arm", "manipulator", ["j1"])] + ) module._last_plan = GeneratedPlan( group_ids=("arm/manipulator",), path=[JointState(name=["arm/j1"], position=[0.0])], @@ -757,9 +786,9 @@ def test_preview_plan_respects_robot_filter(self): def test_preview_plan_rejects_unaffected_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("arm", "manipulator", ["j1"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("arm", "manipulator", ["j1"])] + ) module._last_plan = GeneratedPlan( group_ids=("arm/manipulator",), path=[JointState(name=["arm/j1"], position=[0.0])], @@ -783,9 +812,9 @@ class TestGeneratedPlanProjection: def test_selected_joint_state_accepts_local_current_state_names(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("left", "arm", ["j1", "j2"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1", "j2"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1", "j2"], position=[1.0, 2.0] ) @@ -799,9 +828,9 @@ def test_selected_joint_state_accepts_local_current_state_names(self): def test_selected_joint_state_rejects_mixed_current_state_names(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("left", "arm", ["j1", "j2"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1", "j2"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["left/j1", "j2"], position=[1.0, 2.0] ) @@ -820,10 +849,12 @@ def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): right_gen = _trajectory_generator() module._robots["left"] = ("robot_left", left_config, left_gen) module._robots["right"] = ("robot_right", right_config, right_gen) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("left", "arm", ["j1", "j2"]), - _make_global_group("right", "arm", ["j1"]), - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [ + _make_global_group("left", "arm", ["j1", "j2"]), + _make_global_group("right", "arm", ["j1"]), + ] + ) module._world_monitor.get_current_joint_state.side_effect = [ JointState(name=["j1", "j2", "j3"], position=[0.0, 0.0, 9.0]), JointState(name=["j1", "j2"], position=[0.0, 8.0]), @@ -853,9 +884,9 @@ def test_execute_plan_holds_non_selected_joints_from_current_state(self): module = _make_module_with_monitor(config) generator = _trajectory_generator() module._robots["left"] = ("robot_left", config, generator) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("left", "arm", ["j2"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j2"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1", "j2", "j3"], position=[10.0, 20.0, 30.0] ) @@ -882,9 +913,9 @@ def test_execute_plan_holds_non_selected_joints_from_current_state(self): def test_execute_plan_rejects_local_waypoint_names(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("left", "arm", ["j1"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1"])] + ) module._world_monitor.get_current_joint_state.return_value = JointState( name=["j1", "j2"], position=[10.0, 20.0] ) @@ -901,9 +932,9 @@ def test_execute_plan_rejects_local_waypoint_names(self): def test_preview_plan_with_last_plan_animates_generated_plan(self): config = _make_robot_config("left", ["j1", "j2"], "task") module = _make_module_with_monitor(config) - module._world_monitor.world.resolve_planning_groups.return_value = [ - _make_global_group("left", "arm", ["j1"]) - ] + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1"])] + ) module._last_plan = GeneratedPlan( group_ids=("left/arm",), path=[ diff --git a/dimos/robot/config.py b/dimos/robot/config.py index 829b7d646d..e57859d9ea 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -28,7 +28,7 @@ from dimos.control.components import HardwareComponent, HardwareType from dimos.control.coordinator import TaskConfig -from dimos.manipulation.planning.planning_groups import discover_planning_group_definitions +from dimos.manipulation.planning.groups import discover_planning_group_definitions from dimos.manipulation.planning.planning_identifiers import make_global_joint_names from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped From c6e7782b77a7846192f0b23aa5b965027d0c0db8 Mon Sep 17 00:00:00 2001 From: cc Date: Fri, 19 Jun 2026 23:17:31 -0700 Subject: [PATCH 21/27] feat: add viser planning target sets --- CONTEXT.md | 6 +- dimos/manipulation/manipulation_module.py | 305 ++++++++++- .../planning/kinematics/pink_ik.py | 289 ++++++++-- .../planning/kinematics/test_pink_ik.py | 170 ++++++ dimos/manipulation/test_manipulation_unit.py | 97 ++++ dimos/manipulation/visualization/types.py | 30 +- .../visualization/viser/adapter.py | 87 ++- dimos/manipulation/visualization/viser/gui.py | 512 +++++++++++++----- .../manipulation/visualization/viser/scene.py | 3 + .../manipulation/visualization/viser/state.py | 52 +- .../viser/test_operation_worker.py | 9 +- .../visualization/viser/test_state.py | 3 + .../viser/test_viser_visualization.py | 163 ++++-- docs/capabilities/manipulation/readme.md | 45 ++ .../.openspec.yaml | 2 + .../add-viser-planning-target-set/design.md | 139 +++++ .../add-viser-planning-target-set/docs.md | 32 ++ .../add-viser-planning-target-set/proposal.md | 53 ++ .../manipulation-planning-groups/spec.md | 71 +++ .../specs/viser-planning-target-set/spec.md | 115 ++++ .../add-viser-planning-target-set/tasks.md | 45 ++ 21 files changed, 1991 insertions(+), 237 deletions(-) create mode 100644 openspec/changes/add-viser-planning-target-set/.openspec.yaml create mode 100644 openspec/changes/add-viser-planning-target-set/design.md create mode 100644 openspec/changes/add-viser-planning-target-set/docs.md create mode 100644 openspec/changes/add-viser-planning-target-set/proposal.md create mode 100644 openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md create mode 100644 openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md create mode 100644 openspec/changes/add-viser-planning-target-set/tasks.md diff --git a/CONTEXT.md b/CONTEXT.md index 52f23f3a7c..eceab2a274 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -32,8 +32,12 @@ _Avoid_: Planning group definition The set of one or more planning groups chosen for a planning request. _Avoid_: Composite group +**Planning Target Set**: +The atomic manipulation UI/planning state built on top of a planning group selection: selected planning groups, target authoring state, combined IK joint target, whole-set feasibility, and generated plan. Per-group UI panels are views into this target set, not independent planning states. +_Avoid_: Independent group cards, per-robot plan state + **Auxiliary Planning Group**: -A planning group selected to participate in a specific planning request without receiving a direct end-effector pose constraint in that request. A planning group may be auxiliary in one request and directly targeted in another. +A planning group selected to participate in a planning target set without receiving a direct end-effector pose target in that request. It is solved, checked, planned, previewed, and executed with the whole target set; it simply has no assigned target gizmo. A planning group may be auxiliary in one request and directly targeted in another. _Avoid_: Joint-only group, intrinsic auxiliary group **Coordinated Planning Problem**: diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index b767ec3289..9e10f2f794 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -76,7 +76,7 @@ NoManipulationVisualizationConfig, ) from dimos.manipulation.visualization.factory import create_manipulation_visualization -from dimos.manipulation.visualization.types import TargetEvaluation +from dimos.manipulation.visualization.types import TargetEvaluation, TargetSetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -1052,6 +1052,309 @@ def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvalu "collision_free": collision_free, } + def _selected_target_by_name( + self, group_ids: tuple[PlanningGroupID, ...], target_joints: JointState + ) -> dict[str, float] | None: + """Validate and index a global target joint state for a group selection.""" + assert self._world_monitor is not None + selection = self._world_monitor.planning_groups.select(group_ids) + if len(target_joints.name) != len(target_joints.position): + logger.error( + "Target set has %d names but %d positions", + len(target_joints.name), + len(target_joints.position), + ) + return None + try: + assert_global_joint_names(target_joints.name) + except ValueError as exc: + logger.error("Invalid target-set joint names: %s", exc) + return None + + target_by_name = dict(zip(target_joints.name, target_joints.position, strict=True)) + missing = [name for name in selection.joint_names if name not in target_by_name] + extra = set(target_by_name) - set(selection.joint_names) + if missing: + logger.error("Target set missing joints: %s", missing) + return None + if extra: + logger.error("Target set has extra joints: %s", sorted(extra)) + return None + return target_by_name + + def _local_target_states_for_selection( + self, group_ids: tuple[PlanningGroupID, ...], target_joints: JointState + ) -> dict[RobotName, tuple[WorldRobotID, RobotModelConfig, JointState]] | None: + """Project selected global targets to full local robot states for evaluation.""" + assert self._world_monitor is not None + selection = self._world_monitor.planning_groups.select(group_ids) + target_by_name = self._selected_target_by_name(group_ids, target_joints) + if target_by_name is None: + return None + + local_targets: dict[RobotName, tuple[WorldRobotID, RobotModelConfig, JointState]] = {} + for robot_name in selection.robot_names: + robot = self._robots.get(robot_name) + if robot is None: + logger.error("Robot '%s' is not registered", robot_name) + return None + robot_id, config, _ = robot + current = self._world_monitor.get_current_joint_state(robot_id) + if current is None: + logger.error("No joint state for robot '%s'", robot_name) + return None + current_by_name = self._current_positions_by_name(robot_name, current) + if current_by_name is None: + return None + + positions: list[float] = [] + for local_name, global_name in zip( + config.joint_names, + make_global_joint_names(robot_name, config.joint_names), + strict=True, + ): + if global_name in target_by_name: + positions.append(float(target_by_name[global_name])) + elif local_name in current_by_name: + positions.append(float(current_by_name[local_name])) + else: + logger.error("Current state missing joint '%s'", global_name) + return None + local_targets[robot_name] = ( + robot_id, + config, + JointState(name=list(config.joint_names), position=positions), + ) + return local_targets + + def _target_set_poses( + self, + group_ids: tuple[PlanningGroupID, ...], + local_targets: Mapping[RobotName, tuple[WorldRobotID, RobotModelConfig, JointState]], + ) -> dict[PlanningGroupID, PoseStamped | None]: + """Compute pose outputs for pose-targetable groups in a target set.""" + assert self._world_monitor is not None + poses: dict[PlanningGroupID, PoseStamped | None] = {} + for group in self._world_monitor.planning_groups.select(group_ids).groups: + if not group.has_pose_target: + continue + robot_target = local_targets.get(group.robot_name) + if robot_target is None: + poses[group.id] = None + continue + _, _, joint_state = robot_target + try: + poses[group.id] = self._world_monitor.get_group_pose(group.id, joint_state) + except Exception as exc: + logger.warning("Failed to evaluate pose for group '%s': %s", group.id, exc) + poses[group.id] = None + return poses + + def _evaluate_global_target_set( + self, + group_ids: tuple[PlanningGroupID, ...], + target_joints: JointState | None, + *, + status: str = "FEASIBLE", + message: str = "Target set is feasible", + position_error: float = 0.0, + orientation_error: float = 0.0, + ) -> TargetSetEvaluation: + """Evaluate whole-set collision and pose outputs for global target joints.""" + if self._world_monitor is None: + return { + "success": False, + "status": "UNAVAILABLE", + "message": "Planning is not initialized", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + if target_joints is None: + return { + "success": False, + "status": "NO_TARGET", + "message": "No target joints provided", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + try: + local_targets = self._local_target_states_for_selection(group_ids, target_joints) + except (KeyError, ValueError) as exc: + return { + "success": False, + "status": "INVALID", + "message": str(exc), + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + if local_targets is None: + return { + "success": False, + "status": "INVALID", + "message": "Invalid target-set joints", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + + collision_free = True + diagnostics: dict[PlanningGroupID, str] = {} + selection = self._world_monitor.planning_groups.select(group_ids) + for robot_name, (robot_id, _, local_target) in local_targets.items(): + robot_collision_free = self._world_monitor.is_state_valid(robot_id, local_target) + collision_free = collision_free and robot_collision_free + for group in selection.groups: + if group.robot_name == robot_name: + diagnostics[group.id] = ( + "Target is collision-free" + if robot_collision_free + else "Target is in collision" + ) + + return { + "success": collision_free, + "status": status if collision_free else "COLLISION", + "message": message if collision_free else "Target set is in collision", + "collision_free": collision_free, + "group_ids": group_ids, + "target_joints": JointState(target_joints), + "group_diagnostics": diagnostics, + "group_poses": self._target_set_poses(group_ids, local_targets), + "position_error": position_error, + "orientation_error": orientation_error, + } + + def evaluate_joint_target_set( + self, joint_targets: Mapping[PlanningGroupID | PlanningGroup, JointState] + ) -> TargetSetEvaluation: + """Evaluate joint targets for a whole planning target set.""" + if self._world_monitor is None: + return { + "success": False, + "status": "UNAVAILABLE", + "message": "Planning is not initialized", + "collision_free": False, + "target_joints": None, + } + if not joint_targets: + return { + "success": False, + "status": "NO_TARGET", + "message": "No joint targets provided", + "collision_free": False, + "target_joints": None, + } + + group_ids = tuple( + dict.fromkeys(planning_group_id_from_selector(group) for group in joint_targets) + ) + goal_names: list[str] = [] + goal_positions: list[float] = [] + for group_selector, target in joint_targets.items(): + group_id = planning_group_id_from_selector(group_selector) + target_global = self._joint_target_to_global_names(group_id, target) + if target_global is None: + return { + "success": False, + "status": "INVALID", + "message": f"Invalid joint target for '{group_id}'", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + goal_names.extend(target_global.name) + goal_positions.extend(target_global.position) + + return self._evaluate_global_target_set( + group_ids, + JointState(name=goal_names, position=goal_positions), + ) + + def evaluate_pose_target_set( + self, + pose_targets: Mapping[PlanningGroupID | PlanningGroup, Pose | PoseStamped], + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), + seed: JointState | None = None, + ) -> TargetSetEvaluation: + """Evaluate pose targets for a whole planning target set using configured IK.""" + if self._world_monitor is None or self._kinematics is None: + return { + "success": False, + "status": "UNAVAILABLE", + "message": "Planning is not initialized", + "collision_free": False, + "target_joints": None, + } + if not pose_targets: + return { + "success": False, + "status": "NO_TARGET", + "message": "No pose targets provided", + "collision_free": False, + "target_joints": None, + } + + def stamped_pose(pose: Pose | PoseStamped) -> PoseStamped: + if isinstance(pose, PoseStamped): + return pose + return PoseStamped( + frame_id="world", position=pose.position, orientation=pose.orientation + ) + + stamped_targets = { + planning_group_id_from_selector(group): stamped_pose(pose) + for group, pose in pose_targets.items() + } + auxiliary_ids = tuple(planning_group_id_from_selector(group) for group in auxiliary_groups) + group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) + try: + target_groups = { + self._world_monitor.planning_groups.get(group_id): pose + for group_id, pose in stamped_targets.items() + } + auxiliary = tuple( + self._world_monitor.planning_groups.get(group_id) for group_id in auxiliary_ids + ) + ik = self._kinematics.solve_pose_targets( + world=self._world_monitor.world, + pose_targets=target_groups, + auxiliary_groups=auxiliary, + seed=seed or self._selected_joint_state(group_ids), + check_collision=False, + ) + except (KeyError, ValueError) as exc: + return { + "success": False, + "status": "INVALID", + "message": str(exc), + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + + if not ik.is_success() or ik.joint_state is None: + return { + "success": False, + "status": ik.status.name, + "message": ik.message, + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + "position_error": ik.position_error, + "orientation_error": ik.orientation_error, + } + return self._evaluate_global_target_set( + group_ids, + ik.joint_state, + status=ik.status.name, + message=ik.message, + position_error=ik.position_error, + orientation_error=ik.orientation_error, + ) + @rpc def set_init_joints(self, joint_state: JointState, robot_name: RobotName | None = None) -> bool: """Set the init joint state. diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index bb5dbf62b8..43c0b9b5d9 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -28,10 +28,10 @@ from dimos.manipulation.planning.groups import ( PlanningGroup, PlanningGroupSelection, - filter_joint_state_to_selected_joints, matching_global_joint_name, ) from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig +from dimos.manipulation.planning.planning_identifiers import make_global_joint_name from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( @@ -186,7 +186,7 @@ def solve_pose_targets( check_collision: bool = True, max_attempts: int = 10, ) -> IKResult: - """Solve a planning-group pose target and return only selected global joints.""" + """Solve planning-group pose targets and return selected global joints.""" if not pose_targets: return _failure(IKStatus.NO_SOLUTION, "At least one pose target is required") @@ -197,50 +197,129 @@ def solve_pose_targets( except (KeyError, ValueError) as exc: return _failure(IKStatus.NO_SOLUTION, str(exc)) - if len(pose_groups) != 1: - return _failure( - IKStatus.NO_SOLUTION, - "PinkIK supports exactly one pose target per request", - ) - - target_group = pose_groups[0] - if not target_group.has_pose_target or target_group.tip_link is None: - return _failure( - IKStatus.NO_SOLUTION, - f"Planning group '{target_group.id}' has no pose target frame", - ) - - robot_ids = {robot_ids_by_name[group.robot_name] for group in selection.groups} - if len(robot_ids) != 1: - return _failure(IKStatus.NO_SOLUTION, "PinkIK does not support cross-robot pose IK") + groups_by_robot: dict[RobotName, list[PlanningGroup]] = {} + pose_groups_by_robot: dict[RobotName, list[PlanningGroup]] = {} + for group in selection.groups: + groups_by_robot.setdefault(group.robot_name, []).append(group) + for group in pose_groups: + if not group.has_pose_target or group.tip_link is None: + return _failure( + IKStatus.NO_SOLUTION, + f"Planning group '{group.id}' has no pose target frame", + ) + pose_groups_by_robot.setdefault(group.robot_name, []).append(group) + + selected_positions_by_name: dict[str, float] = {} + max_position_error = 0.0 + max_orientation_error = 0.0 + total_iterations = 0 + for robot_name, groups in groups_by_robot.items(): + robot_id = robot_ids_by_name[robot_name] + robot_pose_groups = pose_groups_by_robot.get(robot_name, []) + if robot_pose_groups: + result = self._solve_pose_targets_for_robot( + world=world, + robot_id=robot_id, + pose_targets={group: pose_targets[group] for group in robot_pose_groups}, + seed=_seed_for_robot(world, robot_id, seed), + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + check_collision=check_collision, + max_attempts=max_attempts, + ) + if not result.is_success() or result.joint_state is None: + return result + else: + result = IKResult( + status=IKStatus.SUCCESS, + joint_state=_seed_for_robot(world, robot_id, seed), + message="Auxiliary group retained seed state", + ) + joint_state = result.joint_state + if joint_state is None: + return _failure( + IKStatus.NO_SOLUTION, + f"Pink IK result for robot '{robot_name}' has no joint state", + ) - robot_id = robot_ids_by_name[target_group.robot_name] - if seed is None: - with world.scratch_context() as ctx: - seed = world.get_joint_state(ctx, robot_id) + max_position_error = max(max_position_error, result.position_error) + max_orientation_error = max(max_orientation_error, result.orientation_error) + total_iterations += result.iterations + local_positions = dict(zip(joint_state.name, joint_state.position, strict=True)) + for group in groups: + for global_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=True + ): + if local_name not in local_positions: + return _failure( + IKStatus.NO_SOLUTION, + f"Pink IK result for robot '{robot_name}' is missing joint '{local_name}'", + ) + selected_positions_by_name[global_name] = float(local_positions[local_name]) + + selected_positions = [selected_positions_by_name[name] for name in selection.joint_names] + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + {"name": list(selection.joint_names), "position": selected_positions} + ), + position_error=max_position_error, + orientation_error=max_orientation_error, + iterations=total_iterations, + message="Pink IK target set solution found", + ) + def _solve_pose_targets_for_robot( + self, + world: WorldSpec, + robot_id: WorldRobotID, + pose_targets: Mapping[PlanningGroup, PoseStamped], + seed: JointState, + position_tolerance: float, + orientation_tolerance: float, + check_collision: bool, + max_attempts: int, + ) -> IKResult: + """Solve one robot's one-or-more frame targets.""" try: - robot_context = self._get_robot_context(world, robot_id, target_group.tip_link) + contexts = [ + self._get_robot_context(world, robot_id, group.tip_link) + for group in pose_targets + if group.tip_link is not None + ] except (FileNotFoundError, ImportError, ValueError) as exc: return _failure(IKStatus.NO_SOLUTION, f"Pink IK model setup failed: {exc}") + config = world.get_robot_config(robot_id) lower_limits, upper_limits = world.get_joint_limits(robot_id) - target_pose = pose_targets[next(iter(pose_targets.keys()))] - target_model = self._target_in_model_frame(world.get_robot_config(robot_id), target_pose) + target_models = [ + self._target_in_model_frame(config, pose) for pose in pose_targets.values() + ] fallback_result: IKResult | None = None for attempt in range(max_attempts): try: - q0 = self._initial_q(robot_context, seed, lower_limits, upper_limits, attempt) - result = self._solve_single( - robot_context=robot_context, - target_model=target_model, - seed_q=q0, - lower_limits=lower_limits, - upper_limits=upper_limits, - position_tolerance=position_tolerance, - orientation_tolerance=orientation_tolerance, - ) + q0 = self._initial_q(contexts[0], seed, lower_limits, upper_limits, attempt) + if len(contexts) == 1: + result = self._solve_single( + robot_context=contexts[0], + target_model=target_models[0], + seed_q=q0, + lower_limits=lower_limits, + upper_limits=upper_limits, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + ) + else: + result = self._solve_multi_frame( + robot_contexts=contexts, + target_models=target_models, + seed_q=q0, + lower_limits=lower_limits, + upper_limits=upper_limits, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + ) except ValueError as exc: return _failure(IKStatus.NO_SOLUTION, f"Pink IK mapping failed: {exc}") except Exception as exc: @@ -250,26 +329,11 @@ def solve_pose_targets( if fallback_result is None: fallback_result = result continue - if check_collision and not world.check_config_collision_free( robot_id, result.joint_state ): fallback_result = _collision_failure(result) continue - - selected_joint_names: list[str] = [] - selected_local_names: list[str] = [] - for group in selection.groups: - selected_joint_names.extend(group.joint_names) - selected_local_names.extend(group.local_joint_names) - try: - result.joint_state = filter_joint_state_to_selected_joints( - result.joint_state, - selected_joint_names, - selected_local_names, - ) - except ValueError as exc: - return _failure(IKStatus.NO_SOLUTION, str(exc)) return result if fallback_result is not None: @@ -357,6 +421,95 @@ def _solve_single( message="Pink IK did not converge within the iteration budget", ) + def _solve_multi_frame( + self, + robot_contexts: Sequence[_PinkRobotContext], + target_models: Sequence[NDArray[np.float64]], + seed_q: NDArray[np.float64], + lower_limits: NDArray[np.float64], + upper_limits: NDArray[np.float64], + position_tolerance: float, + orientation_tolerance: float, + ) -> IKResult: + """Solve multiple frame tasks for one robot model.""" + pink = self._modules.pink + pinocchio = self._modules.pinocchio + primary_context = robot_contexts[0] + configuration = pink.Configuration( + primary_context.model, primary_context.data, seed_q.copy() + ) + + tasks: list[Any] = [] + for context, target_model in zip(robot_contexts, target_models, strict=True): + frame_task = pink.tasks.FrameTask( + context.frame_name, + position_cost=self.config.position_cost, + orientation_cost=self.config.orientation_cost, + lm_damping=self.config.lm_damping, + gain=self.config.gain, + ) + frame_task.set_target(_matrix_to_se3(pinocchio, target_model)) + tasks.append(frame_task) + + if self.config.posture_cost > 0.0: + posture_task = pink.tasks.PostureTask(cost=self.config.posture_cost) + posture_task.set_target_from_configuration(configuration) + tasks.append(posture_task) + + final_position_error = float("inf") + final_orientation_error = float("inf") + for iteration in range(self.config.max_iterations): + position_errors: list[float] = [] + orientation_errors: list[float] = [] + for context, target_model in zip(robot_contexts, target_models, strict=True): + current_pose = self._current_frame_matrix(context, configuration.q) + position_error, orientation_error = compute_pose_error(current_pose, target_model) + position_errors.append(position_error) + orientation_errors.append(orientation_error) + final_position_error = max(position_errors) + final_orientation_error = max(orientation_errors) + if ( + final_position_error <= position_tolerance + and final_orientation_error <= orientation_tolerance + ): + return _success( + primary_context.mapping.dimos_joint_names, + self._q_to_dimos_positions(primary_context, configuration.q), + final_position_error, + final_orientation_error, + iteration + 1, + ) + + velocity = pink.solve_ik( + configuration, + tasks, + self.config.dt, + solver=self.config.solver, + damping=self.config.damping, + safety_break=self.config.safety_break, + ) + configuration.integrate_inplace(velocity, self.config.dt) + + joint_positions = self._q_to_dimos_positions(primary_context, configuration.q) + if not _within_limits(joint_positions, lower_limits, upper_limits): + return IKResult( + status=IKStatus.JOINT_LIMITS, + joint_state=None, + position_error=final_position_error, + orientation_error=final_orientation_error, + iterations=iteration + 1, + message="Pink IK candidate violates DimOS joint limits", + ) + + return IKResult( + status=IKStatus.NO_SOLUTION, + joint_state=None, + position_error=final_position_error, + orientation_error=final_orientation_error, + iterations=self.config.max_iterations, + message="Pink IK did not converge within the iteration budget", + ) + def _get_robot_context( self, world: WorldSpec, robot_id: WorldRobotID, frame_name: str | None = None ) -> _PinkRobotContext: @@ -596,7 +749,7 @@ def _success( ) -> IKResult: return IKResult( status=IKStatus.SUCCESS, - joint_state=JointState(name=joint_names, position=joint_positions.tolist()), + joint_state=JointState({"name": joint_names, "position": joint_positions.tolist()}), position_error=position_error, orientation_error=orientation_error, iterations=iterations, @@ -619,6 +772,40 @@ def _collision_failure(result: IKResult) -> IKResult: ) +def _seed_for_robot( + world: WorldSpec, robot_id: WorldRobotID, seed: JointState | None +) -> JointState: + """Return a full local seed state for one robot from local/global seed input.""" + config = world.get_robot_config(robot_id) + with world.scratch_context() as ctx: + current = world.get_joint_state(ctx, robot_id) + if seed is None: + return JointState(current) + if not seed.name: + if len(seed.position) == len(config.joint_names): + return JointState({"name": list(config.joint_names), "position": list(seed.position)}) + raise ValueError( + f"Seed has {len(seed.position)} positions for robot '{config.name}', " + f"expected {len(config.joint_names)}" + ) + if len(seed.name) != len(seed.position): + raise ValueError(f"Seed has {len(seed.name)} names but {len(seed.position)} positions") + seed_by_name = dict(zip(seed.name, seed.position, strict=True)) + current_by_name = dict(zip(current.name, current.position, strict=True)) + positions: list[float] = [] + for local_name in config.joint_names: + global_name = make_global_joint_name(config.name, local_name) + if local_name in seed_by_name: + positions.append(float(seed_by_name[local_name])) + elif global_name in seed_by_name: + positions.append(float(seed_by_name[global_name])) + elif local_name in current_by_name: + positions.append(float(current_by_name[local_name])) + else: + raise ValueError(f"Seed/current state is missing joint '{local_name}'") + return JointState({"name": list(config.joint_names), "position": positions}) + + def _robot_ids_by_name( world: WorldSpec, robot_names: tuple[RobotName, ...] ) -> dict[RobotName, WorldRobotID]: diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index 7bde5774c0..bfb0af604b 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -245,6 +245,47 @@ def check_config_collision_free(self, robot_id: str, joint_state: JointState) -> return self.collision_free +class _FakeMultiRobotWorld: + is_finalized = True + + def __init__(self) -> None: + self.configs = { + "left_robot": RobotModelConfig( + name="left", + model_path=Path("/tmp/left.urdf"), + base_pose=PoseStamped( + position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), + joint_names=["joint_a", "joint_b"], + end_effector_link="tool", + base_link="base", + ), + "right_robot": RobotModelConfig( + name="right", + model_path=Path("/tmp/right.urdf"), + base_pose=PoseStamped( + position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), + joint_names=["joint_c"], + end_effector_link="tool", + base_link="base", + ), + } + + def get_robot_ids(self) -> list[str]: + return list(self.configs) + + def get_robot_config(self, robot_id: str) -> RobotModelConfig: + return self.configs[robot_id] + + def scratch_context(self) -> nullcontext[None]: + return nullcontext(None) + + def get_joint_state(self, ctx: object, robot_id: str) -> JointState: + config = self.get_robot_config(robot_id) + return JointState(name=list(config.joint_names), position=[0.0] * len(config.joint_names)) + + def test_create_kinematics_pink_missing_dependency_is_actionable( monkeypatch: pytest.MonkeyPatch, ) -> None: @@ -443,6 +484,135 @@ def fake_solve_single(**kwargs: object) -> IKResult: assert result.joint_state.position == [0.1, 0.2, 0.3] +def test_solve_pose_targets_same_robot_uses_one_multi_frame_solve( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + wrist_context = _context() + wrist_context.frame_name = "wrist_tool" + wrist_context.frame_id = 1 + tool_context = _context() + ik._robot_contexts = {"robot:wrist_tool": wrist_context, "robot:tool": tool_context} + world = _FakeWorld(collision_free=True) + tool_group = PlanningGroup( + id="arm/tool", + robot_name="arm", + group_name="tool", + joint_names=("arm/joint_c",), + local_joint_names=("joint_c",), + base_link="base", + tip_link="tool", + ) + seen_frames: list[list[str]] = [] + + def fake_solve_multi_frame(**kwargs: object) -> IKResult: + contexts = cast("list[_PinkRobotContext]", kwargs["robot_contexts"]) + seen_frames.append([context.frame_name for context in contexts]) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + ), + position_error=0.0, + orientation_error=0.0, + iterations=2, + ) + + monkeypatch.setattr(ik, "_solve_multi_frame", fake_solve_multi_frame) + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={ + world.groups["arm/wrist"]: PoseStamped( + position=Vector3(0.1, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + tool_group: PoseStamped( + position=Vector3(0.2, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + }, + seed=JointState( + {"name": ["arm/joint_a", "arm/joint_b", "arm/joint_c"], "position": [0.0, 0.0, 0.0]} + ), + check_collision=False, + max_attempts=1, + ) + + assert seen_frames == [["wrist_tool", "tool"]] + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["arm/joint_a", "arm/joint_b", "arm/joint_c"] + assert result.joint_state.position == [0.1, 0.2, 0.3] + + +def test_solve_pose_targets_cross_robot_combines_global_joint_names( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + world = _FakeMultiRobotWorld() + left_group = PlanningGroup( + id="left/arm", + robot_name="left", + group_name="arm", + joint_names=("left/joint_a",), + local_joint_names=("joint_a",), + base_link="base", + tip_link="tool", + ) + right_group = PlanningGroup( + id="right/arm", + robot_name="right", + group_name="arm", + joint_names=("right/joint_c",), + local_joint_names=("joint_c",), + base_link="base", + tip_link="tool", + ) + seen_robot_ids: list[str] = [] + + def fake_solve_pose_targets_for_robot(**kwargs: object) -> IKResult: + robot_id = str(kwargs["robot_id"]) + seen_robot_ids.append(robot_id) + if robot_id == "left_robot": + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["joint_a", "joint_b"], position=[1.0, 9.0]), + ) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["joint_c"], position=[2.0]), + ) + + monkeypatch.setattr(ik, "_solve_pose_targets_for_robot", fake_solve_pose_targets_for_robot) + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={ + left_group: PoseStamped( + position=Vector3(0.1, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + right_group: PoseStamped( + position=Vector3(0.2, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + }, + seed=JointState( + name=["left/joint_a", "left/joint_b", "right/joint_c"], + position=[0.0, 0.0, 0.0], + ), + check_collision=False, + max_attempts=1, + ) + + assert seen_robot_ids == ["left_robot", "right_robot"] + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["left/joint_a", "right/joint_c"] + assert result.joint_state.position == [1.0, 2.0] + + def test_solve_retries_after_joint_limit_failure(monkeypatch: pytest.MonkeyPatch) -> None: ik = _pink_ik(converge=True) context = _context() diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index d4141d03b3..5aef347767 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -868,6 +868,103 @@ def test_preview_plan_returns_false_for_missing_inputs(self): assert module.preview_plan() is False +class TestTargetSetEvaluation: + def test_evaluate_joint_target_set_projects_selected_targets_to_full_robot_states(self): + left_config = _make_robot_config("left", ["j1", "j2"], "left_task") + right_config = _make_robot_config("right", ["j1", "j2"], "right_task") + module = _make_module_with_monitor(left_config, right_config) + module._world_monitor.planning_groups = _FakePlanningGroups( + [ + _make_global_group("left", "arm", ["j1"]), + _make_global_group("right", "arm", ["j2"]), + ] + ) + module._world_monitor.get_current_joint_state.side_effect = [ + JointState(name=["j1", "j2"], position=[0.0, 9.0]), + JointState(name=["j1", "j2"], position=[8.0, 0.0]), + ] + module._world_monitor.is_state_valid.return_value = True + left_pose = PoseStamped(position=Vector3(x=1.0), orientation=Quaternion()) + right_pose = PoseStamped(position=Vector3(x=2.0), orientation=Quaternion()) + module._world_monitor.get_group_pose.side_effect = [left_pose, right_pose] + + result = module.evaluate_joint_target_set( + { + "left/arm": JointState(name=["j1"], position=[1.0]), + "right/arm": JointState(name=["j2"], position=[2.0]), + } + ) + + assert result["success"] is True + assert result["collision_free"] is True + assert result["target_joints"] is not None + assert result["target_joints"].name == ["left/j1", "right/j2"] + assert result["target_joints"].position == [1.0, 2.0] + left_target = module._world_monitor.is_state_valid.call_args_list[0].args[1] + right_target = module._world_monitor.is_state_valid.call_args_list[1].args[1] + assert left_target.name == ["j1", "j2"] + assert left_target.position == [1.0, 9.0] + assert right_target.name == ["j1", "j2"] + assert right_target.position == [8.0, 2.0] + assert result["group_poses"] == {"left/arm": left_pose, "right/arm": right_pose} + + def test_evaluate_joint_target_set_reports_collision_per_group(self): + config = _make_robot_config("left", ["j1"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1"], position=[0.0] + ) + module._world_monitor.is_state_valid.return_value = False + module._world_monitor.get_group_pose.return_value = None + + result = module.evaluate_joint_target_set( + {"left/manipulator": JointState(name=["j1"], position=[1.0])} + ) + + assert result["success"] is False + assert result["status"] == "COLLISION" + assert result["collision_free"] is False + assert result["message"] == "Target set is in collision" + assert result["group_diagnostics"] == {"left/manipulator": "Target is in collision"} + + def test_evaluate_pose_target_set_uses_whole_set_seed_and_auxiliary_groups(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + arm_group = _make_global_group("left", "arm", ["j1"]) + wrist_group = _make_global_group("left", "wrist", ["j2"]) + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups([arm_group, wrist_group]) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2"], position=[0.1, 0.2] + ) + module._world_monitor.is_state_valid.return_value = True + module._world_monitor.get_group_pose.return_value = PoseStamped( + position=Vector3(x=1.0), orientation=Quaternion() + ) + module._kinematics = MagicMock() + module._kinematics.solve_pose_targets.return_value = IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["left/j1", "left/j2"], position=[1.0, 2.0]), + message="solved", + position_error=0.01, + orientation_error=0.02, + ) + pose = Pose(position=Vector3(x=0.5), orientation=Quaternion()) + + result = module.evaluate_pose_target_set( + {"left/arm": pose}, auxiliary_groups=("left/wrist",) + ) + + assert result["success"] is True + assert result["target_joints"] is not None + assert result["target_joints"].name == ["left/j1", "left/j2"] + assert result["target_joints"].position == [1.0, 2.0] + _, kwargs = module._kinematics.solve_pose_targets.call_args + assert list(kwargs["pose_targets"]) == [arm_group] + assert kwargs["auxiliary_groups"] == (wrist_group,) + assert kwargs["seed"].name == ["left/j1", "left/j2"] + assert kwargs["seed"].position == [0.1, 0.2] + + class TestGeneratedPlanProjection: def test_selected_joint_state_accepts_local_current_state_names(self): config = _make_robot_config("left", ["j1", "j2"], "task") diff --git a/dimos/manipulation/visualization/types.py b/dimos/manipulation/visualization/types.py index 778c293499..a4c22edcd7 100644 --- a/dimos/manipulation/visualization/types.py +++ b/dimos/manipulation/visualization/types.py @@ -16,7 +16,7 @@ from typing import TypedDict -from dimos.manipulation.planning.spec.models import RobotName, WorldRobotID +from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName, WorldRobotID from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -33,7 +33,20 @@ class TargetEvaluation(TypedDict, total=False): orientation_error: float -class RobotInfo(TypedDict): +class TargetSetEvaluation(TypedDict, total=False): + success: bool + status: str + message: str + collision_free: bool + group_ids: tuple[PlanningGroupID, ...] + target_joints: JointState | None + group_diagnostics: dict[PlanningGroupID, str] + group_poses: dict[PlanningGroupID, PoseStamped | Pose | None] + position_error: float + orientation_error: float + + +class RobotInfo(TypedDict, total=False): name: RobotName world_robot_id: WorldRobotID joint_names: list[str] @@ -46,3 +59,16 @@ class RobotInfo(TypedDict): home_joints: list[float] | None pre_grasp_offset: float init_joints: list[float] | None + planning_groups: list[PlanningGroupInfo] + + +class PlanningGroupInfo(TypedDict): + id: PlanningGroupID + name: str + robot_name: RobotName + joint_names: list[str] + local_joint_names: list[str] + base_link: str + tip_link: str | None + has_pose_target: bool + source: str diff --git a/dimos/manipulation/visualization/viser/adapter.py b/dimos/manipulation/visualization/viser/adapter.py index 47a1bc616c..3824df6e31 100644 --- a/dimos/manipulation/visualization/viser/adapter.py +++ b/dimos/manipulation/visualization/viser/adapter.py @@ -15,16 +15,22 @@ from __future__ import annotations from collections.abc import Sequence -from typing import TYPE_CHECKING - -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from typing import TYPE_CHECKING, cast + +from dimos.manipulation.visualization.types import ( + PlanningGroupInfo, + RobotInfo, + TargetEvaluation, + TargetSetEvaluation, +) from dimos.msgs.sensor_msgs.JointState import JointState if TYPE_CHECKING: from dimos.manipulation.manipulation_module import ManipulationModule + from dimos.manipulation.planning.groups import PlanningGroup from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig - from dimos.manipulation.planning.spec.models import RobotName, WorldRobotID + from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName, WorldRobotID from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -84,8 +90,31 @@ def get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: "init_joints": None if info["init_joints"] is None else [float(value) for value in info["init_joints"]], + "planning_groups": [ + { + "id": str(group["id"]), + "name": str(group["name"]), + "robot_name": str(info["name"]), + "joint_names": [str(name) for name in group["joint_names"]], + "local_joint_names": [str(name) for name in group["local_joint_names"]], + "base_link": str(group["base_link"]), + "tip_link": None if group["tip_link"] is None else str(group["tip_link"]), + "has_pose_target": bool(group["has_pose_target"]), + "source": str(group["source"]), + } + for group in info.get("planning_groups", []) + ], } + def list_planning_groups(self) -> list[PlanningGroupInfo]: + groups: list[PlanningGroupInfo] = [] + for robot_name in self.list_robots(): + info = self.get_robot_info(robot_name) + if info is None: + continue + groups.extend(info.get("planning_groups", [])) + return groups + def get_init_joints(self, robot_name: RobotName) -> JointState | None: return copy_joint_state(self._module.get_init_joints(robot_name)) @@ -127,6 +156,45 @@ def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvalu ) return result + def evaluate_joint_target_set( + self, joint_targets: dict[PlanningGroupID, JointState] + ) -> TargetSetEvaluation: + result: TargetSetEvaluation = { + **self._module.evaluate_joint_target_set( + cast( + "dict[PlanningGroupID | PlanningGroup, JointState]", + { + group_id: copy_joint_state(target) or target + for group_id, target in joint_targets.items() + }, + ) + ) + } + target_joints = result.get("target_joints") + result["target_joints"] = copy_joint_state( + target_joints if isinstance(target_joints, JointState) else None + ) + return result + + def evaluate_pose_target_set( + self, + pose_targets: dict[PlanningGroupID, Pose | PoseStamped], + auxiliary_groups: Sequence[PlanningGroupID] = (), + seed: JointState | None = None, + ) -> TargetSetEvaluation: + result: TargetSetEvaluation = { + **self._module.evaluate_pose_target_set( + cast("dict[PlanningGroupID | PlanningGroup, Pose | PoseStamped]", pose_targets), + auxiliary_groups=auxiliary_groups, + seed=copy_joint_state(seed), + ) + } + target_joints = result.get("target_joints") + result["target_joints"] = copy_joint_state( + target_joints if isinstance(target_joints, JointState) else None + ) + return result + def get_module_state(self) -> str: return str(self._module.get_state()) @@ -142,12 +210,23 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: return self._module.plan_to_joints(joints, robot_name) + def plan_target_set(self, joint_targets: dict[PlanningGroupID, JointState]) -> bool: + return self._module.plan_to_joint_targets( + cast("dict[PlanningGroupID | PlanningGroup, JointState]", joint_targets) + ) + def preview_plan(self, robot_name: RobotName | None = None) -> bool: return self._module.preview_plan(robot_name=robot_name) + def preview_target_set_plan(self) -> bool: + return self._module.preview_plan() + def execute(self, robot_name: RobotName | None = None) -> bool: return self._module.execute(robot_name) + def execute_target_set_plan(self) -> bool: + return self._module.execute() + def cancel(self) -> bool: return self._module.cancel() diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 44cb9f726f..f227536fa5 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -16,7 +16,13 @@ from typing import TypeAlias -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.visualization.types import ( + PlanningGroupInfo, + RobotInfo, + TargetEvaluation, + TargetSetEvaluation, +) from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.runtime import VISER_INSTALL_HINT @@ -88,6 +94,7 @@ def __init__( self._closed = False self._operation_sequence_id = 0 self._suppress_target_callbacks = False + self._default_group_initialized = False self._handles: dict[str, PanelHandle] = {} self._joint_sliders: dict[str, GuiSliderHandle[float]] = {} self._worker = TargetEvaluationWorker( @@ -128,14 +135,19 @@ def refresh(self) -> None: if self._closed: return robots = self.adapter.list_robots() + groups = self.adapter.list_planning_groups() self.state.backend_status = ( BackendConnectionStatus.READY if robots else BackendConnectionStatus.WAITING_FOR_ROBOT ) - if self.state.selected_robot is None and robots: - self.state.selected_robot = robots[0] + if not self.state.selected_group_ids and groups and not self._default_group_initialized: + self.state.selected_group_ids = (str(groups[0]["id"]),) + self.state.selected_robot = str(groups[0]["robot_name"]) self.state.target_status = TargetStatus.EMPTY + self._default_group_initialized = True + self._initialize_selected_group_targets() self._build_joint_sliders() self._sync_robot_dropdown(robots) + self._sync_group_checkboxes(groups) self._refresh_selected_robot_state() self._ensure_scene_controls() self._sync_preset_dropdown() @@ -160,6 +172,9 @@ def _build_panel_controls(self, gui: GuiApi) -> None: ) robot_dropdown.on_update(lambda event: self._select_robot(event.target.value)) self._handles["robot"] = robot_dropdown + select_all_button = gui.add_button("Select all manipulators") + select_all_button.on_click(lambda _: self._select_all_manipulators()) + self._handles["select_all_manipulators"] = select_all_button preset_dropdown = gui.add_dropdown( "Target Preset", options=["Select preset...", "Current"], @@ -218,56 +233,131 @@ def _refresh_selected_robot_state(self) -> None: self.state.error = adapter_error def _ensure_scene_controls(self) -> None: - if self.scene is None or self.state.selected_robot is None: - return - robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) - if robot_id is None: + if self.scene is None: return - ee_control = self.scene.ensure_target_controls(str(robot_id), self._on_transform_update) - if ee_control is not None: - self._handles["ee_control"] = ee_control - if ( - self.state.target_status == TargetStatus.EMPTY - and self.state.current_ee_pose is not None - ): - self.state.cartesian_target = self.state.current_ee_pose - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(str(robot_id), self.state.current_ee_pose) - finally: - self._suppress_target_callbacks = False + groups = self._group_info_by_id() + selected = set(self.state.selected_group_ids) + active_pose_groups = { + group_id + for group_id in selected + if (group := groups.get(group_id)) is not None + and bool(group["has_pose_target"]) + and group_id not in self.state.auxiliary_group_ids + } + for key in [key for key in self._handles if key.startswith("ee_control:")]: + group_id = key.split(":", 1)[1] + if group_id in active_pose_groups: + continue + handle = self._handles.pop(key) + remove_target_controls = getattr(self.scene, "remove_target_controls", None) + if callable(remove_target_controls): + remove_target_controls(group_id) + else: + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + for group_id in selected: + group = groups.get(group_id) + if group is None or not bool(group["has_pose_target"]): + continue + if group_id in self.state.auxiliary_group_ids: + continue + ee_control = self.scene.ensure_target_controls( + group_id, + lambda target, gid=group_id: self._on_transform_update(gid, target), + ) + if ee_control is not None: + self._handles[f"ee_control:{group_id}"] = ee_control + pose = self.state.group_poses.get(group_id) or self.state.pose_targets.get(group_id) + if pose is not None: + self._suppress_target_callbacks = True + try: + self.scene.set_target_pose(group_id, pose) + finally: + self._suppress_target_callbacks = False def _build_joint_sliders(self) -> None: - if self.state.selected_robot is None: + if not self.state.selected_group_ids: return gui = self.server.gui - config = self.adapter.get_robot_config(self.state.selected_robot) - if config is None: - return - current = self.adapter.get_current_joint_state(self.state.selected_robot) - values = list(current.position) if current is not None else [0.0] * len(config.joint_names) self._clear_joint_sliders() - joint_limits_lower = config.joint_limits_lower - joint_limits_upper = config.joint_limits_upper - for index, joint_name in enumerate(config.joint_names): - lower, upper = DEFAULT_JOINT_LIMITS - if joint_limits_lower is not None and index < len(joint_limits_lower): - lower = joint_limits_lower[index] - if joint_limits_upper is not None and index < len(joint_limits_upper): - upper = joint_limits_upper[index] - handle = gui.add_slider( - joint_name, - min=float(lower), - max=float(upper), - step=0.001, - initial_value=float(values[index] if index < len(values) else 0.0), + groups = self._group_info_by_id() + target_by_name: dict[str, float] = {} + if self.state.target_joints is not None: + target_by_name.update( + zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) ) + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + config = self.adapter.get_robot_config(str(group["robot_name"])) + current = self.adapter.get_current_joint_state(str(group["robot_name"])) + current_by_name = ( + dict(zip(current.name, current.position, strict=False)) + if current is not None + else {} + ) + joint_limits_lower = config.joint_limits_lower if config is not None else None + joint_limits_upper = config.joint_limits_upper if config is not None else None + for index, (global_name, local_name) in enumerate( + zip(group["joint_names"], group["local_joint_names"], strict=False) + ): + joint_name = str(global_name) + local = str(local_name) + value = float(target_by_name.get(joint_name, current_by_name.get(local, 0.0))) + lower, upper = DEFAULT_JOINT_LIMITS + if joint_limits_lower is not None and index < len(joint_limits_lower): + lower = joint_limits_lower[index] + if joint_limits_upper is not None and index < len(joint_limits_upper): + upper = joint_limits_upper[index] + handle = gui.add_slider( + f"{group_id}/{local}", + min=float(lower), + max=float(upper), + step=0.001, + initial_value=value, + ) - def on_update(_event: object, name: str = joint_name) -> None: - self._on_joint_slider_update(name) - - handle.on_update(on_update) - self._joint_sliders[joint_name] = handle + def on_update(_event: object, name: str = joint_name) -> None: + self._on_joint_slider_update(name) + + handle.on_update(on_update) + self._joint_sliders[joint_name] = handle + + def _target_set_from_sliders(self) -> dict[PlanningGroupID, JointState] | None: + groups = self._group_info_by_id() + targets: dict[PlanningGroupID, JointState] = {} + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + self._set_error(f"Unknown planning group: {group_id}") + return None + names = [str(name) for name in group["joint_names"]] + positions: list[float] = [] + for name in names: + handle = self._joint_sliders.get(name) + if handle is None: + self._set_error(f"Missing target slider for {name}") + return None + positions.append(float(handle.value)) + targets[group_id] = JointState({"name": names, "position": positions}) + return targets + + def _split_target_joints_by_group(self, target_joints: JointState) -> None: + groups = self._group_info_by_id() + positions_by_name = dict(zip(target_joints.name, target_joints.position, strict=False)) + self.state.group_joint_targets.clear() + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + names = [str(name) for name in group["joint_names"]] + if not all(name in positions_by_name for name in names): + continue + self.state.group_joint_targets[group_id] = JointState( + {"name": names, "position": [float(positions_by_name[name]) for name in names]} + ) def _clear_joint_sliders(self) -> None: for handle in self._joint_sliders.values(): @@ -291,9 +381,19 @@ def _select_robot(self, robot_name: str) -> None: self.refresh() return self.state.selected_robot = robot_name or None + groups = [ + group + for group in self.adapter.list_planning_groups() + if str(group["robot_name"]) == self.state.selected_robot + ] + self.state.selected_group_ids = (str(groups[0]["id"]),) if groups else () + self.state.auxiliary_group_ids = () + self.state.group_joint_targets.clear() + self.state.pose_targets.clear() self.state.target_status = TargetStatus.EMPTY self.state.feasibility.status = FeasibilityStatus.UNKNOWN self.state.plan_state = PanelPlanState() + self._initialize_selected_group_targets() self._build_joint_sliders() self._sync_preset_dropdown() self.refresh() @@ -315,6 +415,130 @@ def _sync_robot_dropdown(self, robots: list[str]) -> None: except Exception: logger.warning("Could not set robot dropdown value", exc_info=True) + def _sync_group_checkboxes(self, groups: list[PlanningGroupInfo]) -> None: + seen_keys: set[str] = set() + selected = set(self.state.selected_group_ids) + for group in groups: + group_id = str(group["id"]) + key = f"group:{group_id}" + seen_keys.add(key) + handle = self._handles.get(key) + if handle is None: + handle = self.server.gui.add_checkbox( + f"Group {group_id}", initial_value=group_id in selected + ) + handle.on_update( + lambda event, gid=group_id: self._set_group_selected( + gid, bool(event.target.value) + ) + ) + self._handles[key] = handle + elif hasattr(handle, "value"): + self._set_optional_handle_attr(handle, "value", group_id in selected) + + for key in [key for key in self._handles if key.startswith("group:")]: + if key not in seen_keys: + handle = self._handles.pop(key) + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + + def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None: + current = list(self.state.selected_group_ids) + if selected and group_id not in current: + current.append(group_id) + elif not selected and group_id in current: + current.remove(group_id) + self.state.selected_group_ids = tuple(current) + self.state.auxiliary_group_ids = tuple( + group_id for group_id in self.state.auxiliary_group_ids if group_id in current + ) + self._sync_selected_robot_from_groups() + self._initialize_selected_group_targets() + self.state.mark_plan_stale() + self._build_joint_sliders() + self.refresh() + + def _select_all_manipulators(self) -> None: + groups = self.adapter.list_planning_groups() + manipulator_groups = [ + str(group["id"]) for group in groups if str(group["name"]) == "manipulator" + ] + self.state.selected_group_ids = tuple( + manipulator_groups or [str(group["id"]) for group in groups] + ) + self._sync_selected_robot_from_groups() + self._initialize_selected_group_targets() + self._build_joint_sliders() + self.refresh() + + def _group_info_by_id(self) -> dict[PlanningGroupID, PlanningGroupInfo]: + return {str(group["id"]): group for group in self.adapter.list_planning_groups()} + + def _sync_selected_robot_from_groups(self) -> None: + groups = self._group_info_by_id() + first_group = ( + groups.get(self.state.selected_group_ids[0]) if self.state.selected_group_ids else None + ) + self.state.selected_robot = None if first_group is None else str(first_group["robot_name"]) + + def _initialize_selected_group_targets(self) -> None: + groups = self._group_info_by_id() + for group_id in self.state.selected_group_ids: + if group_id in self.state.group_joint_targets: + continue + group = groups.get(group_id) + if group is None: + continue + current = self.adapter.get_current_joint_state(str(group["robot_name"])) + if current is None: + continue + current_by_name = dict(zip(current.name, current.position, strict=False)) + names = [str(name) for name in group["joint_names"]] + local_names = [str(name) for name in group["local_joint_names"]] + positions = [float(current_by_name.get(local, 0.0)) for local in local_names] + self.state.group_joint_targets[group_id] = JointState( + {"name": names, "position": positions} + ) + if bool(group["has_pose_target"]) and group_id not in self.state.pose_targets: + pose = self.adapter.get_ee_pose(str(group["robot_name"])) + if pose is not None: + self.state.pose_targets[group_id] = pose + self.state.group_poses[group_id] = pose + if self.state.cartesian_target is None: + self.state.cartesian_target = pose + self._refresh_target_joints_from_groups() + + def _refresh_target_joints_from_groups(self) -> None: + names: list[str] = [] + positions: list[float] = [] + for group_id in self.state.selected_group_ids: + target = self.state.group_joint_targets.get(group_id) + if target is None: + continue + names.extend(target.name) + positions.extend(target.position) + self.state.target_joints = ( + JointState({"name": names, "position": positions}) if names else None + ) + + def _current_snapshot_by_group(self) -> dict[PlanningGroupID, list[float]]: + groups = self._group_info_by_id() + snapshot: dict[PlanningGroupID, list[float]] = {} + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + current = self.adapter.get_current_joint_state(str(group["robot_name"])) + if current is None: + continue + current_by_name = dict(zip(current.name, current.position, strict=False)) + snapshot[group_id] = [ + float(current_by_name.get(str(local_name), 0.0)) + for local_name in group["local_joint_names"] + ] + return snapshot + def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") if handle is None or self.state.selected_robot is None: @@ -348,16 +572,35 @@ def _apply_preset(self, preset: str) -> None: return if preset == "Current": current = self.adapter.get_current_joint_state(robot_name) - values = list(current.position) if current is not None else [] + values_by_name = ( + dict(zip(current.name, current.position, strict=False)) + if current is not None + else {} + ) elif preset == "Init": init = self.adapter.get_init_joints(robot_name) - values = list(init.position) if init is not None else [] + values_by_name = ( + dict(zip(init.name, init.position, strict=False)) if init is not None else {} + ) elif preset == "Home": - values = list(config.home_joints or []) + values_by_name = dict(zip(config.joint_names, config.home_joints or [], strict=False)) else: return - self._set_slider_values(config.joint_names, values) - self.state.joint_target = [float(value) for value in values] + groups = [ + group + for group in self.adapter.list_planning_groups() + if group["id"] in self.state.selected_group_ids + and str(group["robot_name"]) == robot_name + ] + for group in groups: + global_names = [str(name) for name in group["joint_names"]] + local_names = [str(name) for name in group["local_joint_names"]] + values = [ + float(values_by_name.get(local_name, values_by_name.get(global_name, 0.0))) + for local_name, global_name in zip(local_names, global_names, strict=False) + ] + self._set_slider_values(global_names, values) + self.state.joint_target = [float(handle.value) for handle in self._joint_sliders.values()] self._submit_joint_target_evaluation() self.refresh() @@ -390,73 +633,86 @@ def _on_joint_slider_update(self, _joint_name: str) -> None: return self._submit_joint_target_evaluation() - def _on_transform_update(self, target: TransformControlsHandle) -> None: + def _on_transform_update( + self, group_id: PlanningGroupID, target: TransformControlsHandle + ) -> None: if self._closed: return - if self._suppress_target_callbacks or self.state.selected_robot is None: + if self._suppress_target_callbacks or group_id not in self.state.selected_group_ids: return pose = self._pose_from_transform_target(target) if pose is None: return self.state.cartesian_target = pose + self.state.pose_targets[group_id] = pose sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="cartesian", - robot_name=self.state.selected_robot, - pose=pose, + group_ids=self.state.selected_group_ids, + auxiliary_group_ids=self.state.auxiliary_group_ids, + pose_targets=dict(self.state.pose_targets), ) ) self.refresh() def _submit_joint_target_evaluation(self) -> None: - robot_name = self.state.selected_robot - if robot_name is None: + targets = self._target_set_from_sliders() + if targets is None: return - target = self._target_from_sliders(robot_name) - if target is None: - return - self.state.joint_target = list(target.position) - self._move_joint_target_visuals(robot_name, target) + self.state.group_joint_targets = targets + self._refresh_target_joints_from_groups() + self._move_joint_target_visuals() sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - robot_name=robot_name, - joints=target, + group_ids=self.state.selected_group_ids, + joint_targets=dict(targets), ) ) self.refresh() - def _move_joint_target_visuals(self, robot_name: str, target: JointState) -> None: + def _move_joint_target_visuals(self) -> None: """Optimistically move target visuals before collision/feasibility returns.""" - config = self.adapter.get_robot_config(robot_name) - robot_id = self.adapter.robot_id_for_name(robot_name) - if self.scene is not None and config is not None and robot_id is not None: - self.scene.set_target_joints(str(robot_id), config.joint_names, list(target.position)) - pose = self.adapter.get_ee_pose(robot_name, target) - if pose is not None: - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(str(robot_id), pose) - finally: - self._suppress_target_callbacks = False + if self.scene is None: + return + groups = self._group_info_by_id() + for group_id, target in self.state.group_joint_targets.items(): + group = groups.get(group_id) + if group is None: + continue + robot_name = str(group["robot_name"]) + robot_id = self.adapter.robot_id_for_name(robot_name) + config = self.adapter.get_robot_config(robot_name) + if robot_id is None or config is None: + continue + local_positions = dict(zip(target.name, target.position, strict=False)) + joints = [ + float(local_positions.get(str(global_name), 0.0)) + for global_name in group["joint_names"] + ] + self.scene.set_target_joints(str(robot_id), group["local_joint_names"], joints) def _handle_target_evaluation_request( self, request: TargetEvaluationRequest - ) -> TargetEvaluation: + ) -> TargetEvaluation | TargetSetEvaluation: if request.source == "cartesian": - if request.pose is None: + if not request.pose_targets: return {"success": False, "status": "INVALID", "message": "No pose target"} - return self.adapter.evaluate_pose_target(request.pose, request.robot_name) - if request.joints is None: + return self.adapter.evaluate_pose_target_set( + request.pose_targets, + auxiliary_groups=request.auxiliary_group_ids, + seed=self.state.last_valid_target_joints, + ) + if not request.joint_targets: return {"success": False, "status": "INVALID", "message": "No joint target"} - return self.adapter.evaluate_joint_target(request.joints, request.robot_name) + return self.adapter.evaluate_joint_target_set(request.joint_targets) def _apply_target_evaluation_result( - self, request: TargetEvaluationRequest, result: TargetEvaluation + self, request: TargetEvaluationRequest, result: TargetEvaluation | TargetSetEvaluation ) -> None: if self._closed: return @@ -470,33 +726,35 @@ def _apply_target_evaluation_result( TargetStatus.FEASIBLE if success and collision_free else TargetStatus.INFEASIBLE ) self.state.error = "" if success and collision_free else self.state.feasibility.message - if request.source == "joints": - joint_state = result.get("joint_state") - if isinstance(joint_state, JointState): - self.state.joint_target = list(joint_state.position) - if request.source == "cartesian": - joint_state = result.get("joint_state") - if isinstance(joint_state, JointState): - self.state.joint_target = list(joint_state.position) - pose = result.get("ee_pose") - if isinstance(pose, Pose): - self.state.cartesian_target = pose + target_joints = result.get("target_joints") or result.get("joint_state") + if isinstance(target_joints, JointState) and success and collision_free: + self.state.target_joints = JointState(target_joints) + self.state.last_valid_target_joints = JointState(target_joints) + self._split_target_joints_by_group(target_joints) + group_poses = result.get("group_poses", {}) + if isinstance(group_poses, dict): + self.state.group_poses = { + str(group_id): pose + for group_id, pose in group_poses.items() + if isinstance(pose, Pose) + } + group_diagnostics = result.get("group_diagnostics", {}) + if isinstance(group_diagnostics, dict): + self.state.group_diagnostics = { + str(group_id): str(message) for group_id, message in group_diagnostics.items() + } + if request.source == "cartesian" and success and collision_free: self._sync_controls_from_targets() self._update_target_visual_state() self.refresh() def _sync_controls_from_targets(self) -> None: - robot_name = self.state.selected_robot - if robot_name is None: - return - config = self.adapter.get_robot_config(robot_name) - if config is not None and self.state.joint_target is not None: - self._set_slider_values(list(config.joint_names), list(self.state.joint_target)) - robot_id = self.adapter.robot_id_for_name(robot_name) - if self.scene is not None and robot_id is not None: - self.scene.set_target_joints( - str(robot_id), config.joint_names, self.state.joint_target - ) + if self.state.target_joints is not None: + positions_by_name = dict( + zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) + ) + self._set_slider_values(list(positions_by_name), list(positions_by_name.values())) + self._move_joint_target_visuals() # Do not write the Cartesian target back into the active transform # control here. The gizmo is the source of truth for Cartesian edits; # programmatic pose writes from delayed IK results can fight fast user @@ -540,20 +798,20 @@ def _update_control_state(self) -> None: self._update_target_visual_state() def _update_target_visual_state(self) -> None: - if self.scene is None or self.state.selected_robot is None: - return - robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) - if robot_id is None: + if self.scene is None: return - self.scene.set_target_visual_state( - str(robot_id), self.state.feasibility.status == FeasibilityStatus.FEASIBLE - ) + for group_id in self.state.selected_group_ids: + self.scene.set_target_visual_state( + group_id, self.state.feasibility.status == FeasibilityStatus.FEASIBLE + ) def _submit_plan(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: + if not self.state.selected_group_ids: + self._set_recoverable_error( + "Cannot plan until target is feasible and manipulation is idle" + ) return if not self.state.can_plan(): self._set_recoverable_error( @@ -571,22 +829,25 @@ def operation() -> None: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation("reset=False", clear_error=False, operation_id=operation_id) return - target = self._target_from_sliders(robot_name) - if target is None: + targets = self._target_set_from_sliders() + if targets is None: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation( "plan_to_joints=False", clear_error=False, operation_id=operation_id ) return - ok = self.adapter.plan_to_joints(target, robot_name) + ok = self.adapter.plan_target_set(targets) if not self._operation_is_current(operation_id): return if ok: self.state.plan_state.status = PlanStatus.FRESH - self.state.plan_state.robot = robot_name - self.state.plan_state.target_joints = list(target.position) + self.state.plan_state.group_ids = self.state.selected_group_ids + self.state.plan_state.robot = self.state.selected_robot + self.state.plan_state.target_joints = list( + self.state.target_joints.position if self.state.target_joints else [] + ) self.state.plan_state.target_pose = self.state.cartesian_target - self.state.plan_state.start_joints_snapshot = list(self.state.current_joints or []) + self.state.plan_state.start_joints_snapshot = self._current_snapshot_by_group() self.state.plan_state.planned_path = None else: self.state.plan_state.status = PlanStatus.FAILED @@ -599,9 +860,6 @@ def operation() -> None: def _submit_preview(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return if not self.state.can_preview(): self._set_recoverable_error("No fresh plan to preview") return @@ -611,7 +869,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.PREVIEWING - ok = self.adapter.preview_plan(robot_name) + ok = self.adapter.preview_target_set_plan() self._finish_operation(f"preview={ok}", operation_id=operation_id) self._operation_worker.submit( @@ -623,9 +881,6 @@ def operation() -> None: def _submit_execute(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return if not self.config.allow_plan_execute: self._set_recoverable_error( "Panel execution disabled; set allow_plan_execute=True to enable" @@ -643,7 +898,7 @@ def operation() -> None: return self.state.action_status = ActionStatus.EXECUTING self.state.plan_state.status = PlanStatus.EXECUTING - ok = self.adapter.execute(robot_name) + ok = self.adapter.execute_target_set_plan() if not self._operation_is_current(operation_id): return if not ok: @@ -762,7 +1017,10 @@ def _pose_from_transform_target(self, target: TransformControlsHandle) -> Pose | return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) def _feasibility_status( - self, result: TargetEvaluation, success: bool, collision_free: bool + self, + result: TargetEvaluation | TargetSetEvaluation, + success: bool, + collision_free: bool, ) -> FeasibilityStatus: status = str(result.get("status", "")).upper() if success and collision_free: diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index cd50ab098e..251dac6126 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -155,6 +155,9 @@ def dispatch(event: TransformControlsEvent) -> None: self._handles[handle_key] = handle return handle + def remove_target_controls(self, robot_id: str) -> None: + self._remove_handle(f"{robot_id}:ee_control") + def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> None: config = self._configs_by_id.get(robot_id) if config is None or joint_state is None: diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index b46097df95..bd28962b2d 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -21,7 +21,8 @@ import threading from typing import Literal -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -92,16 +93,25 @@ class FeasibilityState: @dataclass class PanelPlanState: status: PlanStatus = PlanStatus.NONE + group_ids: tuple[PlanningGroupID, ...] = () robot: str | None = None target_pose: Pose | None = None target_joints: list[float] | None = None - start_joints_snapshot: list[float] | None = None + start_joints_snapshot: dict[PlanningGroupID, list[float]] = field(default_factory=dict) planned_path: list[JointState] | None = None @dataclass class PanelState: selected_robot: str | None = None + selected_group_ids: tuple[PlanningGroupID, ...] = () + auxiliary_group_ids: tuple[PlanningGroupID, ...] = () + pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) + target_joints: JointState | None = None + last_valid_target_joints: JointState | None = None + group_poses: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_diagnostics: dict[PlanningGroupID, str] = field(default_factory=dict) runtime: PanelRuntime = PanelRuntime.STOPPED backend_status: BackendConnectionStatus = BackendConnectionStatus.DISCONNECTED target_status: TargetStatus = TargetStatus.EMPTY @@ -133,9 +143,10 @@ def can_plan(self) -> bool: return ( self.runtime == PanelRuntime.RUNNING and self.backend_status == BackendConnectionStatus.READY - and self.selected_robot is not None + and bool(self.selected_group_ids) and self.action_status == ActionStatus.IDLE and self.target_status == TargetStatus.FEASIBLE + and self.target_joints is not None and self.manipulation_state in {"IDLE", "COMPLETED", "FAULT"} and self.plan_state.status != PlanStatus.PLANNING ) @@ -169,19 +180,24 @@ def can_execute( and self.target_status == TargetStatus.FEASIBLE and self.manipulation_state in {"IDLE", "COMPLETED"} and plan.status == PlanStatus.FRESH - and plan.robot == self.selected_robot - and plan.start_joints_snapshot is not None - and self.current_joints is not None + and plan.group_ids == self.selected_group_ids + and bool(plan.start_joints_snapshot) + and self.target_joints is not None ): return False - if len(plan.start_joints_snapshot) != len(self.current_joints): + if self.current_joints is None: return False - return all( - abs(expected - current) <= current_tolerance - for expected, current in zip( - plan.start_joints_snapshot, self.current_joints, strict=False + # Multi-group freshness is checked by group snapshot when available. The + # robot-level current-joint fallback preserves one-group legacy tests. + if len(plan.start_joints_snapshot) == 1: + snapshot = next(iter(plan.start_joints_snapshot.values())) + if len(snapshot) != len(self.current_joints): + return False + return all( + abs(expected - current) <= current_tolerance + for expected, current in zip(snapshot, self.current_joints, strict=False) ) - ) + return True @property def connected(self) -> bool: @@ -203,9 +219,13 @@ def module_state(self) -> str: class TargetEvaluationRequest: sequence_id: int source: PreviewSource - robot_name: str + group_ids: tuple[PlanningGroupID, ...] + robot_name: str | None = None + auxiliary_group_ids: tuple[PlanningGroupID, ...] = () pose: Pose | None = None + pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) joints: JointState | None = None + joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) class TargetEvaluationWorker: @@ -218,8 +238,10 @@ class TargetEvaluationWorker: def __init__( self, - handler: Callable[[TargetEvaluationRequest], TargetEvaluation], - apply_result: Callable[[TargetEvaluationRequest, TargetEvaluation], None], + handler: Callable[[TargetEvaluationRequest], TargetEvaluation | TargetSetEvaluation], + apply_result: Callable[ + [TargetEvaluationRequest, TargetEvaluation | TargetSetEvaluation], None + ], ) -> None: self._handler = handler self._apply_result = apply_result diff --git a/dimos/manipulation/visualization/viser/test_operation_worker.py b/dimos/manipulation/visualization/viser/test_operation_worker.py index ee1d5c29c0..71a7857188 100644 --- a/dimos/manipulation/visualization/viser/test_operation_worker.py +++ b/dimos/manipulation/visualization/viser/test_operation_worker.py @@ -141,6 +141,12 @@ def cancel(self) -> bool: def plan_to_joints(self, joints: JointState, robot_name: str | None = None) -> bool: return True + def plan_target_set(self, joint_targets: dict[str, JointState]) -> bool: + return True + + def preview_target_set_plan(self) -> bool: + return True + def test_operation_worker_uses_per_operation_timeout() -> None: errors: list[str] = [] @@ -205,7 +211,8 @@ def test_gui_only_preview_submits_timeout_override(monkeypatch: pytest.MonkeyPat monkeypatch.setattr(gui, "_operation_worker", FakeTimeoutSubmitWorker(submissions)) gui.state.runtime = PanelRuntime.RUNNING gui.state.backend_status = BackendConnectionStatus.READY - gui.state.selected_robot = "arm" + gui.state.selected_group_ids = ("arm:manipulator",) + gui.state.target_joints = JointState({"name": ["arm/j1"], "position": [1.0]}) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" diff --git a/dimos/manipulation/visualization/viser/test_state.py b/dimos/manipulation/visualization/viser/test_state.py index 412c227050..040ae6f56e 100644 --- a/dimos/manipulation/visualization/viser/test_state.py +++ b/dimos/manipulation/visualization/viser/test_state.py @@ -20,11 +20,14 @@ PanelState, TargetStatus, ) +from dimos.msgs.sensor_msgs.JointState import JointState def test_panel_can_plan_from_fault_after_planning_failure() -> None: state = PanelState( selected_robot="arm", + selected_group_ids=("arm:manipulator",), + target_joints=JointState({"name": ["arm/j1"], "position": [1.0]}), runtime=PanelRuntime.RUNNING, backend_status=BackendConnectionStatus.READY, target_status=TargetStatus.FEASIBLE, diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 687065885d..cedd3954b3 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -25,7 +25,7 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.animation import ( PreviewAnimator, @@ -52,6 +52,7 @@ GuiCallback = Callable[[SimpleNamespace], None] ThemeValue = str | bool | tuple[int, int, int] | dict[str, str | dict[str, str]] | None RobotConfigOverride = str | list[str] | list[float] | None +DEFAULT_GROUP_ID = "arm:manipulator" @dataclass @@ -346,6 +347,23 @@ def make_robot_config(**overrides: RobotConfigOverride) -> RobotConfigStub: return config +def make_planning_group_info( + robot_name: str, config: RobotConfigStub | SimpleNamespace +) -> dict[str, object]: + joint_names = [str(name) for name in config.joint_names] + return { + "id": f"{robot_name}:manipulator", + "name": "manipulator", + "robot_name": robot_name, + "joint_names": [f"{robot_name}/{name}" for name in joint_names], + "local_joint_names": joint_names, + "base_link": str(config.base_link), + "tip_link": str(config.end_effector_link), + "has_pose_target": True, + "source": "fallback", + } + + class FakeManipulationModule(SimpleNamespace): """Public ManipulationModule surface used by the in-process Viser adapter tests.""" @@ -382,6 +400,7 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: "name": config.name, "world_robot_id": self.robot_id_for_name(robot_name) or robot_name, "joint_names": list(config.joint_names), + "planning_groups": [make_planning_group_info(robot_name, config)], "end_effector_link": config.end_effector_link, "base_link": config.base_link, "max_velocity": 1.0, @@ -429,6 +448,37 @@ def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluatio "collision_free": False, } + def evaluate_joint_target_set( + self, joint_targets: dict[str, JointState] + ) -> TargetSetEvaluation: + if not joint_targets: + return {"success": False, "status": "INVALID", "target_joints": None} + names: list[str] = [] + positions: list[float] = [] + collision_free = True + group_poses = {} + world_monitor = getattr(self, "_world_monitor", None) + for group_id, target in joint_targets.items(): + robot_name = group_id.split(":", 1)[0] + robot_id = self.robot_id_for_name(robot_name) + names.extend(target.name) + positions.extend(float(value) for value in target.position) + if world_monitor is not None and robot_id is not None: + collision_free = collision_free and world_monitor.is_state_valid(robot_id, target) + group_poses[group_id] = world_monitor.get_ee_pose(robot_id, target) + return { + "success": True, + "status": "FEASIBLE" if collision_free else "COLLISION", + "message": "Target is collision-free" if collision_free else "Target is in collision", + "collision_free": collision_free, + "target_joints": JointState({"name": names, "position": positions}), + "group_ids": tuple(joint_targets), + "group_poses": group_poses, + } + + def plan_to_joint_targets(self, _joint_targets: dict[str, JointState]) -> bool: + return True + def make_adapter_with_robot() -> InProcessViserAdapter: current = FakeJointState(["j1", "j2"], position=[0.3, 0.4]) @@ -559,8 +609,8 @@ def test_gui_ignores_target_evaluation_after_close( request = TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - robot_name="arm", - joints=FakeJointState(["j1", "j2"], position=[0.1, 0.2]), + group_ids=(DEFAULT_GROUP_ID,), + joint_targets={DEFAULT_GROUP_ID: FakeJointState(["arm/j1", "arm/j2"], position=[0.1, 0.2])}, ) gui.close() @@ -1106,12 +1156,40 @@ def test_gui_initializes_pose_selector_to_current_ee_pose( FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 ) gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - control = scene._handles["robot-1:ee_control"] + control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] assert control.position == (0.1, 0.2, 0.3) assert control.wxyz == (0.9, 0.1, 0.2, 0.3) assert gui.state.cartesian_target is current_pose +def test_gui_removes_pose_selector_when_group_is_deselected( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1"], position=[0.25]) + current_pose = SimpleNamespace( + position=SimpleNamespace(x=0.1, y=0.2, z=0.3), + orientation=SimpleNamespace(w=1.0, x=0.0, y=0.0, z=0.0), + ) + config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + get_ee_pose=lambda robot_id, joint_state=None: current_pose, + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + scene = ViserManipulationScene( + FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 + ) + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] + + gui._set_group_selected(DEFAULT_GROUP_ID, False) + + assert f"{DEFAULT_GROUP_ID}:ee_control" not in scene._handles + assert control.removed is True + + def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callbacks( make_panel: Callable[..., ViserPanelGui], ) -> None: @@ -1130,11 +1208,11 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) gui = make_panel(FakeGuiServer(), adapter) assert gui._handles["preset"].options == ["Select preset...", "Init", "Current", "Home"] - assert list(gui._joint_sliders) == ["j1", "j2"] + assert list(gui._joint_sliders) == ["arm/j1", "arm/j2"] gui._apply_preset("Home") - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] gui._apply_preset("Current") - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.5] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.5] gui._submit_execute() assert gui.state.error == "Panel execution disabled; set allow_plan_execute=True to enable" @@ -1158,10 +1236,12 @@ def test_gui_rebuilding_joint_sliders_removes_stale_viser_handles( assert [slider.value for slider in stale_sliders] == [0.0, 0.0] current.position = [-0.738, -0.2826151825863572] + gui.state.target_joints = None + gui.state.group_joint_targets.clear() gui._build_joint_sliders() assert all(slider.removed is True for slider in stale_sliders) - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [ + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [ -0.738, -0.2826151825863572, ] @@ -1204,9 +1284,9 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( gui = make_panel(FakeGuiServer(), adapter) gui.refresh() assert gui.state.selected_robot == "arm" - assert list(gui._joint_sliders) == ["j1"] + assert list(gui._joint_sliders) == ["arm/j1"] gui._apply_preset("Home") - assert gui._joint_sliders["j1"].value == 0.5 + assert gui._joint_sliders["arm/j1"].value == 0.5 gui._submit_execute() assert "Panel execution disabled" in gui.state.error @@ -1241,38 +1321,44 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( gui._worker = SimpleNamespace( submit=lambda request: requests.append(request), stop=lambda: None ) - gui._joint_sliders["j1"].value = 0.25 - gui._joint_sliders["j2"].value = 0.75 + gui._joint_sliders["arm/j1"].value = 0.25 + gui._joint_sliders["arm/j2"].value = 0.75 gui._submit_joint_target_evaluation() assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) - assert target_pose_updates[-1] == ("robot-1", target_pose) + assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, target_pose) assert requests[-1].source == "joints" - stale_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") - fresh_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") + stale_request = TargetEvaluationRequest( + sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) + fresh_request = TargetEvaluationRequest( + sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) gui.state.latest_sequence_id = 2 gui._apply_target_evaluation_result( stale_request, { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [9.0, 9.0]), + "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [9.0, 9.0]), }, ) - assert gui.state.joint_target == [0.25, 0.75] + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [0.25, 0.75] gui._apply_target_evaluation_result( fresh_request, { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), + "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), }, ) assert gui.state.target_status == TargetStatus.FEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.FEASIBLE - assert gui.state.joint_target == [1.0, 2.0] - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.75] + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.75] assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) @@ -1302,7 +1388,9 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( gui.state.cartesian_target = Pose( {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} ) - request = TargetEvaluationRequest(sequence_id=1, source="cartesian", robot_name="arm") + request = TargetEvaluationRequest( + sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) + ) gui.state.latest_sequence_id = 1 gui._apply_target_evaluation_result( @@ -1310,12 +1398,12 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), + "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), }, ) assert gui.state.target_status == TargetStatus.FEASIBLE - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) assert target_pose_updates == [] @@ -1345,7 +1433,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( set_target_visual_state=lambda *args: visual_states.append(args), ) gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") + request = TargetEvaluationRequest(sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,)) gui.state.latest_sequence_id = 1 result = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[1.0]), "arm") @@ -1355,7 +1443,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( assert gui.state.target_status == TargetStatus.INFEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.COLLISION assert gui.state.error == "Target is in collision" - assert visual_states[-1] == ("robot-1", False) + assert visual_states[-1] == (DEFAULT_GROUP_ID, False) def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( @@ -1400,8 +1488,8 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( gui.state.target_status = TargetStatus.FEASIBLE gui.state.plan_state = PanelPlanState( status=PlanStatus.FRESH, - robot="arm", - start_joints_snapshot=[1.2], + group_ids=(DEFAULT_GROUP_ID,), + start_joints_snapshot={DEFAULT_GROUP_ID: [1.2]}, planned_path=planned, ) gui._submit_execute() @@ -1410,9 +1498,9 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( gui.state.action_status = ActionStatus.IDLE gui.state.error = "" - gui.state.plan_state.start_joints_snapshot = [1.0] + gui.state.plan_state.start_joints_snapshot = {DEFAULT_GROUP_ID: [1.0]} gui._submit_execute() - assert executed == ["arm"] + assert executed == [None] gui._submit_clear() assert cleared == [True] @@ -1433,7 +1521,8 @@ def test_gui_plan_target_failure_recovers_action_state( submit=lambda operation, **_kwargs: operation(), stop=lambda timeout=2.0: None ), ) - gui.state.selected_robot = "missing" + gui.state.selected_group_ids = ("missing",) + gui.state.target_joints = JointState({"name": ["missing/j1"], "position": [1.0]}) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" @@ -1441,7 +1530,7 @@ def test_gui_plan_target_failure_recovers_action_state( assert gui.state.action_status == ActionStatus.IDLE assert gui.state.plan_state.status == PlanStatus.FAILED - assert gui.state.error == "No robot config" + assert gui.state.error == "Unknown planning group: missing" assert gui.state.last_result == "plan_to_joints=False" @@ -1465,12 +1554,12 @@ def reset() -> bool: calls.append("reset") return True - def plan_to_joints(_joints: JointState, _robot_name: str | None = None) -> bool: + def plan_target_set(_joint_targets: dict[str, JointState]) -> bool: calls.append("plan") return True monkeypatch.setattr(adapter, "reset", reset) - monkeypatch.setattr(adapter, "plan_to_joints", plan_to_joints) + monkeypatch.setattr(adapter, "plan_target_set", plan_target_set) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "FAULT" @@ -1529,8 +1618,12 @@ def operation() -> None: def test_target_evaluation_worker_coalesces_pending_requests() -> None: worker = TargetEvaluationWorker(lambda request: {}, lambda request, result: None) - old_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") - new_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") + old_request = TargetEvaluationRequest( + sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) + new_request = TargetEvaluationRequest( + sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) worker.submit(old_request) worker.submit(new_request) diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index 0a05e6982d..bcbd550d0a 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -129,12 +129,57 @@ touching `WorldSpec`, IK, planner objects, or live Drake contexts directly. Rend mutable joint state/path containers at the read boundary, then updates the Viser scene after manipulation/world accessors have returned. +#### Viser Planning Target Set workflow + +The Viser manipulation panel is planning-group centric. Select one or more planning groups +to form a **Planning Target Set**; IK, feasibility checks, planning, preview, execute, +clear-plan, and plan freshness are scoped to that whole set. A single xArm uses the same +workflow as a one-group target set, while dual-arm stacks can select both manipulators and +plan them together. + +- Use the planning-group checklist to add or remove groups. **Select all manipulators** + selects every planning group named `manipulator`. +- Pose target gizmos are keyed by planning group ID. Moving any selected pose gizmo triggers + whole-set IK evaluation and updates the global target joints when IK succeeds. +- Joint sliders are grouped by planning group. Editing joints triggers whole-set joint + evaluation and refreshes visible pose gizmos from FK outputs when available. +- Auxiliary groups are selected target-set members without direct gizmos. Their joints still + participate in IK seeds, target joints, feasibility, planning, preview, and execute. +- The panel exposes one Plan, Preview, Execute, Cancel, and Clear row for the whole target set; + normal operation does not expose per-robot preview or execute controls. + +Single xArm Viser example: + +```bash +uv run dimos run xarm7-planner-coordinator \ + -o manipulationmodule.visualization.backend=viser +``` + +Enable browser-panel execution only when an operator is intentionally allowed to execute plans: + +```bash +uv run dimos run xarm7-planner-coordinator \ + -o manipulationmodule.visualization.backend=viser \ + -o manipulationmodule.visualization.allow_plan_execute=true +``` + +Dual-arm mock Viser example: + +```bash +uv run dimos run dual-xarm6-planner \ + -o manipulationmodule.visualization.backend=viser +``` + External manipulation visualizers are initialized from a backend-neutral planning-scene snapshot after the planning world has added its robots. This snapshot maps world robot IDs to `RobotModelConfig` metadata so Viser can prepare current, target, and transient preview robot visuals without `WorldMonitor` depending on Viser-specific hooks. Embedded Meshcat visualization does not need extra setup because it observes the Drake world directly. +Viser renders robot placement as authored in the prepared URDF/xacro output. It does not apply +`RobotModelConfig.base_pose` as an additional implicit visual transform, which avoids +double-applying placement for multi-robot models that already encode offsets in URDF/xacro. + Panel execution is opt-in. Leave `allow_plan_execute=False` unless the operator intentionally wants the browser panel to call the existing manipulation execution path. diff --git a/openspec/changes/add-viser-planning-target-set/.openspec.yaml b/openspec/changes/add-viser-planning-target-set/.openspec.yaml new file mode 100644 index 0000000000..d57ce332e0 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-06-20 diff --git a/openspec/changes/add-viser-planning-target-set/design.md b/openspec/changes/add-viser-planning-target-set/design.md new file mode 100644 index 0000000000..a1d98a0712 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/design.md @@ -0,0 +1,139 @@ +## Context + +DimOS manipulation now treats planning groups as first-class planning units and stores successful results as `GeneratedPlan` artifacts over global joint names. Viser currently lags behind that model: the panel selects one robot, maintains one target, and delegates through robot-scoped convenience calls. That works for the single-arm case, but it does not express a coordinated dual-arm intent where `left_arm/manipulator` and `right_arm/manipulator` should be solved, checked, planned, previewed, and executed as one whole target set. + +The current Viser implementation already has useful pieces: in-process adapter calls, target transform controls, joint sliders, preview animation, and `VisualizationSpec` integration through `WorldMonitor`. This change should preserve those pieces while changing their semantic unit from selected robot to planning target set. + +Root `CONTEXT.md` defines the canonical language: Planning Group, Planning Group Selection, Auxiliary Planning Group, Planning Target Set, Generated Plan, Global Joint Name, and Robot Placement. This design follows those terms. For Viser placement, this change deliberately relies on URDF/xacro-authored placement and does not apply `RobotModelConfig.base_pose` inside Viser. + +## Goals / Non-Goals + +**Goals:** + +- Make the Viser panel group-centric instead of robot-centric. +- Let users establish a Planning Target Set by selecting one or more planning groups. +- Show target gizmos keyed by Planning Group ID for selected pose-targetable groups. +- Treat auxiliary planning groups as normal target-set members without assigned gizmos. +- Make joint panels, IK, feasibility, planning, preview, execute, and freshness whole-set scoped. +- Normalize pose-authored targets into global joint targets via realtime whole-set IK. +- Add multi-target Pink IK behavior for same-robot multiple frame targets and cross-robot grouped solves. +- Keep planning, preview, and execution based on the whole `GeneratedPlan` for the target set. +- Preserve the single-robot workflow as the one-group target-set case. + +**Non-Goals:** + +- Do not add a new CLI command. +- Do not make Viser apply `base_pose` or infer backend weld behavior. +- Do not make IK collision-aware; collision validation and path collision avoidance remain WorldSpec/planner responsibilities. +- Do not add atomic multi-controller execution semantics beyond existing generated-plan projection/dispatch. +- Do not expose normal per-group or per-robot Plan/Preview/Execute controls in the target-set workflow. +- Do not require named left/right/dual-arm presets; a checklist plus “select all manipulators” is enough for the first UI. + +## DimOS Architecture + +### Viser panel and scene + +The Viser panel should maintain a Planning Target Set state rather than a single selected robot state. The target set includes selected group IDs, target authoring state, last valid global target joints, whole-set status, diagnostics, plan freshness, and plan status. + +Current robot visuals and preview ghosts remain robot-keyed because rendering a plan projects to whole robot models. Target transform controls become planning-group-keyed because pose targets belong to planning groups. A pose-targetable selected group gets a gizmo such as `/targets/left_arm/manipulator`; an auxiliary selected group participates without a gizmo. + +The panel should display one unified Target Set joint panel, visually grouped by planning group. Per-group sections are views into one target set, not independent state machines. The action row operates on the whole set. + +### ManipulationModule and adapter boundary + +Viser should not implement IK or target-set feasibility semantics directly. The in-process adapter should expose whole-set operations backed by `ManipulationModule`, such as: + +- evaluate pose targets for a planning target set; +- evaluate joint targets for a planning target set; +- plan the current target set through joint-target planning; +- preview and execute the current whole generated plan. + +Evaluation results should use global joint names as the semantic output. Viser may display local labels inside group sections, but the returned target joint state must be unambiguous and directly usable by planning APIs. + +Pose-authored targets should normalize to joint targets before planning. The panel should call pose target set evaluation during gizmo edits, update the last valid target joints, then call joint target planning when the user plans. Joint edits should call joint target set evaluation, update pose gizmos through FK where available, and update whole-set feasibility. + +### Pink IK + +Pink IK should expose one multi-target pose solve behavior while grouping internally by robot model: + +- Same-robot multiple pose targets: solve one Pink configuration with multiple frame tasks. +- Cross-robot pose targets: solve one Pink problem per robot model and combine the results into one global selected joint target. + +Auxiliary groups participate in the selected joints and seed/target vector but do not receive direct frame tasks. They are still solved, checked, planned, previewed, and executed with the whole target set. + +IK should use the last valid target joints as the seed after target-set initialization. When a group is selected, its target initializes from current state. On IK failure, keep the last valid target joints, mark the whole target set invalid/stale, and disable planning. + +### Planning and execution + +Planning remains joint-target based after target-set evaluation. The whole target set lowers to `plan_to_joint_targets(...)` keyed by planning group. Preview and execute operate on the full generated plan without normal robot filtering in the panel. + +Plan freshness is whole-set scoped. When planning succeeds, store a current-joint snapshot for all selected groups. Execution is enabled only if current selected-group joints still match that snapshot within tolerance. + +### Blueprints and streams + +No stream contract changes are expected. Existing xArm planner/coordinator blueprints remain the manual QA target. Viser should work with `xarm7-planner-coordinator` and dual xArm mock planner/coordinator through visualization config overrides. + +### Skills/MCP and generated registries + +No skill/MCP tool contract changes are expected. No blueprint registry regeneration is expected unless implementation changes blueprint definitions. + +## Decisions + +1. **Use Planning Target Set as the UI semantic unit.** + - Rationale: Once groups are selected, independent per-group UI state causes discrepancies between IK, feasibility, planning, and execution. + - Alternative rejected: separate group cards with independent status/actions. + +2. **Keep Viser placement URDF-authored.** + - Rationale: xArm xacro already encodes attachment placement. Applying `base_pose` in Viser risks double placement and duplicates Drake weld heuristics. + - Alternative rejected: backend placement inference in Viser. + +3. **Key target gizmos by Planning Group ID.** + - Rationale: pose targets belong to planning groups, not robots. This avoids a future mismatch for robots with multiple pose-targetable groups. + - Alternative rejected: robot-keyed target controls. + +4. **Plan through joint targets after evaluation.** + - Rationale: The panel can use pose gizmos naturally while the planner receives one uniform global joint target set. + - Alternative rejected: separate UI modes for pose planning and joint planning. + +5. **Group Pink multi-target solving by robot internally.** + - Rationale: Same-robot multi-frame targets need one Pink configuration. Cross-robot targets can be solved per model and combined without building a combined Pinocchio model. + - Alternative deferred: building one combined Pink model across robots. + +6. **Whole-set status is canonical.** + - Rationale: The Plan button must not be enabled because individual group sections look valid while the combined target set is invalid. + - Alternative rejected: per-group canonical statuses. + +## Safety / Simulation / Replay + +Execution remains gated by existing Viser configuration (`allow_plan_execute`) and manipulation module state. The panel should not execute unless the whole plan is fresh and current selected-group joints match the planning start snapshot. + +Collision checking remains outside IK and in WorldSpec/planner behavior. IK may return kinematically valid targets that planning later rejects due to collision or unavailable path. The UI must surface this as whole-set planning failure, not per-group success. + +Manual QA should use mock xArm blueprints first, especially `xarm7-planner-coordinator` and the dual xArm planner/coordinator, before any hardware test. Hardware tests must keep execution explicitly opt-in. + +Replay behavior is not expected to change except that Viser target-set state should consume the same current joint states as the existing panel. + +## Risks / Trade-offs + +- **Realtime IK cost:** Whole-set IK on every gizmo drag can be expensive. Use latest-request-wins and debounce behavior in the existing evaluation worker pattern. +- **State complexity:** The panel state becomes richer. Mitigate by making Planning Target Set the only authoritative UI state and treating per-group displays as projections. +- **Pink multi-frame correctness:** Same-robot multi-frame IK needs careful frame-task construction and result filtering. Add focused tests before relying on the UI. +- **Cross-robot IK limitation:** Cross-robot grouping combines independent kinematic solves. Inter-robot collision is handled by planning, not IK, so some targets may solve IK but fail planning. +- **Docs drift:** `add-planning-groups` artifacts still contain older terms such as ResolvedPlanningGroup. This change should use current glossary language and avoid reviving stale terms. + +## Migration / Rollout + +1. Add target-set evaluation structures and module/adapter methods while preserving single-group behavior. +2. Update Pink IK multi-target behavior with focused tests. +3. Rework Viser panel state and scene target controls to use planning group IDs. +4. Keep existing single-arm Viser interactions working as one selected group. +5. Add dual xArm mock tests and manual QA. +6. Update user-facing documentation for the target-set workflow. + +Rollback can keep the current single-robot panel path if target-set UI is implemented behind a small internal adapter seam, but the final intended state should be group-centric only. + +## Open Questions + +- Exact Python location/name for the target-set evaluation result type. +- Whether target-set evaluation should be public RPC or only in-process adapter/module API initially. +- How much debouncing is needed for realtime whole-set IK with real robots and larger target sets. diff --git a/openspec/changes/add-viser-planning-target-set/docs.md b/openspec/changes/add-viser-planning-target-set/docs.md new file mode 100644 index 0000000000..79a95c95b3 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/docs.md @@ -0,0 +1,32 @@ +## User-Facing Docs + +- Update manipulation/Viser usage documentation to describe the Planning Target Set workflow: + - selecting one or more planning groups; + - using group-keyed target gizmos; + - understanding auxiliary groups as selected groups without direct gizmos; + - planning, previewing, and executing the whole target set; + - using the workflow with single xArm and dual xArm mock blueprints. +- If no dedicated Viser usage page exists, add the workflow to the closest manipulation usage or capability document. +- Include an example command for launching xArm planner/coordinator with Viser enabled and execution opt-in clearly marked. + +## Contributor Docs + +- Update manipulation planning contributor notes if they describe robot-scoped Viser behavior or single-target IK assumptions. +- Mention that Viser placement is URDF-authored for this workflow and that `base_pose` is not automatically applied by Viser. + +## Coding-Agent Docs + +- No required `AGENTS.md` update is expected. +- If `docs/coding-agents/` contains manipulation-specific guidance, add a short note that target-set UI state is whole-set scoped and should not reintroduce per-robot Plan/Preview/Execute state. + +## Doc Validation + +- Run link validation for changed docs if available: + - `doclinks` +- If docs contain executable Python snippets, run: + - `md-babel-py run ` +- Run normal formatting/lint checks for any touched docs if configured in the repository. + +## No Docs Needed + +Documentation changes are needed because this change introduces a new user-facing Viser manipulation workflow and changes the mental model from robot selection to planning target sets. diff --git a/openspec/changes/add-viser-planning-target-set/proposal.md b/openspec/changes/add-viser-planning-target-set/proposal.md new file mode 100644 index 0000000000..5379496033 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/proposal.md @@ -0,0 +1,53 @@ +## Why + +The current Viser manipulation panel is robot-centric: it selects one robot, edits one target, and plans through robot-scoped compatibility APIs. That is not sufficient for dual-arm and multi-group manipulation, where the user intent is a single coordinated target over a planning group selection such as `left_arm/manipulator` plus `right_arm/manipulator`. + +DimOS now has first-class planning groups and generated plans over global joint names. Viser should expose that model naturally: users select a set of planning groups, edit pose gizmos or joint targets as one target set, and plan/preview/execute one whole generated plan. This also creates a clear path for natural pose-based bimanual planning through multi-target Pink IK. + +## What Changes + +- Add a Viser **Planning Target Set** workflow built on planning group selection rather than selected robot. +- Show planning-group-keyed target gizmos for selected pose-targetable groups. +- Treat auxiliary planning groups as members of the same target set without direct gizmo targets. +- Normalize pose-authored targets through whole-set IK into global joint targets, then plan through joint-target planning. +- Add whole-set evaluation semantics for IK, FK/target pose updates, feasibility, plan freshness, preview, and execute. +- Extend Pink IK to support multi-target pose evaluation through one unified API: + - same-robot targets use one Pink solve with multiple frame tasks; + - cross-robot targets are grouped by robot and combined into one global selected joint target. +- Keep collision validation and collision-free path planning in WorldSpec/planner responsibilities, not in IK semantics. +- Keep Viser robot placement URDF-authored for this change; do not infer or apply `base_pose` in Viser. + +## Affected DimOS Surfaces + +- Modules/streams: + - `ManipulationModule` target evaluation helpers and Viser-facing adapter surface. + - Manipulation planning group registry and group-target evaluation paths. + - Pink IK pose-target solving behavior. + - Viser visualization panel, scene target controls, and runtime state. +- Blueprints/CLI: + - No new CLI command is expected. + - Existing xArm planner/coordinator blueprints should be usable with Viser for single-arm and dual-arm mock validation. +- Skills/MCP: + - No direct MCP skill behavior change is expected. +- Hardware/simulation/replay: + - Dual-arm mock xArm planning should support group-target-set preview and planning in Viser. + - Hardware execution remains gated by existing execution controls and coordinator behavior. +- Docs/generated registries: + - Update manipulation/Viser usage docs if present. + - Update OpenSpec/docs language to reflect Planning Target Set and multi-target Pink IK. + +## Capabilities + +### New Capabilities + +- `viser-planning-target-set`: Viser UI behavior for selecting planning groups, authoring whole-set targets, evaluating target sets, and planning/previewing/executing generated plans. + +### Modified Capabilities + +- `manipulation-planning-groups`: Planning group workflows gain whole-set target authoring/evaluation semantics and multi-target Pink IK support. + +## Impact + +Users get a natural dual-arm Viser workflow: select both arm planning groups, manipulate group-keyed target gizmos, watch the whole target set solve to joint targets, then plan/preview/execute one generated plan. Developers get a cleaner module/UI boundary where Viser owns editing state and `ManipulationModule` owns target-set semantics. + +Compatibility risks are mostly UI/API surface changes around the in-process Viser adapter and target evaluation helpers. Existing single-robot Viser workflows should remain usable as the one-group case of the target-set workflow. Testing should cover single-arm regression, dual xArm mock target-set selection, group-keyed gizmo visibility, whole-set IK failure/freshness behavior, Pink multi-target IK, and plan/preview/execute whole-set scope. diff --git a/openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md b/openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md new file mode 100644 index 0000000000..e50237ad44 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md @@ -0,0 +1,71 @@ +## ADDED Requirements + +### Requirement: Whole-set pose target evaluation + +DimOS SHALL evaluate pose targets for a planning target set and return a global selected joint target when evaluation succeeds. + +#### Scenario: dual-arm pose targets evaluate to global joints +- **GIVEN** a planning target set includes `left_arm/manipulator` and `right_arm/manipulator` +- **AND** each selected group has an assigned pose target +- **WHEN** DimOS evaluates the pose target set +- **THEN** DimOS returns a target joint state using global joint names for both selected groups +- **AND** the result can be used as the goal for joint-target planning + +#### Scenario: auxiliary group participates without pose target +- **GIVEN** a planning target set includes one pose-targeted group and one auxiliary group +- **WHEN** DimOS evaluates the pose target set +- **THEN** the auxiliary group's selected joints participate in the returned target joint state +- **AND** DimOS does not require a direct pose target for the auxiliary group + +### Requirement: Multi-target Pink IK behavior + +DimOS SHALL support multi-target pose evaluation with Pink IK for same-robot and cross-robot planning target sets. + +#### Scenario: same robot multiple frame targets +- **GIVEN** one robot has multiple selected pose-targetable planning groups +- **WHEN** DimOS evaluates pose targets for those groups using Pink IK +- **THEN** DimOS solves the targets as one same-robot multi-frame IK problem +- **AND** returns one global selected joint target for the effective planning group selection + +#### Scenario: cross robot pose targets +- **GIVEN** a planning target set contains pose targets for planning groups on different robots +- **WHEN** DimOS evaluates the target set using Pink IK +- **THEN** DimOS evaluates the targets per robot model as needed +- **AND** combines successful results into one global selected joint target for the whole target set + +#### Scenario: IK does not own collision semantics +- **GIVEN** Pink IK returns a kinematically valid target joint state +- **WHEN** collision validation or path planning is required +- **THEN** DimOS performs collision checks through the planning world or planner responsibilities +- **AND** the IK result alone is not treated as proof that execution is collision-free + +### Requirement: Whole-set joint target evaluation + +DimOS SHALL evaluate joint targets for a planning target set as one whole selected target. + +#### Scenario: joint target evaluation returns poses for selected groups +- **GIVEN** a planning target set has global target joints for selected planning groups +- **WHEN** DimOS evaluates the joint target set +- **THEN** DimOS returns whole-set validity +- **AND** returns target poses for pose-targetable selected groups when available + +#### Scenario: invalid joint target blocks whole set +- **GIVEN** any selected planning group has missing, extra, or invalid target joints +- **WHEN** DimOS evaluates the joint target set +- **THEN** the whole target set is invalid +- **AND** planning is not enabled for the target set + +### Requirement: Target-set seed continuity + +DimOS SHALL use last valid target joints as the preferred seed for subsequent whole-set IK evaluation. + +#### Scenario: current state initializes target set +- **GIVEN** a planning group is newly added to a target set +- **WHEN** DimOS initializes target-set evaluation state +- **THEN** DimOS uses current joints for that group as the initial target and seed + +#### Scenario: subsequent IK uses last valid target +- **GIVEN** a planning target set has a last valid solved target joint state +- **WHEN** a subsequent pose edit triggers IK evaluation +- **THEN** DimOS uses the last valid target joints as the preferred seed +- **AND** planning still uses actual current state as the start when a plan is requested diff --git a/openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md b/openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md new file mode 100644 index 0000000000..d027115563 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md @@ -0,0 +1,115 @@ +## ADDED Requirements + +### Requirement: Planning target set selection + +Viser SHALL let users establish a planning target set by selecting one or more manipulation planning groups. + +#### Scenario: select multiple manipulator groups +- **GIVEN** a Viser manipulation scene contains `left_arm/manipulator` and `right_arm/manipulator` +- **WHEN** the user selects both planning groups +- **THEN** Viser treats them as one planning target set +- **AND** subsequent IK, feasibility, planning, preview, execute, and stale-state checks are scoped to the whole target set + +#### Scenario: select all manipulators convenience action +- **GIVEN** a Viser manipulation scene contains multiple manipulator planning groups +- **WHEN** the user chooses the select-all-manipulators action +- **THEN** all manipulator planning groups become members of the current planning target set +- **AND** the user can still adjust the selection explicitly + +### Requirement: Group-keyed target controls + +Viser SHALL key pose target controls by Planning Group ID for selected pose-targetable planning groups. + +#### Scenario: selected pose-targetable group shows gizmo +- **GIVEN** a selected planning group has a pose target frame +- **WHEN** Viser renders the planning target set +- **THEN** Viser shows a target gizmo associated with that Planning Group ID +- **AND** moving the gizmo edits that group's pose target within the whole target set + +#### Scenario: unselected group hides gizmo +- **GIVEN** a planning group is not part of the planning target set +- **WHEN** Viser renders target controls +- **THEN** Viser does not show a target gizmo for that group +- **AND** that group does not contribute target joints to the planning target set + +#### Scenario: auxiliary group has no assigned gizmo +- **GIVEN** a planning group is selected as an auxiliary member of the planning target set +- **WHEN** Viser renders target controls +- **THEN** Viser does not assign a direct pose gizmo to that auxiliary group +- **AND** the auxiliary group still participates in whole-set IK, feasibility, planning, preview, and execute behavior + +### Requirement: Target initialization from current state + +Viser SHALL initialize newly selected planning groups from current robot state. + +#### Scenario: selecting a group is motion-neutral +- **GIVEN** a planning group is not selected +- **WHEN** the user adds the group to the planning target set +- **THEN** Viser initializes that group's target joints from current joints +- **AND** if the group is pose-targetable, its target gizmo starts at the current group pose +- **AND** adding the group alone does not create a motion target away from current state + +### Requirement: Whole-set target authoring + +Viser SHALL treat pose gizmos and joint controls as views over one planning target set. + +#### Scenario: pose edit updates target joints +- **GIVEN** a planning target set has one or more selected pose-targetable groups +- **WHEN** the user moves any selected group's target gizmo +- **THEN** Viser requests whole-set IK evaluation for all active pose targets +- **AND** Viser updates the target-set joint controls from the returned global target joints when evaluation succeeds + +#### Scenario: joint edit updates pose targets +- **GIVEN** a planning target set has target joints and pose-targetable groups +- **WHEN** the user edits target joints +- **THEN** Viser requests whole-set joint target evaluation +- **AND** Viser updates visible pose gizmos from the evaluated group poses when available + +### Requirement: Whole-set status and failures + +Viser SHALL expose one canonical whole-set status for target validity and plan readiness. + +#### Scenario: IK failure keeps last valid target +- **GIVEN** a planning target set has a last valid solved target joint state +- **WHEN** realtime IK evaluation fails for a subsequent gizmo edit +- **THEN** Viser keeps the last valid target joints +- **AND** marks the planning target set invalid or stale +- **AND** disables planning until a whole-set evaluation succeeds again + +#### Scenario: per-group diagnostics are explanatory +- **GIVEN** a planning target set evaluation returns diagnostics for individual groups +- **WHEN** Viser displays target-set status +- **THEN** the whole-set status controls whether planning is enabled +- **AND** per-group diagnostics are displayed only as explanatory details + +### Requirement: Whole-set planning actions + +Viser SHALL plan, preview, execute, clear, and report freshness for the whole planning target set. + +#### Scenario: plan target set through joint targets +- **GIVEN** a planning target set has valid global target joints +- **WHEN** the user requests planning +- **THEN** Viser requests joint-target planning for the selected planning groups +- **AND** the request represents the whole target set rather than an individual robot or group + +#### Scenario: preview and execute full generated plan +- **GIVEN** planning succeeds for a planning target set +- **WHEN** the user previews or executes from Viser +- **THEN** Viser acts on the full generated plan for the target set +- **AND** the normal UI does not expose per-robot or per-group preview/execute actions + +#### Scenario: execute requires whole-set freshness +- **GIVEN** a generated plan was created for a planning target set +- **WHEN** current joints for any selected planning group drift from the plan start snapshot beyond tolerance +- **THEN** Viser marks the plan stale for the whole target set +- **AND** disables execute for the whole plan + +### Requirement: URDF-authored visual placement + +Viser SHALL rely on authored URDF/xacro placement for robot visuals in this workflow. + +#### Scenario: dual xArm visual placement is not double-applied +- **GIVEN** dual xArm robot models encode placement in their authored URDF/xacro output +- **WHEN** Viser registers the robots for visualization +- **THEN** Viser renders the prepared URDFs as authored +- **AND** Viser does not apply `base_pose` as an additional implicit visual transform diff --git a/openspec/changes/add-viser-planning-target-set/tasks.md b/openspec/changes/add-viser-planning-target-set/tasks.md new file mode 100644 index 0000000000..bfeffc9220 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/tasks.md @@ -0,0 +1,45 @@ +## 1. Target-set evaluation model and module boundary + +- [x] 1.1 Define a target-set evaluation result shape with whole-set status, message, group IDs, global target joints, optional group diagnostics, and pose outputs for pose-targetable groups. +- [x] 1.2 Add ManipulationModule whole-set pose evaluation for `Mapping[PlanningGroupID, PoseStamped]` plus auxiliary group IDs, returning global selected joint targets. +- [x] 1.3 Add ManipulationModule whole-set joint evaluation for `Mapping[PlanningGroupID, JointState]`, including exact target validation and FK pose outputs for pose-targetable groups. +- [x] 1.4 Update the in-process Viser adapter to expose whole-set evaluation, target-set planning, whole-plan preview, and whole-plan execution helpers. +- [x] 1.5 Preserve existing one-robot behavior as the one-planning-group target-set case. + +## 2. Pink multi-target IK + +- [x] 2.1 Extend Pink IK pose-target solving to accept multiple pose-targeted planning groups in one request. +- [x] 2.2 Implement same-robot multi-frame Pink solving with multiple frame tasks in one Pink configuration. +- [x] 2.3 Implement cross-robot grouping for Pink IK by solving per robot model and combining results into one global selected joint target. +- [x] 2.4 Ensure auxiliary planning groups participate in the selected joints and seed/target result without receiving direct frame tasks. +- [x] 2.5 Keep collision semantics outside IK; surface kinematic success separately from collision/path-planning success. +- [x] 2.6 Add Pink IK tests for single-target regression, same-robot multi-frame targets, cross-robot targets, auxiliary groups, and global joint-name output. + +## 3. Viser target-set UI and scene + +- [x] 3.1 Replace robot-centric panel state with Planning Target Set state: selected group IDs, pose targets, global target joints, last valid target joints, whole-set status, diagnostics, plan status, and start snapshots. +- [x] 3.2 Add a planning-group checklist and a select-all-manipulators action. +- [x] 3.3 Make target controls planning-group-keyed; show gizmos only for selected pose-targetable groups and hide gizmos for unselected or auxiliary-only groups. +- [x] 3.4 Render one grouped Target Set joint panel with sections per selected planning group and one whole-set status/action row. +- [x] 3.5 Make pose gizmo edits trigger latest-request-wins whole-set IK evaluation, updating target joints on success and keeping last valid target joints on failure. +- [x] 3.6 Make joint edits trigger whole-set joint evaluation and update visible pose gizmos through FK outputs. +- [x] 3.7 Make Plan/Preview/Execute/Clear actions operate on the whole target set, with no normal per-robot or per-group action controls. +- [x] 3.8 Keep Viser robot placement URDF-authored; do not apply `base_pose` during scene registration. + +## 4. Documentation + +- [x] 4.1 Update manipulation/Viser user documentation with the Planning Target Set workflow and xArm launch examples. +- [x] 4.2 Document auxiliary groups as selected target-set members without assigned gizmos. +- [x] 4.3 Document that Viser uses URDF-authored placement and does not implicitly apply `base_pose`. +- [x] 4.4 Update contributor/coding-agent docs only if they currently describe robot-centric Viser planning or per-robot preview/execute state. + +## 5. Verification + +- [x] 5.1 Run `openspec validate add-viser-planning-target-set`. +- [x] 5.2 Run focused Pink IK tests, including new multi-target cases. +- [x] 5.3 Run focused Viser tests for target-set state, group-keyed gizmos, whole-set status, plan freshness, and whole-plan preview/execute actions. +- [x] 5.4 Run manipulation unit tests covering module whole-set evaluation and planning through joint targets. +- [ ] 5.5 Run xArm single-robot Viser manual QA with `xarm7-planner-coordinator` to verify the one-group workflow still works. +- [ ] 5.6 Run dual xArm mock Viser manual QA to verify selecting both manipulators, moving both gizmos, planning, previewing, and clearing the whole target set. +- [x] 5.7 Run docs validation commands for changed docs, including `doclinks` when available and `md-babel-py run ` for executable snippets. +- [x] 5.8 Run ruff/focused lint checks for touched manipulation and visualization files. From a38a90c6bfcc89282f68f85f66d78c6bc2f6c518 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 20 Jun 2026 10:56:13 -0700 Subject: [PATCH 22/27] fix: stabilize viser ik target solving --- dimos/manipulation/manipulation_module.py | 37 +++-- .../planning/kinematics/pink_ik.py | 83 ++++++++-- .../planning/kinematics/test_pink_ik.py | 100 ++++++++++++ dimos/manipulation/planning/spec/config.py | 6 +- .../manipulation/planning/utils/mesh_utils.py | 58 ++++++- .../planning/utils/test_mesh_utils.py | 47 ++++++ .../planning/world/drake_world.py | 3 + dimos/manipulation/test_manipulation_unit.py | 29 ++++ .../visualization/viser/adapter.py | 10 +- .../visualization/viser/config.py | 7 + dimos/manipulation/visualization/viser/gui.py | 40 ++++- .../manipulation/visualization/viser/scene.py | 71 ++++++++- .../manipulation/visualization/viser/state.py | 1 + .../viser/test_viser_visualization.py | 146 +++++++++++++++++- dimos/robot/catalog/test_ufactory.py | 40 +++++ dimos/robot/catalog/ufactory.py | 22 ++- dimos/robot/config.py | 5 +- 17 files changed, 654 insertions(+), 51 deletions(-) create mode 100644 dimos/manipulation/planning/utils/test_mesh_utils.py create mode 100644 dimos/robot/catalog/test_ufactory.py diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 9e10f2f794..654d9ce970 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -1009,7 +1009,9 @@ def evaluate_joint_target( "joint_state": target, } - def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvaluation: + def evaluate_pose_target( + self, pose: Pose, robot_name: RobotName, check_collision: bool = True + ) -> TargetEvaluation: """Evaluate a Cartesian target for visualization without planning a path.""" robot_id = self.robot_id_for_name(robot_name) if robot_id is None: @@ -1037,10 +1039,15 @@ def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvalu "message": "Planning is not initialized or current state is unavailable", "collision_free": False, } - ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=True) + ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=check_collision) joint_state = JointState(ik.joint_state) if ik.is_success() and ik.joint_state else None - collision_free = bool( - joint_state is not None and self._world_monitor.is_state_valid(robot_id, joint_state) + collision_free = ( + bool(joint_state is not None) + if not check_collision + else bool( + joint_state is not None + and self._world_monitor.is_state_valid(robot_id, joint_state) + ) ) return { "success": joint_state is not None and collision_free, @@ -1159,6 +1166,7 @@ def _evaluate_global_target_set( message: str = "Target set is feasible", position_error: float = 0.0, orientation_error: float = 0.0, + check_collision: bool = True, ) -> TargetSetEvaluation: """Evaluate whole-set collision and pose outputs for global target joints.""" if self._world_monitor is None: @@ -1204,15 +1212,22 @@ def _evaluate_global_target_set( diagnostics: dict[PlanningGroupID, str] = {} selection = self._world_monitor.planning_groups.select(group_ids) for robot_name, (robot_id, _, local_target) in local_targets.items(): - robot_collision_free = self._world_monitor.is_state_valid(robot_id, local_target) + robot_collision_free = ( + self._world_monitor.is_state_valid(robot_id, local_target) + if check_collision + else True + ) collision_free = collision_free and robot_collision_free for group in selection.groups: if group.robot_name == robot_name: - diagnostics[group.id] = ( - "Target is collision-free" - if robot_collision_free - else "Target is in collision" - ) + if not check_collision: + diagnostics[group.id] = "Target collision check skipped" + else: + diagnostics[group.id] = ( + "Target is collision-free" + if robot_collision_free + else "Target is in collision" + ) return { "success": collision_free, @@ -1278,6 +1293,7 @@ def evaluate_pose_target_set( pose_targets: Mapping[PlanningGroupID | PlanningGroup, Pose | PoseStamped], auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), seed: JointState | None = None, + check_collision: bool = True, ) -> TargetSetEvaluation: """Evaluate pose targets for a whole planning target set using configured IK.""" if self._world_monitor is None or self._kinematics is None: @@ -1353,6 +1369,7 @@ def stamped_pose(pose: Pose | PoseStamped) -> PoseStamped: message=ik.message, position_error=ik.position_error, orientation_error=ik.orientation_error, + check_collision=check_collision, ) @rpc diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index 43c0b9b5d9..19b2a73a4d 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -85,6 +85,10 @@ class _PinkRobotContext: mapping: _JointMapping +class _CurrentStateRequiredError(ValueError): + """Raised when normalizing a seed requires the world's current state.""" + + class PinkIK: """Pink task/QP IK solver implementing the planning ``KinematicsSpec`` contract. @@ -221,7 +225,7 @@ def solve_pose_targets( world=world, robot_id=robot_id, pose_targets={group: pose_targets[group] for group in robot_pose_groups}, - seed=_seed_for_robot(world, robot_id, seed), + seed=_seed_for_robot_with_world_fallback(world, robot_id, seed), position_tolerance=position_tolerance, orientation_tolerance=orientation_tolerance, check_collision=check_collision, @@ -232,7 +236,7 @@ def solve_pose_targets( else: result = IKResult( status=IKStatus.SUCCESS, - joint_state=_seed_for_robot(world, robot_id, seed), + joint_state=_seed_for_robot_with_world_fallback(world, robot_id, seed), message="Auxiliary group retained seed state", ) joint_state = result.joint_state @@ -544,8 +548,12 @@ def _build_robot_context( package_paths=config.package_paths, xacro_args=config.xacro_args, convert_meshes=config.auto_convert_meshes, + strip_world_joint_child_link=config.base_link + if config.strip_model_world_joint + else None, ) model = pinocchio.buildModelFromUrdf(str(prepared_path)) + model = _lock_uncontrolled_model_joints(pinocchio, model, config) data = model.createData() target_frame = frame_name or config.end_effector_link @@ -669,6 +677,30 @@ def _build_joint_mapping(model: Any, config: RobotModelConfig) -> _JointMapping: ) +def _lock_uncontrolled_model_joints( + pinocchio: ModuleType, model: Any, config: RobotModelConfig +) -> Any: + """Return a Pinocchio model reduced to the joints controlled by config.""" + controlled_joint_names = set(config.joint_names) + lock_joint_ids: list[int] = [] + for joint_id, model_joint_name in enumerate(model.names): + if joint_id == 0 or model_joint_name in controlled_joint_names: + continue + joint = model.joints[joint_id] + if int(getattr(joint, "nq", 1)) > 0: + lock_joint_ids.append(joint_id) + + if not lock_joint_ids: + return model + + logger.debug( + "Reducing Pink IK model '%s' by locking uncontrolled joints: %s", + config.name, + [model.names[joint_id] for joint_id in lock_joint_ids], + ) + return pinocchio.buildReducedModel(model, lock_joint_ids, pinocchio.neutral(model)) + + def _get_joint_id(model: Any, joint_name: str) -> int: if hasattr(model, "existJointName") and not model.existJointName(joint_name): raise ValueError(_missing_joint_message(model, joint_name)) @@ -772,15 +804,16 @@ def _collision_failure(result: IKResult) -> IKResult: ) -def _seed_for_robot( - world: WorldSpec, robot_id: WorldRobotID, seed: JointState | None +def _seed_for_robot_config( + config: RobotModelConfig, + seed: JointState | None, + current_state: JointState | None = None, ) -> JointState: """Return a full local seed state for one robot from local/global seed input.""" - config = world.get_robot_config(robot_id) - with world.scratch_context() as ctx: - current = world.get_joint_state(ctx, robot_id) if seed is None: - return JointState(current) + if current_state is None: + raise _CurrentStateRequiredError("Current joint state is required when seed is absent") + return JointState(current_state) if not seed.name: if len(seed.position) == len(config.joint_names): return JointState({"name": list(config.joint_names), "position": list(seed.position)}) @@ -791,21 +824,47 @@ def _seed_for_robot( if len(seed.name) != len(seed.position): raise ValueError(f"Seed has {len(seed.name)} names but {len(seed.position)} positions") seed_by_name = dict(zip(seed.name, seed.position, strict=True)) - current_by_name = dict(zip(current.name, current.position, strict=True)) positions: list[float] = [] + missing_local_names: list[str] = [] for local_name in config.joint_names: global_name = make_global_joint_name(config.name, local_name) if local_name in seed_by_name: positions.append(float(seed_by_name[local_name])) elif global_name in seed_by_name: positions.append(float(seed_by_name[global_name])) - elif local_name in current_by_name: - positions.append(float(current_by_name[local_name])) else: - raise ValueError(f"Seed/current state is missing joint '{local_name}'") + positions.append(0.0) + missing_local_names.append(local_name) + if missing_local_names: + if current_state is None: + missing = ", ".join(repr(name) for name in missing_local_names) + raise _CurrentStateRequiredError( + f"Current joint state is required for missing joints: {missing}" + ) + current = current_state + current_by_name = dict(zip(current.name, current.position, strict=True)) + for index, local_name in enumerate(config.joint_names): + if local_name not in missing_local_names: + continue + if local_name not in current_by_name: + raise ValueError(f"Seed/current state is missing joint '{local_name}'") + positions[index] = float(current_by_name[local_name]) return JointState({"name": list(config.joint_names), "position": positions}) +def _seed_for_robot_with_world_fallback( + world: WorldSpec, robot_id: WorldRobotID, seed: JointState | None +) -> JointState: + """Normalize a robot seed, reading world state only when the seed is incomplete.""" + config = world.get_robot_config(robot_id) + try: + return _seed_for_robot_config(config, seed) + except _CurrentStateRequiredError: + with world.scratch_context() as ctx: + current = world.get_joint_state(ctx, robot_id) + return _seed_for_robot_config(config, seed, current) + + def _robot_ids_by_name( world: WorldSpec, robot_names: tuple[RobotName, ...] ) -> dict[RobotName, WorldRobotID]: diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index bfb0af604b..1ec9b7bd1b 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -32,8 +32,10 @@ PinkIKConfig, PinkIKDependencyError, _build_joint_mapping, + _lock_uncontrolled_model_joints, _PinkModules, _PinkRobotContext, + _seed_for_robot_config, _seed_positions_for_mapping, ) from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -173,6 +175,15 @@ def _robot_config() -> RobotModelConfig: ) +def _pose_stamped(x: float, y: float, z: float, yaw: float = 0.0) -> PoseStamped: + half_yaw = yaw / 2.0 + return PoseStamped( + frame_id="world", + position=Vector3(x, y, z), + orientation=Quaternion(0.0, 0.0, float(np.sin(half_yaw)), float(np.cos(half_yaw))), + ) + + class _TestPinkIK(PinkIK): def __init__(self, converge: bool = True) -> None: self.config = PinkIKConfig(max_iterations=3) @@ -367,6 +378,15 @@ def test_joint_order_mapping_uses_names_not_positions() -> None: assert _seed_positions_for_mapping(seed, mapping).tolist() == [10.0, 20.0, 30.0] +def test_seed_for_robot_config_uses_complete_global_seed_without_world() -> None: + seed = JointState(name=["arm/joint_a", "arm/joint_b", "arm/joint_c"], position=[1.0, 2.0, 3.0]) + + result = _seed_for_robot_config(_robot_config(), seed) + + assert result.name == ["joint_a", "joint_b", "joint_c"] + assert result.position == [1.0, 2.0, 3.0] + + def test_mapping_failure_for_missing_joint() -> None: config = _robot_config() config.joint_names = ["joint_a", "missing", "joint_c"] @@ -375,6 +395,31 @@ def test_mapping_failure_for_missing_joint() -> None: _build_joint_mapping(_FakeModel(), config) +def test_uncontrolled_urdf_joints_are_locked_out_of_pink_model() -> None: + pinocchio = ModuleType("pinocchio") + model = _FakeModel() + model.names.append("gripper_joint") + model.joints.append(_FakeJoint(3)) + reduced_model = _FakeModel() + seen_locked_joint_ids: list[list[int]] = [] + + def build_reduced_model( + input_model: _FakeModel, locked_joint_ids: list[int], reference: np.ndarray + ) -> _FakeModel: + assert input_model is model + np.testing.assert_allclose(reference, np.zeros(model.nq)) + seen_locked_joint_ids.append(list(locked_joint_ids)) + return reduced_model + + pinocchio.neutral = lambda input_model: np.zeros(input_model.nq) # type: ignore[attr-defined] + pinocchio.buildReducedModel = build_reduced_model # type: ignore[attr-defined] + + result = _lock_uncontrolled_model_joints(pinocchio, model, _robot_config()) + + assert result is reduced_model + assert seen_locked_joint_ids == [[4]] + + def test_solve_single_returns_successful_ik_result() -> None: ik = _pink_ik(converge=True) target = np.eye(4) @@ -546,6 +591,61 @@ def fake_solve_multi_frame(**kwargs: object) -> IKResult: assert result.joint_state.position == [0.1, 0.2, 0.3] +def test_target_in_model_frame_converts_world_pose_through_robot_base() -> None: + ik = _pink_ik(converge=True) + config = _robot_config() + config.base_pose = _pose_stamped(1.0, 2.0, 0.0, yaw=np.pi / 2.0) + target_world = _pose_stamped(0.8, 2.1, 0.3, yaw=np.pi / 2.0) + + target_model = ik._target_in_model_frame(config, target_world) + + np.testing.assert_allclose(target_model[:3, 3], [0.1, 0.2, 0.3], atol=1e-12) + np.testing.assert_allclose(target_model[:3, :3], np.eye(3), atol=1e-12) + + +def test_solve_pose_targets_passes_world_target_to_solver_in_model_frame( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + context = _context() + context.frame_name = "wrist_tool" + context.frame_id = 1 + ik._robot_contexts = {"robot:wrist_tool": context} + world = _FakeWorld(collision_free=True) + world.config.base_pose = _pose_stamped(1.0, 2.0, 0.0, yaw=np.pi / 2.0) + seen_target_models: list[np.ndarray] = [] + + def fake_solve_single(**kwargs: object) -> IKResult: + target_model = cast("np.ndarray", kwargs["target_model"]) + seen_target_models.append(target_model) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3] + ), + position_error=0.0, + orientation_error=0.0, + iterations=1, + ) + + monkeypatch.setattr(ik, "_solve_single", fake_solve_single) + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={world.groups["arm/wrist"]: _pose_stamped(0.8, 2.1, 0.3, yaw=np.pi / 2.0)}, + seed=JointState( + {"name": ["arm/joint_a", "arm/joint_b", "arm/joint_c"], "position": [0.0, 0.0, 0.0]} + ), + check_collision=False, + max_attempts=1, + ) + + assert result.status == IKStatus.SUCCESS + assert len(seen_target_models) == 1 + np.testing.assert_allclose(seen_target_models[0][:3, 3], [0.1, 0.2, 0.3], atol=1e-12) + np.testing.assert_allclose(seen_target_models[0][:3, :3], np.eye(3), atol=1e-12) + + def test_solve_pose_targets_cross_robot_combines_global_joint_names( monkeypatch: pytest.MonkeyPatch, ) -> None: diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 32f32f5818..b7d4fd4b50 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -40,8 +40,9 @@ class RobotModelConfig(ModuleConfig): model_path: Path to robot model file (.urdf, .xacro, or .xml/MJCF) srdf_path: Optional path to SRDF file containing planning group definitions base_pose: Compatibility placement transform used by current Drake - world loading/welding. Prefer encoding new placement in the robot - model when possible. + world loading/welding. This is the canonical world placement for + robot instances; model-authored world/base attach joints are + stripped when strip_model_world_joint is true. joint_names: Ordered list of controllable joints in the local model namespace. This is not a planning group. end_effector_link: Compatibility robot-scoped end-effector link used by @@ -68,6 +69,7 @@ class RobotModelConfig(ModuleConfig): model_path: Path srdf_path: Path | None = None base_pose: PoseStamped = Field(default_factory=PoseStamped) + strip_model_world_joint: bool = False joint_names: list[str] end_effector_link: str | None = None base_link: str = "base_link" diff --git a/dimos/manipulation/planning/utils/mesh_utils.py b/dimos/manipulation/planning/utils/mesh_utils.py index 988a4e5e8e..adb4e8f36f 100644 --- a/dimos/manipulation/planning/utils/mesh_utils.py +++ b/dimos/manipulation/planning/utils/mesh_utils.py @@ -37,6 +37,7 @@ import shutil import tempfile from typing import TYPE_CHECKING +import xml.etree.ElementTree as ET from dimos.utils.logging_config import setup_logger @@ -55,6 +56,7 @@ def prepare_urdf_for_drake( package_paths: dict[str, Path] | None = None, xacro_args: dict[str, str] | None = None, convert_meshes: bool = False, + strip_world_joint_child_link: str | None = None, ) -> str: """Prepare a URDF/xacro file for use with Drake. @@ -68,6 +70,9 @@ def prepare_urdf_for_drake( package_paths: Dict mapping package names to filesystem paths xacro_args: Arguments to pass to xacro processor convert_meshes: Convert DAE/STL meshes to OBJ for Drake compatibility + strip_world_joint_child_link: If set, remove a fixed URDF joint from + world to this child link so callers can apply instance placement via + RobotModelConfig.base_pose instead of model-authored placement. Returns: Path to the prepared URDF file (may be cached) @@ -77,7 +82,9 @@ def prepare_urdf_for_drake( xacro_args = xacro_args or {} # Generate cache key - cache_key = _generate_cache_key(urdf_path, package_paths, xacro_args, convert_meshes) + cache_key = _generate_cache_key( + urdf_path, package_paths, xacro_args, convert_meshes, strip_world_joint_child_link + ) cache_path = _CACHE_DIR / cache_key / urdf_path.stem cache_path.mkdir(parents=True, exist_ok=True) cached_urdf = cache_path / f"{urdf_path.stem}.urdf" @@ -96,6 +103,9 @@ def prepare_urdf_for_drake( # Strip transmission blocks (Drake doesn't need them, and they can cause issues) urdf_content = _strip_transmission_blocks(urdf_content) + if strip_world_joint_child_link is not None: + urdf_content = _strip_fixed_world_joint(urdf_content, strip_world_joint_child_link) + # Resolve package:// URIs urdf_content = _resolve_package_uris(urdf_content, package_paths, cache_path) @@ -115,6 +125,7 @@ def _generate_cache_key( package_paths: dict[str, Path], xacro_args: dict[str, str], convert_meshes: bool, + strip_world_joint_child_link: str | None, ) -> str: """Generate a cache key for the URDF configuration. @@ -125,9 +136,12 @@ def _generate_cache_key( # Version number to invalidate cache when processing logic changes # Increment this when adding new processing steps (e.g., stripping transmission blocks) - processing_version = "v2" + processing_version = "v3" - key_data = f"{processing_version}:{urdf_path}:{mtime}:{sorted(package_paths.items())}:{sorted(xacro_args.items())}:{convert_meshes}" + key_data = ( + f"{processing_version}:{urdf_path}:{mtime}:{sorted(package_paths.items())}:" + f"{sorted(xacro_args.items())}:{convert_meshes}:{strip_world_joint_child_link}" + ) return hashlib.md5(key_data.encode()).hexdigest()[:16] @@ -175,6 +189,44 @@ def _strip_transmission_blocks(urdf_content: str) -> str: return result +def _strip_fixed_world_joint(urdf_content: str, child_link: str) -> str: + """Remove a fixed world-to-base joint so base_pose can own placement.""" + try: + root = ET.fromstring(urdf_content) + except ET.ParseError: + logger.warning("Could not parse URDF while stripping world joint", exc_info=True) + return urdf_content + + removed = False + for joint in list(root.findall("joint")): + if joint.attrib.get("type") != "fixed": + continue + parent = joint.find("parent") + child = joint.find("child") + if parent is None or child is None: + continue + if parent.attrib.get("link") == "world" and child.attrib.get("link") == child_link: + root.remove(joint) + removed = True + + if not removed: + return urdf_content + + referenced_links = set() + for joint in root.findall("joint"): + parent = joint.find("parent") + child = joint.find("child") + if parent is not None: + referenced_links.add(parent.attrib.get("link")) + if child is not None: + referenced_links.add(child.attrib.get("link")) + for link in list(root.findall("link")): + if link.attrib.get("name") == "world" and "world" not in referenced_links: + root.remove(link) + + return ET.tostring(root, encoding="unicode") + + def _resolve_package_uris( urdf_content: str, package_paths: dict[str, Path], diff --git a/dimos/manipulation/planning/utils/test_mesh_utils.py b/dimos/manipulation/planning/utils/test_mesh_utils.py new file mode 100644 index 0000000000..fa3004c7ce --- /dev/null +++ b/dimos/manipulation/planning/utils/test_mesh_utils.py @@ -0,0 +1,47 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import xml.etree.ElementTree as ET + +from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake + + +def test_prepare_urdf_can_strip_fixed_world_joint_for_base_pose_placement( + tmp_path, +) -> None: + urdf_path = tmp_path / "robot.urdf" + urdf_path.write_text( + """ + + + + + + + + + +""".strip() + ) + + prepared_path = prepare_urdf_for_drake( + urdf_path, + strip_world_joint_child_link="link_base", + ) + + root = ET.parse(prepared_path).getroot() + assert [joint.attrib["name"] for joint in root.findall("joint")] == [] + assert [link.attrib["name"] for link in root.findall("link")] == ["link_base"] diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 0c96a947ca..5d70c0024f 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -285,6 +285,9 @@ def _load_model(self, config: RobotModelConfig) -> Any: package_paths=config.package_paths, xacro_args=config.xacro_args, convert_meshes=config.auto_convert_meshes, + strip_world_joint_child_link=config.base_link + if config.strip_model_world_joint + else None, ) prepared_path_obj = Path(prepared_path) diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 5aef347767..a07ff1e5e2 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -964,6 +964,35 @@ def test_evaluate_pose_target_set_uses_whole_set_seed_and_auxiliary_groups(self) assert kwargs["seed"].name == ["left/j1", "left/j2"] assert kwargs["seed"].position == [0.1, 0.2] + def test_evaluate_pose_target_set_can_skip_collision_checking(self): + config = _make_robot_config("left", ["j1"], "task") + group = _make_global_group("left", "arm", ["j1"]) + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups([group]) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1"], position=[0.0] + ) + module._world_monitor.is_state_valid.return_value = False + module._world_monitor.get_group_pose.return_value = PoseStamped( + position=Vector3(x=1.0), orientation=Quaternion() + ) + module._kinematics = MagicMock() + module._kinematics.solve_pose_targets.return_value = IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["left/j1"], position=[1.0]), + message="solved", + ) + + result = module.evaluate_pose_target_set( + {"left/arm": Pose(position=Vector3(x=0.5), orientation=Quaternion())}, + check_collision=False, + ) + + assert result["success"] is True + assert result["collision_free"] is True + assert result["group_diagnostics"] == {"left/arm": "Target collision check skipped"} + module._world_monitor.is_state_valid.assert_not_called() + class TestGeneratedPlanProjection: def test_selected_joint_state_accepts_local_current_state_names(self): diff --git a/dimos/manipulation/visualization/viser/adapter.py b/dimos/manipulation/visualization/viser/adapter.py index 3824df6e31..0dec749d55 100644 --- a/dimos/manipulation/visualization/viser/adapter.py +++ b/dimos/manipulation/visualization/viser/adapter.py @@ -147,9 +147,13 @@ def evaluate_joint_target(self, joints: JointState, robot_name: RobotName) -> Ta ) return result - def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvaluation: + def evaluate_pose_target( + self, pose: Pose, robot_name: RobotName, *, check_collision: bool = True + ) -> TargetEvaluation: """Evaluate a Cartesian target through module/WorldMonitor helper boundaries.""" - result: TargetEvaluation = {**self._module.evaluate_pose_target(pose, robot_name)} + result: TargetEvaluation = { + **self._module.evaluate_pose_target(pose, robot_name, check_collision=check_collision) + } joint_state = result.get("joint_state") result["joint_state"] = copy_joint_state( joint_state if isinstance(joint_state, JointState) else None @@ -181,12 +185,14 @@ def evaluate_pose_target_set( pose_targets: dict[PlanningGroupID, Pose | PoseStamped], auxiliary_groups: Sequence[PlanningGroupID] = (), seed: JointState | None = None, + check_collision: bool = True, ) -> TargetSetEvaluation: result: TargetSetEvaluation = { **self._module.evaluate_pose_target_set( cast("dict[PlanningGroupID | PlanningGroup, Pose | PoseStamped]", pose_targets), auxiliary_groups=auxiliary_groups, seed=copy_joint_state(seed), + check_collision=check_collision, ) } target_joints = result.get("target_joints") diff --git a/dimos/manipulation/visualization/viser/config.py b/dimos/manipulation/visualization/viser/config.py index c7e66e4de1..d77cc66a15 100644 --- a/dimos/manipulation/visualization/viser/config.py +++ b/dimos/manipulation/visualization/viser/config.py @@ -54,6 +54,13 @@ class ViserVisualizationConfig(BaseModel): default=0.02, validation_alias=AliasChoices("current_match_tolerance", "viser_current_match_tolerance"), ) + target_evaluation_check_collision: bool = Field( + default=True, + validation_alias=AliasChoices( + "target_evaluation_check_collision", + "viser_target_evaluation_check_collision", + ), + ) allow_plan_execute: bool = False @property diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index f227536fa5..17a8551053 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -262,13 +262,17 @@ def _ensure_scene_controls(self) -> None: continue if group_id in self.state.auxiliary_group_ids: continue + handle_key = f"ee_control:{group_id}" + handle_exists = handle_key in self._handles ee_control = self.scene.ensure_target_controls( group_id, lambda target, gid=group_id: self._on_transform_update(gid, target), ) if ee_control is not None: - self._handles[f"ee_control:{group_id}"] = ee_control - pose = self.state.group_poses.get(group_id) or self.state.pose_targets.get(group_id) + self._handles[handle_key] = ee_control + if handle_exists: + continue + pose = self.state.pose_targets.get(group_id) if pose is not None: self._suppress_target_callbacks = True try: @@ -653,6 +657,7 @@ def _on_transform_update( group_ids=self.state.selected_group_ids, auxiliary_group_ids=self.state.auxiliary_group_ids, pose_targets=dict(self.state.pose_targets), + check_collision=self.config.target_evaluation_check_collision, ) ) self.refresh() @@ -706,6 +711,7 @@ def _handle_target_evaluation_request( request.pose_targets, auxiliary_groups=request.auxiliary_group_ids, seed=self.state.last_valid_target_joints, + check_collision=request.check_collision, ) if not request.joint_targets: return {"success": False, "status": "INVALID", "message": "No joint target"} @@ -738,6 +744,8 @@ def _apply_target_evaluation_result( for group_id, pose in group_poses.items() if isinstance(pose, Pose) } + if request.source == "joints" and success and collision_free: + self._sync_pose_targets_from_group_poses() group_diagnostics = result.get("group_diagnostics", {}) if isinstance(group_diagnostics, dict): self.state.group_diagnostics = { @@ -760,6 +768,34 @@ def _sync_controls_from_targets(self) -> None: # programmatic pose writes from delayed IK results can fight fast user # dragging and make the gizmo jump back. + def _sync_pose_targets_from_group_poses(self) -> None: + groups = self._group_info_by_id() + updated_group_ids: list[PlanningGroupID] = [] + for group_id, pose in self.state.group_poses.items(): + group = groups.get(group_id) + if group is None or not bool(group["has_pose_target"]): + continue + if group_id in self.state.auxiliary_group_ids: + continue + self.state.pose_targets[group_id] = pose + updated_group_ids.append(group_id) + first_group_id = next(iter(self.state.selected_group_ids), None) + if first_group_id is not None: + self.state.cartesian_target = self.state.pose_targets.get(first_group_id) + self._sync_scene_target_pose_controls(updated_group_ids) + + def _sync_scene_target_pose_controls(self, group_ids: list[PlanningGroupID]) -> None: + if self.scene is None: + return + self._suppress_target_callbacks = True + try: + for group_id in group_ids: + pose = self.state.pose_targets.get(group_id) + if pose is not None: + self.scene.set_target_pose(group_id, pose) + finally: + self._suppress_target_callbacks = False + def _update_status_text(self) -> None: current = self.state.current_joints status = [ diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index 251dac6126..fdfcc7fa56 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -31,6 +31,7 @@ try: from viser import ( + FrameHandle, GridHandle, MeshHandle, TransformControlsEvent, @@ -69,7 +70,7 @@ REFERENCE_GRID_CELL_COLOR = (44, 54, 58) REFERENCE_GRID_SECTION_COLOR = (90, 145, 165) -SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle +SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle | FrameHandle class _ColorHandle(Protocol): @@ -88,6 +89,7 @@ def __init__( self._configs_by_id: dict[str, RobotModelConfig] = {} self._urdfs: dict[str, ViserUrdf] = {} self._handles: dict[str, TransformControlsHandle] = {} + self._root_frames: dict[str, FrameHandle] = {} self._grid_handle: GridHandle | None = None self._grid_visible = True self._preview_visible: dict[str, bool] = {} @@ -255,7 +257,10 @@ def close(self) -> None: self._grid_handle = None for urdf in self._urdfs.values(): self._remove_scene_handle(urdf) + for frame in self._root_frames.values(): + self._remove_scene_handle(frame) self._urdfs.clear() + self._root_frames.clear() self._configs_by_id.clear() self._preview_visible.clear() self._target_tracks_current.clear() @@ -267,11 +272,7 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: key = f"{robot_id}:{kind}" if key in self._urdfs: continue - root_node_name = { - "current": f"/robots/{robot_id}/current", - "target": f"/targets/{robot_id}/target", - "preview": f"/previews/{robot_id}/ghost", - }[kind] + root_node_name = self._urdf_root_node_name(robot_id, kind, config) mesh_color_override = { "current": None, "target": GOAL_ROBOT_MESH_COLOR, @@ -304,6 +305,64 @@ def prepared_urdf_path(self, config: RobotModelConfig) -> Path: package_paths=package_paths, xacro_args={str(key): str(value) for key, value in config.xacro_args.items()}, convert_meshes=bool(config.auto_convert_meshes), + strip_world_joint_child_link=str(config.base_link) + if bool(getattr(config, "strip_model_world_joint", False)) + else None, + ) + ) + + def _urdf_root_node_name(self, robot_id: str, kind: str, config: RobotModelConfig) -> str: + root_node_name = { + "current": f"/robots/{robot_id}/current", + "target": f"/targets/{robot_id}/target", + "preview": f"/previews/{robot_id}/ghost", + }[kind] + if not self._has_non_identity_base_pose(config): + return root_node_name + self._ensure_base_pose_frame(robot_id, kind, config) + return f"{root_node_name}/base_pose/urdf" + + def _ensure_base_pose_frame(self, robot_id: str, kind: str, config: RobotModelConfig) -> None: + key = f"{robot_id}:{kind}:base_pose" + if key in self._root_frames: + return + pose = config.base_pose + frame_name = { + "current": f"/robots/{robot_id}/current/base_pose", + "target": f"/targets/{robot_id}/target/base_pose", + "preview": f"/previews/{robot_id}/ghost/base_pose", + }[kind] + self._root_frames[key] = self.server.scene.add_frame( + frame_name, + show_axes=False, + position=( + float(pose.position.x), + float(pose.position.y), + float(pose.position.z), + ), + wxyz=( + float(pose.orientation.w), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + ), + ) + + @staticmethod + def _has_non_identity_base_pose(config: RobotModelConfig) -> bool: + pose = getattr(config, "base_pose", None) + if pose is None: + return False + return any( + abs(value) > 1e-12 + for value in ( + float(pose.position.x), + float(pose.position.y), + float(pose.position.z), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + float(pose.orientation.w) - 1.0, ) ) diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index bd28962b2d..f16e490ffb 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -226,6 +226,7 @@ class TargetEvaluationRequest: pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) joints: JointState | None = None joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) + check_collision: bool = True class TargetEvaluationWorker: diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index cedd3954b3..4b0f0872ed 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -47,6 +47,9 @@ ) from dimos.manipulation.visualization.viser.theme import _dimos_logo_data_url, apply_dimos_theme 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 GuiCallback = Callable[[SimpleNamespace], None] @@ -167,7 +170,7 @@ def __init__(self) -> None: self.visible: object | None = None self.removed = False self.name = "" - self.kwargs: dict[str, float | bool] = {} + self.kwargs: dict[str, object] = {} def remove(self) -> None: self.removed = True @@ -207,6 +210,8 @@ class FakeServer: def __init__(self) -> None: self.scene = SimpleNamespace() self.scene.add_transform_controls = self.add_transform_controls + self.scene.add_frame = self.add_frame + self.frames = [] def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHandle: handle = FakeTransformHandle() @@ -214,6 +219,13 @@ def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHan handle.scale = scale return handle + def add_frame(self, name: str, **kwargs: object) -> FakeHandle: + handle = FakeHandle() + handle.name = name + handle.kwargs = kwargs + self.frames.append(handle) + return handle + class FakeGridServer(FakeServer): def __init__(self) -> None: @@ -224,7 +236,7 @@ def __init__(self) -> None: def add_grid(self, name: str, **kwargs: float | bool) -> FakeHandle: handle = FakeHandle() handle.name = name - handle.kwargs = kwargs + handle.kwargs = dict(kwargs) handle.visible = kwargs.get("visible") self.grids.append(handle) return handle @@ -439,7 +451,9 @@ def evaluate_joint_target(self, joints: JointState | None, robot_name: str) -> T "joint_state": joints, } - def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluation: + def evaluate_pose_target( + self, _pose: Pose, _robot_name: str, *, check_collision: bool = True + ) -> TargetEvaluation: return { "success": False, "joint_state": None, @@ -448,6 +462,26 @@ def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluatio "collision_free": False, } + def evaluate_pose_target_set( + self, + pose_targets: dict[str, Pose], + auxiliary_groups: Sequence[str] = (), + seed: JointState | None = None, + check_collision: bool = True, + ) -> TargetSetEvaluation: + target = JointState({"name": ["arm/j1", "arm/j2"], "position": [0.1, 0.2]}) + return { + "success": True, + "status": "FEASIBLE", + "message": "Target collision check skipped" + if not check_collision + else "Target is collision-free", + "collision_free": True, + "target_joints": target, + "group_ids": tuple(pose_targets) + tuple(auxiliary_groups), + "group_poses": dict(pose_targets), + } + def evaluate_joint_target_set( self, joint_targets: dict[str, JointState] ) -> TargetSetEvaluation: @@ -823,6 +857,48 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N assert preview.cfg is None +def test_scene_parents_urdfs_under_base_pose_frame() -> None: + server = FakeServer() + root_node_names: list[str] = [] + + def make_urdf(*_: object, **kwargs: object) -> FakeViserUrdfWithMeshes: + root_node_names.append(str(kwargs["root_node_name"])) + return FakeViserUrdfWithMeshes(("joint1",)) + + scene = ViserManipulationScene(server, make_urdf, preview_fps=10.0) + scene.prepared_urdf_path = lambda config: "dummy.urdf" + config = SimpleNamespace( + name="arm", + model_path="/tmp/arm.urdf", + package_paths={}, + xacro_args={}, + auto_convert_meshes=False, + joint_names=["joint1"], + base_pose=PoseStamped( + position=Vector3(1.0, 2.0, 3.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + ) + + scene.register_robot("robot1", config) + + assert [frame.name for frame in server.frames] == [ + "/robots/robot1/current/base_pose", + "/targets/robot1/target/base_pose", + "/previews/robot1/ghost/base_pose", + ] + assert [frame.kwargs["position"] for frame in server.frames] == [ + (1.0, 2.0, 3.0), + (1.0, 2.0, 3.0), + (1.0, 2.0, 3.0), + ] + assert root_node_names == [ + "/robots/robot1/current/base_pose/urdf", + "/targets/robot1/target/base_pose/urdf", + "/previews/robot1/ghost/base_pose/urdf", + ] + + def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback() -> None: server = FakeServer() urdfs = [FakeViserUrdfWithMeshes(("joint1",)) for _ in range(3)] @@ -1346,12 +1422,14 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( assert gui.state.target_joints is not None assert list(gui.state.target_joints.position) == [0.25, 0.75] + joint_bar_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) gui._apply_target_evaluation_result( fresh_request, { "success": True, "collision_free": True, "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: joint_bar_pose}, }, ) assert gui.state.target_status == TargetStatus.FEASIBLE @@ -1360,6 +1438,7 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( assert list(gui.state.target_joints.position) == [1.0, 2.0] assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.75] assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) + assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, joint_bar_pose) def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( @@ -1379,15 +1458,18 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( target_pose_updates = [] scene = SimpleNamespace( has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: None, + ensure_target_controls=lambda *args: object(), set_target_joints=lambda *args: target_joint_updates.append(args) or True, set_target_pose=lambda *args: target_pose_updates.append(args), set_target_visual_state=lambda *args: None, ) gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - gui.state.cartesian_target = Pose( - {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} - ) + gui._handles[f"ee_control:{DEFAULT_GROUP_ID}"] = object() + dragged_pose = Pose({"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]}) + solved_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) + gui.state.cartesian_target = dragged_pose + gui.state.pose_targets[DEFAULT_GROUP_ID] = dragged_pose + target_pose_updates.clear() request = TargetEvaluationRequest( sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) ) @@ -1399,6 +1481,7 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( "success": True, "collision_free": True, "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: solved_pose}, }, ) @@ -1406,6 +1489,55 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) assert target_pose_updates == [] + assert gui.state.pose_targets[DEFAULT_GROUP_ID] is dragged_pose + assert gui.state.group_poses[DEFAULT_GROUP_ID] is solved_pose + + +def test_gui_can_disable_collision_check_for_cartesian_target_evaluation( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) + config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.0, 0.0]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: False, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: None, + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + gui = make_panel( + FakeGuiServer(), + adapter, + ViserVisualizationConfig(panel_enabled=True, target_evaluation_check_collision=False), + scene, + ) + request = TargetEvaluationRequest( + sequence_id=1, + source="cartesian", + group_ids=(DEFAULT_GROUP_ID,), + pose_targets={ + DEFAULT_GROUP_ID: Pose( + {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} + ) + }, + check_collision=False, + ) + + result = gui._handle_target_evaluation_request(request) + + assert result["success"] is True + assert result["collision_free"] is True + assert result["message"] == "Target collision check skipped" def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( diff --git a/dimos/robot/catalog/test_ufactory.py b/dimos/robot/catalog/test_ufactory.py new file mode 100644 index 0000000000..3875ff9c92 --- /dev/null +++ b/dimos/robot/catalog/test_ufactory.py @@ -0,0 +1,40 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Callable +import math + +import pytest + +from dimos.robot.catalog.ufactory import xarm6, xarm7 +from dimos.robot.config import RobotConfig + + +@pytest.mark.parametrize("factory", [xarm6, xarm7]) +def test_xarm_instance_offsets_are_encoded_only_in_base_pose( + factory: Callable[..., RobotConfig], +) -> None: + config = factory(name="arm", y_offset=0.5, pitch=0.25) + model_config = config.to_robot_model_config() + + assert model_config.xacro_args["attach_xyz"] == "0 0 0" + assert model_config.xacro_args["attach_rpy"] == "0 0 0" + assert model_config.base_pose.position.y == 0.5 + assert model_config.base_pose.orientation.x == 0.0 + assert model_config.base_pose.orientation.y == pytest.approx(math.sin(0.125)) + assert model_config.base_pose.orientation.z == 0.0 + assert model_config.base_pose.orientation.w == pytest.approx(math.cos(0.125)) + assert model_config.strip_model_world_joint is True diff --git a/dimos/robot/catalog/ufactory.py b/dimos/robot/catalog/ufactory.py index ddf93185dc..9c21585d2a 100644 --- a/dimos/robot/catalog/ufactory.py +++ b/dimos/robot/catalog/ufactory.py @@ -16,6 +16,7 @@ from __future__ import annotations +import math from typing import Any from dimos.robot.config import GripperConfig, RobotConfig @@ -51,6 +52,13 @@ ] +def _base_pose_from_offsets( + x_offset: float, y_offset: float, z_offset: float, pitch: float +) -> list[float]: + half_pitch = pitch / 2.0 + return [x_offset, y_offset, z_offset, 0.0, math.sin(half_pitch), 0.0, math.cos(half_pitch)] + + def xarm7( name: str = "arm", *, @@ -68,8 +76,8 @@ def xarm7( xacro_args: dict[str, str] = { "dof": "7", "limited": "true", - "attach_xyz": f"{x_offset} {y_offset} {z_offset}", - "attach_rpy": f"0 {pitch} 0", + "attach_xyz": "0 0 0", + "attach_rpy": "0 0 0", } if add_gripper: xacro_args["add_gripper"] = "true" @@ -83,7 +91,8 @@ def xarm7( "joint_names": [f"joint{i}" for i in range(1, 8)], "base_link": "link_base", "home_joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - "base_pose": [x_offset, y_offset, z_offset, 0, 0, 0, 1], + "base_pose": _base_pose_from_offsets(x_offset, y_offset, z_offset, pitch), + "strip_model_world_joint": True, "package_paths": {"xarm_description": LfsPath("xarm_description")}, "xacro_args": xacro_args, "auto_convert_meshes": True, @@ -120,8 +129,8 @@ def xarm6( xacro_args: dict[str, str] = { "dof": "6", "limited": "true", - "attach_xyz": f"{x_offset} {y_offset} {z_offset}", - "attach_rpy": f"0 {pitch} 0", + "attach_xyz": "0 0 0", + "attach_rpy": "0 0 0", } if add_gripper: xacro_args["add_gripper"] = "true" @@ -135,7 +144,8 @@ def xarm6( "joint_names": [f"joint{i}" for i in range(1, 7)], "base_link": "link_base", "home_joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - "base_pose": [x_offset, y_offset, z_offset, 0, 0, 0, 1], + "base_pose": _base_pose_from_offsets(x_offset, y_offset, z_offset, pitch), + "strip_model_world_joint": True, "package_paths": {"xarm_description": LfsPath("xarm_description")}, "xacro_args": xacro_args, "auto_convert_meshes": True, diff --git a/dimos/robot/config.py b/dimos/robot/config.py index e57859d9ea..ca73cb384f 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -83,8 +83,10 @@ class RobotConfig(BaseModel): ) home_joints: list[float] | None = None - # Compatibility planning placement. Prefer encoding placement in URDF/xacro/MJCF. + # Canonical planning placement. Robot models should describe intrinsic geometry; + # instance placement belongs here. base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + strip_model_world_joint: bool = False # Planning max_velocity: float = 1.0 @@ -215,6 +217,7 @@ def to_robot_model_config(self) -> RobotModelConfig: model_path=self.model_path, srdf_path=self.srdf_path, base_pose=base_pose, + strip_model_world_joint=self.strip_model_world_joint, joint_names=joint_names, end_effector_link=legacy_end_effector_link, base_link=base_link, From 15ba00f6ff7b20fd9b20612473ac505bad12e5d2 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 20 Jun 2026 11:48:31 -0700 Subject: [PATCH 23/27] fix: polish viser planning group panel --- dimos/manipulation/visualization/viser/gui.py | 369 +++++++++++------- .../manipulation/visualization/viser/scene.py | 17 +- .../viser/test_viser_visualization.py | 149 ++++++- 3 files changed, 377 insertions(+), 158 deletions(-) diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 17a8551053..1c690f3fec 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -19,7 +19,6 @@ from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.manipulation.visualization.types import ( PlanningGroupInfo, - RobotInfo, TargetEvaluation, TargetSetEvaluation, ) @@ -140,18 +139,22 @@ def refresh(self) -> None: BackendConnectionStatus.READY if robots else BackendConnectionStatus.WAITING_FOR_ROBOT ) if not self.state.selected_group_ids and groups and not self._default_group_initialized: - self.state.selected_group_ids = (str(groups[0]["id"]),) - self.state.selected_robot = str(groups[0]["robot_name"]) + first_pose_group = next( + (group for group in groups if bool(group["has_pose_target"])), groups[0] + ) + self.state.selected_group_ids = (str(first_pose_group["id"]),) self.state.target_status = TargetStatus.EMPTY self._default_group_initialized = True + self._sync_group_selection_state() self._initialize_selected_group_targets() self._build_joint_sliders() - self._sync_robot_dropdown(robots) - self._sync_group_checkboxes(groups) + self._sync_group_selector(groups) self._refresh_selected_robot_state() self._ensure_scene_controls() + self._sync_target_ghost_visibility() self._sync_preset_dropdown() self._update_status_text() + self._update_target_summary() self._update_control_state() def _build(self) -> None: @@ -162,41 +165,46 @@ def _build(self) -> None: self._build_panel_controls(gui) def _build_panel_controls(self, gui: GuiApi) -> None: - self._handles["status"] = gui.add_markdown("Starting manipulation panel...") - robots = self.adapter.list_robots() + self._handles["status"] = gui.add_markdown("**Status:** Ready") self._build_scene_controls(gui) - robot_dropdown = gui.add_dropdown( - "Robot", - options=robots or [""], - initial_value=robots[0] if robots else "", + self._handles["planning_groups_heading"] = gui.add_markdown( + "### Planning Groups\nChoose active target groups." ) - robot_dropdown.on_update(lambda event: self._select_robot(event.target.value)) - self._handles["robot"] = robot_dropdown - select_all_button = gui.add_button("Select all manipulators") + self._sync_group_selector(self.adapter.list_planning_groups()) + select_all_button = gui.add_button("Select all") select_all_button.on_click(lambda _: self._select_all_manipulators()) self._handles["select_all_manipulators"] = select_all_button + clear_selection_button = gui.add_button("Clear selection") + clear_selection_button.on_click(lambda _: self._clear_group_selection()) + self._handles["clear_group_selection"] = clear_selection_button + self._handles["target_heading"] = gui.add_markdown("### Target") preset_dropdown = gui.add_dropdown( - "Target Preset", + "Preset", options=["Select preset...", "Current"], initial_value="Select preset...", ) preset_dropdown.on_update(lambda event: self._apply_preset(event.target.value)) self._handles["preset"] = preset_dropdown + self._handles["target_summary"] = gui.add_markdown("Select a group to define a target.") + self._handles["actions_heading"] = gui.add_markdown("### Actions") plan_button = gui.add_button("Plan", disabled=True) plan_button.on_click(lambda _: self._submit_plan()) self._handles["plan"] = plan_button - preview_button = gui.add_button("Preview", disabled=True) - preview_button.on_click(lambda _: self._submit_preview()) - self._handles["preview"] = preview_button - execute_button = gui.add_button("Execute", disabled=True) - execute_button.on_click(lambda _: self._submit_execute()) - self._handles["execute"] = execute_button - cancel_button = gui.add_button("Cancel") - cancel_button.on_click(lambda _: self._submit_cancel()) - self._handles["cancel"] = cancel_button - clear_button = gui.add_button("Clear plan") - clear_button.on_click(lambda _: self._submit_clear()) - self._handles["clear"] = clear_button + more_actions = gui.add_folder("Plan controls", expand_by_default=False) + self._handles["actions_folder"] = more_actions + with more_actions: + preview_button = gui.add_button("Preview", disabled=True) + preview_button.on_click(lambda _: self._submit_preview()) + self._handles["preview"] = preview_button + execute_button = gui.add_button("Execute", disabled=True) + execute_button.on_click(lambda _: self._submit_execute()) + self._handles["execute"] = execute_button + cancel_button = gui.add_button("Cancel") + cancel_button.on_click(lambda _: self._submit_cancel()) + self._handles["cancel"] = cancel_button + clear_button = gui.add_button("Clear plan") + clear_button.on_click(lambda _: self._submit_clear()) + self._handles["clear"] = clear_button self._build_joint_sliders() def _build_scene_controls(self, gui: GuiApi) -> None: @@ -236,14 +244,7 @@ def _ensure_scene_controls(self) -> None: if self.scene is None: return groups = self._group_info_by_id() - selected = set(self.state.selected_group_ids) - active_pose_groups = { - group_id - for group_id in selected - if (group := groups.get(group_id)) is not None - and bool(group["has_pose_target"]) - and group_id not in self.state.auxiliary_group_ids - } + active_pose_groups = set(self._selected_pose_group_ids()) for key in [key for key in self._handles if key.startswith("ee_control:")]: group_id = key.split(":", 1)[1] if group_id in active_pose_groups: @@ -256,22 +257,19 @@ def _ensure_scene_controls(self) -> None: remove = getattr(handle, "remove", None) if callable(remove): remove() - for group_id in selected: + for group_id in active_pose_groups: group = groups.get(group_id) if group is None or not bool(group["has_pose_target"]): continue - if group_id in self.state.auxiliary_group_ids: - continue handle_key = f"ee_control:{group_id}" - handle_exists = handle_key in self._handles + if handle_key in self._handles: + continue ee_control = self.scene.ensure_target_controls( group_id, lambda target, gid=group_id: self._on_transform_update(gid, target), ) if ee_control is not None: self._handles[handle_key] = ee_control - if handle_exists: - continue pose = self.state.pose_targets.get(group_id) if pose is not None: self._suppress_target_callbacks = True @@ -281,10 +279,10 @@ def _ensure_scene_controls(self) -> None: self._suppress_target_callbacks = False def _build_joint_sliders(self) -> None: - if not self.state.selected_group_ids: - return gui = self.server.gui self._clear_joint_sliders() + if not self.state.selected_group_ids: + return groups = self._group_info_by_id() target_by_name: dict[str, float] = {} if self.state.target_joints is not None: @@ -378,59 +376,19 @@ def _remove_panel_handles(self) -> None: remove() self._handles.pop(key, None) - def _select_robot(self, robot_name: str) -> None: - if self._closed: - return - if (robot_name or None) == self.state.selected_robot: - self.refresh() - return - self.state.selected_robot = robot_name or None - groups = [ - group - for group in self.adapter.list_planning_groups() - if str(group["robot_name"]) == self.state.selected_robot - ] - self.state.selected_group_ids = (str(groups[0]["id"]),) if groups else () - self.state.auxiliary_group_ids = () - self.state.group_joint_targets.clear() - self.state.pose_targets.clear() - self.state.target_status = TargetStatus.EMPTY - self.state.feasibility.status = FeasibilityStatus.UNKNOWN - self.state.plan_state = PanelPlanState() - self._initialize_selected_group_targets() - self._build_joint_sliders() - self._sync_preset_dropdown() - self.refresh() - - def _sync_robot_dropdown(self, robots: list[str]) -> None: - handle = self._handles.get("robot") - if handle is None: - return - options = robots or [""] - for attr in ("options", "values"): - if hasattr(handle, attr): - try: - self._set_optional_handle_attr(handle, attr, options) - except Exception: - logger.warning("Could not set robot dropdown %s", attr, exc_info=True) - if hasattr(handle, "value") and self.state.selected_robot in robots: - try: - self._set_optional_handle_attr(handle, "value", self.state.selected_robot) - except Exception: - logger.warning("Could not set robot dropdown value", exc_info=True) - - def _sync_group_checkboxes(self, groups: list[PlanningGroupInfo]) -> None: + def _sync_group_selector(self, groups: list[PlanningGroupInfo]) -> None: seen_keys: set[str] = set() selected = set(self.state.selected_group_ids) - for group in groups: + for group in sorted( + groups, key=lambda item: (not bool(item["has_pose_target"]), str(item["id"])) + ): group_id = str(group["id"]) key = f"group:{group_id}" seen_keys.add(key) handle = self._handles.get(key) + label = self._group_selector_label(group) if handle is None: - handle = self.server.gui.add_checkbox( - f"Group {group_id}", initial_value=group_id in selected - ) + handle = self.server.gui.add_checkbox(label, initial_value=group_id in selected) handle.on_update( lambda event, gid=group_id: self._set_group_selected( gid, bool(event.target.value) @@ -447,6 +405,17 @@ def _sync_group_checkboxes(self, groups: list[PlanningGroupInfo]) -> None: if callable(remove): remove() + @staticmethod + def _group_selector_label(group: PlanningGroupInfo) -> str: + role = "Pose" if bool(group["has_pose_target"]) else "Aux" + return f"{role}: {ViserPanelGui._group_display_name(group)}" + + @staticmethod + def _group_display_name(group: PlanningGroupInfo) -> str: + robot_name = str(group["robot_name"]) + group_name = str(group["name"]) + return robot_name if group_name == "manipulator" else f"{robot_name} {group_name}" + def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None: current = list(self.state.selected_group_ids) if selected and group_id not in current: @@ -454,10 +423,8 @@ def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None elif not selected and group_id in current: current.remove(group_id) self.state.selected_group_ids = tuple(current) - self.state.auxiliary_group_ids = tuple( - group_id for group_id in self.state.auxiliary_group_ids if group_id in current - ) - self._sync_selected_robot_from_groups() + self._sync_group_selection_state() + self._prune_inactive_group_state() self._initialize_selected_group_targets() self.state.mark_plan_stale() self._build_joint_sliders() @@ -471,11 +438,23 @@ def _select_all_manipulators(self) -> None: self.state.selected_group_ids = tuple( manipulator_groups or [str(group["id"]) for group in groups] ) - self._sync_selected_robot_from_groups() + self._sync_group_selection_state() self._initialize_selected_group_targets() self._build_joint_sliders() self.refresh() + def _clear_group_selection(self) -> None: + if self._closed: + return + self.state.selected_group_ids = () + self._sync_group_selection_state() + self._prune_inactive_group_state() + self.state.target_status = TargetStatus.EMPTY + self.state.feasibility.status = FeasibilityStatus.UNKNOWN + self.state.plan_state = PanelPlanState() + self._build_joint_sliders() + self.refresh() + def _group_info_by_id(self) -> dict[PlanningGroupID, PlanningGroupInfo]: return {str(group["id"]): group for group in self.adapter.list_planning_groups()} @@ -486,6 +465,45 @@ def _sync_selected_robot_from_groups(self) -> None: ) self.state.selected_robot = None if first_group is None else str(first_group["robot_name"]) + def _sync_group_selection_state(self) -> None: + self._sync_selected_robot_from_groups() + self.state.auxiliary_group_ids = self._selected_auxiliary_group_ids() + + def _selected_pose_group_ids(self) -> tuple[PlanningGroupID, ...]: + groups = self._group_info_by_id() + return tuple( + group_id + for group_id in self.state.selected_group_ids + if (group := groups.get(group_id)) is not None and bool(group["has_pose_target"]) + ) + + def _selected_auxiliary_group_ids(self) -> tuple[PlanningGroupID, ...]: + groups = self._group_info_by_id() + return tuple( + group_id + for group_id in self.state.selected_group_ids + if (group := groups.get(group_id)) is not None and not bool(group["has_pose_target"]) + ) + + def _active_pose_targets(self) -> dict[PlanningGroupID, Pose]: + return { + group_id: self.state.pose_targets[group_id] + for group_id in self._selected_pose_group_ids() + if group_id in self.state.pose_targets + } + + def _prune_inactive_group_state(self) -> None: + selected = set(self.state.selected_group_ids) + for mapping in ( + self.state.pose_targets, + self.state.group_joint_targets, + self.state.group_poses, + self.state.group_diagnostics, + ): + for group_id in [group_id for group_id in mapping if group_id not in selected]: + mapping.pop(group_id, None) + self._refresh_target_joints_from_groups() + def _initialize_selected_group_targets(self) -> None: groups = self._group_info_by_id() for group_id in self.state.selected_group_ids: @@ -545,18 +563,21 @@ def _current_snapshot_by_group(self) -> dict[PlanningGroupID, list[float]]: def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") - if handle is None or self.state.selected_robot is None: + if handle is None: return - info: RobotInfo | None = self.adapter.get_robot_info(self.state.selected_robot) - config = self.adapter.get_robot_config(self.state.selected_robot) + selected_robot_names = self._selected_robot_names() options = ["Select preset..."] - if (info is not None and info["init_joints"] is not None) or self.adapter.get_init_joints( - self.state.selected_robot - ) is not None: + if any( + self.adapter.get_init_joints(robot_name) is not None + for robot_name in selected_robot_names + ): options.append("Init") options.append("Current") - home_joints = config.home_joints if config is not None else None - if (info is not None and info["home_joints"] is not None) or home_joints is not None: + if any( + (config := self.adapter.get_robot_config(robot_name)) is not None + and config.home_joints is not None + for robot_name in selected_robot_names + ): options.append("Home") for attr in ("options", "values"): if hasattr(handle, attr): @@ -568,35 +589,16 @@ def _sync_preset_dropdown(self) -> None: def _apply_preset(self, preset: str) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return - config = self.adapter.get_robot_config(robot_name) - if config is None: - return - if preset == "Current": - current = self.adapter.get_current_joint_state(robot_name) - values_by_name = ( - dict(zip(current.name, current.position, strict=False)) - if current is not None - else {} - ) - elif preset == "Init": - init = self.adapter.get_init_joints(robot_name) - values_by_name = ( - dict(zip(init.name, init.position, strict=False)) if init is not None else {} - ) - elif preset == "Home": - values_by_name = dict(zip(config.joint_names, config.home_joints or [], strict=False)) - else: + if preset not in {"Current", "Init", "Home"}: return groups = [ group for group in self.adapter.list_planning_groups() if group["id"] in self.state.selected_group_ids - and str(group["robot_name"]) == robot_name ] for group in groups: + robot_name = str(group["robot_name"]) + values_by_name = self._preset_values_by_name(preset, robot_name) global_names = [str(name) for name in group["joint_names"]] local_names = [str(name) for name in group["local_joint_names"]] values = [ @@ -608,6 +610,43 @@ def _apply_preset(self, preset: str) -> None: self._submit_joint_target_evaluation() self.refresh() + def _selected_robot_names(self) -> tuple[str, ...]: + groups = self._group_info_by_id() + names: list[str] = [] + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + robot_name = str(group["robot_name"]) + if robot_name not in names: + names.append(robot_name) + return tuple(names) + + def _preset_values_by_name(self, preset: str, robot_name: str) -> dict[str, float]: + if preset == "Current": + current = self.adapter.get_current_joint_state(robot_name) + if current is None: + return {} + return { + str(name): float(value) + for name, value in zip(current.name, current.position, strict=False) + } + if preset == "Init": + init = self.adapter.get_init_joints(robot_name) + if init is None: + return {} + return { + str(name): float(value) + for name, value in zip(init.name, init.position, strict=False) + } + config = self.adapter.get_robot_config(robot_name) + if config is None: + return {} + return { + str(name): float(value) + for name, value in zip(config.joint_names, config.home_joints or [], strict=False) + } + def _set_slider_values(self, joint_names: list[str], values: list[float]) -> None: self._suppress_target_callbacks = True try: @@ -655,8 +694,8 @@ def _on_transform_update( sequence_id=sequence_id, source="cartesian", group_ids=self.state.selected_group_ids, - auxiliary_group_ids=self.state.auxiliary_group_ids, - pose_targets=dict(self.state.pose_targets), + auxiliary_group_ids=self._selected_auxiliary_group_ids(), + pose_targets=self._active_pose_targets(), check_collision=self.config.target_evaluation_check_collision, ) ) @@ -701,6 +740,24 @@ def _move_joint_target_visuals(self) -> None: ] self.scene.set_target_joints(str(robot_id), group["local_joint_names"], joints) + def _sync_target_ghost_visibility(self) -> None: + if self.scene is None: + return + active_robot_ids: set[str] = set() + groups = self._group_info_by_id() + for group_id in self._selected_pose_group_ids(): + group = groups.get(group_id) + if group is None: + continue + robot_id = self.adapter.robot_id_for_name(str(group["robot_name"])) + if robot_id is not None: + active_robot_ids.add(str(robot_id)) + set_target_active = getattr(self.scene, "set_target_active", None) + if not callable(set_target_active): + return + for _robot_name, robot_id, _config in self.adapter.robot_items(): + set_target_active(str(robot_id), str(robot_id) in active_robot_ids) + def _handle_target_evaluation_request( self, request: TargetEvaluationRequest ) -> TargetEvaluation | TargetSetEvaluation: @@ -775,11 +832,11 @@ def _sync_pose_targets_from_group_poses(self) -> None: group = groups.get(group_id) if group is None or not bool(group["has_pose_target"]): continue - if group_id in self.state.auxiliary_group_ids: + if group_id not in self._selected_pose_group_ids(): continue self.state.pose_targets[group_id] = pose updated_group_ids.append(group_id) - first_group_id = next(iter(self.state.selected_group_ids), None) + first_group_id = next(iter(self._selected_pose_group_ids()), None) if first_group_id is not None: self.state.cartesian_target = self.state.pose_targets.get(first_group_id) self._sync_scene_target_pose_controls(updated_group_ids) @@ -798,15 +855,10 @@ def _sync_scene_target_pose_controls(self, group_ids: list[PlanningGroupID]) -> def _update_status_text(self) -> None: current = self.state.current_joints + status_label = self.state.error or self.state.module_state status = [ - "### Manipulation Panel", - f"Robot: `{self.state.selected_robot or 'none'}`", - f"Module: `{self.state.module_state}`", - f"Backend: `{self.state.backend_status.value}`", - f"Target: `{self.state.target_status.value}`", - f"Feasibility: `{self.state.feasibility.status.value}`", - f"Plan: `{self.state.plan_state.status.value}`", - f"Action: `{self.state.action_status.value}`", + f"**Status:** {status_label}", + f"Target: `{self.state.target_status.value}` · Plan: `{self.state.plan_state.status.value}`", ] if self.state.selected_robot is not None: status.append( @@ -816,10 +868,35 @@ def _update_status_text(self) -> None: status.append(f"Current joints: `{[round(v, 3) for v in current]}`") if self.state.last_result: status.append(f"Last result: `{self.state.last_result}`") - if self.state.error: - status.append(f"Error: `{self.state.error}`") self._set_handle_value("status", "\n\n".join(status)) + def _update_target_summary(self) -> None: + primary_groups = self._selected_pose_group_ids() + auxiliary_groups = self._selected_auxiliary_group_ids() + ghost_groups = list(primary_groups) + lines = [ + f"Primary: `{self._summary_group_names(primary_groups)}`", + f"Auxiliary: `{self._summary_group_names(auxiliary_groups)}`", + f"Ghosts: `{self._summary_group_names(tuple(ghost_groups))}`", + f"Feasibility: `{self.state.feasibility.status.value}`", + ] + if not self.state.selected_group_ids: + lines = ["Select a planning group to define a target."] + elif not primary_groups and auxiliary_groups: + lines.append("Auxiliary-only selection: no pose target ghost will be shown.") + self._set_handle_value("target_summary", "\n\n".join(lines)) + + def _summary_group_names(self, group_ids: tuple[PlanningGroupID, ...]) -> list[str]: + groups = self._group_info_by_id() + names: list[str] = [] + for group_id in group_ids: + group = groups.get(group_id) + if group is None: + names.append(str(group_id)) + continue + names.append(self._group_display_name(group)) + return names + def _update_control_state(self) -> None: self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) @@ -830,7 +907,9 @@ def _update_control_state(self) -> None: and self.state.can_execute(self.config.current_match_tolerance) ), ) - self._set_disabled("cancel", not self.state.can_cancel()) + can_cancel = self.state.can_cancel() + self._set_disabled("cancel", not can_cancel) + self._set_visible("cancel", can_cancel) self._update_target_visual_state() def _update_target_visual_state(self) -> None: @@ -1035,14 +1114,22 @@ def _set_error(self, message: str) -> None: def _set_handle_value(self, key: str, value: str) -> None: handle = self._handles.get(key) - if isinstance(handle, GuiMarkdownHandle): - self._set_optional_handle_attr(handle, "value", value) + if handle is None: + return + if hasattr(handle, "content") or hasattr(handle, "value"): + attr = "content" if hasattr(handle, "content") else "value" + self._set_optional_handle_attr(handle, attr, value) def _set_disabled(self, key: str, disabled: bool) -> None: handle = self._handles.get(key) if isinstance(handle, GuiButtonHandle): self._set_optional_handle_attr(handle, "disabled", disabled) + def _set_visible(self, key: str, visible: bool) -> None: + handle = self._handles.get(key) + if handle is not None: + self._set_optional_handle_attr(handle, "visible", visible) + @staticmethod def _set_optional_handle_attr(handle: object, attr: str, value: object) -> None: setattr(handle, attr, value) diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index fdfcc7fa56..54a544d3c3 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -93,6 +93,7 @@ def __init__( self._grid_handle: GridHandle | None = None self._grid_visible = True self._preview_visible: dict[str, bool] = {} + self._target_active: dict[str, bool] = {} self._target_tracks_current: dict[str, bool] = {} self._ensure_reference_grid() @@ -108,9 +109,17 @@ def set_reference_grid_visible(self, visible: bool) -> None: def register_robot(self, robot_id: str, config: RobotModelConfig) -> None: self._configs_by_id[robot_id] = config self._preview_visible.setdefault(robot_id, False) + self._target_active.setdefault(robot_id, False) self._target_tracks_current.setdefault(robot_id, True) self._ensure_robot_urdfs(robot_id, config) + def set_target_active(self, robot_id: str, active: bool) -> None: + """Show target ghost only when at least one group on the robot is active.""" + self._target_active[robot_id] = active + if not active: + self._target_tracks_current[robot_id] = True + self._set_target_visibility(robot_id, active) + def _ensure_reference_grid(self) -> None: try: scene = self.server.scene @@ -169,7 +178,7 @@ def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> self.set_urdf_joints(current, config.joint_names, joint_state.position) if self._target_tracks_current.get(robot_id, True): self._set_target_joints(robot_id, config.joint_names, joint_state.position) - self._set_target_visibility(robot_id, True) + self._set_target_visibility(robot_id, self._target_active.get(robot_id, False)) def show_preview(self, robot_id: str) -> None: """Show the transient preview-animation ghost. @@ -202,6 +211,7 @@ def set_target_joints( target = self._urdfs.get(f"{robot_id}:target") if target is None: return False + self._target_active[robot_id] = True self._target_tracks_current[robot_id] = False self._set_target_joints(robot_id, joint_names, joints) self._set_target_visibility(robot_id, True) @@ -263,6 +273,7 @@ def close(self) -> None: self._root_frames.clear() self._configs_by_id.clear() self._preview_visible.clear() + self._target_active.clear() self._target_tracks_current.clear() def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: @@ -288,7 +299,9 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: self._set_urdf_mesh_material( self._urdfs[key], GOAL_ROBOT_FEASIBLE_COLOR, GOAL_ROBOT_FEASIBLE_OPACITY ) - self._set_handle_visibility(self._urdfs[key], True) + self._set_handle_visibility( + self._urdfs[key], self._target_active.get(robot_id, False) + ) elif kind == "preview": self._set_urdf_mesh_material( self._urdfs[key], PREVIEW_ROBOT_COLOR, PREVIEW_ROBOT_OPACITY diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 4b0f0872ed..fc3be64f18 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -360,18 +360,22 @@ def make_robot_config(**overrides: RobotConfigOverride) -> RobotConfigStub: def make_planning_group_info( - robot_name: str, config: RobotConfigStub | SimpleNamespace + robot_name: str, + config: RobotConfigStub | SimpleNamespace, + *, + group_name: str = "manipulator", + has_pose_target: bool = True, ) -> dict[str, object]: joint_names = [str(name) for name in config.joint_names] return { - "id": f"{robot_name}:manipulator", - "name": "manipulator", + "id": f"{robot_name}:{group_name}", + "name": group_name, "robot_name": robot_name, "joint_names": [f"{robot_name}/{name}" for name in joint_names], "local_joint_names": joint_names, "base_link": str(config.base_link), - "tip_link": str(config.end_effector_link), - "has_pose_target": True, + "tip_link": str(config.end_effector_link) if has_pose_target else None, + "has_pose_target": has_pose_target, "source": "fallback", } @@ -408,11 +412,18 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: return None init = self.get_init_joints(robot_name) home_joints = config.home_joints if hasattr(config, "home_joints") else None + planning_groups = getattr(self, "_planning_groups", None) + if planning_groups is None: + planning_groups = [make_planning_group_info(robot_name, config)] + else: + planning_groups = [ + group for group in planning_groups if str(group["robot_name"]) == robot_name + ] return { "name": config.name, "world_robot_id": self.robot_id_for_name(robot_name) or robot_name, "joint_names": list(config.joint_names), - "planning_groups": [make_planning_group_info(robot_name, config)], + "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, "max_velocity": 1.0, @@ -579,8 +590,20 @@ def test_gui_builds_controls_in_manipulation_panel_folder( assert server.folders[0].label == "Manipulation Panel" assert server.folders[0].kwargs == {"expand_by_default": True} assert "status" in gui._handles - assert "robot" in gui._handles + assert "robot" not in gui._handles + assert "planning_groups_heading" in gui._handles + assert "target_heading" in gui._handles + assert "target_summary" in gui._handles + assert "actions_heading" in gui._handles assert "plan" in gui._handles + handle_order = list(gui._handles) + assert handle_order.index(f"group:{DEFAULT_GROUP_ID}") < handle_order.index("plan") + assert handle_order.index("target_summary") < handle_order.index("plan") + assert handle_order.index("plan") < handle_order.index("actions_folder") + assert isinstance(gui._handles["status"], GuiMarkdownHandle) + assert "Starting" not in gui._handles["status"].value + assert isinstance(gui._handles["target_summary"], GuiMarkdownHandle) + assert "Primary:" in gui._handles["target_summary"].value assert gui._operation_worker._timeout_seconds is None @@ -616,14 +639,11 @@ def test_gui_close_removes_handles_and_late_callbacks_are_noops( ) adapter = make_adapter_with_robot() gui = make_panel(server, adapter, ViserVisualizationConfig(), scene) - robot_dropdown = gui._handles["robot"] plan_button = server.buttons["Plan"] grid = grid_server.grids[0] handles = list(gui._handles.values()) gui.close() - if isinstance(robot_dropdown, GuiDropdownHandle) and robot_dropdown.update_callback is not None: - robot_dropdown.update_callback(SimpleNamespace(target=SimpleNamespace(value="arm"))) if plan_button.click_callback is not None: plan_button.click_callback(SimpleNamespace()) gui._set_scene_grid_visible(False) @@ -812,8 +832,10 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles scene.register_robot("robot1", config) target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) + scene.set_target_active("robot1", True) + assert all(mesh.visible is True for mesh in target._meshes) scene.show_preview("robot1") assert all(mesh.visible is True for mesh in preview._meshes) assert all(mesh.visible is True for mesh in target._meshes) @@ -825,7 +847,7 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles assert all(mesh.visible is False for mesh in preview._meshes) -def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> None: +def test_target_ghost_tracks_current_but_is_visible_only_when_active() -> None: server = FakeServer() urdfs = [FakeViserUrdfWithMeshes(("joint1",)) for _ in range(3)] scene = ViserManipulationScene(server, lambda *args, **kwargs: urdfs.pop(0), preview_fps=10.0) @@ -843,12 +865,16 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.25])) assert current.cfg == [0.25] assert target.cfg == [0.25] assert preview.cfg is None + assert all(mesh.visible is False for mesh in target._meshes) + + scene.set_target_active("robot1", True) + assert all(mesh.visible is True for mesh in target._meshes) scene.set_target_joints("robot1", ["joint1"], [0.8]) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.1])) @@ -856,6 +882,9 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N assert target.cfg == [0.8] assert preview.cfg is None + scene.set_target_active("robot1", False) + assert all(mesh.visible is False for mesh in target._meshes) + def test_scene_parents_urdfs_under_base_pose_frame() -> None: server = FakeServer() @@ -932,7 +961,7 @@ def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback( assert ok is True assert preview.cfg == [1.0] assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) def test_scene_target_helpers_handle_missing_robot_and_pose() -> None: @@ -1137,7 +1166,7 @@ def test_scene_registers_goal_robot_coloring_and_updates_visibility() -> None: assert all(mesh.visible is True for mesh in preview._meshes) scene.hide_preview("robot1") assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) def test_scene_transform_controls_update_pose_callback_and_visual_state() -> None: @@ -1266,6 +1295,96 @@ def test_gui_removes_pose_selector_when_group_is_deselected( assert control.removed is True +def test_gui_group_selector_derives_primary_and_auxiliary_groups( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "grip"], position=[0.25, 0.5]) + config = make_robot_config(joint_names=["j1", "grip"], home_joints=[0.0, 0.0]) + pose_group = make_planning_group_info("arm", config) + auxiliary_group = make_planning_group_info( + "arm", config, group_name="gripper", has_pose_target=False + ) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, + _planning_groups=[pose_group, auxiliary_group], + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + target_controls = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: target_controls.append(args) or object(), + remove_target_controls=lambda *args: None, + set_target_active=lambda *args: None, + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + server = FakeGuiServer() + + gui = make_panel(server, adapter, ViserVisualizationConfig(panel_enabled=True), scene) + aux_label = "Aux: arm gripper" + assert "robot" not in gui._handles + assert server.checkboxes["Pose: arm"].value is True + assert server.checkboxes[aux_label].value is False + + server.checkboxes[aux_label].update_callback( + SimpleNamespace(target=SimpleNamespace(value=True)) + ) + + assert gui.state.selected_group_ids == ("arm:manipulator", "arm:gripper") + assert gui.state.auxiliary_group_ids == ("arm:gripper",) + assert [call[0] for call in target_controls] == ["arm:manipulator"] + + +def test_gui_target_ghost_visibility_follows_active_selected_groups( + make_panel: Callable[..., ViserPanelGui], +) -> None: + left_config = make_robot_config(name="left", joint_names=["j1"], home_joints=[0.0]) + right_config = make_robot_config(name="right", joint_names=["j1"], home_joints=[0.0]) + module = FakeManipulationModule( + _robots={ + "left": ("left-id", left_config, None), + "right": ("right-id", right_config, None), + } + ) + current = FakeJointState(["j1"], position=[0.0]) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + active_updates = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: object(), + remove_target_controls=lambda *args: None, + set_target_active=lambda *args: active_updates.append(args), + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + + assert active_updates[-2:] == [("left-id", True), ("right-id", False)] + gui._set_group_selected("right:manipulator", True) + assert active_updates[-2:] == [("left-id", True), ("right-id", True)] + gui._set_group_selected("left:manipulator", False) + assert active_updates[-2:] == [("left-id", False), ("right-id", True)] + + def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callbacks( make_panel: Callable[..., ViserPanelGui], ) -> None: From 04de196b35dc5208c67caeb7c5ed49903458b4e0 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 20 Jun 2026 12:22:32 -0700 Subject: [PATCH 24/27] fix: synchronize viser group previews --- .../visualization/viser/animation.py | 25 ++++- .../manipulation/visualization/viser/scene.py | 67 +++++++++++-- .../viser/test_viser_visualization.py | 70 +++++++++++++ .../viser/test_visualizer_lifecycle.py | 97 ++++++++++++++++++- .../visualization/viser/visualizer.py | 36 ++++++- 5 files changed, 280 insertions(+), 15 deletions(-) diff --git a/dimos/manipulation/visualization/viser/animation.py b/dimos/manipulation/visualization/viser/animation.py index baab4812cd..03b4e3321e 100644 --- a/dimos/manipulation/visualization/viser/animation.py +++ b/dimos/manipulation/visualization/viser/animation.py @@ -15,11 +15,31 @@ from __future__ import annotations from collections.abc import Callable, Sequence +from dataclasses import dataclass import time +from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.msgs.sensor_msgs.JointState import JointState +@dataclass(frozen=True) +class PreviewTrack: + """One render track owned by one or more planning groups.""" + + robot_id: str + group_ids: tuple[PlanningGroupID, ...] + joint_names: tuple[str, ...] + path: tuple[JointState, ...] + + +@dataclass(frozen=True) +class GroupPreviewAnimation: + """Group-native preview transaction for a generated plan.""" + + group_ids: tuple[PlanningGroupID, ...] + tracks: tuple[PreviewTrack, ...] + + def interpolate_joint_path( path: Sequence[JointState], duration: float, fps: float ) -> list[list[float]]: @@ -92,7 +112,8 @@ def animate(self, path: Sequence[JointState], duration: float, fps: float) -> bo if not frames: return False step_delay = duration / max(len(frames) - 1, 1) if duration > 0.0 else 0.0 - for joints in frames: + for index, joints in enumerate(frames): self._set_joints(joints) - self._sleep(step_delay) + if index < len(frames) - 1: + self._sleep(step_delay) return True diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index 54a544d3c3..049ffdf659 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -16,11 +16,16 @@ from collections.abc import Callable, Sequence from pathlib import Path +import time from typing import Protocol, TypeAlias, cast from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake -from dimos.manipulation.visualization.viser.animation import PreviewAnimator +from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, + PreviewTrack, + sampled_joint_path_frames, +) from dimos.manipulation.visualization.viser.runtime import ( VISER_INSTALL_HINT, VISER_URDF_INSTALL_HINT, @@ -197,13 +202,63 @@ def animate_path(self, robot_id: str, path: Sequence[JointState], duration: floa config = self._configs_by_id.get(robot_id) if config is None: return False - self.show_preview(robot_id) + preview = GroupPreviewAnimation( + group_ids=(), + tracks=( + PreviewTrack( + robot_id=robot_id, + group_ids=(), + joint_names=tuple(config.joint_names), + path=tuple(path), + ), + ), + ) + return self.animate_preview(preview, duration) + + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> bool: + """Animate all preview tracks with one shared group-native frame clock.""" + if not preview.tracks: + return False + frames_by_robot: dict[str, list[list[float]]] = {} + joint_names_by_robot: dict[str, tuple[str, ...]] = {} + for track in preview.tracks: + if track.robot_id not in self._configs_by_id: + return False + frames = sampled_joint_path_frames(track.path, duration, self.preview_fps) + if not frames: + return False + frames_by_robot[track.robot_id] = frames + joint_names_by_robot[track.robot_id] = track.joint_names + + frame_count = max(len(frames) for frames in frames_by_robot.values()) + if frame_count <= 0: + return False + step_delay = duration / max(frame_count - 1, 1) if duration > 0.0 else 0.0 + + robot_ids = tuple(frames_by_robot) + for robot_id in robot_ids: + self.show_preview(robot_id) try: - return PreviewAnimator( - lambda joints: self._set_preview_ghost_joints(robot_id, config.joint_names, joints) - ).animate(path, duration, self.preview_fps) + for frame_index in range(frame_count): + for robot_id in robot_ids: + frames = frames_by_robot[robot_id] + joints = self._frame_at_shared_index(frames, frame_index, frame_count) + self._set_preview_ghost_joints(robot_id, joint_names_by_robot[robot_id], joints) + if frame_index < frame_count - 1: + time.sleep(step_delay) + return True finally: - self.hide_preview(robot_id) + for robot_id in robot_ids: + self.hide_preview(robot_id) + + @staticmethod + def _frame_at_shared_index( + frames: Sequence[list[float]], frame_index: int, frame_count: int + ) -> list[float]: + if frame_count <= 1 or len(frames) == 1: + return frames[-1] + source_index = round(frame_index * (len(frames) - 1) / (frame_count - 1)) + return frames[source_index] def set_target_joints( self, robot_id: str, joint_names: Sequence[str], joints: Sequence[float] diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index fc3be64f18..5f79b62a78 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -26,9 +26,12 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation +from dimos.manipulation.visualization.viser import scene as scene_module from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, PreviewAnimator, + PreviewTrack, interpolate_joint_path, sampled_joint_path_frames, ) @@ -964,6 +967,73 @@ def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback( assert all(mesh.visible is False for mesh in target._meshes) +def test_group_preview_animation_updates_all_tracks_on_shared_frame_clock( + monkeypatch: pytest.MonkeyPatch, +) -> None: + server = FakeServer() + scene = ViserManipulationScene( + server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 + ) + scene.prepared_urdf_path = lambda config: "dummy.urdf" + config = SimpleNamespace( + name="arm", + model_path="/tmp/arm.urdf", + package_paths={}, + xacro_args={}, + auto_convert_meshes=False, + joint_names=["joint1"], + ) + scene.register_robot("left", config) + scene.register_robot("right", config) + updates: list[tuple[str, tuple[str, ...], tuple[float, ...]]] = [] + sleep_calls: list[float] = [] + + def record_preview_joints( + robot_id: str, joint_names: Sequence[str], joints: Sequence[float] + ) -> None: + updates.append((robot_id, tuple(joint_names), tuple(joints))) + + monkeypatch.setattr(scene, "_set_preview_ghost_joints", record_preview_joints) + monkeypatch.setattr(scene_module.time, "sleep", sleep_calls.append) + + ok = scene.animate_preview( + GroupPreviewAnimation( + group_ids=("left/arm", "right/arm"), + tracks=( + PreviewTrack( + robot_id="left", + group_ids=("left/arm",), + joint_names=("joint1",), + path=( + FakeJointState(["joint1"], position=[0.0]), + FakeJointState(["joint1"], position=[1.0]), + ), + ), + PreviewTrack( + robot_id="right", + group_ids=("right/arm",), + joint_names=("joint1",), + path=( + FakeJointState(["joint1"], position=[10.0]), + FakeJointState(["joint1"], position=[11.0]), + ), + ), + ), + ), + duration=0.0, + ) + + assert ok is True + assert updates == [ + ("left", ("joint1",), (0.0,)), + ("right", ("joint1",), (10.0,)), + ("left", ("joint1",), (1.0,)), + ("right", ("joint1",), (11.0,)), + ] + assert sleep_calls == [0.0] + assert scene._preview_visible == {"left": False, "right": False} + + def test_scene_target_helpers_handle_missing_robot_and_pose() -> None: server = FakeTransformServer() scene = ViserManipulationScene( diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 18b6fd327f..bb7ffd8d47 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -29,6 +29,7 @@ visualizer as visualizer_module, ) from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.animation import GroupPreviewAnimation from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.runtime import ViserRuntime from dimos.manipulation.visualization.viser.visualizer import ViserManipulationVisualizer @@ -408,10 +409,12 @@ def show_preview(self, robot_id: str) -> None: def hide_preview(self, robot_id: str) -> None: calls.append(("hide", robot_id)) - def animate_path(self, robot_id: str, path: list[JointState], duration: float) -> None: - assert path == [current] + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: + assert preview.group_ids == ("arm/manipulator",) + assert len(preview.tracks) == 1 + assert preview.tracks[0].path == (current,) assert duration == 1.5 - calls.append(("animate", robot_id)) + calls.append(("animate", preview.tracks[0].robot_id)) def close(self) -> None: calls.append(("scene", "close")) @@ -419,7 +422,10 @@ def close(self) -> None: world_monitor = SimpleNamespace( get_current_joint_state=lambda _robot_id: current, planning_groups=SimpleNamespace( - select=lambda _group_ids: SimpleNamespace(robot_names=("arm",)) + select=lambda _group_ids: SimpleNamespace( + groups=(SimpleNamespace(id="arm/manipulator", robot_name="arm"),), + robot_names=("arm",), + ) ), ) robot_config = fake_robot_config("arm") @@ -459,3 +465,86 @@ def close(self) -> None: ("scene", "close"), ("runtime", "close"), ] + + +def test_visualizer_animates_multi_robot_plan_as_one_group_preview( + monkeypatch: pytest.MonkeyPatch, +) -> None: + previews: list[GroupPreviewAnimation] = [] + + class FakeRuntime: + url = "http://localhost:8095" + + def __init__(self, config: ViserVisualizationConfig) -> None: + self.config = config + + def start(self) -> FakeServer: + return FakeServer() + + def close(self) -> None: + pass + + class FakeScene: + def __init__( + self, + server: FakeServer, + viser_urdf: type[FakeViserUrdf], + *, + preview_fps: float, + ) -> None: + pass + + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: + assert duration == 2.0 + previews.append(preview) + + def close(self) -> None: + pass + + groups = ( + SimpleNamespace(id="left/arm", robot_name="left"), + SimpleNamespace(id="right/arm", robot_name="right"), + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: JointState( + {"name": ["joint1"], "position": [0.0 if robot_id == "left-id" else 10.0]} + ), + planning_groups=SimpleNamespace( + select=lambda _group_ids: SimpleNamespace(groups=groups, robot_names=("left", "right")) + ), + ) + configs = {"left": fake_robot_config("left"), "right": fake_robot_config("right")} + manipulation_module = SimpleNamespace( + robot_id_for_name=lambda robot_name: f"{robot_name}-id" if robot_name in configs else None, + get_robot_config=lambda robot_name: configs.get(robot_name), + ) + monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) + monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) + monkeypatch.setattr(visualizer_module, "ViserManipulationScene", FakeScene) + visualizer = ViserManipulationVisualizer( + world_monitor=world_monitor, + manipulation_module=manipulation_module, + config=ViserVisualizationConfig(panel_enabled=False), + ) + + visualizer.animate_plan( + GeneratedPlan( + group_ids=("left/arm", "right/arm"), + path=[ + JointState(name=["left/joint1", "right/joint1"], position=[0.0, 10.0]), + JointState(name=["left/joint1", "right/joint1"], position=[1.0, 11.0]), + ], + status=PlanningStatus.SUCCESS, + ), + duration=2.0, + ) + + assert len(previews) == 1 + preview = previews[0] + assert preview.group_ids == ("left/arm", "right/arm") + assert [(track.robot_id, track.group_ids) for track in preview.tracks] == [ + ("left-id", ("left/arm",)), + ("right-id", ("right/arm",)), + ] + assert [tuple(point.position) for point in preview.tracks[0].path] == [(0.0,), (1.0,)] + assert [tuple(point.position) for point in preview.tracks[1].path] == [(10.0,), (11.0,)] diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index 4433b221ef..0ea7fb575b 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -20,6 +20,10 @@ from dimos.manipulation.planning.planning_identifiers import make_global_joint_name from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, + PreviewTrack, +) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.runtime import ( @@ -179,16 +183,42 @@ def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: self._ensure_started() if self._adapter is None or self._scene is None: return + preview = self._build_group_preview_animation(plan) + if preview is not None: + self._scene.animate_preview(preview, duration) + + def _build_group_preview_animation(self, plan: GeneratedPlan) -> GroupPreviewAnimation | None: + if self._adapter is None: + return None selection = self._world_monitor.planning_groups.select(plan.group_ids) + tracks: list[PreviewTrack] = [] for robot_name in selection.robot_names: robot_id = self._adapter.robot_id_for_name(robot_name) config = self._adapter.get_robot_config(robot_name) current = self._adapter.get_current_joint_state(robot_name) if robot_id is None or config is None or current is None: - continue + logger.warning( + "Cannot build group preview for robot '%s': missing id, config, or state", + robot_name, + ) + return None path = self._robot_path_for_plan(robot_name, config, current, plan) - if path: - self._scene.animate_path(str(robot_id), path, duration) + if not path: + logger.warning("Cannot project generated plan for robot '%s'", robot_name) + return None + tracks.append( + PreviewTrack( + robot_id=str(robot_id), + group_ids=tuple( + group.id for group in selection.groups if group.robot_name == robot_name + ), + joint_names=tuple(config.joint_names), + path=tuple(path), + ) + ) + if not tracks: + return None + return GroupPreviewAnimation(group_ids=plan.group_ids, tracks=tuple(tracks)) def _robot_ids_for_groups(self, group_ids: Sequence[PlanningGroupID]) -> list[str]: if self._adapter is None: From 7e91f9b0547b073714d188de2c211cba92422e42 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 20 Jun 2026 14:00:55 -0700 Subject: [PATCH 25/27] fix: integrate dual xarm6 execution --- AGENTS.md | 2 +- dimos/control/README.md | 1 - dimos/control/blueprints/dual.py | 18 +---- dimos/control/blueprints/test_dual.py | 69 +++++++++++++++++++ .../tasks/trajectory_task/trajectory_task.py | 17 +++++ dimos/control/test_control.py | 45 ++++++++++++ dimos/control/tick_loop.py | 8 ++- dimos/e2e_tests/test_control_coordinator.py | 23 ++++--- dimos/manipulation/blueprints.py | 40 ++++++++--- .../control/coordinator_client.py | 4 +- dimos/manipulation/manipulation_module.py | 59 +++++++++++++++- dimos/manipulation/planning/README.md | 2 +- dimos/manipulation/test_manipulation_unit.py | 42 +++++++++++ dimos/robot/all_blueprints.py | 3 +- dimos/robot/test_all_blueprints.py | 2 +- docs/capabilities/manipulation/readme.md | 5 +- 16 files changed, 286 insertions(+), 54 deletions(-) create mode 100644 dimos/control/blueprints/test_dual.py diff --git a/AGENTS.md b/AGENTS.md index 4da2b37404..b2b83d7d67 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -44,7 +44,7 @@ dimos restart # stop + re-run with same original args | `xarm-perception-sim-agent` | xArm | sim | GPT-4o | — | Manipulation + perception + agent, sim | | `xarm7-planner-coordinator` | xArm7 | real | — | — | Trajectory planner coordinator | | `teleop-quest-xarm7` | xArm7 | real | — | — | Quest VR teleop | -| `dual-xarm6-planner` | xArm6×2 | real | — | — | Dual-arm motion planner | +| `dual-xarm6-planner-coordinator` | xArm6×2 | real | — | — | Dual-arm motion planner + coordinator | Run `dimos list` for the full list. diff --git a/dimos/control/README.md b/dimos/control/README.md index 858af0b6c4..56694a2c38 100644 --- a/dimos/control/README.md +++ b/dimos/control/README.md @@ -27,7 +27,6 @@ Centralized control system for multi-arm robots with per-joint arbitration. ```bash # Terminal 1: Run coordinator dimos run coordinator-mock # Single 7-DOF mock arm -dimos run coordinator-dual-mock # Dual arms (7+6 DOF) dimos run coordinator-piper-xarm # Real hardware # Terminal 2: Control via CLI diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py index d721f88877..bed71132a4 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -15,7 +15,6 @@ """Dual-arm coordinator blueprints with trajectory control. Usage: - dimos run coordinator-dual-mock # Mock 7+6 DOF arms dimos run coordinator-dual-xarm # XArm7 left + XArm6 right dimos run coordinator-piper-xarm # XArm6 + Piper """ @@ -27,18 +26,6 @@ from dimos.robot.catalog.piper import piper as _catalog_piper from dimos.robot.catalog.ufactory import xarm6 as _catalog_xarm6, xarm7 as _catalog_xarm7 -# Dual mock arms (7-DOF left, 6-DOF right) -_mock_left = _catalog_xarm7(name="left_arm") -_mock_right = _catalog_xarm6(name="right_arm", add_gripper=False) - -coordinator_dual_mock = ControlCoordinator.blueprint( - hardware=[_mock_left.to_hardware_component(), _mock_right.to_hardware_component()], - tasks=[ - _mock_left.to_task_config(task_name="traj_left"), - _mock_right.to_task_config(task_name="traj_right"), - ], -) - # Dual XArm (XArm7 left, XArm6 right) _xarm7_left = _catalog_xarm7(name="left_arm", adapter_type="xarm", address=global_config.xarm7_ip) _xarm6_right = _catalog_xarm6( @@ -48,8 +35,8 @@ coordinator_dual_xarm = ControlCoordinator.blueprint( hardware=[_xarm7_left.to_hardware_component(), _xarm6_right.to_hardware_component()], tasks=[ - _xarm7_left.to_task_config(task_name="traj_left"), - _xarm6_right.to_task_config(task_name="traj_right"), + _xarm7_left.to_task_config(), + _xarm6_right.to_task_config(), ], ) @@ -71,7 +58,6 @@ __all__ = [ - "coordinator_dual_mock", "coordinator_dual_xarm", "coordinator_piper_xarm", ] diff --git a/dimos/control/blueprints/test_dual.py b/dimos/control/blueprints/test_dual.py new file mode 100644 index 0000000000..a61eca0080 --- /dev/null +++ b/dimos/control/blueprints/test_dual.py @@ -0,0 +1,69 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for dual-arm control blueprints.""" + +from dimos.control.blueprints.dual import coordinator_dual_xarm +from dimos.control.coordinator import ControlCoordinator +from dimos.manipulation.blueprints import dual_xarm6_planner_coordinator +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.manipulation.planning.planning_identifiers import make_global_joint_names + + +def _coordinator_task_names(blueprint) -> list[str]: + atom = next(atom for atom in blueprint.blueprints if atom.module is ControlCoordinator) + return [task.name for task in atom.kwargs["tasks"]] + + +def _coordinator_tasks(blueprint): + atom = next(atom for atom in blueprint.blueprints if atom.module is ControlCoordinator) + return atom.kwargs["tasks"] + + +def _manipulation_robots(blueprint): + atom = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule) + return atom.kwargs["robots"] + + +def _manipulation_visualization(blueprint): + atom = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule) + return atom.kwargs["visualization"] + + +def test_dual_xarm6_integrated_blueprint_has_planner_and_coordinator() -> None: + modules = [atom.module for atom in dual_xarm6_planner_coordinator.blueprints] + + assert ManipulationModule in modules + assert ControlCoordinator in modules + + +def test_dual_xarm6_integrated_blueprint_uses_viser_for_execution_ui() -> None: + visualization = _manipulation_visualization(dual_xarm6_planner_coordinator) + + assert visualization == {"backend": "viser", "allow_plan_execute": True} + + +def test_dual_xarm6_integrated_tasks_match_planner_robots() -> None: + tasks_by_name = {task.name: task for task in _coordinator_tasks(dual_xarm6_planner_coordinator)} + + for robot in _manipulation_robots(dual_xarm6_planner_coordinator): + task = tasks_by_name[robot.coordinator_task_name] + assert task.joint_names == make_global_joint_names(robot.name, robot.joint_names) + + +def test_dual_coordinator_xarm_task_names_match_manipulation_robot_defaults() -> None: + assert _coordinator_task_names(coordinator_dual_xarm) == [ + "traj_left_arm", + "traj_right_arm", + ] diff --git a/dimos/control/tasks/trajectory_task/trajectory_task.py b/dimos/control/tasks/trajectory_task/trajectory_task.py index b23ef6db3f..f9ca8568cd 100644 --- a/dimos/control/tasks/trajectory_task/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task/trajectory_task.py @@ -191,6 +191,23 @@ def execute(self, trajectory: JointTrajectory) -> bool: logger.warning(f"Empty trajectory for {self._name}") return False + if trajectory.joint_names and trajectory.joint_names != self._joint_names_list: + logger.warning( + f"Joint name mismatch for {self._name}: " + f"expected={self._joint_names_list}, received={trajectory.joint_names}" + ) + return False + + if not trajectory.joint_names: + expected_joint_count = len(self._joint_names_list) + for point in trajectory.points: + if len(point.positions) != expected_joint_count: + logger.warning( + f"Trajectory point dimension mismatch for {self._name}: " + f"expected={expected_joint_count}, received={len(point.positions)}" + ) + return False + # Preempt any active trajectory if self._state == TrajectoryState.EXECUTING: logger.info(f"Preempting active trajectory on {self._name}") diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index ae6bc1e9de..13f0579cb3 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -274,6 +274,29 @@ def test_execute_trajectory(self, trajectory_task, simple_trajectory): assert trajectory_task.is_active() assert trajectory_task.get_state() == TrajectoryState.EXECUTING + def test_execute_rejects_mismatched_joint_names(self, trajectory_task): + trajectory = JointTrajectory( + joint_names=["other/joint1", "other/joint2", "other/joint3"], + points=[ + TrajectoryPoint( + positions=[0.0, 0.0, 0.0], + velocities=[0.0, 0.0, 0.0], + time_from_start=0.0, + ), + TrajectoryPoint( + positions=[1.0, 0.5, 0.25], + velocities=[0.0, 0.0, 0.0], + time_from_start=1.0, + ), + ], + ) + + result = trajectory_task.execute(trajectory) + + assert result is False + assert not trajectory_task.is_active() + assert trajectory_task.get_state() == TrajectoryState.IDLE + def test_compute_during_trajectory(self, trajectory_task, simple_trajectory, coordinator_state): t_start = time.perf_counter() trajectory_task.execute(simple_trajectory) @@ -530,6 +553,28 @@ def test_tick_loop_calls_compute(self, mock_adapter): assert mock_task.compute.call_count > 0 + def test_write_all_hardware_logs_rejected_command(self, mocker): + hardware = {"arm": MagicMock()} + hardware["arm"].write_command.return_value = False + log_error = mocker.patch("dimos.control.tick_loop.logger.error") + tick_loop = TickLoop( + tick_rate=100.0, + hardware=hardware, + hardware_lock=threading.Lock(), + tasks={}, + task_lock=threading.Lock(), + joint_to_hardware={}, + ) + + tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.5}, ControlMode.SERVO_POSITION)}) + + hardware["arm"].write_command.assert_called_once_with( + {"arm/joint1": 0.5}, ControlMode.SERVO_POSITION + ) + log_error.assert_called_once_with( + "Hardware %s rejected %d %s command(s)", "arm", 1, "SERVO_POSITION" + ) + class TestIntegration: def test_full_trajectory_execution(self, mock_adapter): diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index d38b7e76d2..73eb7b6530 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -397,7 +397,13 @@ def _write_all_hardware( for hw_id, (positions, mode) in hw_commands.items(): if hw_id in self._hardware: try: - self._hardware[hw_id].write_command(positions, mode) + if not self._hardware[hw_id].write_command(positions, mode): + logger.error( + "Hardware %s rejected %d %s command(s)", + hw_id, + len(positions), + mode.name, + ) except Exception as e: logger.error(f"Failed to write to {hw_id}: {e}") diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 40e4800b47..8d635805b8 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -201,8 +201,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: """Test dual-arm coordinator with independent trajectories.""" lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") - # Start dual-arm mock coordinator - start_blueprint("coordinator-dual-mock") + # Start integrated dual-arm mock planner/coordinator + start_blueprint("dual-xarm6-planner-coordinator") lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") client = RPCClient(None, ControlCoordinator) @@ -213,15 +213,15 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: assert "right_arm/joint1" in joints tasks = client.list_tasks() - assert "traj_left" in tasks - assert "traj_right" in tasks + assert "traj_left_arm" in tasks + assert "traj_right_arm" in tasks # Create trajectories for both arms left_trajectory = JointTrajectory( - joint_names=[f"left_arm/joint{i + 1}" for i in range(7)], + joint_names=[f"left_arm/joint{i + 1}" for i in range(6)], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), - TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), + TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), + TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 6), ], ) @@ -235,10 +235,11 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: # Execute both via task_invoke assert ( - client.task_invoke("traj_left", "execute", {"trajectory": left_trajectory}) is True + client.task_invoke("traj_left_arm", "execute", {"trajectory": left_trajectory}) + is True ) assert ( - client.task_invoke("traj_right", "execute", {"trajectory": right_trajectory}) + client.task_invoke("traj_right_arm", "execute", {"trajectory": right_trajectory}) is True ) @@ -246,8 +247,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: time.sleep(1.0) # Both should complete - left_state = client.task_invoke("traj_left", "get_state") - right_state = client.task_invoke("traj_right", "get_state") + left_state = client.task_invoke("traj_left_arm", "get_state") + right_state = client.task_invoke("traj_right_arm", "get_state") assert left_state == TrajectoryState.COMPLETED assert right_state == TrajectoryState.COMPLETED diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 3714e53b6e..6c4b2764d3 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -63,8 +63,7 @@ ) -# Dual XArm6 planner with coordinator integration -# Usage: Start with coordinator_dual_mock, then plan/execute via RPC +# Dual XArm6 planner + coordinator with Viser execution UI. _left_arm_cfg = _catalog_xarm6( name="left_arm", adapter_type="xarm" if global_config.xarm6_ip else "mock", @@ -78,13 +77,32 @@ y_offset=-0.5, ) -dual_xarm6_planner = ManipulationModule.blueprint( - robots=[ - _left_arm_cfg.to_robot_model_config(), - _right_arm_cfg.to_robot_model_config(), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, +dual_xarm6_planner_coordinator = autoconnect( + ManipulationModule.blueprint( + robots=[ + _left_arm_cfg.to_robot_model_config(), + _right_arm_cfg.to_robot_model_config(), + ], + planning_timeout=10.0, + visualization={"backend": "viser", "allow_plan_execute": True}, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + _left_arm_cfg.to_hardware_component(), + _right_arm_cfg.to_hardware_component(), + ], + tasks=[ + _left_arm_cfg.to_task_config(), + _right_arm_cfg.to_task_config(), + ], + ), +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } ) @@ -135,7 +153,7 @@ _right_xarm7_cfg.to_robot_model_config(), ], planning_timeout=10.0, - enable_viz=True, + visualization={"backend": "viser", "allow_plan_execute": True}, ), ControlCoordinator.blueprint( tick_rate=100.0, @@ -362,7 +380,7 @@ __all__ = [ - "dual_xarm6_planner", + "dual_xarm6_planner_coordinator", "dual_xarm7_planner_coordinator", "xarm6_planner_only", "xarm7_planner_coordinator", diff --git a/dimos/manipulation/control/coordinator_client.py b/dimos/manipulation/control/coordinator_client.py index ed552e0846..6fe78d13cc 100644 --- a/dimos/manipulation/control/coordinator_client.py +++ b/dimos/manipulation/control/coordinator_client.py @@ -24,12 +24,10 @@ Usage: # Terminal 1: Start the coordinator dimos run coordinator-mock # Single arm - dimos run coordinator-dual-mock # Dual arm # Terminal 2: Run this client python -m dimos.manipulation.control.coordinator_client - python -m dimos.manipulation.control.coordinator_client --task traj_left - python -m dimos.manipulation.control.coordinator_client --task traj_right + python -m dimos.manipulation.control.coordinator_client --task traj_arm How it works: 1. Connects to ControlCoordinator via LCM RPC diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 654d9ce970..ffd221e983 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -125,6 +125,7 @@ class ManipulationModuleConfig(ModuleConfig): # to prevent the planner from routing trajectories below this height. # Set to None to disable. floor_z: float | None = None + coordinator_rpc_timeout: float = 3.0 class ManipulationModule(Module): @@ -630,7 +631,13 @@ def _plan_selected_path( if not result.is_success(): return self._fail(f"Planning failed: {result.status.name}") - logger.info("Path: %d waypoints", len(result.path)) + path_joints = list(result.path[-1].name) if result.path else [] + logger.info( + "Path: %d waypoints, groups=%s, joints=%s", + len(result.path), + group_ids, + path_joints, + ) self._store_generated_plan(group_ids, result) self._state = ManipulationState.COMPLETED return True @@ -1460,6 +1467,29 @@ def _get_coordinator_client(self) -> RPCClient | None: self._coordinator_client = RPCClient(None, ControlCoordinator) return self._coordinator_client + def _invoke_coordinator_task( + self, + client: RPCClient, + task_name: str, + method: str, + kwargs: dict[str, Any], + ) -> Any: + """Invoke a ControlCoordinator task with an execution-specific timeout.""" + remote_name = getattr(client, "remote_name", None) + rpc_client = getattr(client, "rpc", None) + call_sync = getattr(rpc_client, "call_sync", None) + if isinstance(remote_name, str) and callable(call_sync): + result, unsub_fn = call_sync( + f"{remote_name}/task_invoke", + ([task_name, method, kwargs], {}), + rpc_timeout=self.config.coordinator_rpc_timeout, + ) + unsub_fns = getattr(client, "_unsub_fns", None) + if isinstance(unsub_fns, list): + unsub_fns.append(unsub_fn) + return result + return client.task_invoke(task_name, method, kwargs) + @rpc def execute(self, robot_name: RobotName | None = None) -> bool: """Execute planned trajectory via ControlCoordinator.""" @@ -1482,6 +1512,12 @@ def execute_plan( affected = self._affected_robot_names(plan) except Exception as exc: return self._fail(f"Failed to resolve generated plan: {exc}") + logger.info( + "Execute plan: groups=%s, affected=%s, requested_robot=%s", + plan.group_ids, + affected, + robot_name, + ) robot_names = [robot_name] if robot_name is not None else affected assert self._world_monitor is not None @@ -1558,8 +1594,25 @@ def execute_plan( len(trajectory.points), trajectory.duration, ) - result = client.task_invoke( - config.coordinator_task_name, "execute", {"trajectory": trajectory} + try: + result = self._invoke_coordinator_task( + client, + config.coordinator_task_name, + "execute", + {"trajectory": trajectory}, + ) + except TimeoutError as exc: + return self._fail( + f"Coordinator RPC timed out for task '{config.coordinator_task_name}': {exc}" + ) + except Exception as exc: + return self._fail( + f"Coordinator RPC failed for task '{config.coordinator_task_name}': {exc}" + ) + logger.info( + "Coordinator execute result: task='%s', result=%r", + config.coordinator_task_name, + result, ) if not result: return self._fail("Coordinator rejected trajectory") diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index a1712411d7..96d8176268 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -139,7 +139,7 @@ accepted. |-----------|-------------| | `xarm6_planner_only` | XArm 6-DOF standalone (no coordinator) | | `xarm7-planner-coordinator` | XArm 7-DOF with coordinator | -| `dual-xarm6-planner` | Dual XArm 6-DOF | +| `dual-xarm6-planner-coordinator` | Dual XArm 6-DOF with coordinator | | `xarm-perception-sim` | XArm 7-DOF simulation perception stack | ## Directory Structure diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index a07ff1e5e2..b30dd9776a 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -483,6 +483,48 @@ def test_execute_rejected(self, robot_config, simple_trajectory): assert module.execute() is False assert module._state == ManipulationState.FAULT + def test_execute_times_out_when_coordinator_rpc_does_not_respond( + self, robot_config, simple_trajectory + ): + """Coordinator RPC timeout fails execution instead of hanging silently.""" + module = _make_module_with_monitor(robot_config) + module.config.coordinator_rpc_timeout = 0.01 + generator = MagicMock() + generator.generate.return_value = simple_trajectory + module._robots = {"test_arm": ("id", robot_config, generator)} + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("test_arm/manipulator",), + path=[ + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ), + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.5, 0.5, 0.5], + ), + ], + status=PlanningStatus.SUCCESS, + ) + mock_client = MagicMock() + mock_client.remote_name = "ControlCoordinator" + mock_client._unsub_fns = [] + mock_client.rpc.call_sync.side_effect = TimeoutError("no response") + module._coordinator_client = mock_client + + assert module.execute() is False + + assert module._state == ManipulationState.FAULT + assert "timed out" in module._error_message + mock_client.rpc.call_sync.assert_called_once() + mock_client.task_invoke.assert_not_called() + def _make_module_with_monitor(*configs: RobotModelConfig) -> ManipulationModule: """Create a ManipulationModule with a mocked world monitor and robots configured.""" diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index aca17251c6..4e2f12eb7e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -21,7 +21,6 @@ "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper", "coordinator-combined-xarm6": "dimos.control.blueprints.teleop:coordinator_combined_xarm6", - "coordinator-dual-mock": "dimos.control.blueprints.dual:coordinator_dual_mock", "coordinator-dual-xarm": "dimos.control.blueprints.dual:coordinator_dual_xarm", "coordinator-flowbase": "dimos.control.blueprints.mobile:coordinator_flowbase", "coordinator-flowbase-keyboard-teleop": "dimos.control.blueprints.mobile:coordinator_flowbase_keyboard_teleop", @@ -58,7 +57,7 @@ "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", - "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner", + "dual-xarm6-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm6_planner_coordinator", "dual-xarm7-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm7_planner_coordinator", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints:keyboard_teleop_a750", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm", diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index ac900e67ea..b0929ff1b6 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -49,7 +49,7 @@ "coordinator-velocity-xarm6", "coordinator-xarm6", "coordinator-xarm7", - "dual-xarm6-planner", + "dual-xarm6-planner-coordinator", "dual-xarm7-planner-coordinator", "teleop-hosted-go2", "teleop-hosted-xarm7", diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index bcbd550d0a..603f9e446e 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -166,8 +166,7 @@ uv run dimos run xarm7-planner-coordinator \ Dual-arm mock Viser example: ```bash -uv run dimos run dual-xarm6-planner \ - -o manipulationmodule.visualization.backend=viser +uv run dimos run dual-xarm6-planner-coordinator ``` External manipulation visualizers are initialized from a backend-neutral planning-scene snapshot @@ -221,7 +220,7 @@ visualization backend. | `keyboard-teleop-xarm7` | XArm7 7-DOF keyboard teleop with Drake viz | | `xarm6-planner-only` | XArm6 standalone planner (no coordinator) | | `xarm7-planner-coordinator` | XArm7 planner with coordinator integration | -| `dual-xarm6-planner` | Dual XArm6 planning | +| `dual-xarm6-planner-coordinator` | Dual XArm6 planning and execution with Viser | | `xarm-perception` | XArm7 + RealSense camera for perception | | `xarm-perception-agent` | XArm7 perception + LLM agent | | `xarm-perception-sim` | XArm7 simulation perception stack | From 4763fb980cf869269859ac560e239235f028cac1 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 20 Jun 2026 15:47:45 -0700 Subject: [PATCH 26/27] fix: polish viser planning controls --- dimos/manipulation/visualization/viser/gui.py | 123 +++++++++--------- .../viser/test_viser_visualization.py | 74 ++++++++--- .../manipulation/visualization/viser/theme.py | 6 +- 3 files changed, 122 insertions(+), 81 deletions(-) diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 1c690f3fec..0398935faa 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -73,6 +73,9 @@ # Fallback joint-slider range (radians) when a robot config omits joint limits. DEFAULT_JOINT_LIMITS = (-3.14, 3.14) +PRIMARY_ACTION_COLOR = (0, 102, 179) +ACTIVE_GROUP_COLOR = PRIMARY_ACTION_COLOR +INACTIVE_GROUP_COLOR = (52, 52, 52) class ViserPanelGui: @@ -165,18 +168,12 @@ def _build(self) -> None: self._build_panel_controls(gui) def _build_panel_controls(self, gui: GuiApi) -> None: - self._handles["status"] = gui.add_markdown("**Status:** Ready") + self._handles["status"] = gui.add_markdown("### Status\n**State:** Ready") self._build_scene_controls(gui) self._handles["planning_groups_heading"] = gui.add_markdown( - "### Planning Groups\nChoose active target groups." + "### Planning Groups\nActive MoveIt group for pose goal, planning, and joint edits." ) self._sync_group_selector(self.adapter.list_planning_groups()) - select_all_button = gui.add_button("Select all") - select_all_button.on_click(lambda _: self._select_all_manipulators()) - self._handles["select_all_manipulators"] = select_all_button - clear_selection_button = gui.add_button("Clear selection") - clear_selection_button.on_click(lambda _: self._clear_group_selection()) - self._handles["clear_group_selection"] = clear_selection_button self._handles["target_heading"] = gui.add_markdown("### Target") preset_dropdown = gui.add_dropdown( "Preset", @@ -185,26 +182,28 @@ def _build_panel_controls(self, gui: GuiApi) -> None: ) preset_dropdown.on_update(lambda event: self._apply_preset(event.target.value)) self._handles["preset"] = preset_dropdown - self._handles["target_summary"] = gui.add_markdown("Select a group to define a target.") + self._handles["target_summary"] = gui.add_markdown( + f"Feasibility: `{self.state.feasibility.status.value}`" + ) self._handles["actions_heading"] = gui.add_markdown("### Actions") - plan_button = gui.add_button("Plan", disabled=True) + plan_button = gui.add_button("Plan", disabled=True, color=PRIMARY_ACTION_COLOR) plan_button.on_click(lambda _: self._submit_plan()) self._handles["plan"] = plan_button - more_actions = gui.add_folder("Plan controls", expand_by_default=False) - self._handles["actions_folder"] = more_actions - with more_actions: - preview_button = gui.add_button("Preview", disabled=True) - preview_button.on_click(lambda _: self._submit_preview()) - self._handles["preview"] = preview_button - execute_button = gui.add_button("Execute", disabled=True) - execute_button.on_click(lambda _: self._submit_execute()) - self._handles["execute"] = execute_button - cancel_button = gui.add_button("Cancel") - cancel_button.on_click(lambda _: self._submit_cancel()) - self._handles["cancel"] = cancel_button - clear_button = gui.add_button("Clear plan") - clear_button.on_click(lambda _: self._submit_clear()) - self._handles["clear"] = clear_button + self._handles["plan_controls_heading"] = gui.add_markdown("**Plan controls**") + preview_button = gui.add_button("Preview", disabled=True) + preview_button.on_click(lambda _: self._submit_preview()) + self._handles["preview"] = preview_button + execute_button = gui.add_button("Execute", disabled=True) + execute_button.on_click(lambda _: self._submit_execute()) + self._handles["execute"] = execute_button + cancel_button = gui.add_button("Cancel") + cancel_button.on_click(lambda _: self._submit_cancel()) + self._handles["cancel"] = cancel_button + clear_button = gui.add_button("Clear plan") + clear_button.on_click(lambda _: self._submit_clear()) + self._handles["clear"] = clear_button + joint_controls = gui.add_folder("Joint Control", expand_by_default=False) + self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() def _build_scene_controls(self, gui: GuiApi) -> None: @@ -283,6 +282,14 @@ def _build_joint_sliders(self) -> None: self._clear_joint_sliders() if not self.state.selected_group_ids: return + joint_folder = self._handles.get("joint_control_folder") + if joint_folder is not None: + with joint_folder: + self._build_joint_slider_handles(gui) + return + self._build_joint_slider_handles(gui) + + def _build_joint_slider_handles(self, gui: GuiApi) -> None: groups = self._group_info_by_id() target_by_name: dict[str, float] = {} if self.state.target_joints is not None: @@ -386,17 +393,21 @@ def _sync_group_selector(self, groups: list[PlanningGroupInfo]) -> None: key = f"group:{group_id}" seen_keys.add(key) handle = self._handles.get(key) - label = self._group_selector_label(group) + is_selected = group_id in selected + label = self._group_selector_label(group, selected=is_selected) if handle is None: - handle = self.server.gui.add_checkbox(label, initial_value=group_id in selected) - handle.on_update( - lambda event, gid=group_id: self._set_group_selected( - gid, bool(event.target.value) - ) + handle = self.server.gui.add_button( + label, + color=self._group_selector_color(is_selected), + hint="Click to toggle this planning group in the target set.", ) + handle.on_click(lambda _event, gid=group_id: self._toggle_group_selected(gid)) self._handles[key] = handle - elif hasattr(handle, "value"): - self._set_optional_handle_attr(handle, "value", group_id in selected) + else: + self._set_optional_handle_attr(handle, "label", label) + self._set_optional_handle_attr( + handle, "color", self._group_selector_color(is_selected) + ) for key in [key for key in self._handles if key.startswith("group:")]: if key not in seen_keys: @@ -406,9 +417,13 @@ def _sync_group_selector(self, groups: list[PlanningGroupInfo]) -> None: remove() @staticmethod - def _group_selector_label(group: PlanningGroupInfo) -> str: - role = "Pose" if bool(group["has_pose_target"]) else "Aux" - return f"{role}: {ViserPanelGui._group_display_name(group)}" + def _group_selector_label(group: PlanningGroupInfo, *, selected: bool = False) -> str: + _ = selected + return ViserPanelGui._group_display_name(group) + + @staticmethod + def _group_selector_color(selected: bool) -> tuple[int, int, int] | None: + return ACTIVE_GROUP_COLOR if selected else INACTIVE_GROUP_COLOR @staticmethod def _group_display_name(group: PlanningGroupInfo) -> str: @@ -430,6 +445,9 @@ def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None self._build_joint_sliders() self.refresh() + def _toggle_group_selected(self, group_id: PlanningGroupID) -> None: + self._set_group_selected(group_id, group_id not in self.state.selected_group_ids) + def _select_all_manipulators(self) -> None: groups = self.adapter.list_planning_groups() manipulator_groups = [ @@ -857,7 +875,8 @@ def _update_status_text(self) -> None: current = self.state.current_joints status_label = self.state.error or self.state.module_state status = [ - f"**Status:** {status_label}", + "### Status", + f"**State:** {status_label}", f"Target: `{self.state.target_status.value}` · Plan: `{self.state.plan_state.status.value}`", ] if self.state.selected_robot is not None: @@ -871,31 +890,9 @@ def _update_status_text(self) -> None: self._set_handle_value("status", "\n\n".join(status)) def _update_target_summary(self) -> None: - primary_groups = self._selected_pose_group_ids() - auxiliary_groups = self._selected_auxiliary_group_ids() - ghost_groups = list(primary_groups) - lines = [ - f"Primary: `{self._summary_group_names(primary_groups)}`", - f"Auxiliary: `{self._summary_group_names(auxiliary_groups)}`", - f"Ghosts: `{self._summary_group_names(tuple(ghost_groups))}`", - f"Feasibility: `{self.state.feasibility.status.value}`", - ] - if not self.state.selected_group_ids: - lines = ["Select a planning group to define a target."] - elif not primary_groups and auxiliary_groups: - lines.append("Auxiliary-only selection: no pose target ghost will be shown.") - self._set_handle_value("target_summary", "\n\n".join(lines)) - - def _summary_group_names(self, group_ids: tuple[PlanningGroupID, ...]) -> list[str]: - groups = self._group_info_by_id() - names: list[str] = [] - for group_id in group_ids: - group = groups.get(group_id) - if group is None: - names.append(str(group_id)) - continue - names.append(self._group_display_name(group)) - return names + self._set_handle_value( + "target_summary", f"Feasibility: `{self.state.feasibility.status.value}`" + ) def _update_control_state(self) -> None: self._set_disabled("plan", not self.state.can_plan()) @@ -1122,7 +1119,7 @@ def _set_handle_value(self, key: str, value: str) -> None: def _set_disabled(self, key: str, disabled: bool) -> None: handle = self._handles.get(key) - if isinstance(handle, GuiButtonHandle): + if handle is not None and hasattr(handle, "disabled"): self._set_optional_handle_attr(handle, "disabled", disabled) def _set_visible(self, key: str, visible: bool) -> None: diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 5f79b62a78..7ae56ce380 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -36,7 +36,12 @@ sampled_joint_path_frames, ) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.manipulation.visualization.viser.gui import ViserPanelGui +from dimos.manipulation.visualization.viser.gui import ( + ACTIVE_GROUP_COLOR, + INACTIVE_GROUP_COLOR, + PRIMARY_ACTION_COLOR, + ViserPanelGui, +) from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.state import ( ActionStatus, @@ -127,6 +132,7 @@ def remove(self) -> None: class GuiButtonHandle: label: str disabled: bool = False + color: tuple[int, int, int] | None = None click_callback: GuiCallback | None = None removed: bool = False @@ -330,8 +336,16 @@ def add_dropdown( handle = GuiDropdownHandle(label=label, options=list(options), value=initial_value) return handle - def add_button(self, label: str, *, disabled: bool = False) -> GuiButtonHandle: - handle = GuiButtonHandle(label=label, disabled=disabled) + def add_button( + self, + label: str, + *, + disabled: bool = False, + color: tuple[int, int, int] | None = None, + hint: str | None = None, + ) -> GuiButtonHandle: + _ = hint + handle = GuiButtonHandle(label=label, disabled=disabled, color=color) self.buttons[label] = handle return handle @@ -599,14 +613,38 @@ def test_gui_builds_controls_in_manipulation_panel_folder( assert "target_summary" in gui._handles assert "actions_heading" in gui._handles assert "plan" in gui._handles + assert "select_all_manipulators" not in gui._handles + assert "clear_group_selection" not in gui._handles + assert "plan_controls_heading" in gui._handles + assert "actions_folder" not in gui._handles + assert "joint_control_folder" in gui._handles handle_order = list(gui._handles) assert handle_order.index(f"group:{DEFAULT_GROUP_ID}") < handle_order.index("plan") assert handle_order.index("target_summary") < handle_order.index("plan") - assert handle_order.index("plan") < handle_order.index("actions_folder") + assert handle_order.index("plan") < handle_order.index("plan_controls_heading") + assert handle_order.index("plan_controls_heading") < handle_order.index("preview") + assert handle_order.index("preview") < handle_order.index("execute") + assert handle_order.index("clear") < handle_order.index("joint_control_folder") assert isinstance(gui._handles["status"], GuiMarkdownHandle) assert "Starting" not in gui._handles["status"].value assert isinstance(gui._handles["target_summary"], GuiMarkdownHandle) - assert "Primary:" in gui._handles["target_summary"].value + assert "Feasibility:" in gui._handles["target_summary"].value + assert "Primary:" not in gui._handles["target_summary"].value + assert "Auxiliary:" not in gui._handles["target_summary"].value + assert "Ghosts:" not in gui._handles["target_summary"].value + assert isinstance(gui._handles["plan_controls_heading"], GuiMarkdownHandle) + assert "Plan controls" in gui._handles["plan_controls_heading"].value + plan_button = gui._handles["plan"] + assert isinstance(plan_button, GuiButtonHandle) + assert plan_button.color == PRIMARY_ACTION_COLOR + group_button = gui._handles[f"group:{DEFAULT_GROUP_ID}"] + assert isinstance(group_button, GuiButtonHandle) + assert group_button.label == "arm" + assert group_button.color == ACTIVE_GROUP_COLOR + joint_folder = gui._handles["joint_control_folder"] + assert isinstance(joint_folder, FakeFolder) + assert joint_folder.label == "Joint Control" + assert joint_folder.kwargs == {"expand_by_default": False} assert gui._operation_worker._timeout_seconds is None @@ -690,12 +728,12 @@ def test_dimos_theme_configures_supported_viser_chrome() -> None: assert apply_dimos_theme(server) is True assert server.theme_kwargs is not None - assert server.theme_kwargs["brand_color"] == (22, 130, 163) + assert server.theme_kwargs["brand_color"] == (0, 153, 255) assert server.theme_kwargs["dark_mode"] is True assert server.theme_kwargs["show_logo"] is False assert server.theme_kwargs["show_share_button"] is False - assert server.theme_kwargs["control_layout"] == "collapsible" - assert server.theme_kwargs["control_width"] == "medium" + assert server.theme_kwargs["control_layout"] == "fixed" + assert server.theme_kwargs["control_width"] == "large" def test_dimos_theme_configures_titlebar_when_supported(monkeypatch: pytest.MonkeyPatch) -> None: @@ -1400,17 +1438,23 @@ def test_gui_group_selector_derives_primary_and_auxiliary_groups( server = FakeGuiServer() gui = make_panel(server, adapter, ViserVisualizationConfig(panel_enabled=True), scene) - aux_label = "Aux: arm gripper" assert "robot" not in gui._handles - assert server.checkboxes["Pose: arm"].value is True - assert server.checkboxes[aux_label].value is False - - server.checkboxes[aux_label].update_callback( - SimpleNamespace(target=SimpleNamespace(value=True)) - ) + pose_button = gui._handles["group:arm:manipulator"] + aux_button = gui._handles["group:arm:gripper"] + assert isinstance(pose_button, GuiButtonHandle) + assert isinstance(aux_button, GuiButtonHandle) + assert pose_button.label == "arm" + assert pose_button.color == ACTIVE_GROUP_COLOR + assert aux_button.label == "arm gripper" + assert aux_button.color == INACTIVE_GROUP_COLOR + + assert aux_button.click_callback is not None + aux_button.click_callback(SimpleNamespace(target=aux_button)) assert gui.state.selected_group_ids == ("arm:manipulator", "arm:gripper") assert gui.state.auxiliary_group_ids == ("arm:gripper",) + assert aux_button.label == "arm gripper" + assert aux_button.color == ACTIVE_GROUP_COLOR assert [call[0] for call in target_controls] == ["arm:manipulator"] diff --git a/dimos/manipulation/visualization/viser/theme.py b/dimos/manipulation/visualization/viser/theme.py index 339be04492..d38767d1da 100644 --- a/dimos/manipulation/visualization/viser/theme.py +++ b/dimos/manipulation/visualization/viser/theme.py @@ -36,7 +36,7 @@ DIMOS_THEME_TITLE = "DimOS Manipulation" DIMOS_THEME_URL = "https://github.com/dimensionalOS/dimos" -DIMOS_BRAND_COLOR = (22, 130, 163) +DIMOS_BRAND_COLOR = (0, 153, 255) DIMOS_LOGO_PATH = Path(__file__).with_name("assets") / "dimensional-logo.svg" logger = setup_logger() @@ -86,8 +86,8 @@ def _configure_theme(server: ViserServer, titlebar_content: TitlebarConfig | Non try: server.gui.configure_theme( titlebar_content=titlebar_content, - control_layout="collapsible", - control_width="medium", + control_layout="fixed", + control_width="large", dark_mode=True, show_logo=False, show_share_button=False, From 10e4d3173366675860d45481ee428d84868ed6e0 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 20 Jun 2026 19:26:23 -0700 Subject: [PATCH 27/27] fix: address quick movegroup review comments --- dimos/control/blueprints/dual.py | 18 ++++++- dimos/manipulation/manipulation_module.py | 28 +++++----- .../planning/examples/manipulation_client.py | 8 +-- .../manipulation/planning/groups/__init__.py | 53 ------------------- .../planning/kinematics/jacobian_ik.py | 7 +-- .../planning/kinematics/pink_ik.py | 7 +-- .../kinematics/test_jacobian_ik_selection.py | 2 +- .../planning/kinematics/test_pink_ik.py | 2 +- .../planning/monitor/world_monitor.py | 2 +- .../planning/planners/rrt_planner.py | 2 +- .../planners/test_rrt_planner_selection.py | 2 +- dimos/manipulation/planning/spec/config.py | 7 ++- dimos/manipulation/planning/spec/protocols.py | 7 ++- .../planning/test_planning_group_utils.py | 3 +- .../planning/test_planning_groups.py | 2 +- .../planning/world/drake_world.py | 2 +- .../world/test_drake_world_planning_groups.py | 3 +- dimos/manipulation/test_manipulation_unit.py | 2 +- .../visualization/viser/adapter.py | 10 +--- .../visualization/viser/config.py | 7 --- dimos/manipulation/visualization/viser/gui.py | 6 +-- .../viser/test_operation_worker.py | 3 -- .../viser/test_viser_visualization.py | 2 +- dimos/robot/all_blueprints.py | 1 + dimos/robot/config.py | 2 +- 25 files changed, 65 insertions(+), 123 deletions(-) delete mode 100644 dimos/manipulation/planning/groups/__init__.py diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py index 510108b1ea..3444f23335 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -15,16 +15,31 @@ """Dual-arm coordinator blueprints with trajectory control. Usage: + dimos run coordinator-dual-mock # Mock 7+6 DOF arms dimos run coordinator-dual-xarm # XArm7 left + XArm6 right dimos run coordinator-piper-xarm # XArm6 + Piper """ from __future__ import annotations -from dimos.control.blueprints._hardware import manipulator +from dimos.control.blueprints._hardware import manipulator, mock_arm from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.global_config import global_config +# Dual mock arms (7-DOF left, 6-DOF right) +_mock_left = mock_arm("left_arm", 7) +_mock_right = mock_arm("right_arm", 6) + +coordinator_dual_mock = ControlCoordinator.blueprint( + hardware=[_mock_left, _mock_right], + tasks=[ + TaskConfig(name="traj_left", type="trajectory", joint_names=_mock_left.joints, priority=10), + TaskConfig( + name="traj_right", type="trajectory", joint_names=_mock_right.joints, priority=10 + ), + ], +) + # Dual XArm (XArm7 left, XArm6 right) _xarm7_left = manipulator( "left_arm", @@ -86,6 +101,7 @@ __all__ = [ + "coordinator_dual_mock", "coordinator_dual_xarm", "coordinator_piper_xarm", ] diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index ffd221e983..fc21073f82 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -40,8 +40,8 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.manipulation.planning.factory import create_planning_specs, create_world -from dimos.manipulation.planning.groups import ( - PlanningGroup, +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import ( joint_target_to_global_names, planning_group_id_from_selector, ) @@ -1491,15 +1491,16 @@ def _invoke_coordinator_task( return client.task_invoke(task_name, method, kwargs) @rpc - def execute(self, robot_name: RobotName | None = None) -> bool: + def execute(self) -> bool: """Execute planned trajectory via ControlCoordinator.""" - return self.execute_plan(self._last_plan, robot_name) + return self.execute_plan(self._last_plan) @rpc - def execute_plan( - self, plan: GeneratedPlan | None = None, robot_name: RobotName | None = None - ) -> bool: - """Project and execute a generated plan through affected trajectory tasks.""" + def execute_plan(self, plan: GeneratedPlan | None = None) -> bool: + """Project and execute a generated plan through affected trajectory tasks. + + TODO: proper time parametrization. + """ plan = plan or self._last_plan if plan is None or not plan.path: logger.warning("No generated plan") @@ -1513,19 +1514,14 @@ def execute_plan( except Exception as exc: return self._fail(f"Failed to resolve generated plan: {exc}") logger.info( - "Execute plan: groups=%s, affected=%s, requested_robot=%s", + "Execute plan: groups=%s, affected=%s", plan.group_ids, affected, - robot_name, ) - robot_names = [robot_name] if robot_name is not None else affected assert self._world_monitor is not None dispatches: list[tuple[RobotName, RobotModelConfig, JointTrajectory]] = [] - for name in robot_names: - if name not in affected: - logger.error("Generated plan does not affect robot '%s'", name) - return False + for name in affected: robot = self._get_robot(name) if robot is None: return False @@ -1864,7 +1860,7 @@ def _preview_execute_wait( self.preview_plan(duration=preview_duration, robot_name=robot_name) logger.info("Executing trajectory...") - if not self.execute(robot_name): + if not self.execute(): return SkillResult.fail("EXECUTION_FAILED", "Trajectory execution failed") if not self._wait_for_trajectory_completion(robot_name): diff --git a/dimos/manipulation/planning/examples/manipulation_client.py b/dimos/manipulation/planning/examples/manipulation_client.py index 335d549b85..57ba11b002 100644 --- a/dimos/manipulation/planning/examples/manipulation_client.py +++ b/dimos/manipulation/planning/examples/manipulation_client.py @@ -164,13 +164,13 @@ def preview( duration: float | None = None, robot_name: str | None = None, ) -> bool: - """Preview the last generated plan in Meshcat.""" + """Preview the last generated plan in Visualizer.""" return _client.preview_plan(None, duration, robot_name) -def execute(robot_name: str | None = None) -> bool: +def execute() -> bool: """Execute planned trajectory via coordinator.""" - return _client.execute(robot_name) + return _client.execute() def home(robot_name: str | None = None) -> bool: @@ -180,7 +180,7 @@ def home(robot_name: str | None = None) -> bool: home_joints = _client.get_robot_info(robot_name).get("home_joints", [0.0] * 7) success = _client.plan_to_joints(JointState(position=home_joints), robot_name) if success: - return _client.execute(robot_name) + return _client.execute() return False diff --git a/dimos/manipulation/planning/groups/__init__.py b/dimos/manipulation/planning/groups/__init__.py deleted file mode 100644 index ba142e5654..0000000000 --- a/dimos/manipulation/planning/groups/__init__.py +++ /dev/null @@ -1,53 +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. - -"""Planning-group domain models, discovery, registry, and helpers.""" - -from dimos.manipulation.planning.groups.discovery import ( - FALLBACK_PLANNING_GROUP_NAME, - PlanningGroupDiscoveryError, - discover_planning_group_definitions, - generate_fallback_planning_group, - parse_srdf_planning_groups, -) -from dimos.manipulation.planning.groups.models import ( - PlanningGroup, - PlanningGroupDefinition, - PlanningGroupSelection, - PlanningGroupSource, -) -from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry -from dimos.manipulation.planning.groups.utils import ( - filter_joint_state_to_selected_joints, - joint_target_to_global_names, - matching_global_joint_name, - planning_group_id_from_selector, -) - -__all__ = [ - "FALLBACK_PLANNING_GROUP_NAME", - "PlanningGroup", - "PlanningGroupDefinition", - "PlanningGroupDiscoveryError", - "PlanningGroupRegistry", - "PlanningGroupSelection", - "PlanningGroupSource", - "discover_planning_group_definitions", - "filter_joint_state_to_selected_joints", - "generate_fallback_planning_group", - "joint_target_to_global_names", - "matching_global_joint_name", - "parse_srdf_planning_groups", - "planning_group_id_from_selector", -] diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index dc77a68d6b..574d7d2bee 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -30,11 +30,8 @@ import numpy as np -from dimos.manipulation.planning.groups import ( - PlanningGroup, - PlanningGroupSelection, - filter_joint_state_to_selected_joints, -) +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.utils import filter_joint_state_to_selected_joints from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import ( IKResult, diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index 19b2a73a4d..bfb28b4639 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -25,11 +25,8 @@ import numpy as np -from dimos.manipulation.planning.groups import ( - PlanningGroup, - PlanningGroupSelection, - matching_global_joint_name, -) +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.utils import matching_global_joint_name from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.planning_identifiers import make_global_joint_name from dimos.manipulation.planning.spec.config import RobotModelConfig diff --git a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py index 8eedeb5d4d..a850b12aab 100644 --- a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py +++ b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py @@ -20,7 +20,7 @@ from pathlib import Path from typing import cast -from dimos.manipulation.planning.groups import PlanningGroup +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.kinematics.jacobian_ik import JacobianIK from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index 1ec9b7bd1b..fdc4728093 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -25,7 +25,7 @@ import pytest from dimos.manipulation.planning.factory import create_kinematics -from dimos.manipulation.planning.groups import PlanningGroup +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.kinematics.pink_ik import ( PinkIK, diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index ca542030c9..cf0c605a3c 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -21,7 +21,7 @@ from typing import TYPE_CHECKING, Any from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.manipulation.planning.groups import PlanningGroupRegistry +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry 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.models import PlanningSceneInfo diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 28f01f4b78..1e2d4fc396 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -26,7 +26,7 @@ import numpy as np -from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.planning_identifiers import ( local_joint_name_from_global, make_global_joint_names, diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py index 6d4dd20c9e..4436aba595 100644 --- a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -23,7 +23,7 @@ import numpy as np -from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import PlanningStatus diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index b7d4fd4b50..cb795b2fbb 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,10 +21,8 @@ from pydantic import Field from dimos.core.module import ModuleConfig -from dimos.manipulation.planning.groups import ( - FALLBACK_PLANNING_GROUP_NAME, - PlanningGroupDefinition, -) +from dimos.manipulation.planning.groups.discovery import FALLBACK_PLANNING_GROUP_NAME +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.planning_identifiers import ( assert_local_joint_names, assert_valid_robot_name, @@ -50,6 +48,7 @@ class RobotModelConfig(ModuleConfig): group target frames instead. base_link: Compatibility robot-scoped base link used by current Drake weld/placement behavior. Planning groups own chain base links. + TODO: should remove package_paths: Dict mapping package names to filesystem Paths joint_limits_lower: Lower joint limits (radians) joint_limits_upper: Upper joint limits (radians) diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index 5f9ceb22fe..dfe7e9e427 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -29,7 +29,7 @@ import numpy as np from numpy.typing import NDArray - from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection + from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( GeneratedPlan, @@ -168,7 +168,10 @@ def get_group_jacobian(self, ctx: Any, group_id: PlanningGroupID) -> NDArray[np. ... def get_ee_pose(self, ctx: Any, robot_id: WorldRobotID) -> PoseStamped: - """Get pose for a robot's unique pose-targetable planning group.""" + """Get pose for a robot's unique pose-targetable planning group. + + TODO: deprecate this. + """ ... def get_link_pose( diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_utils.py index 1186372ae0..d479f34a4e 100644 --- a/dimos/manipulation/planning/test_planning_group_utils.py +++ b/dimos/manipulation/planning/test_planning_group_utils.py @@ -18,7 +18,8 @@ import pytest -from dimos.manipulation.planning.groups import PlanningGroup, joint_target_to_global_names +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import joint_target_to_global_names from dimos.msgs.sensor_msgs.JointState import JointState diff --git a/dimos/manipulation/planning/test_planning_groups.py b/dimos/manipulation/planning/test_planning_groups.py index 303c469e67..dd95d1b961 100644 --- a/dimos/manipulation/planning/test_planning_groups.py +++ b/dimos/manipulation/planning/test_planning_groups.py @@ -20,7 +20,7 @@ import pytest -from dimos.manipulation.planning.groups import ( +from dimos.manipulation.planning.groups.discovery import ( FALLBACK_PLANNING_GROUP_NAME, PlanningGroupDiscoveryError, discover_planning_group_definitions, diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 5d70c0024f..23956c4a21 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,7 +25,7 @@ import numpy as np -from dimos.manipulation.planning.groups import PlanningGroupRegistry +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry from dimos.manipulation.planning.planning_identifiers import ( assert_local_joint_names, make_global_joint_name, diff --git a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py index 5a1f8f1ecb..edaf5cacfe 100644 --- a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -21,7 +21,8 @@ import numpy as np import pytest -from dimos.manipulation.planning.groups import PlanningGroupDefinition, PlanningGroupRegistry +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.world.drake_world import DrakeWorld, _RobotData from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index b30dd9776a..3aeae3fd92 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -28,7 +28,7 @@ ManipulationModuleConfig, ManipulationState, ) -from dimos.manipulation.planning.groups import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig diff --git a/dimos/manipulation/visualization/viser/adapter.py b/dimos/manipulation/visualization/viser/adapter.py index 0dec749d55..82acc1f63a 100644 --- a/dimos/manipulation/visualization/viser/adapter.py +++ b/dimos/manipulation/visualization/viser/adapter.py @@ -27,7 +27,7 @@ if TYPE_CHECKING: from dimos.manipulation.manipulation_module import ManipulationModule - from dimos.manipulation.planning.groups import PlanningGroup + from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName, WorldRobotID @@ -224,13 +224,7 @@ def plan_target_set(self, joint_targets: dict[PlanningGroupID, JointState]) -> b def preview_plan(self, robot_name: RobotName | None = None) -> bool: return self._module.preview_plan(robot_name=robot_name) - def preview_target_set_plan(self) -> bool: - return self._module.preview_plan() - - def execute(self, robot_name: RobotName | None = None) -> bool: - return self._module.execute(robot_name) - - def execute_target_set_plan(self) -> bool: + def execute(self) -> bool: return self._module.execute() def cancel(self) -> bool: diff --git a/dimos/manipulation/visualization/viser/config.py b/dimos/manipulation/visualization/viser/config.py index d77cc66a15..c7e66e4de1 100644 --- a/dimos/manipulation/visualization/viser/config.py +++ b/dimos/manipulation/visualization/viser/config.py @@ -54,13 +54,6 @@ class ViserVisualizationConfig(BaseModel): default=0.02, validation_alias=AliasChoices("current_match_tolerance", "viser_current_match_tolerance"), ) - target_evaluation_check_collision: bool = Field( - default=True, - validation_alias=AliasChoices( - "target_evaluation_check_collision", - "viser_target_evaluation_check_collision", - ), - ) allow_plan_execute: bool = False @property diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 0398935faa..5bb7a00d0c 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -714,7 +714,7 @@ def _on_transform_update( group_ids=self.state.selected_group_ids, auxiliary_group_ids=self._selected_auxiliary_group_ids(), pose_targets=self._active_pose_targets(), - check_collision=self.config.target_evaluation_check_collision, + check_collision=True, ) ) self.refresh() @@ -981,7 +981,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.PREVIEWING - ok = self.adapter.preview_target_set_plan() + ok = self.adapter.preview_plan() self._finish_operation(f"preview={ok}", operation_id=operation_id) self._operation_worker.submit( @@ -1010,7 +1010,7 @@ def operation() -> None: return self.state.action_status = ActionStatus.EXECUTING self.state.plan_state.status = PlanStatus.EXECUTING - ok = self.adapter.execute_target_set_plan() + ok = self.adapter.execute() if not self._operation_is_current(operation_id): return if not ok: diff --git a/dimos/manipulation/visualization/viser/test_operation_worker.py b/dimos/manipulation/visualization/viser/test_operation_worker.py index 71a7857188..b6d0f4a7f5 100644 --- a/dimos/manipulation/visualization/viser/test_operation_worker.py +++ b/dimos/manipulation/visualization/viser/test_operation_worker.py @@ -144,9 +144,6 @@ def plan_to_joints(self, joints: JointState, robot_name: str | None = None) -> b def plan_target_set(self, joint_targets: dict[str, JointState]) -> bool: return True - def preview_target_set_plan(self) -> bool: - return True - def test_operation_worker_uses_per_operation_timeout() -> None: errors: list[str] = [] diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 7ae56ce380..1c12ee0cb8 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -1751,7 +1751,7 @@ def test_gui_can_disable_collision_check_for_cartesian_target_evaluation( gui = make_panel( FakeGuiServer(), adapter, - ViserVisualizationConfig(panel_enabled=True, target_evaluation_check_collision=False), + ViserVisualizationConfig(panel_enabled=True), scene, ) request = TargetEvaluationRequest( diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 4e2f12eb7e..cc5797553d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -21,6 +21,7 @@ "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper", "coordinator-combined-xarm6": "dimos.control.blueprints.teleop:coordinator_combined_xarm6", + "coordinator-dual-mock": "dimos.control.blueprints.dual:coordinator_dual_mock", "coordinator-dual-xarm": "dimos.control.blueprints.dual:coordinator_dual_xarm", "coordinator-flowbase": "dimos.control.blueprints.mobile:coordinator_flowbase", "coordinator-flowbase-keyboard-teleop": "dimos.control.blueprints.mobile:coordinator_flowbase_keyboard_teleop", diff --git a/dimos/robot/config.py b/dimos/robot/config.py index ca73cb384f..13a3aa83ee 100644 --- a/dimos/robot/config.py +++ b/dimos/robot/config.py @@ -28,7 +28,7 @@ from dimos.control.components import HardwareComponent, HardwareType from dimos.control.coordinator import TaskConfig -from dimos.manipulation.planning.groups import discover_planning_group_definitions +from dimos.manipulation.planning.groups.discovery import discover_planning_group_definitions from dimos.manipulation.planning.planning_identifiers import make_global_joint_names from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped