From fe7a7de5874ffbc4ba290e97aae6a863205dd4f0 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 4 Jun 2026 13:41:32 -0700 Subject: [PATCH 01/14] 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/14] 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 4e25297e72a260e5dcba0365941aceaee3a72993 Mon Sep 17 00:00:00 2001 From: cc Date: Sun, 19 Jul 2026 22:44:26 -0700 Subject: [PATCH 03/14] add mattskill --- docs/agents/domain.md | 60 ++++++++++++++++++++++++++++++++++ docs/agents/issue-tracker.md | 62 ++++++++++++++++++++++++++++++++++++ 2 files changed, 122 insertions(+) create mode 100644 docs/agents/domain.md create mode 100644 docs/agents/issue-tracker.md diff --git a/docs/agents/domain.md b/docs/agents/domain.md new file mode 100644 index 0000000000..e1de27973a --- /dev/null +++ b/docs/agents/domain.md @@ -0,0 +1,60 @@ +# DimOS agent domain context + +## Context loading + +Before working on a change, load the repository context in this order: + +1. Read `AGENTS.md` and follow its applicable instructions. +2. Read `openspec/config.yaml` for the OpenSpec schema, terminology, and rules. +3. Read the relevant files under `openspec/specs/`. +4. Read the root `CONTEXT.md` if it exists. +5. Read relevant records under `docs/adr/` if that directory exists. + +`CONTEXT.md` and `docs/adr/` are optional. If either is absent, continue +silently; do not report the absence as an error. Select specs and ADRs based on +the affected behavior and implementation surface rather than reading +unrelated material. + +## Two meanings of “spec” + +Keep these terms separate: + +- An **OpenSpec spec** is a behavior specification under `openspec/specs/`. + It describes observable behavior, user or developer outcomes, public + interfaces, safety constraints, and testable scenarios. +- A **DimOS Python Spec Protocol** is a code-level interface contract, usually + a `Protocol` inheriting from `dimos.spec.utils.Spec`, often found in a + `*_spec.py` file. It describes module RPCs and injected interfaces. + +An OpenSpec spec is not a Python Protocol, and a Python Protocol does not +replace an OpenSpec behavioral requirement. Keep implementation details such as +class names, module wiring, stream types, generated registries, and rollout +steps in the OpenSpec change design or tasks unless they are externally +observable. + +## Work layout + +Organize work through this chain: + +```text +Linear issue -> OpenSpec change -> implementation tasks -> pull request +``` + +Linear provides intake and tracking. The OpenSpec change is the source of truth +for the behavioral change, design, and tasks. The pull request implements and +reviews those tasks. Keep the identifiers and links aligned across all three +artifacts; any Linear link edit requires user confirmation before it is made. + +When a task affects behavior, update the relevant OpenSpec change and, where +appropriate, the corresponding spec under `openspec/specs/`. Include concrete +scenarios for behavioral requirements. Call out DimOS Python Spec Protocols, +blueprint composition, streams, skills/MCP exposure, generated files, and +hardware, simulation, or replay assumptions in design and task material when +they are relevant. + +## Conflicting guidance + +Surface conflicts between an ADR and an OpenSpec spec explicitly. Do not +silently reconcile, overwrite, or guess which decision applies. Report the +conflict, identify the affected behavior or implementation, and ask for the +decision or update the authoritative document only when instructed. diff --git a/docs/agents/issue-tracker.md b/docs/agents/issue-tracker.md new file mode 100644 index 0000000000..c0db692d0f --- /dev/null +++ b/docs/agents/issue-tracker.md @@ -0,0 +1,62 @@ +# Issue tracking with Linear + +## Workspace + +DimOS work is tracked in the **DIM** team in Linear: + + + +Access Linear through the configured Linear MCP. Do not assume that a local +copy, an unconfigured client, or a direct API call is an alternative source of +truth. + +## Confirmation policy + +User confirmation is required immediately before **every** Linear edit. This +includes, without limitation: + +- creating an issue; +- changing any issue field, including title, description, assignee, priority, + project, or due date; +- adding, removing, or changing labels; +- posting comments; +- changing state or making any other state transition; and +- adding, removing, or changing links. + +Reading Linear is not an edit. Before an edit, state exactly what will change +and wait for explicit user confirmation. One confirmation does not authorize +later edits, even when they concern the same issue or change. + +## Linking convention + +Keep the work chain navigable: + +```text +Linear issue <-> openspec/changes/ <-> pull request +``` + +Use the OpenSpec change ID as the stable identifier in the relationship. Link +the Linear issue to the relevant OpenSpec change and link the pull request to +both when the tools support those links. If a link must be created or changed, +it is a Linear edit and requires confirmation under the policy above. + +## Source of truth and workflow + +Linear is the intake and tracking system. It records requests, ownership, +status, discussion, and delivery progress. OpenSpec is the source of truth for +the behavioral change, its design, and its implementation tasks. The pull +request is the review and delivery vehicle. + +Use this sequence: + +1. Capture or find the Linear issue in the DIM team. +2. Create or update `openspec/changes//` for the proposed behavior, + design, and tasks. +3. Implement the tasks and keep the OpenSpec change current. +4. Open the pull request and connect it to the issue and OpenSpec change. +5. Reflect progress in Linear only after confirming each requested edit. + +Do not use a Linear description, comment, or state as a substitute for an +OpenSpec requirement, design decision, or task. If Linear and OpenSpec +disagree about behavior, treat OpenSpec as authoritative and surface the +discrepancy to the user rather than silently choosing a version. From e96832d0d3c71331ac27582a5741e0eca10d9999 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 29 Jul 2026 21:15:41 -0700 Subject: [PATCH 04/14] spec: formalize trajectory parametrization --- CONTEXT.md | 20 ++- ...-one-trajectory-parametrization-backend.md | 3 + ...scope-roboplan-toppra-to-roboplan-world.md | 3 + ...parametrize-during-plan-materialization.md | 3 + ...-post-processing-out-of-parametrization.md | 3 + ...ust-parametrizer-collision-preservation.md | 3 + ...-urdf-motion-limits-for-roboplan-toppra.md | 3 + .../.openspec.yaml | 2 + .../add-trajectory-parametrization/design.md | 170 ++++++++++++++++++ .../add-trajectory-parametrization/docs.md | 38 ++++ .../proposal.md | 41 +++++ .../spec.md | 124 +++++++++++++ .../add-trajectory-parametrization/tasks.md | 52 ++++++ pyproject.toml | 4 +- uv.lock | 4 +- 15 files changed, 468 insertions(+), 5 deletions(-) create mode 100644 docs/adr/0001-select-one-trajectory-parametrization-backend.md create mode 100644 docs/adr/0002-scope-roboplan-toppra-to-roboplan-world.md create mode 100644 docs/adr/0003-parametrize-during-plan-materialization.md create mode 100644 docs/adr/0004-keep-geometric-post-processing-out-of-parametrization.md create mode 100644 docs/adr/0005-trust-parametrizer-collision-preservation.md create mode 100644 docs/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md create mode 100644 openspec/changes/add-trajectory-parametrization/.openspec.yaml create mode 100644 openspec/changes/add-trajectory-parametrization/design.md create mode 100644 openspec/changes/add-trajectory-parametrization/docs.md create mode 100644 openspec/changes/add-trajectory-parametrization/proposal.md create mode 100644 openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md create mode 100644 openspec/changes/add-trajectory-parametrization/tasks.md diff --git a/CONTEXT.md b/CONTEXT.md index d97571b990..2a69e36dcf 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -1,6 +1,8 @@ # Manipulation Planning -This context describes requests for planning robot motion through joint and Cartesian spaces. +This context describes requests for planning robot motion through joint and +Cartesian spaces, assigning time to that motion, and executing it through robot +control. ## Language @@ -25,3 +27,19 @@ A Cartesian timing policy that resolves the requested path into joint space and **Custom Planner Components**: Backend-native solver tasks, constraints, and barriers injected as live objects. These are outside standard Cartesian planning and require a separate constrained-IK interface. + +**Geometric Path**: +An ordered sequence of robot configurations describing where a robot may move, without prescribing when it reaches them. +_Avoid_: Untimed trajectory, parametrized path + +**Timed Trajectory**: +A robot motion expressed on a shared time domain, including timed configurations and their motion derivatives where available. +_Avoid_: Parametrized path, timed path + +**Generated Plan**: +The accepted manipulation result pairing a geometric path with the timed trajectory prepared for preview and execution. +_Avoid_: Path, trajectory + +**Trajectory Parametrization**: +The conversion of a geometric path into a timed trajectory under motion limits, including the bounded interpolation needed to define continuous motion between waypoints. +_Avoid_: Path planning, trajectory generation diff --git a/docs/adr/0001-select-one-trajectory-parametrization-backend.md b/docs/adr/0001-select-one-trajectory-parametrization-backend.md new file mode 100644 index 0000000000..b0733e23ce --- /dev/null +++ b/docs/adr/0001-select-one-trajectory-parametrization-backend.md @@ -0,0 +1,3 @@ +# Select one trajectory parametrization backend at startup + +Each manipulation deployment selects exactly one trajectory parametrization backend at startup. If that backend cannot parametrize a geometric path, plan materialization fails explicitly; the system does not fall back to another parametrizer because doing so would silently change trajectory semantics, timing, and failure behavior. A selected backend may use its own documented safety behavior between internal curve-fitting modes, such as RoboPlan TOPP-RA falling back from a colliding linear blend to Hermite fitting. diff --git a/docs/adr/0002-scope-roboplan-toppra-to-roboplan-world.md b/docs/adr/0002-scope-roboplan-toppra-to-roboplan-world.md new file mode 100644 index 0000000000..b3975d2710 --- /dev/null +++ b/docs/adr/0002-scope-roboplan-toppra-to-roboplan-world.md @@ -0,0 +1,3 @@ +# Scope RoboPlan TOPP-RA to RoboPlanWorld + +The RoboPlan TOPP-RA parametrization backend accepts geometric paths produced by any planner, but it is available only when the manipulation world is `RoboPlanWorld`. This preserves planner independence without introducing and synchronizing a second RoboPlan robot model for other world backends; unsupported backend combinations fail during startup. diff --git a/docs/adr/0003-parametrize-during-plan-materialization.md b/docs/adr/0003-parametrize-during-plan-materialization.md new file mode 100644 index 0000000000..3edcc52952 --- /dev/null +++ b/docs/adr/0003-parametrize-during-plan-materialization.md @@ -0,0 +1,3 @@ +# Parametrize during plan materialization + +Trajectory parametrization runs immediately after geometric planning, before a `GeneratedPlan` is accepted or cached. Preview and execution therefore consume the same validated timed trajectory, and parametrization failures prevent an untimed plan from being presented as ready rather than surfacing during execution. diff --git a/docs/adr/0004-keep-geometric-post-processing-out-of-parametrization.md b/docs/adr/0004-keep-geometric-post-processing-out-of-parametrization.md new file mode 100644 index 0000000000..e15946b1a8 --- /dev/null +++ b/docs/adr/0004-keep-geometric-post-processing-out-of-parametrization.md @@ -0,0 +1,3 @@ +# Keep geometric post-processing out of trajectory parametrization + +Trajectory parametrization converts an accepted geometric path into a timed trajectory under motion limits. It may perform bounded interpolation or curve fitting needed to define continuous motion between the supplied waypoints, but it does not rewrite the source path through shortcutting, waypoint simplification, or path-class-specific resampling; those operations belong to plan generation or its post-processing stage. diff --git a/docs/adr/0005-trust-parametrizer-collision-preservation.md b/docs/adr/0005-trust-parametrizer-collision-preservation.md new file mode 100644 index 0000000000..6a1dafbcd2 --- /dev/null +++ b/docs/adr/0005-trust-parametrizer-collision-preservation.md @@ -0,0 +1,3 @@ +# Trust the parametrizer to preserve collision validity + +DimOS does not independently collision-check every sample of a returned timed trajectory during plan materialization. A parametrization backend that fits a curve away from the source waypoint polyline is responsible for collision-checking that curve against its authoritative world; DimOS validates the returned trajectory's structure and motion limits without duplicating the backend's potentially expensive collision pass. diff --git a/docs/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md b/docs/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md new file mode 100644 index 0000000000..6afaae2d5a --- /dev/null +++ b/docs/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md @@ -0,0 +1,3 @@ +# Use URDF motion limits for RoboPlan TOPP-RA + +The RoboPlan TOPP-RA backend uses velocity and acceleration limits loaded by its RoboPlan scene from the robot URDF. Missing required limits fail explicitly rather than falling back to DimOS's current generic motion-limit fields; formal, globally named per-joint DimOS overrides are deferred to a separate change and will later map into RoboPlan's supported limit-override mechanism. diff --git a/openspec/changes/add-trajectory-parametrization/.openspec.yaml b/openspec/changes/add-trajectory-parametrization/.openspec.yaml new file mode 100644 index 0000000000..d581a3210f --- /dev/null +++ b/openspec/changes/add-trajectory-parametrization/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-07-30 diff --git a/openspec/changes/add-trajectory-parametrization/design.md b/openspec/changes/add-trajectory-parametrization/design.md new file mode 100644 index 0000000000..7b3585ec6e --- /dev/null +++ b/openspec/changes/add-trajectory-parametrization/design.md @@ -0,0 +1,170 @@ +## Context + +`ManipulationModule._materialize_generated_plan()` currently validates a planner path, resolves selected-joint limits, directly constructs `JointTrajectoryGenerator`, and stores its output beside the source path in `GeneratedPlan`. `JointTrajectoryGenerator` creates an independent trapezoidal profile for each adjacent waypoint pair, so every interior waypoint is a stop. Dense paths consequently execute much more slowly than their geometric length and robot limits imply. + +The current execution architecture is intentionally atomic: a cached `GeneratedPlan` contains both its source path and executable `JointTrajectory`, and `PlanExecutionManager` dispatches that stored trajectory without regenerating it. This design preserves that contract while introducing a deep path-to-trajectory adapter seam. + +RoboPlan 0.5.1 provides TOPP-RA with Hermite, cubic, adaptive, and linear-blend curve-fitting modes. Its parameterizer owns collision preservation for fitted curves and obtains absolute velocity and acceleration limits from its RoboPlan scene. The DimOS `RobotModelConfig` motion-limit fields are currently informal and are not authoritative for this backend. + +## Goals / Non-Goals + +**Goals:** + +- Select one parametrization backend at manipulation-stack startup. +- Preserve the existing simple segmented-trapezoid behavior as a compatibility backend. +- Add RoboPlan TOPP-RA for any planner path represented in `RoboPlanWorld`. +- Convert and validate a path before constructing or caching `GeneratedPlan`. +- Preserve the source path while allowing bounded backend interpolation between waypoints. +- Keep preview and execution on the exact trajectory accepted during planning. +- Use URDF-backed RoboPlan velocity and acceleration limits and fail clearly when they are unavailable. +- Retain current public manipulation RPC, skill, MCP, stream, and execution signatures. + +**Non-Goals:** + +- Path shortcutting, waypoint simplification, or path-class-specific resampling. +- Linear-TCP constraint metadata or constraint-aware geometric post-processing. +- Reparametrizing one stored geometric path at multiple speeds. +- Runtime backend switching or cross-backend fallback. +- Formal per-joint DimOS motion-limit models or RoboPlan YAML overrides. +- Independent DimOS collision resampling of trajectories already checked by RoboPlan. +- Jerk-limited execution or changes to the trajectory message schema. + +## DimOS Architecture + +### Configuration and startup + +Add a typed `TrajectoryParametrizationConfig` under manipulation planning configuration. It selects `simple_trapezoid` or `roboplan_toppra` and carries backend-specific options: + +- common operating scales and output sample period; +- simple-backend point density/minimum segment controls needed for compatibility; +- RoboPlan spline-fitting mode and its adaptive/blend controls. + +The configuration factory validates the complete backend combination during startup. `roboplan_toppra` requires a finalized `RoboPlanWorld`; a non-RoboPlan world is rejected before planning. The selected parametrizer is constructed once and retained by `ManipulationModule`. + +### Adapter Protocol + +Introduce a small adapter `Protocol`, distinct from an RPC-oriented DimOS `Spec`, for path-to-trajectory conversion. Its input contains: + +- selected planning-group IDs; +- exact global joint ordering; +- the validated source `JointState` path; +- backend configuration. + +Its output is the canonical `JointTrajectory`, or it raises/returns a typed failure that plan materialization converts into the module's existing planning error surface. The adapter must not mutate the input path. + +The simple adapter wraps `JointTrajectoryGenerator` and receives its existing DimOS-resolved limits. The RoboPlan adapter owns a finalized `RoboPlanWorld`/`RoboPlanModel` reference, resolves the selected group from the model, converts global names to native RoboPlan ordering, invokes `PathParameterizerTOPPRA`, and converts the native result back to exact selected global ordering. + +The RoboPlan adapter may cache one native TOPP-RA parameterizer per selected group set. This is internal optimization; construction and use must remain safe under the manipulation module's existing planning concurrency rules. + +### Plan materialization + +Retain `GeneratedPlan` as the canonical accepted aggregate: + +```text +PlanningResult.path + │ + ▼ +canonical input validation + │ + ▼ +startup-selected TrajectoryParametrizer + │ + ▼ +canonical timed-output validation + │ + ▼ +GeneratedPlan(path + trajectory) +``` + +Replace the direct `JointTrajectoryGenerator` construction inside materialization with the selected adapter. A failure at either parametrization or validation leaves `_last_plan` unset and follows the existing planning-epoch failure path. No separate public `GeneratedTrajectory` lifecycle is added. + +Canonical validation retains the current strong invariants: exact global joint ordering, finite and dimensionally aligned positions/velocities, first time at zero, strictly increasing times, positive duration for non-noop motion, and preserved start/goal. It also checks returned motion against the applicable velocity and acceleration limits with a documented numerical tolerance. Where RoboPlan exposes native accelerations, validate them before converting to the current positions/velocities-only message; otherwise derive the acceleration check consistently from velocity samples. + +### RoboPlan limits and fitting + +Pin RoboPlan to `0.5.1`. `RoboPlanModel.scene` remains the source of native joint order and absolute TOPP-RA limits: + +- velocity from the URDF model; +- acceleration from extended URDF joint limits. + +DimOS `max_velocity`, `velocity_limits`, and `max_acceleration` do not override RoboPlan TOPP-RA in this change. Startup or first selected-group construction fails explicitly when a required URDF limit is missing or invalid. Common TOPP-RA velocity and acceleration scales may reduce, but never increase, the scene limits. + +Default RoboPlan fitting is `LinearBlend`, subject to confirming the 0.5.1 Python binding names during implementation. Other supported modes remain startup-selectable. Curve fitting is part of path-to-trajectory conversion, but preprocessing that rewrites the source waypoint sequence is not. + +RoboPlan owns collision checking for a fitted curve against its authoritative scene. DimOS does not repeat that expensive collision pass. RoboPlan's documented internal transition to a safe fitting mode remains within the selected `roboplan_toppra` backend and is allowed; failure of the backend as a whole does not invoke `simple_trapezoid`. + +### Other DimOS surfaces + +No streams, transports, module references, blueprint composition, RPC signatures, skills, MCP tools, CLI commands, or generated registry inputs change. Existing preview and execution flows consume the stored `GeneratedPlan.trajectory`. No `all_blueprints.py` regeneration is expected. + +## Decisions + +### Keep `GeneratedPlan` as the accepted aggregate + +The feature does not introduce separately cached geometric-plan and timed-trajectory artifacts because no current caller retimes one plan multiple ways. A narrow internal adapter provides extensibility without changing the public lifecycle. + +Alternative: restore frontier's public `GeneratedPlan`/`GeneratedTrajectory`/dispatch split. Rejected because it conflicts with the newer atomic execution architecture and solves no current use case. + +### Select one backend for the run + +Backend selection is startup configuration. A selected backend's failure fails materialization; no other backend is attempted. + +Alternative: fall back to the simple backend after TOPP-RA failure. Rejected because it silently changes timing and stop behavior. + +### Scope RoboPlan TOPP-RA to `RoboPlanWorld` + +TOPP-RA accepts paths from any planner, but it reuses the authoritative RoboPlan scene and planning groups rather than building and synchronizing a second RoboPlan model for other worlds. + +Alternative: make RoboPlan TOPP-RA work with Drake by constructing a shadow RoboPlan scene. Deferred because model, naming, group, and limit synchronization add complexity without a current deployment need. + +### Keep geometric post-processing outside this feature + +The parametrizer may fit a bounded continuous curve while producing a trajectory. It does not shortcut, simplify, resample, or replace the source waypoint sequence. + +Alternative: port frontier's adaptive uniform waypoint decimator. Rejected because RoboPlan provides path-shortcutting and path-specific resampling facilities, and those operations change planning geometry. + +### Trust backend collision preservation + +RoboPlan owns collision checking introduced by its fitting mode. DimOS validates representation and motion constraints without repeating collision checks. + +Alternative: sample and collision-check the complete returned trajectory again in DimOS. Rejected for duplicated cost and competing backend logic. + +### Use URDF limits for RoboPlan + +RoboPlan scene limits are authoritative. Generic existing DimOS defaults are not injected. + +Alternative: wire current scalar/list DimOS fields into RoboPlan. Rejected because their ordering, provenance, defaults, and test coverage are insufficient. Formal globally named per-joint overrides are future work. + +## Safety / Simulation / Replay + +- Hardware never receives a trajectory that failed canonical validation or whose selected backend failed. +- Missing or invalid URDF motion limits fail rather than selecting generic defaults. +- The TOPP-RA reduction scales are constrained to safe ranges and cannot raise URDF limits. +- Simulation uses the same materialized trajectory path as hardware and is the primary manual QA surface. +- Preview must show the exact stored trajectory later dispatched by execution. +- Replay behavior is unaffected because no stream or replay-data format changes. +- Manual QA should compare simple and TOPP-RA trajectories for the same RoboPlan-world path, check smooth traversal of interior waypoints, and verify explicit failures for missing limits and incompatible startup configuration before any hardware trial. + +## Risks / Trade-offs + +- Existing robot URDFs may lack acceleration attributes. Mitigation: inventory relevant manipulation models, add valid model limits where authoritative, and test the failure diagnostic. +- RoboPlan's Python binding may expose names or return shapes different from the C++ documentation. Mitigation: add a focused API contract test against pinned 0.5.1 before integrating. +- `LinearBlend` can deviate from the waypoint polyline. Mitigation: bound deviation through backend configuration and rely on RoboPlan's scene collision check/internal safe-mode behavior. +- Finite-difference acceleration validation can be sensitive to output sampling. Mitigation: prefer native accelerations when available and document a numerical tolerance. +- Pinning RoboPlan 0.5.1 upgrades Pinocchio/Coal transitive dependencies. Mitigation: retain the regenerated lockfile and run focused RoboPlan world/planning tests. +- Simple and RoboPlan backends use different absolute-limit sources. Mitigation: document this explicitly; formal unified limit overrides remain separate work. + +## Migration / Rollout + +1. Land the RoboPlan 0.5.1 pin and compatible lock update. +2. Add configuration, adapter protocol, factory validation, and the wrapped simple backend while preserving its default behavior. +3. Add the RoboPlan TOPP-RA adapter and URDF-limit validation. +4. Route plan materialization through the startup-selected adapter. +5. Update manipulation planning docs with backend compatibility, limit requirements, and configuration examples. +6. Run focused manipulation/RoboPlan tests and manual simulation preview/execute QA before enabling TOPP-RA on hardware. + +Rollback is configuration-only while the simple backend remains available. No generated blueprint registry update or persistent data migration is required. + +## Open Questions + +None. Implementation must verify the exact RoboPlan 0.5.1 Python binding surface and choose numerical validation tolerances, but the intended behavior and ownership boundaries are decided. diff --git a/openspec/changes/add-trajectory-parametrization/docs.md b/openspec/changes/add-trajectory-parametrization/docs.md new file mode 100644 index 0000000000..d532b6d65e --- /dev/null +++ b/openspec/changes/add-trajectory-parametrization/docs.md @@ -0,0 +1,38 @@ +## User-Facing Docs + +- Update `docs/capabilities/manipulation/index.md` with the path-to-trajectory lifecycle and the fact that preview and execution use the same materialized trajectory. +- Update `docs/capabilities/manipulation/adding_a_custom_arm.md` to require valid URDF velocity and extended acceleration limits when RoboPlan TOPP-RA is selected. +- Update `dimos/manipulation/planning/README.md` with: + - startup backend selection and configuration examples; + - `simple_trapezoid` versus `roboplan_toppra`; + - the `RoboPlanWorld` compatibility requirement; + - supported RoboPlan fitting modes and bounded deviation; + - no cross-backend fallback; + - URDF limit ownership and explicit missing-limit failures. + +## Contributor Docs + +- No new standalone contributor guide is required. +- If implementation reveals a non-obvious RoboPlan packaging or URDF 1.2 limit convention, add a focused note under `docs/development/` rather than expanding user-facing architecture prose. +- Keep the architecture decisions under `docs/adr/` and ensure the OpenSpec design remains consistent with them. + +## Coding-Agent Docs + +- Update `AGENTS.md` only if trajectory-parametrizer extension guidance becomes a stable coding-agent workflow. If updated, document: + - the geometric-path versus timed-trajectory boundary; + - startup-only backend selection; + - RoboPlan URDF limit ownership; + - the prohibition on silent cross-backend fallback. +- No coding-agent doc update is required merely for private class or file names. + +## Doc Validation + +- Run `doclinks` for changed Markdown links. +- Run `md-babel-py run dimos/manipulation/planning/README.md` if executable Python or shell examples are added or modified. +- Run `md-babel-py run docs/capabilities/manipulation/adding_a_custom_arm.md` if executable examples are changed. +- Run `bin/gen-diagrams` only if a checked-in generated diagram source is introduced or changed. +- Run the repository's documentation build/check command applicable to changed capability pages. + +## No Docs Needed + +Not applicable. Backend selection and URDF motion-limit requirements affect robot configuration and failure behavior, so user-facing documentation is required. diff --git a/openspec/changes/add-trajectory-parametrization/proposal.md b/openspec/changes/add-trajectory-parametrization/proposal.md new file mode 100644 index 0000000000..5395eed721 --- /dev/null +++ b/openspec/changes/add-trajectory-parametrization/proposal.md @@ -0,0 +1,41 @@ +## Why + +Manipulation planning currently turns every pair of geometric waypoints into an independent trapezoidal segment. The robot therefore stops at every waypoint, so dense planner output produces slow and mechanically awkward motion instead of one continuous trajectory constrained by the robot's actual motion limits. + +DimOS needs an explicit path-to-trajectory parametrization boundary that can retain the current simple implementation while allowing RoboPlan TOPP-RA to generate continuous, time-optimal trajectories. The selected behavior must be deterministic at startup, fail before a plan is exposed as executable, and use authoritative robot limits. + +## What Changes + +- Add startup configuration that selects exactly one manipulation trajectory parametrization backend for the lifetime of the stack. +- Preserve the existing simple trapezoid behavior as a selectable compatibility backend. +- Add a RoboPlan TOPP-RA backend for any geometric path planned against `RoboPlanWorld`, independent of which planner produced that path. +- Parametrize immediately after geometric planning and only construct/cache a `GeneratedPlan` after trajectory generation and validation succeed. +- Allow the selected backend to perform bounded interpolation or curve fitting while converting the source path into a timed trajectory. +- Use RoboPlan scene limits sourced from URDF velocity and acceleration limits; fail explicitly when required limits or the selected backend are unavailable. +- Do not switch parametrization backends after startup or fall back to another backend when parametrization fails. +- Exclude geometric path shortcutting, waypoint simplification, path-specific resampling, and formal DimOS per-joint limit overrides from this change. +- Pin the optional RoboPlan dependency to version `0.5.1`. + +## Affected DimOS Surfaces + +- Modules/streams: manipulation plan materialization, planning configuration/models, a trajectory-parametrizer adapter protocol, RoboPlan world/model integration, and timed-trajectory validation; no stream contract changes. +- Blueprints/CLI: manipulation blueprint configuration gains a startup-selectable parametrization backend; no new CLI command or blueprint name is introduced. +- Skills/MCP: existing plan, preview, and execute surfaces retain their signatures; unsuccessful parametrization makes planning fail before preview or execution. +- Hardware/simulation/replay: hardware and simulation execute the exact trajectory accepted during planning; RoboPlan TOPP-RA requires URDF velocity and acceleration limits. Replay behavior is unchanged. +- Docs/generated registries: manipulation planning documentation and dependency guidance require updates; no generated blueprint registry change is expected. + +## Capabilities + +### New Capabilities + +- `manipulation-trajectory-parametrization`: Startup backend selection and conversion of accepted geometric manipulation paths into validated timed trajectories. + +### Modified Capabilities + +None. + +## Impact + +Users may choose the existing simple backend or RoboPlan TOPP-RA at startup. RoboPlan TOPP-RA configurations become stricter: they require `RoboPlanWorld`, RoboPlan `0.5.1`, and usable URDF velocity and acceleration limits. Parametrization failures are reported as planning/materialization failures rather than being deferred to execution or hidden by fallback. + +The implementation touches manipulation planning internals and dependency resolution but does not intentionally break existing plan, preview, execute, skill, MCP, stream, or CLI signatures. Verification requires backend/configuration tests, trajectory invariant and failure tests, RoboPlan adapter tests, dependency lock validation, simulation/manual preview and execution QA, and documentation validation. diff --git a/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md b/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md new file mode 100644 index 0000000000..fb56d8b21c --- /dev/null +++ b/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md @@ -0,0 +1,124 @@ +## ADDED Requirements + +### Requirement: A single trajectory parametrization backend is selected at startup + +The manipulation stack SHALL select exactly one trajectory parametrization backend during startup and SHALL use that backend for every plan materialized during that run. + +#### Scenario: Simple backend is selected +- **GIVEN** a manipulation stack configured with the simple trajectory parametrization backend +- **WHEN** the stack starts successfully +- **THEN** every accepted geometric path MUST be converted by the simple backend +- **AND** the backend MUST remain unchanged for the lifetime of the running stack + +#### Scenario: RoboPlan TOPP-RA backend is selected +- **GIVEN** a manipulation stack configured with the RoboPlan TOPP-RA backend and `RoboPlanWorld` +- **WHEN** the stack starts successfully +- **THEN** every accepted geometric path MUST be converted by RoboPlan TOPP-RA +- **AND** the planner that produced the path MUST NOT be required to be RoboPlan's planner + +#### Scenario: Backend and world are incompatible +- **GIVEN** a manipulation stack configured with RoboPlan TOPP-RA and a non-RoboPlan world +- **WHEN** the stack is initialized +- **THEN** initialization MUST fail with an actionable configuration error +- **AND** planning MUST NOT begin with a backend that cannot operate against the configured world + +### Requirement: Parametrization completes before a generated plan is accepted + +The manipulation stack SHALL convert an accepted geometric path into a timed trajectory before exposing or caching the corresponding generated plan. + +#### Scenario: Parametrization succeeds +- **GIVEN** a planner has returned a successful geometric path +- **WHEN** the selected backend produces a valid timed trajectory +- **THEN** the system SHALL construct and cache one generated plan containing the source path and timed trajectory +- **AND** preview and execution MUST consume that same accepted trajectory + +#### Scenario: Parametrization fails +- **GIVEN** a planner has returned a successful geometric path +- **WHEN** the selected backend cannot produce a valid timed trajectory +- **THEN** the system MUST report plan materialization failure +- **AND** it MUST NOT cache or expose that path as an executable generated plan + +### Requirement: Parametrization converts a path into continuous timed motion + +The selected backend SHALL convert the source geometric path into one trajectory with a shared time domain across every selected joint. + +#### Scenario: Multi-waypoint path is converted +- **GIVEN** a valid path containing two or more consistently ordered joint configurations +- **WHEN** the path is parametrized +- **THEN** the result MUST contain timed positions and velocities for every selected joint +- **AND** its time values MUST start at zero and increase strictly through the final trajectory duration + +#### Scenario: Backend performs bounded curve fitting +- **GIVEN** a backend mode that fits continuous motion between supplied waypoints +- **WHEN** the path is parametrized +- **THEN** the backend MAY produce samples that do not coincide with every interior waypoint +- **AND** it MUST preserve the source start and goal within the configured numerical tolerance +- **AND** it MUST honor its configured geometric-deviation and collision-preservation contract + +#### Scenario: Source geometric path remains available +- **GIVEN** a path is successfully converted with interpolation or bounded curve fitting +- **WHEN** the generated plan is inspected +- **THEN** its source geometric path MUST remain unchanged +- **AND** its timed trajectory MUST be stored as a distinct representation within the generated plan + +### Requirement: Timed trajectories satisfy canonical invariants + +The manipulation stack MUST reject timed trajectories that are malformed, non-finite, incorrectly ordered, or inconsistent with the selected joints and motion limits. + +#### Scenario: Valid trajectory is accepted +- **GIVEN** a backend returns a trajectory with the expected global joint ordering +- **WHEN** all samples have finite positions and velocities, strictly increasing finite times, preserved endpoints, and motion within applicable limits +- **THEN** the trajectory SHALL be accepted for preview and execution + +#### Scenario: Malformed trajectory is rejected +- **GIVEN** a backend returns missing samples, inconsistent dimensions, duplicate or decreasing times, non-finite values, unexpected joint names, or a mismatched endpoint +- **WHEN** the result is validated +- **THEN** plan materialization MUST fail with a diagnostic identifying the violated invariant +- **AND** the invalid trajectory MUST NOT reach execution + +#### Scenario: Motion limits are exceeded +- **GIVEN** a backend returns motion exceeding an applicable joint velocity or acceleration limit beyond numerical tolerance +- **WHEN** the result is validated +- **THEN** plan materialization MUST fail +- **AND** the generated motion MUST NOT be exposed as executable + +### Requirement: RoboPlan TOPP-RA uses authoritative URDF limits + +The RoboPlan TOPP-RA backend SHALL use the RoboPlan scene's joint velocity and acceleration limits sourced from the robot URDF. + +#### Scenario: Required URDF limits are present +- **GIVEN** every selected joint has usable velocity and acceleration limits in the URDF-backed RoboPlan scene +- **WHEN** RoboPlan TOPP-RA parametrizes a path +- **THEN** it MUST constrain the trajectory using those limits and the configured reduction scales + +#### Scenario: A required URDF limit is missing +- **GIVEN** at least one selected joint lacks a usable URDF velocity or acceleration limit +- **WHEN** RoboPlan TOPP-RA is initialized for or applied to that planning group +- **THEN** the operation MUST fail with a diagnostic naming the missing limit and affected joint +- **AND** the system MUST NOT substitute DimOS's generic motion-limit defaults + +### Requirement: Backend failures do not trigger cross-backend fallback + +The manipulation stack MUST NOT silently switch to another trajectory parametrization backend when the startup-selected backend fails. + +#### Scenario: Selected backend rejects a path +- **GIVEN** exactly one parametrization backend was selected at startup +- **WHEN** that backend rejects or fails to parametrize a path +- **THEN** plan materialization MUST fail using that backend's diagnostic +- **AND** no other parametrization backend may be invoked for the path + +#### Scenario: RoboPlan uses an internal safety fitting mode +- **GIVEN** RoboPlan TOPP-RA remains the selected backend +- **WHEN** RoboPlan applies its documented internal safety behavior between curve-fitting modes +- **THEN** the result MAY be accepted if the backend reports a valid trajectory +- **AND** this MUST NOT be treated as switching to a different parametrization backend + +### Requirement: Existing manipulation control surfaces remain compatible + +Trajectory parametrization SHALL integrate without changing the public plan, preview, execute, skill, MCP, or stream signatures. + +#### Scenario: Existing preview and execution flow +- **GIVEN** a generated plan was successfully materialized by either supported backend +- **WHEN** a caller invokes the existing preview or execute surface +- **THEN** the caller MUST use the same public operation and argument shape as before +- **AND** the accepted stored trajectory MUST be previewed or dispatched without retiming diff --git a/openspec/changes/add-trajectory-parametrization/tasks.md b/openspec/changes/add-trajectory-parametrization/tasks.md new file mode 100644 index 0000000000..611a52c726 --- /dev/null +++ b/openspec/changes/add-trajectory-parametrization/tasks.md @@ -0,0 +1,52 @@ +## 1. Configuration and Adapter Boundary + +- [x] 1.1 Pin `roboplan==0.5.1` in manipulation and lint dependencies and regenerate `uv.lock`. +- [ ] 1.2 Add a focused RoboPlan 0.5.1 binding contract test covering TOPP-RA construction, fitting-mode names, options, native trajectory fields, and missing-limit behavior. +- [ ] 1.3 Add typed startup configuration for `simple_trapezoid` and `roboplan_toppra`, including validated common scales/output period and backend-specific fitting controls. +- [ ] 1.4 Add the internal trajectory-parametrizer adapter Protocol and typed request/failure boundary without introducing a separate public generated-trajectory lifecycle. +- [ ] 1.5 Extend planning factory validation so exactly one parametrizer is constructed at startup and `roboplan_toppra` with a non-RoboPlan world fails before planning. + +## 2. Parametrization Backends + +- [ ] 2.1 Wrap the existing `JointTrajectoryGenerator` as the `simple_trapezoid` adapter while preserving current limit resolution, waypoint, and timing behavior. +- [ ] 2.2 Implement the RoboPlan TOPP-RA adapter using the finalized `RoboPlanWorld` model, selected-group lookup, and exact global-to-native joint mapping. +- [ ] 2.3 Validate that every selected RoboPlan joint has finite positive URDF-backed velocity and acceleration limits, with no fallback to generic DimOS motion-limit fields. +- [ ] 2.4 Map configured TOPP-RA fitting mode, output period, velocity/acceleration reduction scales, and adaptive/blend options into the pinned 0.5.1 API. +- [ ] 2.5 Convert RoboPlan native trajectory output back to exact selected global joint order and retain positions, velocities, timestamps, and native acceleration data long enough for limit validation. +- [ ] 2.6 Ensure a selected backend failure returns one actionable materialization error and never invokes the other backend; retain documented RoboPlan internal safe fitting-mode behavior. + +## 3. Plan Materialization and Validation + +- [ ] 3.1 Construct and retain the selected trajectory parametrizer during manipulation planning initialization. +- [ ] 3.2 Route `_materialize_generated_plan()` through the selected adapter while preserving the source `JointState` path unchanged in `GeneratedPlan`. +- [ ] 3.3 Preserve planning-epoch atomicity so parametrization or output-validation failure leaves no cached executable plan. +- [ ] 3.4 Extend canonical timed-trajectory validation for exact global joint ordering, dimensions, finite values, zero start time, strictly increasing times, positive non-noop duration, and start/goal preservation. +- [ ] 3.5 Validate returned velocity and acceleration against the backend's applicable limits with documented numerical tolerances, preferring native acceleration samples when available. +- [ ] 3.6 Verify preview and execution reuse the accepted stored trajectory without regeneration or retiming. + +## 4. Automated Tests + +- [ ] 4.1 Add adapter tests for valid simple and RoboPlan trajectories, multi-waypoint continuity, global/native reordering, composite planning groups, and configurable fitting modes. +- [ ] 4.2 Add startup/configuration tests for each backend, unknown backends, invalid scales/options, incompatible world selection, and startup-only backend lifetime. +- [ ] 4.3 Add RoboPlan limit tests for valid URDF velocity/acceleration limits, missing limits, non-finite or non-positive limits, reduction scales, and proof that generic DimOS defaults are not substituted. +- [ ] 4.4 Add materialization tests for backend failure, no cross-backend fallback, malformed/native output rejection, motion-limit rejection, and no plan caching after failure. +- [ ] 4.5 Update preview/execution tests to prove the exact accepted timed trajectory reaches visualization and the coordinator without regeneration. +- [ ] 4.6 Run focused test targets including `dimos/manipulation/test_generated_plan_materialization.py`, `dimos/manipulation/test_planning_factory.py`, `dimos/manipulation/test_roboplan_world.py`, `dimos/manipulation/test_plan_execution.py`, and new parametrizer tests. + +## 5. Documentation + +- [ ] 5.1 Update `dimos/manipulation/planning/README.md` with the path-to-trajectory lifecycle, backend configuration examples, RoboPlan fitting modes, `RoboPlanWorld` compatibility, no cross-backend fallback, and URDF limit requirements. +- [ ] 5.2 Update `docs/capabilities/manipulation/index.md` to explain that a plan is accepted only after parametrization and that preview and execution share the stored trajectory. +- [ ] 5.3 Update `docs/capabilities/manipulation/adding_a_custom_arm.md` with RoboPlan 0.5.1 URDF velocity and extended acceleration-limit requirements and missing-limit failure behavior. +- [ ] 5.4 Reconcile `CONTEXT.md` and `docs/adr/0001` through `docs/adr/0006` with the implemented names and behavior; update `AGENTS.md` only if stable extension guidance is added. + +## 6. Verification and Manual QA + +- [ ] 6.1 Run `OPENSPEC_TELEMETRY=0 openspec validate add-trajectory-parametrization`. +- [ ] 6.2 Run `uv lock --check` and verify RoboPlan resolves to exactly `0.5.1` on supported Python/platform markers. +- [ ] 6.3 Run `uv run mypy dimos/manipulation` and the repository's Ruff/pre-commit checks for changed Python files. +- [ ] 6.4 Run the focused tests from task 4.6 and the broader fast manipulation test suite. +- [ ] 6.5 Run `doclinks` and applicable `md-babel-py run` commands for changed documentation examples; run `bin/gen-diagrams` only if generated diagram sources changed. +- [ ] 6.6 Manually plan, preview, and execute a nontrivial multi-waypoint path in a manipulation simulation with `simple_trapezoid`, confirming compatibility behavior and identical preview/execution timing. +- [ ] 6.7 Manually repeat the simulation with `RoboPlanWorld` and `roboplan_toppra`, confirming smooth interior traversal, URDF-limit compliance, and identical preview/execution timing. +- [ ] 6.8 Manually verify actionable pre-motion failures for an incompatible world/backend combination, a missing URDF acceleration limit, and a TOPP-RA parametrization failure. diff --git a/pyproject.toml b/pyproject.toml index 35d88fe7db..762b8a70ad 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -291,7 +291,7 @@ manipulation = [ # Other "matplotlib>=3.7.1", "pyyaml>=6.0", - "roboplan>=0.5.1", + "roboplan==0.5.1", ] cpu = [ @@ -456,7 +456,7 @@ lint = [ "pytest==8.3.5", "python-can>=4", "python-socketio>=5.16.1", - "roboplan>=0.5.1", + "roboplan==0.5.1", "sounddevice>=0.5.5", "trimesh>=4.12", "watchdog>=3.0.0", diff --git a/uv.lock b/uv.lock index 3aa5e08a64..510e0a5c77 100644 --- a/uv.lock +++ b/uv.lock @@ -2117,7 +2117,7 @@ requires-dist = [ { name = "reportlab", marker = "extra == 'apriltag'", specifier = ">=4.5.0" }, { name = "rerun-sdk", specifier = "==0.32.0" }, { name = "rerun-sdk", marker = "extra == 'visualization'", specifier = "==0.32.0" }, - { name = "roboplan", marker = "extra == 'manipulation'", specifier = ">=0.5.1" }, + { name = "roboplan", marker = "extra == 'manipulation'", specifier = "==0.5.1" }, { name = "scipy", specifier = ">=1.15.1" }, { name = "sortedcontainers", specifier = "==2.4.0" }, { name = "sounddevice", marker = "extra == 'agents'" }, @@ -2179,7 +2179,7 @@ lint = [ { name = "pytest", specifier = "==8.3.5" }, { name = "python-can", specifier = ">=4" }, { name = "python-socketio", specifier = ">=5.16.1" }, - { name = "roboplan", specifier = ">=0.5.1" }, + { name = "roboplan", specifier = "==0.5.1" }, { name = "ruff", specifier = "==0.14.3" }, { name = "sounddevice", specifier = ">=0.5.5" }, { name = "tensorboard", specifier = "==2.20.0" }, From cd6139bbb28a6c506d815a6c300839bd88578d78 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 29 Jul 2026 23:10:14 -0700 Subject: [PATCH 05/14] feat(manipulation): add trajectory parametrization --- CONTEXT.md | 28 ++ dimos/manipulation/manipulation_module.py | 184 +++++++++++-- dimos/manipulation/planning/README.md | 95 ++++++- dimos/manipulation/planning/factory.py | 50 ++++ .../planning/trajectory_generator/config.py | 49 ++++ .../trajectory_generator/parametrizer.py | 62 +++++ .../roboplan_toppra_parametrizer.py | 241 ++++++++++++++++++ .../simple_parametrizer.py | 76 ++++++ .../trajectory_generator/test_config.py | 62 +++++ .../test_roboplan_toppra_contract.py | 111 ++++++++ .../test_roboplan_toppra_parametrizer.py | 225 ++++++++++++++++ .../test_simple_parametrizer.py | 94 +++++++ .../planning/world/roboplan_world.py | 6 + .../test_generated_plan_materialization.py | 148 ++++++++++- dimos/manipulation/test_manipulation_unit.py | 35 +++ dimos/manipulation/test_plan_execution.py | 11 +- dimos/manipulation/test_planning_factory.py | 56 ++++ dimos/manipulation/visualization/operator.py | 8 + .../visualization/test_operator.py | 17 ++ dimos/manipulation/visualization/viser/gui.py | 49 +++- .../viser/test_viser_visualization.py | 69 ++++- ...-one-trajectory-parametrization-backend.md | 3 - ...parametrize-during-plan-materialization.md | 3 - .../manipulation/adding_a_custom_arm.md | 45 +++- docs/capabilities/manipulation/index.md | 39 ++- ...-one-trajectory-parametrization-backend.md | 3 + ...scope-roboplan-toppra-to-roboplan-world.md | 0 ...parametrize-during-plan-materialization.md | 3 + ...-post-processing-out-of-parametrization.md | 0 ...ust-parametrizer-collision-preservation.md | 0 ...-urdf-motion-limits-for-roboplan-toppra.md | 0 .../add-trajectory-parametrization/design.md | 61 +++-- .../add-trajectory-parametrization/docs.md | 5 +- .../proposal.md | 9 +- .../spec.md | 47 +++- .../add-trajectory-parametrization/tasks.md | 73 +++--- 36 files changed, 1868 insertions(+), 99 deletions(-) create mode 100644 dimos/manipulation/planning/trajectory_generator/config.py create mode 100644 dimos/manipulation/planning/trajectory_generator/parametrizer.py create mode 100644 dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py create mode 100644 dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py create mode 100644 dimos/manipulation/planning/trajectory_generator/test_config.py create mode 100644 dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py create mode 100644 dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py create mode 100644 dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py delete mode 100644 docs/adr/0001-select-one-trajectory-parametrization-backend.md delete mode 100644 docs/adr/0003-parametrize-during-plan-materialization.md create mode 100644 docs/development/adr/0001-select-one-trajectory-parametrization-backend.md rename docs/{ => development}/adr/0002-scope-roboplan-toppra-to-roboplan-world.md (100%) create mode 100644 docs/development/adr/0003-parametrize-during-plan-materialization.md rename docs/{ => development}/adr/0004-keep-geometric-post-processing-out-of-parametrization.md (100%) rename docs/{ => development}/adr/0005-trust-parametrizer-collision-preservation.md (100%) rename docs/{ => development}/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md (100%) diff --git a/CONTEXT.md b/CONTEXT.md index 2a69e36dcf..98de271185 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -43,3 +43,31 @@ _Avoid_: Path, trajectory **Trajectory Parametrization**: The conversion of a geometric path into a timed trajectory under motion limits, including the bounded interpolation needed to define continuous motion between waypoints. _Avoid_: Path planning, trajectory generation + +**Planner-Native Timed Result**: +A planner result that already contains authoritative timestamps and velocities. +It bypasses trajectory parametrization, retains its time domain, and still +receives canonical timed-trajectory validation. +_Avoid_: Parametrization fallback + +## Trajectory Parametrization Boundary + +Each manipulation stack selects one parametrization backend at startup. +Untimed geometric paths use that backend before a `GeneratedPlan` can be +accepted. A failure does not switch backends and leaves no executable plan +cached. Planner-native timed results skip conversion because they are already +timed trajectories, not because the selected backend failed. + +`simple_trapezoid` uses the current DimOS motion-limit resolution. +`roboplan_toppra` is available only with `RoboPlanWorld` and uses finite, +positive URDF velocity and extended acceleration limits from the RoboPlan +scene. It does not substitute the current generic DimOS limit fields. Preview +and execution share the accepted trajectory time domain; execution may only +project global joints into robot-local order without regenerating or retiming. + +**Next-Plan Speed**: +A runtime reduction scale in `(0, 1]` applied when generating a future +trajectory. Changing it never mutates or retimes an accepted `GeneratedPlan`; +the operator must plan again. Viser exposes this policy through its +`Next plan speed` slider. +_Avoid_: Playback speed, execution override diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c8ced56229..a15ee2fc81 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -49,6 +49,7 @@ KinematicsName, WorldBackend, create_planning_specs, + create_trajectory_parametrizer, create_world, ) from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection @@ -81,9 +82,17 @@ WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, + TrajectoryParametrizationConfig, +) from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( JointTrajectoryGenerator, ) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + TrajectoryParametrizationRequest, + TrajectoryParametrizer, +) from dimos.manipulation.skill_errors import ManipulationSkillError from dimos.manipulation.visualization.config import ( ManipulationVisualizationConfig, @@ -103,6 +112,10 @@ logger = setup_logger() +_TRAJECTORY_POSITION_TOLERANCE = 1e-6 +_TRAJECTORY_LIMIT_RELATIVE_TOLERANCE = 1e-2 +_TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE = 1e-8 + # Composite type aliases for readability (using semantic IDs from planning.spec) RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] """(world_robot_id, config, trajectory_generator)""" @@ -145,6 +158,9 @@ class ManipulationModuleConfig(ModuleConfig): default_factory=NoManipulationVisualizationConfig ) planner: ManipulationPlannerConfig = Field(default_factory=RoboPlanPlannerConfig) + trajectory_parametrization: TrajectoryParametrizationConfig = Field( + default_factory=SimpleTrapezoidParametrizationConfig + ) kinematics: ManipulationKinematicsConfig = Field(default_factory=PinkKinematicsConfig) # Deprecated: use kinematics.backend instead. kinematics_name: KinematicsName | None = None @@ -177,11 +193,13 @@ def __init__(self, **kwargs: Any) -> None: self._lock = threading.Lock() self._error_message = "" self._planning_epoch = 0 + self._motion_speed_scale = 1.0 # Planning components (initialized in start()) self._world_monitor: WorldMonitor | None = None self._planner: PlannerSpec | None = None self._kinematics: KinematicsSpec | None = None + self._trajectory_parametrizer: TrajectoryParametrizer | None = None # Robot registry: maps robot_name -> (world_robot_id, config, trajectory_gen) self._robots: RobotRegistry = {} @@ -234,6 +252,7 @@ def _initialize_planning(self) -> None: planner=self.config.planner, kinematics_name=self.config.kinematics_name, kinematics=self.config.kinematics, + trajectory_parametrization=self.config.trajectory_parametrization, ) self._world_monitor = planning_specs.world_monitor self._planner = planning_specs.planner @@ -256,6 +275,11 @@ def _initialize_planning(self) -> None: operator = ManipulationOperator(self, self._world_monitor) self._world_monitor.finalize(visualization, operator=operator) + self._trajectory_parametrizer = create_trajectory_parametrizer( + self.config.trajectory_parametrization, + world=world, + world_backend=self.config.world_backend, + ) # Add floor obstacle to prevent trajectories below the table surface if self.config.floor_z is not None: @@ -422,6 +446,27 @@ def get_error(self) -> str: """ return self._error_message + @rpc + def set_motion_speed(self, speed_scale: float) -> bool: + """Set a runtime speed reduction for plans generated in the future. + + Existing accepted plans and dispatched trajectories remain unchanged. + Plan again after changing this value. + """ + if not math.isfinite(speed_scale) or speed_scale <= 0.0 or speed_scale > 1.0: + self._record_error("motion speed scale must be finite, > 0, and <= 1") + return False + with self._lock: + self._motion_speed_scale = float(speed_scale) + self._error_message = "" + return True + + @rpc + def get_motion_speed(self) -> float: + """Return the runtime speed reduction used for future plans.""" + with self._lock: + return self._motion_speed_scale + @rpc def cancel(self) -> bool: """Cancel current motion or invalidate an in-progress plan.""" @@ -621,6 +666,10 @@ def _validate_generated_trajectory( trajectory: JointTrajectory, expected_names: Sequence[str], waypoints: Sequence[Sequence[float]], + *, + velocity_limits: Sequence[float] | None = None, + acceleration_limits: Sequence[float] | None = None, + accelerations: Sequence[Sequence[float]] | None = None, ) -> None: expected = list(expected_names) if list(trajectory.joint_names) != expected: @@ -647,14 +696,97 @@ def _validate_generated_trajectory( non_noop = any(list(waypoint) != list(waypoints[0]) for waypoint in waypoints[1:]) if non_noop and trajectory.duration <= 0.0: raise ValueError("Generated trajectory duration must be positive") - waypoint_index = 0 - for point in trajectory.points: - if list(point.positions) == list(waypoints[waypoint_index]): - waypoint_index += 1 - if waypoint_index == len(waypoints): - break - if waypoint_index != len(waypoints): - raise ValueError("Generated trajectory does not contain ordered waypoint boundaries") + if not self._positions_close(trajectory.points[0].positions, waypoints[0]): + raise ValueError("Generated trajectory does not preserve the path start") + if not self._positions_close(trajectory.points[-1].positions, waypoints[-1]): + raise ValueError("Generated trajectory does not preserve the path goal") + if velocity_limits is not None: + self._validate_motion_limits( + trajectory, + velocity_limits, + acceleration_limits, + accelerations, + ) + + @staticmethod + def _positions_close(first: Sequence[float], second: Sequence[float]) -> bool: + return len(first) == len(second) and all( + math.isclose( + left, + right, + rel_tol=0.0, + abs_tol=_TRAJECTORY_POSITION_TOLERANCE, + ) + for left, right in zip(first, second, strict=True) + ) + + def _validate_motion_limits( + self, + trajectory: JointTrajectory, + velocity_limits: Sequence[float], + acceleration_limits: Sequence[float] | None, + accelerations: Sequence[Sequence[float]] | None, + ) -> None: + expected_dimension = len(trajectory.joint_names) + if len(velocity_limits) != expected_dimension: + raise ValueError("Velocity limits do not match selected joints") + self._assert_valid_motion_limits(velocity_limits, "velocity") + for point_index, point in enumerate(trajectory.points): + self._assert_within_limits( + point.velocities, + velocity_limits, + f"Generated point {point_index} velocity", + ) + if acceleration_limits is None: + return + if len(acceleration_limits) != expected_dimension: + raise ValueError("Acceleration limits do not match selected joints") + self._assert_valid_motion_limits(acceleration_limits, "acceleration") + if accelerations is not None: + if len(accelerations) != len(trajectory.points): + raise ValueError("Acceleration samples do not match trajectory points") + for point_index, values in enumerate(accelerations): + if len(values) != expected_dimension: + raise ValueError( + f"Generated point {point_index} acceleration dimension mismatch" + ) + self._assert_finite_sequence(values, f"Generated point {point_index} accelerations") + self._assert_within_limits( + values, + acceleration_limits, + f"Generated point {point_index} acceleration", + ) + return + for point_index in range(1, len(trajectory.points)): + previous = trajectory.points[point_index - 1] + current = trajectory.points[point_index] + dt = current.time_from_start - previous.time_from_start + derived = [ + (current_velocity - previous_velocity) / dt + for previous_velocity, current_velocity in zip( + previous.velocities, current.velocities, strict=True + ) + ] + self._assert_within_limits( + derived, + acceleration_limits, + f"Generated interval {point_index - 1}:{point_index} acceleration", + ) + + @staticmethod + def _assert_valid_motion_limits(values: Sequence[float], label: str) -> None: + if any(not math.isfinite(value) or value <= 0.0 for value in values): + raise ValueError(f"Invalid {label} limits") + + @staticmethod + def _assert_within_limits(values: Sequence[float], limits: Sequence[float], label: str) -> None: + for joint_index, (value, limit) in enumerate(zip(values, limits, strict=True)): + tolerance = max( + _TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE, + limit * _TRAJECTORY_LIMIT_RELATIVE_TOLERANCE, + ) + if abs(value) > limit + tolerance: + raise ValueError(f"{label} exceeds joint {joint_index} limit: {value} vs {limit}") def _materialize_generated_plan( self, group_ids: tuple[PlanningGroupID, ...], result_path: Sequence[JointState] @@ -664,19 +796,33 @@ def _materialize_generated_plan( expected_names = list(selection.joint_names) path = [JointState(state) for state in result_path] waypoints = self._validate_selected_path(path, expected_names) - velocities, accelerations = self._limits_for_global_joints(expected_names) - generator = JointTrajectoryGenerator( - num_joints=len(expected_names), - max_velocity=velocities, - max_acceleration=accelerations, + if self._trajectory_parametrizer is None: + raise ValueError("Trajectory parametrizer is not initialized") + velocity_limits: tuple[float, ...] | None = None + acceleration_limits: tuple[float, ...] | None = None + if self._trajectory_parametrizer.uses_request_limits: + velocities, accelerations = self._limits_for_global_joints(expected_names) + velocity_limits = tuple(velocities) + acceleration_limits = tuple(accelerations) + parametrized = self._trajectory_parametrizer.parametrize( + TrajectoryParametrizationRequest( + group_ids=group_ids, + joint_names=tuple(expected_names), + path=tuple(path), + velocity_limits=velocity_limits, + acceleration_limits=acceleration_limits, + speed_scale=self.get_motion_speed(), + ) ) - generated = generator.generate(waypoints) - trajectory = JointTrajectory( - joint_names=expected_names, - points=generated.points, - timestamp=generated.timestamp, + trajectory = parametrized.trajectory + self._validate_generated_trajectory( + trajectory, + expected_names, + waypoints, + velocity_limits=parametrized.velocity_limits, + acceleration_limits=parametrized.acceleration_limits, + accelerations=parametrized.accelerations, ) - self._validate_generated_trajectory(trajectory, expected_names, waypoints) return path, trajectory def _materialize_timed_generated_plan( diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index a6bcb64c43..3acfea151b 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -1,6 +1,7 @@ # Manipulation Planning Stack -Motion planning for robotic manipulators. Backend-agnostic design with Drake implementation. +Motion planning for robotic manipulators. The stack separates geometric path +planning from conversion to an executable timed trajectory. ## Quick Start @@ -17,7 +18,7 @@ python -i -m dimos.manipulation.planning.examples.manipulation_client # termina ``` In the interactive client: -```python +```python skip commands() # List available commands joints() # Get current joint positions plan([0.1] * 7) # Plan to target @@ -58,7 +59,7 @@ execute() # Execute via coordinator ## Using ManipulationModule -```python +```python skip from pathlib import Path from dimos.manipulation import ManipulationModule from dimos.manipulation.planning.spec import RobotModelConfig @@ -79,6 +80,7 @@ module = ManipulationModule( enable_viz=True, world_backend="drake", # RoboPlan is the default planner={"backend": "rrt_connect"}, # RoboPlan is the default + trajectory_parametrization={"backend": "simple_trapezoid"}, kinematics={"backend": "drake_optimization"}, # Or "jacobian" / "pink" ) module.start() @@ -86,6 +88,92 @@ module.plan_to_joints([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) module.execute() # Sends to coordinator ``` +## Path-to-Trajectory Lifecycle + +A joint-space planner normally returns an untimed geometric path. Before DimOS +accepts a `GeneratedPlan`, the one trajectory-parametrization backend selected +at startup converts that path into a timed `JointTrajectory`. DimOS then +validates joint ordering, dimensions, finite values, strictly increasing time, +start and goal preservation, and applicable velocity and acceleration limits. +A failure leaves no executable plan cached. + +A planner may instead return a trajectory that already contains timestamps and +velocities. That result is already on the trajectory side of the boundary, so +DimOS skips parametrization, preserves its timing, and applies the same +canonical structural validation. This is not fallback: a failure of the +selected parametrizer never invokes another backend. + +Preview and execution both consume the accepted stored trajectory. Execution +may project globally named joints into each robot's local order, but it does not +regenerate or retime the trajectory. + +When Viser is enabled, its **Next plan speed** slider selects a runtime +reduction from `0.05` to `1.0`. The value multiplies the configured velocity +and acceleration scales for the next plan. It does not modify the currently +accepted plan: move the slider, then press **Plan** again. Joint-space paths +apply the value during trajectory parametrization; Viser Cartesian requests +pass it to the native planner before that planner produces timestamps. + +## Trajectory Parametrization + +The default compatibility backend retains the existing segmented trapezoidal +behavior: + +```python skip +ManipulationModuleConfig( + trajectory_parametrization={ + "backend": "simple_trapezoid", + "velocity_scale": 1.0, + "acceleration_scale": 1.0, + "points_per_segment": 50, + }, +) +``` + +RoboPlan TOPP-RA produces continuous timing across a geometric path: + +```python skip +ManipulationModuleConfig( + world_backend="roboplan", + trajectory_parametrization={ + "backend": "roboplan_toppra", + "output_period": 0.01, + "velocity_scale": 0.8, + "acceleration_scale": 0.8, + "fitting_mode": "linear_blend", + "max_blend_deviation": 0.01, + }, +) +``` + +The selectable fitting modes are `hermite`, `cubic`, `adaptive`, and +`linear_blend`. Adaptive fitting also exposes `max_adaptive_iterations` and +`max_adaptive_step_size`. `linear_blend` exposes `max_blend_deviation`. + +`roboplan_toppra` can parametrize a geometric path from any planner, but only +when `world_backend="roboplan"`: it reuses the finalized `RoboPlanWorld` model +and planning groups. Selecting it with another world fails during startup. +DimOS pins RoboPlan to `0.5.1` for this integration. + +For every selected movable joint, the RoboPlan URDF must provide a finite, +positive velocity limit and an extended acceleration limit: + +```xml + +``` + +RoboPlan scene limits are authoritative for this backend. The current +`RobotModelConfig.max_velocity`, `velocity_limits`, and `max_acceleration` +fields are not substituted when a URDF limit is missing. Missing or invalid +limits fail plan materialization with the affected joint named. Formal +globally named per-joint overrides are future work. + ## RobotModelConfig Fields | Field | Description | @@ -133,6 +221,7 @@ accepted. | Backend | Description | |---------|-------------| | `DrakeWorld` | Drake physics with Meshcat visualization | +| `RoboPlanWorld` | RoboPlan model, collision scene, native planner, and TOPP-RA support | ## Blueprints diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index f3e25dd5f0..9195ad49d2 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -31,6 +31,14 @@ RoboPlanPlannerConfig, ) from dimos.manipulation.planning.spec.protocols import PlannerSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, + SimpleTrapezoidParametrizationConfig, + TrajectoryParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + TrajectoryParametrizer, +) from dimos.manipulation.visualization.config import ( ManipulationVisualizationConfig, NoManipulationVisualizationConfig, @@ -73,6 +81,7 @@ def validate_backend_combination( world_backend: str = "roboplan", planner_backend: str = "roboplan", kinematics_name: str = DEFAULT_KINEMATICS_NAME, + trajectory_parametrization_backend: str = "simple_trapezoid", ) -> None: """Validate manipulation backend choices before constructing the stack.""" if world_backend not in SUPPORTED_WORLD_BACKENDS: @@ -87,11 +96,48 @@ def validate_backend_combination( raise ValueError( f"Unknown kinematics solver: {kinematics_name}. Available: {list(SUPPORTED_KINEMATICS)}" ) + if trajectory_parametrization_backend not in ("simple_trapezoid", "roboplan_toppra"): + raise ValueError( + f"Unknown trajectory parametrization backend: {trajectory_parametrization_backend}" + ) if planner_backend == "roboplan" and world_backend != "roboplan": raise ValueError(_ROBOPLAN_PLANNER_REQUIRES_ROBOPLAN_WORLD) if kinematics_name == "drake_optimization" and world_backend != "drake": raise ValueError('kinematics_name="drake_optimization" requires world_backend="drake"') + if trajectory_parametrization_backend == "roboplan_toppra" and world_backend != "roboplan": + raise ValueError( + 'trajectory_parametrization.backend="roboplan_toppra" requires world_backend="roboplan"' + ) + + +def create_trajectory_parametrizer( + config: TrajectoryParametrizationConfig, + *, + world: WorldSpec, + world_backend: str, +) -> TrajectoryParametrizer: + """Construct the one startup-selected path parametrizer.""" + if config.backend == "roboplan_toppra" and world_backend != "roboplan": + raise ValueError( + 'trajectory_parametrization.backend="roboplan_toppra" requires world_backend="roboplan"' + ) + if isinstance(config, SimpleTrapezoidParametrizationConfig): + from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, + ) + + return SimpleTrapezoidParametrizer(config) + if isinstance(config, RoboPlanTOPPRAParametrizationConfig): + from dimos.manipulation.planning.trajectory_generator.roboplan_toppra_parametrizer import ( + RoboPlanTOPPRAParametrizer, + ) + from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld + + if not isinstance(world, RoboPlanWorld): + raise ValueError("RoboPlan TOPP-RA requires a finalized RoboPlanWorld instance") + return RoboPlanTOPPRAParametrizer(world, config) + raise TypeError(f"Unsupported trajectory parametrization config: {type(config).__name__}") def create_world( @@ -173,6 +219,7 @@ def create_planning_specs( planner: ManipulationPlannerConfig | None = None, kinematics_name: str | None = None, kinematics: ManipulationKinematicsConfig | None = None, + trajectory_parametrization: TrajectoryParametrizationConfig | None = None, ) -> PlanningSpecs: """Create planning specs around an already-created world.""" from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor @@ -183,11 +230,14 @@ def create_planning_specs( kinematics = kinematics_config_from_name(DEFAULT_KINEMATICS_NAME) if planner is None: planner = RoboPlanPlannerConfig() + if trajectory_parametrization is None: + trajectory_parametrization = SimpleTrapezoidParametrizationConfig() validate_backend_combination( world_backend=world_backend, planner_backend=planner.backend, kinematics_name=kinematics.backend, + trajectory_parametrization_backend=trajectory_parametrization.backend, ) return PlanningSpecs( diff --git a/dimos/manipulation/planning/trajectory_generator/config.py b/dimos/manipulation/planning/trajectory_generator/config.py new file mode 100644 index 0000000000..78e1d189a7 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/config.py @@ -0,0 +1,49 @@ +# 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. + +"""Typed configuration for manipulation trajectory parametrization.""" + +from typing import Annotated, Literal + +from pydantic import Field + +from dimos.protocol.service.spec import BaseConfig + + +class SimpleTrapezoidParametrizationConfig(BaseConfig): + """Configuration for the compatibility segmented-trapezoid backend.""" + + backend: Literal["simple_trapezoid"] = "simple_trapezoid" + velocity_scale: float = Field(default=1.0, gt=0.0, le=1.0) + acceleration_scale: float = Field(default=1.0, gt=0.0, le=1.0) + points_per_segment: int = Field(default=50, ge=1) + + +class RoboPlanTOPPRAParametrizationConfig(BaseConfig): + """Configuration for RoboPlan TOPP-RA path parametrization.""" + + backend: Literal["roboplan_toppra"] = "roboplan_toppra" + output_period: float = Field(default=0.01, gt=0.0) + velocity_scale: float = Field(default=1.0, gt=0.0, le=1.0) + acceleration_scale: float = Field(default=1.0, gt=0.0, le=1.0) + fitting_mode: Literal["hermite", "cubic", "adaptive", "linear_blend"] = "linear_blend" + max_adaptive_iterations: int = Field(default=10, ge=1) + max_adaptive_step_size: float = Field(default=0.05, gt=0.0) + max_blend_deviation: float = Field(default=0.01, ge=0.0) + + +TrajectoryParametrizationConfig = Annotated[ + SimpleTrapezoidParametrizationConfig | RoboPlanTOPPRAParametrizationConfig, + Field(discriminator="backend"), +] diff --git a/dimos/manipulation/planning/trajectory_generator/parametrizer.py b/dimos/manipulation/planning/trajectory_generator/parametrizer.py new file mode 100644 index 0000000000..52f8dff539 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/parametrizer.py @@ -0,0 +1,62 @@ +# 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. + +"""Internal path-to-trajectory parametrization boundary.""" + +from dataclasses import dataclass +import math +from typing import Protocol + +from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory + + +class TrajectoryParametrizationError(ValueError): + """A path could not be converted into a valid timed trajectory.""" + + +@dataclass(frozen=True) +class TrajectoryParametrizationRequest: + """Canonical input for an untimed selected-joint path.""" + + group_ids: tuple[PlanningGroupID, ...] + joint_names: tuple[str, ...] + path: tuple[JointState, ...] + velocity_limits: tuple[float, ...] | None = None + acceleration_limits: tuple[float, ...] | None = None + speed_scale: float = 1.0 + + def __post_init__(self) -> None: + if not math.isfinite(self.speed_scale) or self.speed_scale <= 0.0 or self.speed_scale > 1.0: + raise ValueError("speed_scale must be finite, > 0, and <= 1") + + +@dataclass(frozen=True) +class ParametrizedTrajectory: + """Canonical output plus the limits and accelerations used to validate it.""" + + trajectory: JointTrajectory + velocity_limits: tuple[float, ...] + acceleration_limits: tuple[float, ...] + accelerations: tuple[tuple[float, ...], ...] | None = None + + +class TrajectoryParametrizer(Protocol): + """Convert an untimed geometric path into one timed trajectory.""" + + @property + def uses_request_limits(self) -> bool: ... + + def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: ... diff --git a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py new file mode 100644 index 0000000000..ebb678781f --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py @@ -0,0 +1,241 @@ +# 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. + +"""RoboPlan TOPP-RA trajectory parametrization adapter.""" + +from dataclasses import dataclass +import math +import sys +from typing import Any + +import numpy as np +import roboplan.core as roboplan_core +import roboplan.toppra as roboplan_toppra + +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + ParametrizedTrajectory, + TrajectoryParametrizationError, + TrajectoryParametrizationRequest, +) +from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel +from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint + + +@dataclass(frozen=True) +class _GroupParametrizer: + group: RoboPlanGroup + native: Any + velocity_limits: tuple[float, ...] + acceleration_limits: tuple[float, ...] + + +class RoboPlanTOPPRAParametrizer: + """Convert selected-joint paths with a finalized RoboPlan scene.""" + + def __init__( + self, + world: RoboPlanWorld, + config: RoboPlanTOPPRAParametrizationConfig, + ) -> None: + self._world = world + self._config = config + self._groups: dict[frozenset[str], _GroupParametrizer] = {} + + @property + def uses_request_limits(self) -> bool: + """RoboPlan uses only limits from its authoritative scene.""" + return False + + def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: + try: + with self._world.parametrization_model() as model: + resolved = self._resolve_group(model, request) + native_path = self._native_path(resolved.group, request) + native_trajectory = resolved.native.generate( + native_path, self._options(request.speed_scale) + ) + return self._canonical_result(resolved, request, native_trajectory) + except TrajectoryParametrizationError: + raise + except (IndexError, KeyError, RuntimeError, TypeError, ValueError) as exc: + raise TrajectoryParametrizationError( + f"RoboPlan TOPP-RA parametrization failed: {exc}" + ) from exc + + def _resolve_group( + self, + model: RoboPlanModel, + request: TrajectoryParametrizationRequest, + ) -> _GroupParametrizer: + key = frozenset(request.group_ids) + cached = self._groups.get(key) + if cached is not None: + return cached + group = model.groups.get(key) + if group is None: + raise TrajectoryParametrizationError( + f"RoboPlan has no generated group for {list(request.group_ids)}" + ) + expected = set(request.joint_names) + if expected != set(group.public_names): + raise TrajectoryParametrizationError( + f"RoboPlan group '{group.name}' does not match selected joints" + ) + velocity_limits = self._limits( + model.scene.getVelocityLimitVectors(group.name), + group, + "velocity", + ) + acceleration_limits = self._limits( + model.scene.getAccelerationLimitVectors(group.name), + group, + "acceleration", + ) + resolved = _GroupParametrizer( + group=group, + native=roboplan_toppra.PathParameterizerTOPPRA(model.scene, group.name), + velocity_limits=tuple(value * self._config.velocity_scale for value in velocity_limits), + acceleration_limits=tuple( + value * self._config.acceleration_scale for value in acceleration_limits + ), + ) + self._groups[key] = resolved + return resolved + + @staticmethod + def _limits( + bounds: tuple[Any, Any], + group: RoboPlanGroup, + label: str, + ) -> tuple[float, ...]: + lower = np.asarray(bounds[0], dtype=np.float64) + upper = np.asarray(bounds[1], dtype=np.float64) + if lower.shape != upper.shape or len(lower) != len(group.native_names): + raise TrajectoryParametrizationError( + f"RoboPlan {label} limits do not match group '{group.name}'" + ) + by_public: dict[str, float] = {} + for public_name, low, high in zip(group.public_names, lower, upper, strict=True): + magnitude = min(abs(float(low)), abs(float(high))) + if not math.isfinite(magnitude) or magnitude <= 0.0 or magnitude >= sys.float_info.max: + raise TrajectoryParametrizationError( + f"RoboPlan group '{group.name}' has no usable URDF {label} " + f"limit for joint '{public_name}'" + ) + by_public[public_name] = magnitude + return tuple(by_public[name] for name in group.public_names) + + @staticmethod + def _native_path( + group: RoboPlanGroup, + request: TrajectoryParametrizationRequest, + ) -> Any: + public_index = {name: index for index, name in enumerate(request.joint_names)} + path = roboplan_core.JointPath() + path.joint_names = list(group.native_names) + path.positions = [ + np.asarray( + [state.position[public_index[public_name]] for public_name in group.public_names], + dtype=np.float64, + ) + for state in request.path + ] + return path + + def _options(self, speed_scale: float) -> Any: + return roboplan_toppra.TOPPRAOptions( + dt=self._config.output_period, + mode={ + "hermite": roboplan_toppra.SplineFittingMode.Hermite, + "cubic": roboplan_toppra.SplineFittingMode.Cubic, + "adaptive": roboplan_toppra.SplineFittingMode.Adaptive, + "linear_blend": roboplan_toppra.SplineFittingMode.LinearBlend, + }[self._config.fitting_mode], + velocity_scale=self._config.velocity_scale * speed_scale, + acceleration_scale=self._config.acceleration_scale * speed_scale, + max_adaptive_iterations=self._config.max_adaptive_iterations, + max_adaptive_step_size=self._config.max_adaptive_step_size, + max_blend_deviation=self._config.max_blend_deviation, + ) + + @staticmethod + def _canonical_result( + resolved: _GroupParametrizer, + request: TrajectoryParametrizationRequest, + native_trajectory: Any, + ) -> ParametrizedTrajectory: + native_names = tuple(native_trajectory.joint_names) + if set(native_names) != set(resolved.group.native_names): + raise TrajectoryParametrizationError("RoboPlan TOPP-RA returned unexpected joint names") + native_index = {name: index for index, name in enumerate(native_names)} + native_by_public = dict( + zip( + resolved.group.public_names, + resolved.group.native_names, + strict=True, + ) + ) + output_indices = [native_index[native_by_public[name]] for name in request.joint_names] + times = [float(value) for value in native_trajectory.times] + positions = list(native_trajectory.positions) + velocities = list(native_trajectory.velocities) + accelerations = list(native_trajectory.accelerations) + if not (len(times) == len(positions) == len(velocities) == len(accelerations)): + raise TrajectoryParametrizationError( + "RoboPlan TOPP-RA returned inconsistent trajectory fields" + ) + points = [ + TrajectoryPoint( + time_from_start=time, + positions=[float(position[index]) for index in output_indices], + velocities=[float(velocity[index]) for index in output_indices], + ) + for time, position, velocity in zip(times, positions, velocities, strict=True) + ] + canonical_accelerations = tuple( + tuple(float(acceleration[index]) for index in output_indices) + for acceleration in accelerations + ) + velocity_by_public = dict( + zip( + resolved.group.public_names, + resolved.velocity_limits, + strict=True, + ) + ) + acceleration_by_public = dict( + zip( + resolved.group.public_names, + resolved.acceleration_limits, + strict=True, + ) + ) + return ParametrizedTrajectory( + trajectory=JointTrajectory( + joint_names=list(request.joint_names), + points=points, + ), + velocity_limits=tuple( + velocity_by_public[name] * request.speed_scale for name in request.joint_names + ), + acceleration_limits=tuple( + acceleration_by_public[name] * request.speed_scale for name in request.joint_names + ), + accelerations=canonical_accelerations, + ) diff --git a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py new file mode 100644 index 0000000000..e938732548 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py @@ -0,0 +1,76 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Compatibility trajectory parametrizer using segmented trapezoids.""" + +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( + JointTrajectoryGenerator, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + ParametrizedTrajectory, + TrajectoryParametrizationError, + TrajectoryParametrizationRequest, +) +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory + + +class SimpleTrapezoidParametrizer: + """Wrap the existing trajectory generator behind the adapter protocol.""" + + def __init__(self, config: SimpleTrapezoidParametrizationConfig) -> None: + self._config = config + + @property + def uses_request_limits(self) -> bool: + """The compatibility backend uses limits resolved from DimOS config.""" + return True + + def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: + if request.velocity_limits is None or request.acceleration_limits is None: + raise TrajectoryParametrizationError( + "Simple trapezoid parametrization requires DimOS motion limits" + ) + velocity_limits = tuple( + value * self._config.velocity_scale * request.speed_scale + for value in request.velocity_limits + ) + acceleration_limits = tuple( + value * self._config.acceleration_scale * request.speed_scale + for value in request.acceleration_limits + ) + try: + generator = JointTrajectoryGenerator( + num_joints=len(request.joint_names), + max_velocity=list(velocity_limits), + max_acceleration=list(acceleration_limits), + points_per_segment=self._config.points_per_segment, + ) + generated = generator.generate([list(state.position) for state in request.path]) + except (IndexError, RuntimeError, TypeError, ValueError) as exc: + raise TrajectoryParametrizationError( + f"Simple trapezoid parametrization failed: {exc}" + ) from exc + trajectory = JointTrajectory( + joint_names=list(request.joint_names), + points=generated.points, + timestamp=generated.timestamp, + ) + return ParametrizedTrajectory( + trajectory=trajectory, + velocity_limits=velocity_limits, + acceleration_limits=acceleration_limits, + ) diff --git a/dimos/manipulation/planning/trajectory_generator/test_config.py b/dimos/manipulation/planning/trajectory_generator/test_config.py new file mode 100644 index 0000000000..0a583f81f6 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_config.py @@ -0,0 +1,62 @@ +# 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 trajectory-parametrization startup configuration.""" + +from pydantic import TypeAdapter, ValidationError +import pytest + +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, + SimpleTrapezoidParametrizationConfig, + TrajectoryParametrizationConfig, +) + + +@pytest.mark.parametrize( + ("payload", "expected_type"), + [ + ({"backend": "simple_trapezoid"}, SimpleTrapezoidParametrizationConfig), + ({"backend": "roboplan_toppra"}, RoboPlanTOPPRAParametrizationConfig), + ], +) +def test_trajectory_parametrization_config_selects_one_backend( + payload: dict[str, str], + expected_type: type[object], +) -> None: + result = TypeAdapter(TrajectoryParametrizationConfig).validate_python(payload) + + assert isinstance(result, expected_type) + + +@pytest.mark.parametrize( + "payload", + [ + {"backend": "unknown"}, + {"backend": "simple_trapezoid", "velocity_scale": 0.0}, + {"backend": "simple_trapezoid", "acceleration_scale": 1.01}, + {"backend": "simple_trapezoid", "points_per_segment": 0}, + {"backend": "roboplan_toppra", "output_period": 0.0}, + {"backend": "roboplan_toppra", "velocity_scale": 1.01}, + {"backend": "roboplan_toppra", "acceleration_scale": -0.1}, + {"backend": "roboplan_toppra", "max_adaptive_iterations": 0}, + {"backend": "roboplan_toppra", "max_adaptive_step_size": 0.0}, + {"backend": "roboplan_toppra", "max_blend_deviation": -0.1}, + ], +) +def test_trajectory_parametrization_config_rejects_invalid_options( + payload: dict[str, object], +) -> None: + with pytest.raises(ValidationError): + TypeAdapter(TrajectoryParametrizationConfig).validate_python(payload) diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py new file mode 100644 index 0000000000..5f918557bf --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py @@ -0,0 +1,111 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Contract tests for the pinned RoboPlan TOPP-RA Python binding.""" + +from pathlib import Path +import sys + +import numpy as np +import pytest + +roboplan_core = pytest.importorskip("roboplan.core") +roboplan_toppra = pytest.importorskip("roboplan.toppra") + +pytestmark = pytest.mark.self_hosted + + +def _scene(tmp_path: Path, *, acceleration: float | None) -> object: + acceleration_attribute = "" if acceleration is None else f' acceleration="{acceleration}"' + urdf = tmp_path / "robot.urdf" + urdf.write_text( + f"""\ + + + + + + + + + + +""" + ) + srdf = tmp_path / "robot.srdf" + srdf.write_text( + """\ + + + + + +""" + ) + return roboplan_core.Scene("contract_robot", urdf, srdf, []) + + +def test_roboplan_051_toppra_options_and_native_trajectory_contract(tmp_path: Path) -> None: + scene = _scene(tmp_path, acceleration=2.0) + options = roboplan_toppra.TOPPRAOptions( + dt=0.02, + mode=roboplan_toppra.SplineFittingMode.LinearBlend, + velocity_scale=0.5, + acceleration_scale=0.25, + max_adaptive_iterations=7, + max_adaptive_step_size=0.03, + max_blend_deviation=0.01, + ) + path = roboplan_core.JointPath() + path.joint_names = ["joint"] + path.positions = [ + np.asarray([0.0], dtype=np.float64), + np.asarray([0.2], dtype=np.float64), + np.asarray([0.4], dtype=np.float64), + ] + + trajectory = roboplan_toppra.PathParameterizerTOPPRA(scene, "arm").generate(path, options) + + assert list(roboplan_toppra.SplineFittingMode) == [ + roboplan_toppra.SplineFittingMode.Hermite, + roboplan_toppra.SplineFittingMode.Cubic, + roboplan_toppra.SplineFittingMode.Adaptive, + roboplan_toppra.SplineFittingMode.LinearBlend, + ] + assert options.dt == 0.02 + assert options.mode is roboplan_toppra.SplineFittingMode.LinearBlend + assert options.velocity_scale == 0.5 + assert options.acceleration_scale == 0.25 + assert options.max_adaptive_iterations == 7 + assert options.max_adaptive_step_size == 0.03 + assert options.max_blend_deviation == 0.01 + assert trajectory.joint_names == ["joint"] + assert len(trajectory.times) == len(trajectory.positions) + assert len(trajectory.velocities) == len(trajectory.positions) + assert len(trajectory.accelerations) == len(trajectory.positions) + assert trajectory.times[0] == 0.0 + assert trajectory.times[-1] > 0.0 + assert np.allclose(trajectory.positions[0], [0.0]) + assert np.allclose(trajectory.positions[-1], [0.4]) + + +def test_roboplan_051_missing_urdf_acceleration_is_effectively_unbounded( + tmp_path: Path, +) -> None: + scene = _scene(tmp_path, acceleration=None) + + lower, upper = scene.getAccelerationLimitVectors("arm") + + assert lower.tolist() == [-sys.float_info.max] + assert upper.tolist() == [sys.float_info.max] diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py new file mode 100644 index 0000000000..c6f476fcef --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -0,0 +1,225 @@ +# 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 the RoboPlan TOPP-RA trajectory parametrizer.""" + +from contextlib import contextmanager +from types import SimpleNamespace + +import numpy as np +import pytest +from pytest_mock import MockerFixture + +pytest.importorskip("roboplan.toppra") + +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + TrajectoryParametrizationError, + TrajectoryParametrizationRequest, +) +from dimos.manipulation.planning.trajectory_generator.roboplan_toppra_parametrizer import ( + RoboPlanTOPPRAParametrizer, +) +from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel +from dimos.msgs.sensor_msgs.JointState import JointState + +pytestmark = pytest.mark.self_hosted + + +class _Scene: + def __init__(self, *, missing_acceleration: bool = False) -> None: + self.missing_acceleration = missing_acceleration + + def getVelocityLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarray]: + assert group_name == "composite" + return np.asarray([-2.0, -1.0]), np.asarray([2.0, 1.0]) + + def getAccelerationLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarray]: + assert group_name == "composite" + maximum = np.finfo(np.float64).max if self.missing_acceleration else 4.0 + return np.asarray([-6.0, -maximum]), np.asarray([6.0, maximum]) + + +class _World: + def __init__(self, model: RoboPlanModel) -> None: + self.model = model + + @contextmanager + def parametrization_model(self): + yield self.model + + +def _model(*, missing_acceleration: bool = False) -> RoboPlanModel: + group = RoboPlanGroup( + group_ids=("left/arm", "right/arm"), + name="composite", + native_names=("native_b", "native_a"), + public_names=("right/b", "left/a"), + ) + return RoboPlanModel( + scene=_Scene(missing_acceleration=missing_acceleration), + groups={frozenset(group.group_ids): group}, + legacy_group_ids={}, + native_joint_by_global={}, + native_link_by_robot={}, + all_group=group, + ) + + +def _request( + names: tuple[str, str] = ("left/a", "right/b"), + *, + speed_scale: float = 1.0, +) -> TrajectoryParametrizationRequest: + positions_by_name = { + "left/a": (0.0, 0.3), + "right/b": (0.1, 0.4), + } + return TrajectoryParametrizationRequest( + group_ids=("right/arm", "left/arm"), + joint_names=names, + path=( + JointState( + name=list(names), + position=[positions_by_name[name][0] for name in names], + ), + JointState( + name=list(names), + position=[positions_by_name[name][1] for name in names], + ), + ), + velocity_limits=(999.0, 999.0), + acceleration_limits=(999.0, 999.0), + speed_scale=speed_scale, + ) + + +@pytest.mark.parametrize( + "fitting_mode", + ["hermite", "cubic", "adaptive", "linear_blend"], +) +def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( + mocker: MockerFixture, + fitting_mode: str, +) -> None: + generated = SimpleNamespace( + joint_names=["native_a", "native_b"], + times=[0.0, 0.5], + positions=[np.asarray([0.0, 0.1]), np.asarray([0.3, 0.4])], + velocities=[np.asarray([0.0, 0.0]), np.asarray([0.6, 0.2])], + accelerations=[np.asarray([0.0, 0.0]), np.asarray([1.2, 0.4])], + ) + native = mocker.MagicMock() + native.generate.return_value = generated + constructor = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + parametrizer = RoboPlanTOPPRAParametrizer( + _World(_model()), + RoboPlanTOPPRAParametrizationConfig( + fitting_mode=fitting_mode, + output_period=0.02, + velocity_scale=0.5, + acceleration_scale=0.25, + ), + ) + request = _request(speed_scale=0.5) + + result = parametrizer.parametrize(request) + + assert not parametrizer.uses_request_limits + constructor.assert_called_once() + native_path, options = native.generate.call_args.args + assert native_path.joint_names == ["native_b", "native_a"] + assert [row.tolist() for row in native_path.positions] == [ + [0.1, 0.0], + [0.4, 0.3], + ] + assert options.dt == 0.02 + assert options.mode.name.lower().replace("linearblend", "linear_blend") == fitting_mode + assert options.velocity_scale == 0.25 + assert options.acceleration_scale == 0.125 + assert result.velocity_limits == (0.25, 0.5) + assert result.acceleration_limits == (0.5, 0.75) + assert result.trajectory.joint_names == ["left/a", "right/b"] + assert [point.positions for point in result.trajectory.points] == [ + [0.0, 0.1], + [0.3, 0.4], + ] + assert [point.velocities for point in result.trajectory.points] == [ + [0.0, 0.0], + [0.6, 0.2], + ] + assert result.accelerations == ((0.0, 0.0), (1.2, 0.4)) + assert [state.position for state in request.path] == [[0.0, 0.1], [0.3, 0.4]] + + +def test_roboplan_parametrizer_rejects_missing_urdf_acceleration_without_fallback( + mocker: MockerFixture, +) -> None: + constructor = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA" + ) + parametrizer = RoboPlanTOPPRAParametrizer( + _World(_model(missing_acceleration=True)), + RoboPlanTOPPRAParametrizationConfig(), + ) + + with pytest.raises( + TrajectoryParametrizationError, + match="no usable URDF acceleration limit for joint 'left/a'", + ): + parametrizer.parametrize(_request()) + + constructor.assert_not_called() + + +def test_cached_group_limits_follow_each_request_joint_order( + mocker: MockerFixture, +) -> None: + generated = SimpleNamespace( + joint_names=["native_b", "native_a"], + times=[0.0, 0.5], + positions=[np.asarray([0.0, 0.1]), np.asarray([0.3, 0.4])], + velocities=[np.asarray([0.0, 0.0]), np.asarray([0.6, 0.2])], + accelerations=[np.asarray([0.0, 0.0]), np.asarray([1.2, 0.4])], + ) + native = mocker.MagicMock() + native.generate.return_value = generated + constructor = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + parametrizer = RoboPlanTOPPRAParametrizer( + _World(_model()), + RoboPlanTOPPRAParametrizationConfig( + velocity_scale=0.5, + acceleration_scale=0.25, + ), + ) + + canonical = parametrizer.parametrize(_request()) + reversed_order = parametrizer.parametrize(_request(("right/b", "left/a"))) + + constructor.assert_called_once() + assert canonical.velocity_limits == (0.5, 1.0) + assert canonical.acceleration_limits == (1.0, 1.5) + assert reversed_order.velocity_limits == (1.0, 0.5) + assert reversed_order.acceleration_limits == (1.5, 1.0) diff --git a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py new file mode 100644 index 0000000000..e8a623c244 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py @@ -0,0 +1,94 @@ +# 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 the compatibility trajectory parametrizer.""" + +import pytest + +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + TrajectoryParametrizationError, + TrajectoryParametrizationRequest, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _request(*, speed_scale: float = 1.0) -> TrajectoryParametrizationRequest: + names = ("arm/a", "arm/b") + return TrajectoryParametrizationRequest( + group_ids=("arm/manipulator",), + joint_names=names, + path=( + JointState(name=list(names), position=[0.0, 0.0]), + JointState(name=list(names), position=[0.2, 0.1]), + JointState(name=list(names), position=[0.4, 0.0]), + ), + velocity_limits=(2.0, 4.0), + acceleration_limits=(6.0, 8.0), + speed_scale=speed_scale, + ) + + +def test_simple_parametrizer_preserves_segmented_trapezoid_behavior() -> None: + request = _request(speed_scale=0.5) + parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig( + velocity_scale=0.5, + acceleration_scale=0.25, + points_per_segment=4, + ) + ) + + result = parametrizer.parametrize(request) + + assert parametrizer.uses_request_limits + assert result.velocity_limits == (0.5, 1.0) + assert result.acceleration_limits == (0.75, 1.0) + assert result.accelerations is None + assert result.trajectory.joint_names == list(request.joint_names) + assert len(result.trajectory.points) == 9 + assert result.trajectory.points[0].positions == [0.0, 0.0] + assert result.trajectory.points[4].positions == [0.2, 0.1] + assert result.trajectory.points[-1].positions == [0.4, 0.0] + assert [state.position for state in request.path] == [ + [0.0, 0.0], + [0.2, 0.1], + [0.4, 0.0], + ] + + +def test_simple_parametrizer_requires_dimos_limits() -> None: + request = _request() + request = TrajectoryParametrizationRequest( + group_ids=request.group_ids, + joint_names=request.joint_names, + path=request.path, + ) + + with pytest.raises( + TrajectoryParametrizationError, + match="requires DimOS motion limits", + ): + SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()).parametrize(request) + + +@pytest.mark.parametrize("speed_scale", [0.0, -0.1, 1.01, float("inf"), float("nan")]) +def test_parametrization_request_rejects_invalid_runtime_speed(speed_scale: float) -> None: + with pytest.raises(ValueError, match="speed_scale"): + _request(speed_scale=speed_scale) diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 3f1580c7af..ee918218cb 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -968,6 +968,12 @@ def _require_model(self) -> RoboPlanModel: raise RuntimeError("RoboPlan model is not initialized; finalize the world first") return self._model + @contextmanager + def parametrization_model(self) -> Generator[RoboPlanModel, None, None]: + """Yield the finalized trajectory model under the world scene lock.""" + with self._lock: + yield self._require_model() + def _full_scene_q( self, ctx: RoboPlanContext, diff --git a/dimos/manipulation/test_generated_plan_materialization.py b/dimos/manipulation/test_generated_plan_materialization.py index ec75185577..acbc7d7810 100644 --- a/dimos/manipulation/test_generated_plan_materialization.py +++ b/dimos/manipulation/test_generated_plan_materialization.py @@ -26,6 +26,16 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + ParametrizedTrajectory, + TrajectoryParametrizationRequest, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -41,9 +51,14 @@ class RecordingGenerator: fail = False def __init__( - self, num_joints: int, max_velocity: list[float], max_acceleration: list[float] + self, + num_joints: int, + max_velocity: list[float], + max_acceleration: list[float], + points_per_segment: int = 50, ) -> None: self.num_joints = num_joints + self.points_per_segment = points_per_segment RecordingGenerator.limits = (list(max_velocity), list(max_acceleration)) def generate(self, waypoints: list[list[float]]) -> JointTrajectory: @@ -62,6 +77,18 @@ def generate(self, waypoints: list[list[float]]) -> JointTrajectory: ) +class FixedParametrizer: + uses_request_limits = False + + def __init__(self, result: ParametrizedTrajectory) -> None: + self.result = result + self.requests: list[TrajectoryParametrizationRequest] = [] + + def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: + self.requests.append(request) + return self.result + + def _robot(name: str, joints: list[str], velocity: float, acceleration: float) -> RobotModelConfig: return RobotModelConfig( name=name, @@ -84,7 +111,9 @@ def _module(monkeypatch: pytest.MonkeyPatch, module_factory): RecordingGenerator.limits = None RecordingGenerator.fail = False monkeypatch.setattr( - "dimos.manipulation.manipulation_module.JointTrajectoryGenerator", RecordingGenerator + "dimos.manipulation.planning.trajectory_generator." + "simple_parametrizer.JointTrajectoryGenerator", + RecordingGenerator, ) left = _robot("left", ["a", "b"], 1.0, 2.0) right = _robot("right", ["c"], 3.0, 4.0) @@ -97,6 +126,9 @@ def _module(monkeypatch: pytest.MonkeyPatch, module_factory): module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) module._planner = MagicMock() + module._trajectory_parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig() + ) module._state = ManipulationState.PLANNING module._planning_epoch = 1 return module @@ -261,3 +293,115 @@ def test_zero_generation_after_caching_for_status_and_completion(monkeypatch, mo module._wait_for_trajectory_completion(timeout=0.0) assert RecordingGenerator.calls == [] + + +def test_materialization_accepts_bounded_fitting_without_interior_waypoint( + monkeypatch, + module_factory, +): + module = _module(monkeypatch, module_factory) + names = ["left/b", "left/a"] + source_path = [ + JointState(name=names, position=[0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1]), + JointState(name=names, position=[0.4, 0.0]), + ] + trajectory = JointTrajectory( + joint_names=names, + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0, 0.0], + velocities=[0.0, 0.0], + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[0.4, 0.0], + velocities=[0.0, 0.0], + ), + ], + ) + parametrizer = FixedParametrizer( + ParametrizedTrajectory( + trajectory=trajectory, + velocity_limits=(1.0, 1.0), + acceleration_limits=(2.0, 2.0), + accelerations=((0.0, 0.0), (0.0, 0.0)), + ) + ) + module._trajectory_parametrizer = parametrizer + assert module.set_motion_speed(0.4) + + path, result = module._materialize_generated_plan(("left/group",), source_path) + + assert [state.position for state in path] == [ + [0.0, 0.0], + [0.2, 0.1], + [0.4, 0.0], + ] + assert result is trajectory + assert len(parametrizer.requests) == 1 + assert parametrizer.requests[0].speed_scale == pytest.approx(0.4) + + +@pytest.mark.parametrize( + ("velocities", "accelerations", "message"), + [ + ([[0.0, 0.0], [1.1, 0.0]], ((0.0, 0.0), (0.0, 0.0)), "velocity exceeds"), + ([[0.0, 0.0], [0.0, 0.0]], ((0.0, 0.0), (2.1, 0.0)), "acceleration exceeds"), + ], +) +def test_materialization_rejects_parametrized_motion_limit_violations( + monkeypatch, + module_factory, + velocities, + accelerations, + message, +): + module = _module(monkeypatch, module_factory) + names = ["left/b", "left/a"] + path = _path(names, [0.0, 0.0], [0.4, 0.0]) + module._trajectory_parametrizer = FixedParametrizer( + ParametrizedTrajectory( + trajectory=JointTrajectory( + joint_names=names, + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0, 0.0], + velocities=velocities[0], + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[0.4, 0.0], + velocities=velocities[1], + ), + ], + ), + velocity_limits=(1.0, 1.0), + acceleration_limits=(2.0, 2.0), + accelerations=accelerations, + ) + ) + + with pytest.raises(ValueError, match=message): + module._materialize_generated_plan(("left/group",), path) + + +def test_materialization_validates_real_simple_backend( + monkeypatch, + module_factory, +): + module = _module(monkeypatch, module_factory) + module._trajectory_parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig() + ) + names = ["left/b", "left/a"] + + path, trajectory = module._materialize_generated_plan( + ("left/group",), + _path(names, [0.0, 0.0], [0.2, 0.1]), + ) + + assert path[-1].position == [0.2, 0.1] + assert trajectory.points[-1].positions == [0.2, 0.1] diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 2bcc8a9afc..8ac2662b92 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -45,6 +45,12 @@ IKResult, PlanningResult, ) +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -196,6 +202,12 @@ def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: ) +def _enable_simple_parametrization(module: ManipulationModule) -> None: + module._trajectory_parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig() + ) + + class TestObstacleUpdates: def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> None: module = module_factory() @@ -348,6 +360,25 @@ def test_fail_sets_fault_state(self, module_factory): assert module._state == ManipulationState.FAULT assert module._error_message == "Test error" + @pytest.mark.parametrize("invalid", [0.0, -0.1, 1.01, float("inf"), float("nan")]) + def test_motion_speed_applies_to_future_plans_only(self, module_factory, invalid: float): + module = module_factory() + accepted = GeneratedPlan( + trajectory=JointTrajectory(), + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j0"], position=[0.0])], + ) + module._last_plan = accepted + + assert module.set_motion_speed(0.5) is True + assert module.get_motion_speed() == pytest.approx(0.5) + assert module._last_plan is accepted + + assert module.set_motion_speed(invalid) is False + assert module.get_motion_speed() == pytest.approx(0.5) + assert module._last_plan is accepted + assert "motion speed scale" in module.get_error() + def test_begin_planning_state_checks(self, robot_config, module_factory): """_begin_planning only allowed from IDLE or COMPLETED.""" module = module_factory() @@ -468,6 +499,7 @@ def test_kinematics_config_is_passed_to_factory( planner=module.config.planner, kinematics_name=None, kinematics=kinematics, + trajectory_parametrization=module.config.trajectory_parametrization, ) def test_legacy_kinematics_name_still_selects_backend( @@ -491,6 +523,7 @@ def test_legacy_kinematics_name_still_selects_backend( planner=module.config.planner, kinematics_name="pink", kinematics=module.config.kinematics, + trajectory_parametrization=module.config.trajectory_parametrization, ) def test_nested_kinematics_config_parses_cli_override_shape(self) -> None: @@ -623,6 +656,7 @@ def test_plan_to_joint_targets_stores_generated_plan_and_legacy_caches( (0.0, [0.0, 0.0, 0.0]), (1.0, [0.1, 0.2, 0.3]) ) module._robots = {"test_arm": ("robot_id", robot_config, traj_gen)} + _enable_simple_parametrization(module) module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = registry @@ -707,6 +741,7 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( (0.0, [0.0, 0.0, 0.0]), (1.0, [0.1, 0.2, 0.3]) ) module._robots = {"test_arm": ("robot_id", robot_config, traj_gen)} + _enable_simple_parametrization(module) module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = registry diff --git a/dimos/manipulation/test_plan_execution.py b/dimos/manipulation/test_plan_execution.py index 5a14c224bc..62fbe9ed78 100644 --- a/dimos/manipulation/test_plan_execution.py +++ b/dimos/manipulation/test_plan_execution.py @@ -102,11 +102,20 @@ def test_execute_plan_can_dispatch_cached_plan_repeatedly( ) -> None: coordinator = _coordinator() module = _module_with_coordinator(coordinator, module_factory) - module._last_plan = _plan() + plan = _plan() + module._last_plan = plan assert module.execute_plan() assert module.execute_plan() assert coordinator.execute_trajectory.call_count == 2 + for call in coordinator.execute_trajectory.call_args_list: + dispatched = call.args[0] + assert [point.time_from_start for point in dispatched.points] == [ + point.time_from_start for point in plan.trajectory.points + ] + assert [point.velocities for point in dispatched.points] == [ + point.velocities for point in plan.trajectory.points + ] def test_direct_plan_does_not_replace_cached_plan(module_factory) -> None: diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index 2613363c29..80b82b7416 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -30,6 +30,7 @@ create_kinematics, create_planner, create_planning_stack, + create_trajectory_parametrizer, create_world, validate_backend_combination, ) @@ -46,6 +47,13 @@ from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.protocols import PlannerSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -125,6 +133,42 @@ def test_validate_backend_combination_rejects_invalid_combinations() -> None: ): validate_backend_combination(world_backend="roboplan", kinematics_name="drake_optimization") + with pytest.raises( + ValueError, + match='trajectory_parametrization.backend="roboplan_toppra" requires', + ): + validate_backend_combination( + world_backend="drake", + planner_backend="rrt_connect", + trajectory_parametrization_backend="roboplan_toppra", + ) + + +def test_create_trajectory_parametrizer_selects_simple_backend( + mocker: MockerFixture, +) -> None: + result = create_trajectory_parametrizer( + SimpleTrapezoidParametrizationConfig(), + world=mocker.MagicMock(), + world_backend="drake", + ) + + assert isinstance(result, SimpleTrapezoidParametrizer) + + +def test_create_trajectory_parametrizer_rejects_toppra_with_non_roboplan_world( + mocker: MockerFixture, +) -> None: + with pytest.raises( + ValueError, + match='trajectory_parametrization.backend="roboplan_toppra" requires', + ): + create_trajectory_parametrizer( + RoboPlanTOPPRAParametrizationConfig(), + world=mocker.MagicMock(), + world_backend="drake", + ) + def test_create_planner_uses_roboplan_world_as_native_planner(mocker: MockerFixture) -> None: world = mocker.MagicMock(spec=PlannerSpec) @@ -238,6 +282,11 @@ def test_start_uses_configured_planner_and_kinematics( "dimos.manipulation.manipulation_module.create_planning_specs", return_value=planning_specs, ) + parametrizer = mocker.MagicMock(name="trajectory_parametrizer") + create_parametrizer_mock = mocker.patch( + "dimos.manipulation.manipulation_module.create_trajectory_parametrizer", + return_value=parametrizer, + ) module._initialize_planning() @@ -250,7 +299,14 @@ def test_start_uses_configured_planner_and_kinematics( planner=planner_config, kinematics_name=None, kinematics=module.config.kinematics, + trajectory_parametrization=module.config.trajectory_parametrization, + ) + create_parametrizer_mock.assert_called_once_with( + module.config.trajectory_parametrization, + world=world, + world_backend="roboplan", ) assert module._planner is planner assert module._kinematics is kinematics + assert module._trajectory_parametrizer is parametrizer assert module._robots["arm"][0] == "robot-id" diff --git a/dimos/manipulation/visualization/operator.py b/dimos/manipulation/visualization/operator.py index ac0ef65609..ebb1b67070 100644 --- a/dimos/manipulation/visualization/operator.py +++ b/dimos/manipulation/visualization/operator.py @@ -96,6 +96,14 @@ def status(self) -> OperatorStatus: has_plan=self._module.has_planned_path(), ) + def get_motion_speed(self) -> float: + """Return the runtime speed reduction used for future plans.""" + return self._module.get_motion_speed() + + def set_motion_speed(self, speed_scale: float) -> bool: + """Set the runtime speed reduction used for future plans.""" + return self._module.set_motion_speed(speed_scale) + def get_init_joints(self, robot_name: RobotName) -> JointState | None: """Return the operator-authoritative init joint state for a robot.""" init = self._module.get_init_joints(robot_name) diff --git a/dimos/manipulation/visualization/test_operator.py b/dimos/manipulation/visualization/test_operator.py index a302fe9642..1820848ed2 100644 --- a/dimos/manipulation/visualization/test_operator.py +++ b/dimos/manipulation/visualization/test_operator.py @@ -81,6 +81,7 @@ def __init__(self) -> None: self.state = "COMPLETED" self.error = "" self.has_plan = True + self.motion_speed = 1.0 self.plan = GeneratedPlan( group_ids=("arm/manipulator",), trajectory=JointTrajectory( @@ -126,6 +127,13 @@ def get_error(self) -> str: def has_planned_path(self) -> bool: return self.has_plan + def get_motion_speed(self) -> float: + return self.motion_speed + + def set_motion_speed(self, speed_scale: float) -> bool: + self.motion_speed = speed_scale + return True + def get_robot_config(self, robot_name: RobotName) -> RobotModelConfig | None: self.topology_calls += 1 return self.robot_configs.get(robot_name) @@ -277,6 +285,15 @@ def test_status_is_compact_and_does_not_read_topology_or_telemetry() -> None: assert monitor.telemetry_calls == 0 +def test_motion_speed_delegates_to_module() -> None: + operator, module, _ = _operator() + + assert operator.get_motion_speed() == 1.0 + assert operator.set_motion_speed(0.5) is True + assert operator.get_motion_speed() == 0.5 + assert module.motion_speed == 0.5 + + def test_evaluate_joint_target_accepts_exact_global_selection_domain() -> None: operator, _, _ = _operator() diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 02145d9fc5..5695fb85cd 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -15,6 +15,7 @@ from __future__ import annotations from collections.abc import Mapping, MutableMapping, Sequence +import math from typing import TypeAlias, cast from dimos.manipulation.planning.groups.models import PlanningGroup @@ -78,6 +79,7 @@ | GuiDropdownHandle[str] | GuiButtonHandle | GuiCheckboxHandle + | GuiSliderHandle[float] | TransformControlsHandle ) @@ -324,10 +326,14 @@ def plan_cartesian( ) for group_id, pose in pose_targets.items() } + speed_scale = self.operator.get_motion_speed() plan = self.operator.plan_cartesian( CartesianTargetRequest( stamped, - RoboPlanCartesianPathConfig(), + RoboPlanCartesianPathConfig( + velocity_scale=speed_scale, + acceleration_scale=speed_scale, + ), tuple(auxiliary_group_ids), ) ) @@ -394,6 +400,7 @@ def _build_panel_controls(self, gui: GuiApi) -> None: self._handles["preset"] = preset_dropdown self._handles["target_summary"] = gui.add_markdown("Feasibility: `unknown`") self._handles["actions_heading"] = gui.add_markdown("### Actions") + self._build_motion_settings(gui) planning_mode = gui.add_dropdown( "Planning mode", options=list(PLANNING_MODES_BY_LABEL), @@ -421,6 +428,43 @@ def _build_panel_controls(self, gui: GuiApi) -> None: self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() + def _build_motion_settings(self, gui: GuiApi) -> None: + """Build controls that affect trajectories generated in the future.""" + speed_slider = gui.add_slider( + "Next plan speed", + min=0.05, + max=1.0, + step=0.05, + initial_value=self._motion_speed_scale_for_slider(), + ) + speed_slider.on_update(lambda event: self._set_next_plan_speed(event.target.value)) + self._handles["next_plan_speed"] = speed_slider + + def _motion_speed_scale_for_slider(self) -> float: + """Return the module's speed setting bounded to the slider range.""" + try: + speed_scale = float(self.operator.get_motion_speed()) + except Exception: + logger.warning("Could not read manipulation motion speed", exc_info=True) + return 1.0 + if not math.isfinite(speed_scale) or speed_scale <= 0.0: + return 1.0 + return min(max(speed_scale, 0.05), 1.0) + + def _set_next_plan_speed(self, speed_scale: float) -> None: + """Update future-plan speed without invalidating the accepted plan.""" + if self._closed: + return + if self.state.action_status != ActionStatus.IDLE: + self._set_recoverable_error( + "Cannot change next-plan speed while an operation is active" + ) + return + if not self.operator.set_motion_speed(float(speed_scale)): + self._set_error(self.get_error() or "Invalid next-plan speed") + return + self.refresh() + def _sync_group_selector(self, groups: list[PlanningGroup]) -> None: """Render source-order group toggle buttons without a robot dropdown.""" selected = set(self.state.selected_group_ids) @@ -1119,6 +1163,7 @@ def _update_status_text(self) -> None: ) def _update_control_state(self) -> None: + self._set_disabled("next_plan_speed", self.state.action_status != ActionStatus.IDLE) self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) self._set_disabled( @@ -1463,7 +1508,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 8e1428214f..15b701e10e 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -61,6 +61,7 @@ ViserManipulationScene, ) from dimos.manipulation.visualization.viser.state import ( + ActionStatus, PanelPlanState, PlanningMode, PlanStatus, @@ -217,6 +218,8 @@ def __init__(self, groups: list[PlanningGroup], states: dict[str, JointState]) - self.cancelled = 0 self.cleared = 0 self.last_plan: GeneratedPlan | None = None + self.motion_speed = 1.0 + self.motion_speed_updates: list[float] = [] def make_plan(self, group_ids: tuple[str, ...]) -> GeneratedPlan: names = [ @@ -263,6 +266,14 @@ def get_state(self) -> str: def get_error(self) -> str: return self.error + def get_motion_speed(self) -> float: + return self.motion_speed + + def set_motion_speed(self, speed_scale: float) -> bool: + self.motion_speed = float(speed_scale) + self.motion_speed_updates.append(float(speed_scale)) + return True + def reset(self) -> SimpleNamespace: return SimpleNamespace(is_success=lambda: True) @@ -321,6 +332,12 @@ def status(self) -> SimpleNamespace: has_plan=True, ) + def get_motion_speed(self) -> float: + return self.module.get_motion_speed() + + def set_motion_speed(self, speed_scale: float) -> bool: + return self.module.set_motion_speed(speed_scale) + def get_init_joints(self, robot_name: str) -> JointState | None: return self.module.get_init_joints(robot_name) @@ -515,10 +532,14 @@ def test_panel_contract_group_order_defaults_and_controls( assert server.gui.dropdowns[1].options == ["Joint space", "Cartesian space"] assert [ (slider.label, slider.min, slider.max, slider.value) for slider in server.gui.sliders - ] == [("arm/manipulator/j1", -1.0, 1.0, 0.1)] + ] == [ + ("Next plan speed", 0.05, 1.0, 1.0), + ("arm/manipulator/j1", -1.0, 1.0, 0.1), + ] server.gui.buttons[1].callback(SimpleNamespace()) assert gui.state.selected_group_ids == ("arm/manipulator", "arm/gripper") assert [slider.label for slider in server.gui.sliders if not slider.removed] == [ + "Next plan speed", "arm/manipulator/j1", "arm/gripper/j2", ] @@ -599,6 +620,7 @@ def test_cartesian_space_mode_plans_absolute_pose_targets_with_auxiliary_groups( pose_group = group("arm", "manipulator", ("j1",), pose=True) auxiliary_group = group("arm", "gripper", ("j2",)) gui, module, server = panel([pose_group, auxiliary_group], states("arm")) + module.motion_speed = 0.4 gui._toggle_group_selected(auxiliary_group.id) gui.state.target_status = TargetStatus.FEASIBLE gui._operation_worker.stop() @@ -618,6 +640,8 @@ def test_cartesian_space_mode_plans_absolute_pose_targets_with_auxiliary_groups( assert tuple(targets) == (pose_group.id,) assert targets[pose_group.id].frame_id == "world" assert config.speed_mode == "bounded" # type: ignore[attr-defined] + assert config.velocity_scale == pytest.approx(0.4) # type: ignore[attr-defined] + assert config.acceleration_scale == pytest.approx(0.4) # type: ignore[attr-defined] assert auxiliary_ids == (auxiliary_group.id,) assert gui.state.plan_state.status == PlanStatus.FRESH assert gui.state.last_result == "plan_cartesian_space=True" @@ -704,13 +728,14 @@ def test_valid_init_preset_builds_sliders_after_incomplete_initial_telemetry( ) assert gui.state.group_joint_targets == {} - assert server.gui.sliders == [] + assert [slider.label for slider in server.gui.sliders] == ["Next plan speed"] module.configs["arm"].home_joints = [-0.5, -1.0] gui._apply_preset("Init") assert gui.state.group_joint_targets[selected.id].position == [-0.5, -1.0] assert [slider.label for slider in server.gui.sliders if not slider.removed] == [ + "Next plan speed", "arm/manipulator/j1", "arm/manipulator/j2", ] @@ -895,6 +920,7 @@ def test_panel_preset_defaults_and_joint_slider_limits( (slider.label, slider.min, slider.max, slider.step, slider.value) for slider in server.gui.sliders ] == [ + ("Next plan speed", 0.05, 1.0, 0.05, 1.0), ("arm/manipulator/j1", -1.0, 1.0, 0.001, 0.1), ("arm/manipulator/j2", -2.0, 2.0, 0.001, 0.2), ] @@ -964,6 +990,45 @@ def test_panel_action_controls_are_present_in_source_order( "Manipulation Panel", "Joint Control", ] + assert len(server.gui.sliders) == 2 + speed_slider = server.gui.sliders[0] + assert speed_slider.label == "Next plan speed" + assert speed_slider.min == pytest.approx(0.05) + assert speed_slider.max == pytest.approx(1.0) + assert speed_slider.step == pytest.approx(0.05) + assert speed_slider.value == pytest.approx(1.0) + + +def test_next_plan_speed_slider_updates_future_speed_without_staling_plan( + panel: Callable[..., tuple[ViserPanelGui, Module, Server]], +) -> None: + selected = group("arm", "manipulator", ("j1",), pose=True) + gui, module, server = panel([selected], states("arm")) + accepted = module.make_plan((selected.id,)) + gui.state.plan_state = PanelPlanState(status=PlanStatus.FRESH, plan=accepted) + speed_slider = server.gui.sliders[0] + speed_slider.value = 0.5 + assert speed_slider.callback is not None + + speed_slider.callback(SimpleNamespace(target=speed_slider)) + + assert module.motion_speed_updates == [0.5] + assert module.last_plan is accepted + assert gui.state.plan_state.plan is accepted + assert gui.state.plan_state.status == PlanStatus.FRESH + + +def test_next_plan_speed_slider_is_disabled_during_panel_operation( + panel: Callable[..., tuple[ViserPanelGui, Module, Server]], +) -> None: + selected = group("arm", "manipulator", ("j1",), pose=True) + gui, _module, server = panel([selected], states("arm")) + speed_slider = server.gui.sliders[0] + + gui.state.action_status = ActionStatus.RUNNING + gui.refresh() + + assert speed_slider.disabled is True def test_target_callbacks_require_current_target_identity( diff --git a/docs/adr/0001-select-one-trajectory-parametrization-backend.md b/docs/adr/0001-select-one-trajectory-parametrization-backend.md deleted file mode 100644 index b0733e23ce..0000000000 --- a/docs/adr/0001-select-one-trajectory-parametrization-backend.md +++ /dev/null @@ -1,3 +0,0 @@ -# Select one trajectory parametrization backend at startup - -Each manipulation deployment selects exactly one trajectory parametrization backend at startup. If that backend cannot parametrize a geometric path, plan materialization fails explicitly; the system does not fall back to another parametrizer because doing so would silently change trajectory semantics, timing, and failure behavior. A selected backend may use its own documented safety behavior between internal curve-fitting modes, such as RoboPlan TOPP-RA falling back from a colliding linear blend to Hermite fitting. diff --git a/docs/adr/0003-parametrize-during-plan-materialization.md b/docs/adr/0003-parametrize-during-plan-materialization.md deleted file mode 100644 index 3edcc52952..0000000000 --- a/docs/adr/0003-parametrize-during-plan-materialization.md +++ /dev/null @@ -1,3 +0,0 @@ -# Parametrize during plan materialization - -Trajectory parametrization runs immediately after geometric planning, before a `GeneratedPlan` is accepted or cached. Preview and execution therefore consume the same validated timed trajectory, and parametrization failures prevent an untimed plan from being presented as ready rather than surfacing during execution. diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 4f9df72a86..c486db9b54 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -474,12 +474,37 @@ coordinator_yourarm = ControlCoordinator.blueprint( ## Step 4: Add URDF and Planning Integration (Optional) -If you want motion planning (collision-free trajectories via Drake), you need a URDF and a planning blueprint. Add these to your robot's own `blueprints.py`. +If you want motion planning, you need a URDF and a planning blueprint. Add these +to your robot's own `blueprints.py`. ### 4a. Add your URDF Place your URDF/xacro files under LFS data so they can be resolved via `LfsPath`. `LfsPath` is a `Path` subclass that lazily downloads LFS data on first access — this avoids downloading at import time when the blueprint module is loaded. +If the planning blueprint selects the RoboPlan TOPP-RA trajectory +parametrizer, DimOS currently pins RoboPlan to `0.5.1`. Every movable joint in +each selected planning group must provide finite, positive velocity and +extended acceleration limits: + +```xml + + + + +``` + +RoboPlan loads both limits from its scene model. If either is absent, zero, +negative, or non-finite, plan materialization fails before preview or execution +and identifies the affected joint. DimOS does not substitute +`RobotModelConfig.max_velocity`, `velocity_limits`, or `max_acceleration` for +this backend. Formal per-joint DimOS overrides will be added separately. + ```python skip from dimos.utils.data import LfsPath from dimos.manipulation.manipulation_module import manipulation_module @@ -551,12 +576,30 @@ yourarm_planner = manipulation_module( robots=[_make_yourarm_config("arm")], planning_timeout=10.0, visualization={"backend": "meshcat"}, + trajectory_parametrization={"backend": "simple_trapezoid"}, ) # The planner's `coordinator_joint_state` input auto-connects to the # ControlCoordinator's output on the default `/coordinator_joint_state` # topic, so no `.transports(...)` override is needed. ``` +To use continuous TOPP-RA timing instead, select RoboPlan for the world and +parametrizer after adding the URDF limits described above: + +```python skip +yourarm_planner = manipulation_module( + robots=[_make_yourarm_config("arm")], + world_backend="roboplan", + trajectory_parametrization={ + "backend": "roboplan_toppra", + "fitting_mode": "linear_blend", + "velocity_scale": 0.8, + "acceleration_scale": 0.8, + }, + visualization={"backend": "viser"}, +) +``` + ### Key config fields | Field | Description | diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index a0aa40f927..e9b635aeef 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -115,6 +115,37 @@ request. For example, `planner.backend=roboplan` requires `world_backend=roboplan`, and `kinematics.backend=drake_optimization` requires `world_backend=drake`. +Trajectory parametrization is a separate startup choice. Joint-space planners +normally return an untimed geometric path; DimOS accepts the plan only after +the selected backend converts that path to a validated timed trajectory: + +```bash +# Compatibility behavior: independent trapezoids between path waypoints +dimos run xarm7-planner-coordinator \ + -o manipulationmodule.trajectory_parametrization.backend=simple_trapezoid + +# Continuous TOPP-RA timing, available with RoboPlanWorld +dimos run xarm7-planner-coordinator \ + -o manipulationmodule.world_backend=roboplan \ + -o manipulationmodule.trajectory_parametrization.backend=roboplan_toppra \ + -o manipulationmodule.trajectory_parametrization.fitting_mode=linear_blend +``` + +Exactly one backend is constructed for the stack lifetime. There is no +cross-backend fallback. `roboplan_toppra` may parametrize paths from either +RoboPlan's planner or the generic RRT planner, but it requires +`world_backend=roboplan` because it reuses that world's model, groups, and URDF +motion limits. A planner-native result that already has timestamps and +velocities bypasses path parametrization and retains its existing timing after +canonical validation. + +The Viser panel's **Next plan speed** slider provides runtime speed tuning from +`0.05` to `1.0`. Changing it leaves the accepted plan and any active execution +unchanged; press **Plan** again to generate motion at the new scale. For +joint-space planning the value reduces the selected parametrizer's configured +velocity and acceleration scales. For Cartesian planning Viser puts the same +scale into the native planning request before its timestamps are generated. + RoboPlan Cartesian options are supplied per planning request: ```python skip @@ -250,9 +281,11 @@ not need extra setup because it observes the Drake world directly. Previews use the stored synchronized `JointTrajectory` from the generated plan. Viser projects the globally named trajectory into robot-local preview ghosts and plays the stored timestamped points directly; optional preview duration only -scales the stored delays. Execute freshness is enforced by the manipulation -module/operator immediately before dispatch, not by Viser-side telemetry -snapshots. +scales the stored delays. Execution projects that same accepted trajectory into +each robot's local joint order while preserving timestamps and velocities; it +does not regenerate or retime it. Execute freshness is enforced by the +manipulation module/operator immediately before dispatch, not by Viser-side +telemetry snapshots. ### Perception + Agent diff --git a/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md b/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md new file mode 100644 index 0000000000..3ca932ac28 --- /dev/null +++ b/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md @@ -0,0 +1,3 @@ +# Select one trajectory parametrization backend at startup + +Each manipulation deployment selects exactly one trajectory parametrization backend at startup. Every untimed geometric path uses that backend. Planner-native timed results bypass parametrization because they are already trajectories, not because a backend failed. If the selected backend cannot parametrize a geometric path, plan materialization fails explicitly; the system does not fall back to another parametrizer because doing so would silently change trajectory semantics, timing, and failure behavior. A selected backend may use its own documented safety behavior between internal curve-fitting modes, such as RoboPlan TOPP-RA falling back from a colliding linear blend to Hermite fitting. diff --git a/docs/adr/0002-scope-roboplan-toppra-to-roboplan-world.md b/docs/development/adr/0002-scope-roboplan-toppra-to-roboplan-world.md similarity index 100% rename from docs/adr/0002-scope-roboplan-toppra-to-roboplan-world.md rename to docs/development/adr/0002-scope-roboplan-toppra-to-roboplan-world.md diff --git a/docs/development/adr/0003-parametrize-during-plan-materialization.md b/docs/development/adr/0003-parametrize-during-plan-materialization.md new file mode 100644 index 0000000000..9b36b7e541 --- /dev/null +++ b/docs/development/adr/0003-parametrize-during-plan-materialization.md @@ -0,0 +1,3 @@ +# Parametrize during plan materialization + +Trajectory parametrization runs immediately after untimed geometric planning, before a `GeneratedPlan` is accepted or cached. Planner-native timed results already sit on the trajectory side of this boundary, so they bypass parametrization while retaining canonical validation. Preview and execution therefore consume the same validated time domain; execution may project globally named joints into robot-local order but does not regenerate or retime the trajectory. A runtime next-plan speed reduction is captured while producing a new trajectory and never changes an accepted plan. Parametrization failures prevent an untimed plan from being presented as ready rather than surfacing during execution. diff --git a/docs/adr/0004-keep-geometric-post-processing-out-of-parametrization.md b/docs/development/adr/0004-keep-geometric-post-processing-out-of-parametrization.md similarity index 100% rename from docs/adr/0004-keep-geometric-post-processing-out-of-parametrization.md rename to docs/development/adr/0004-keep-geometric-post-processing-out-of-parametrization.md diff --git a/docs/adr/0005-trust-parametrizer-collision-preservation.md b/docs/development/adr/0005-trust-parametrizer-collision-preservation.md similarity index 100% rename from docs/adr/0005-trust-parametrizer-collision-preservation.md rename to docs/development/adr/0005-trust-parametrizer-collision-preservation.md diff --git a/docs/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md b/docs/development/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md similarity index 100% rename from docs/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md rename to docs/development/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md diff --git a/openspec/changes/add-trajectory-parametrization/design.md b/openspec/changes/add-trajectory-parametrization/design.md index 7b3585ec6e..89fbfbd8f5 100644 --- a/openspec/changes/add-trajectory-parametrization/design.md +++ b/openspec/changes/add-trajectory-parametrization/design.md @@ -14,8 +14,12 @@ RoboPlan 0.5.1 provides TOPP-RA with Hermite, cubic, adaptive, and linear-blend - Preserve the existing simple segmented-trapezoid behavior as a compatibility backend. - Add RoboPlan TOPP-RA for any planner path represented in `RoboPlanWorld`. - Convert and validate a path before constructing or caching `GeneratedPlan`. +- Preserve and validate planner-native timed trajectories without parametrizing them again. +- Allow an operator to reduce the speed of future plans from Viser without + changing the selected backend or mutating an accepted plan. - Preserve the source path while allowing bounded backend interpolation between waypoints. -- Keep preview and execution on the exact trajectory accepted during planning. +- Keep preview and execution on the accepted trajectory's time domain, allowing + robot-local joint projection but no regeneration or retiming. - Use URDF-backed RoboPlan velocity and acceleration limits and fail clearly when they are unavailable. - Retain current public manipulation RPC, skill, MCP, stream, and execution signatures. @@ -41,6 +45,14 @@ Add a typed `TrajectoryParametrizationConfig` under manipulation planning config The configuration factory validates the complete backend combination during startup. `roboplan_toppra` requires a finalized `RoboPlanWorld`; a non-RoboPlan world is rejected before planning. The selected parametrizer is constructed once and retained by `ManipulationModule`. +The module also owns a runtime next-plan speed scale in `(0, 1]`, initially +`1.0`. This is not backend selection or persistent configuration. Each new +untimed path materialization captures the current scale and multiplies the +configured velocity and acceleration reductions. Planner-native Cartesian +planning receives the same scale through its per-request velocity and +acceleration fields before it creates authoritative timing. Changing the scale +does not invalidate, reparametrize, or retime an existing `GeneratedPlan`. + ### Adapter Protocol Introduce a small adapter `Protocol`, distinct from an RPC-oriented DimOS `Spec`, for path-to-trajectory conversion. Its input contains: @@ -58,26 +70,23 @@ The RoboPlan adapter may cache one native TOPP-RA parameterizer per selected gro ### Plan materialization -Retain `GeneratedPlan` as the canonical accepted aggregate: - -```text -PlanningResult.path - │ - ▼ -canonical input validation - │ - ▼ -startup-selected TrajectoryParametrizer - │ - ▼ -canonical timed-output validation - │ - ▼ -GeneratedPlan(path + trajectory) -``` +Retain `GeneratedPlan` as the canonical accepted aggregate. An untimed +`PlanningResult.path` passes through canonical input validation, the +startup-selected `TrajectoryParametrizer`, and canonical timed-output +validation before becoming `GeneratedPlan(path + trajectory)`. + +A planner-native timed result already sits on the trajectory side of this +boundary. It bypasses `TrajectoryParametrizer`, retains its planner-defined +timestamps and velocities, and passes through the same canonical timed-output +validation before becoming `GeneratedPlan(path + trajectory)`. This bypass is +not fallback: no alternative parametrization backend is selected or invoked. Replace the direct `JointTrajectoryGenerator` construction inside materialization with the selected adapter. A failure at either parametrization or validation leaves `_last_plan` unset and follows the existing planning-epoch failure path. No separate public `GeneratedTrajectory` lifecycle is added. +Keep the existing planner-native timed materialization path for results such as +RoboPlan Cartesian planning. It must not invoke the selected parametrizer or +discard bounded/time-optimal TCP timing semantics. + Canonical validation retains the current strong invariants: exact global joint ordering, finite and dimensionally aligned positions/velocities, first time at zero, strictly increasing times, positive duration for non-noop motion, and preserved start/goal. It also checks returned motion against the applicable velocity and acceleration limits with a documented numerical tolerance. Where RoboPlan exposes native accelerations, validate them before converting to the current positions/velocities-only message; otherwise derive the acceleration check consistently from velocity samples. ### RoboPlan limits and fitting @@ -95,7 +104,12 @@ RoboPlan owns collision checking for a fitted curve against its authoritative sc ### Other DimOS surfaces -No streams, transports, module references, blueprint composition, RPC signatures, skills, MCP tools, CLI commands, or generated registry inputs change. Existing preview and execution flows consume the stored `GeneratedPlan.trajectory`. No `all_blueprints.py` regeneration is expected. +No streams, transports, module references, blueprint composition, existing RPC signatures, skills, MCP tools, CLI commands, or generated registry inputs change. The Viser control uses additive speed-setting RPCs on the existing manipulation operator seam. Existing preview and execution flows consume the stored `GeneratedPlan.trajectory`. No `all_blueprints.py` regeneration is expected. + +Viser exposes the runtime scale as a `Next plan speed` slider from `0.05` to +`1.0` in `0.05` steps. The control is disabled during an active panel +operation. It calls the UI-neutral `ManipulationOperator`, which delegates to +the module's runtime getter/setter; Viser does not own trajectory generation. ## Decisions @@ -107,7 +121,11 @@ Alternative: restore frontier's public `GeneratedPlan`/`GeneratedTrajectory`/dis ### Select one backend for the run -Backend selection is startup configuration. A selected backend's failure fails materialization; no other backend is attempted. +Backend selection is startup configuration. Every untimed geometric path that +requires path-to-trajectory conversion uses the selected backend. A selected +backend's failure fails materialization; no other backend is attempted. +Planner-native timed results skip conversion because they are already +trajectories, not because a backend failed. Alternative: fall back to the simple backend after TOPP-RA failure. Rejected because it silently changes timing and stop behavior. @@ -141,7 +159,8 @@ Alternative: wire current scalar/list DimOS fields into RoboPlan. Rejected becau - Missing or invalid URDF motion limits fail rather than selecting generic defaults. - The TOPP-RA reduction scales are constrained to safe ranges and cannot raise URDF limits. - Simulation uses the same materialized trajectory path as hardware and is the primary manual QA surface. -- Preview must show the exact stored trajectory later dispatched by execution. +- Preview must show the stored trajectory later projected into robot-local + joint order for execution without regeneration or retiming. - Replay behavior is unaffected because no stream or replay-data format changes. - Manual QA should compare simple and TOPP-RA trajectories for the same RoboPlan-world path, check smooth traversal of interior waypoints, and verify explicit failures for missing limits and incompatible startup configuration before any hardware trial. diff --git a/openspec/changes/add-trajectory-parametrization/docs.md b/openspec/changes/add-trajectory-parametrization/docs.md index d532b6d65e..efd4fe1e44 100644 --- a/openspec/changes/add-trajectory-parametrization/docs.md +++ b/openspec/changes/add-trajectory-parametrization/docs.md @@ -8,13 +8,14 @@ - the `RoboPlanWorld` compatibility requirement; - supported RoboPlan fitting modes and bounded deviation; - no cross-backend fallback; - - URDF limit ownership and explicit missing-limit failures. + - URDF limit ownership and explicit missing-limit failures; + - Viser next-plan speed behavior and its non-retroactive boundary. ## Contributor Docs - No new standalone contributor guide is required. - If implementation reveals a non-obvious RoboPlan packaging or URDF 1.2 limit convention, add a focused note under `docs/development/` rather than expanding user-facing architecture prose. -- Keep the architecture decisions under `docs/adr/` and ensure the OpenSpec design remains consistent with them. +- Keep the architecture decisions under `docs/development/adr/` and ensure the OpenSpec design remains consistent with them. ## Coding-Agent Docs diff --git a/openspec/changes/add-trajectory-parametrization/proposal.md b/openspec/changes/add-trajectory-parametrization/proposal.md index 5395eed721..ca22242e90 100644 --- a/openspec/changes/add-trajectory-parametrization/proposal.md +++ b/openspec/changes/add-trajectory-parametrization/proposal.md @@ -10,18 +10,21 @@ DimOS needs an explicit path-to-trajectory parametrization boundary that can ret - Preserve the existing simple trapezoid behavior as a selectable compatibility backend. - Add a RoboPlan TOPP-RA backend for any geometric path planned against `RoboPlanWorld`, independent of which planner produced that path. - Parametrize immediately after geometric planning and only construct/cache a `GeneratedPlan` after trajectory generation and validation succeed. +- Preserve planner-native timed trajectories without parametrizing them again; validate and store their existing timing. - Allow the selected backend to perform bounded interpolation or curve fitting while converting the source path into a timed trajectory. - Use RoboPlan scene limits sourced from URDF velocity and acceleration limits; fail explicitly when required limits or the selected backend are unavailable. - Do not switch parametrization backends after startup or fall back to another backend when parametrization fails. +- Add a Viser "Next plan speed" control that applies a bounded runtime reduction + scale to future plans without mutating an already accepted plan. - Exclude geometric path shortcutting, waypoint simplification, path-specific resampling, and formal DimOS per-joint limit overrides from this change. - Pin the optional RoboPlan dependency to version `0.5.1`. ## Affected DimOS Surfaces -- Modules/streams: manipulation plan materialization, planning configuration/models, a trajectory-parametrizer adapter protocol, RoboPlan world/model integration, and timed-trajectory validation; no stream contract changes. -- Blueprints/CLI: manipulation blueprint configuration gains a startup-selectable parametrization backend; no new CLI command or blueprint name is introduced. +- Modules/streams: manipulation plan materialization, planning configuration/models, a trajectory-parametrizer adapter protocol, RoboPlan world/model integration, and timed-trajectory validation; planner-native timed results bypass path parametrization and no stream contracts change. +- Blueprints/CLI: manipulation blueprint configuration gains a startup-selectable parametrization backend; Viser gains a next-plan speed slider; no new CLI command or blueprint name is introduced. - Skills/MCP: existing plan, preview, and execute surfaces retain their signatures; unsuccessful parametrization makes planning fail before preview or execution. -- Hardware/simulation/replay: hardware and simulation execute the exact trajectory accepted during planning; RoboPlan TOPP-RA requires URDF velocity and acceleration limits. Replay behavior is unchanged. +- Hardware/simulation/replay: hardware and simulation preserve the accepted trajectory's time domain during robot-local joint projection; RoboPlan TOPP-RA requires URDF velocity and acceleration limits. Replay behavior is unchanged. - Docs/generated registries: manipulation planning documentation and dependency guidance require updates; no generated blueprint registry change is expected. ## Capabilities diff --git a/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md b/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md index fb56d8b21c..3677a6f48d 100644 --- a/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md +++ b/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md @@ -2,7 +2,7 @@ ### Requirement: A single trajectory parametrization backend is selected at startup -The manipulation stack SHALL select exactly one trajectory parametrization backend during startup and SHALL use that backend for every plan materialized during that run. +The manipulation stack SHALL select exactly one trajectory parametrization backend during startup and SHALL use that backend for every untimed geometric path materialized during that run. #### Scenario: Simple backend is selected - **GIVEN** a manipulation stack configured with the simple trajectory parametrization backend @@ -22,6 +22,13 @@ The manipulation stack SHALL select exactly one trajectory parametrization backe - **THEN** initialization MUST fail with an actionable configuration error - **AND** planning MUST NOT begin with a backend that cannot operate against the configured world +#### Scenario: Planner returns a timed trajectory +- **GIVEN** a planner returns a trajectory with authoritative timestamps and velocities +- **WHEN** the generated plan is materialized +- **THEN** the system MUST preserve and canonically validate the planner-provided timing +- **AND** it MUST NOT invoke a trajectory parametrization backend +- **AND** this bypass MUST NOT be treated as backend fallback + ### Requirement: Parametrization completes before a generated plan is accepted The manipulation stack SHALL convert an accepted geometric path into a timed trajectory before exposing or caching the corresponding generated plan. @@ -38,6 +45,12 @@ The manipulation stack SHALL convert an accepted geometric path into a timed tra - **THEN** the system MUST report plan materialization failure - **AND** it MUST NOT cache or expose that path as an executable generated plan +#### Scenario: Existing planner timing is accepted +- **GIVEN** a planner has returned a valid timed trajectory +- **WHEN** canonical timed-output validation succeeds +- **THEN** the system SHALL construct and cache one generated plan containing the source path and planner-native trajectory +- **AND** preview and execution MUST consume that same trajectory without retiming + ### Requirement: Parametrization converts a path into continuous timed motion The selected backend SHALL convert the source geometric path into one trajectory with a shared time domain across every selected joint. @@ -118,7 +131,35 @@ The manipulation stack MUST NOT silently switch to another trajectory parametriz Trajectory parametrization SHALL integrate without changing the public plan, preview, execute, skill, MCP, or stream signatures. #### Scenario: Existing preview and execution flow -- **GIVEN** a generated plan was successfully materialized by either supported backend +- **GIVEN** a generated plan was successfully materialized from either an untimed path or a planner-native timed trajectory - **WHEN** a caller invokes the existing preview or execute surface - **THEN** the caller MUST use the same public operation and argument shape as before -- **AND** the accepted stored trajectory MUST be previewed or dispatched without retiming +- **AND** execution MAY project the accepted stored trajectory into robot-local joint order +- **AND** the accepted timestamps and velocities MUST be previewed or dispatched without regeneration or retiming + +### Requirement: Viser controls the speed of future plans + +The manipulation Viser panel SHALL expose a bounded runtime speed scale for +plans generated after the setting changes. + +#### Scenario: Operator reduces next-plan speed +- **GIVEN** the Viser panel is idle and displays a fresh accepted plan +- **WHEN** the operator moves `Next plan speed` to a value in `(0, 1]` +- **THEN** the module MUST retain that value for future planning +- **AND** the existing accepted plan MUST remain unchanged and executable + +#### Scenario: A new untimed path is planned +- **GIVEN** a runtime next-plan speed below `1.0` +- **WHEN** an untimed geometric path is materialized +- **THEN** the selected parametrizer MUST multiply its configured velocity and acceleration reduction scales by the runtime scale + +#### Scenario: A new Cartesian path is planned from Viser +- **GIVEN** a runtime next-plan speed below `1.0` +- **WHEN** Viser requests planner-native Cartesian planning +- **THEN** the request MUST carry that velocity and acceleration scale +- **AND** the returned planner-native timing MUST still bypass path parametrization + +#### Scenario: Speed changes during an active panel operation +- **GIVEN** the Viser panel is planning, previewing, executing, cancelling, or clearing +- **WHEN** the speed control is rendered +- **THEN** it MUST be disabled until the operation becomes idle diff --git a/openspec/changes/add-trajectory-parametrization/tasks.md b/openspec/changes/add-trajectory-parametrization/tasks.md index 611a52c726..e176975310 100644 --- a/openspec/changes/add-trajectory-parametrization/tasks.md +++ b/openspec/changes/add-trajectory-parametrization/tasks.md @@ -1,52 +1,63 @@ ## 1. Configuration and Adapter Boundary - [x] 1.1 Pin `roboplan==0.5.1` in manipulation and lint dependencies and regenerate `uv.lock`. -- [ ] 1.2 Add a focused RoboPlan 0.5.1 binding contract test covering TOPP-RA construction, fitting-mode names, options, native trajectory fields, and missing-limit behavior. -- [ ] 1.3 Add typed startup configuration for `simple_trapezoid` and `roboplan_toppra`, including validated common scales/output period and backend-specific fitting controls. -- [ ] 1.4 Add the internal trajectory-parametrizer adapter Protocol and typed request/failure boundary without introducing a separate public generated-trajectory lifecycle. -- [ ] 1.5 Extend planning factory validation so exactly one parametrizer is constructed at startup and `roboplan_toppra` with a non-RoboPlan world fails before planning. +- [x] 1.2 Add a focused RoboPlan 0.5.1 binding contract test covering TOPP-RA construction, fitting-mode names, options, native trajectory fields, and missing-limit behavior. +- [x] 1.3 Add typed startup configuration for `simple_trapezoid` and `roboplan_toppra`, including validated common scales/output period and backend-specific fitting controls. +- [x] 1.4 Add the internal trajectory-parametrizer adapter Protocol and typed request/failure boundary without introducing a separate public generated-trajectory lifecycle. +- [x] 1.5 Extend planning factory validation so exactly one parametrizer is constructed at startup and `roboplan_toppra` with a non-RoboPlan world fails before planning. ## 2. Parametrization Backends -- [ ] 2.1 Wrap the existing `JointTrajectoryGenerator` as the `simple_trapezoid` adapter while preserving current limit resolution, waypoint, and timing behavior. -- [ ] 2.2 Implement the RoboPlan TOPP-RA adapter using the finalized `RoboPlanWorld` model, selected-group lookup, and exact global-to-native joint mapping. -- [ ] 2.3 Validate that every selected RoboPlan joint has finite positive URDF-backed velocity and acceleration limits, with no fallback to generic DimOS motion-limit fields. -- [ ] 2.4 Map configured TOPP-RA fitting mode, output period, velocity/acceleration reduction scales, and adaptive/blend options into the pinned 0.5.1 API. -- [ ] 2.5 Convert RoboPlan native trajectory output back to exact selected global joint order and retain positions, velocities, timestamps, and native acceleration data long enough for limit validation. -- [ ] 2.6 Ensure a selected backend failure returns one actionable materialization error and never invokes the other backend; retain documented RoboPlan internal safe fitting-mode behavior. +- [x] 2.1 Wrap the existing `JointTrajectoryGenerator` as the `simple_trapezoid` adapter while preserving current limit resolution, waypoint, and timing behavior. +- [x] 2.2 Implement the RoboPlan TOPP-RA adapter using the finalized `RoboPlanWorld` model, selected-group lookup, and exact global-to-native joint mapping. +- [x] 2.3 Validate that every selected RoboPlan joint has finite positive URDF-backed velocity and acceleration limits, with no fallback to generic DimOS motion-limit fields. +- [x] 2.4 Map configured TOPP-RA fitting mode, output period, velocity/acceleration reduction scales, and adaptive/blend options into the pinned 0.5.1 API. +- [x] 2.5 Convert RoboPlan native trajectory output back to exact selected global joint order and retain positions, velocities, timestamps, and native acceleration data long enough for limit validation. +- [x] 2.6 Ensure a selected backend failure returns one actionable materialization error and never invokes the other backend; retain documented RoboPlan internal safe fitting-mode behavior. ## 3. Plan Materialization and Validation -- [ ] 3.1 Construct and retain the selected trajectory parametrizer during manipulation planning initialization. -- [ ] 3.2 Route `_materialize_generated_plan()` through the selected adapter while preserving the source `JointState` path unchanged in `GeneratedPlan`. -- [ ] 3.3 Preserve planning-epoch atomicity so parametrization or output-validation failure leaves no cached executable plan. -- [ ] 3.4 Extend canonical timed-trajectory validation for exact global joint ordering, dimensions, finite values, zero start time, strictly increasing times, positive non-noop duration, and start/goal preservation. -- [ ] 3.5 Validate returned velocity and acceleration against the backend's applicable limits with documented numerical tolerances, preferring native acceleration samples when available. -- [ ] 3.6 Verify preview and execution reuse the accepted stored trajectory without regeneration or retiming. +- [x] 3.1 Construct and retain the selected trajectory parametrizer during manipulation planning initialization. +- [x] 3.2 Route `_materialize_generated_plan()` through the selected adapter while preserving the source `JointState` path unchanged in `GeneratedPlan`. +- [x] 3.2a Preserve the existing planner-native timed-result path so it bypasses parametrization, retains its timestamps and velocities, and still receives canonical timed-output validation. +- [x] 3.3 Preserve planning-epoch atomicity so parametrization or output-validation failure leaves no cached executable plan. +- [x] 3.4 Extend canonical timed-trajectory validation for exact global joint ordering, dimensions, finite values, zero start time, strictly increasing times, positive non-noop duration, and start/goal preservation. +- [x] 3.5 Validate returned velocity and acceleration against the backend's applicable limits with documented numerical tolerances, preferring native acceleration samples when available. +- [x] 3.6 Verify preview and execution reuse the accepted stored trajectory without regeneration or retiming. +- [x] 3.7 Add a runtime next-plan speed setting, apply it to untimed + parametrization and Viser Cartesian request timing, and keep accepted plans + immutable when the setting changes. +- [x] 3.8 Add the Viser `Next plan speed` slider through + `ManipulationOperator`, including active-operation disabling. ## 4. Automated Tests -- [ ] 4.1 Add adapter tests for valid simple and RoboPlan trajectories, multi-waypoint continuity, global/native reordering, composite planning groups, and configurable fitting modes. -- [ ] 4.2 Add startup/configuration tests for each backend, unknown backends, invalid scales/options, incompatible world selection, and startup-only backend lifetime. -- [ ] 4.3 Add RoboPlan limit tests for valid URDF velocity/acceleration limits, missing limits, non-finite or non-positive limits, reduction scales, and proof that generic DimOS defaults are not substituted. -- [ ] 4.4 Add materialization tests for backend failure, no cross-backend fallback, malformed/native output rejection, motion-limit rejection, and no plan caching after failure. -- [ ] 4.5 Update preview/execution tests to prove the exact accepted timed trajectory reaches visualization and the coordinator without regeneration. -- [ ] 4.6 Run focused test targets including `dimos/manipulation/test_generated_plan_materialization.py`, `dimos/manipulation/test_planning_factory.py`, `dimos/manipulation/test_roboplan_world.py`, `dimos/manipulation/test_plan_execution.py`, and new parametrizer tests. +- [x] 4.1 Add adapter tests for valid simple and RoboPlan trajectories, multi-waypoint continuity, global/native reordering, composite planning groups, and configurable fitting modes. +- [x] 4.2 Add startup/configuration tests for each backend, unknown backends, invalid scales/options, incompatible world selection, and startup-only backend lifetime. +- [x] 4.3 Add RoboPlan limit tests for valid URDF velocity/acceleration limits, missing limits, non-finite or non-positive limits, reduction scales, and proof that generic DimOS defaults are not substituted. +- [x] 4.4 Add materialization tests for backend failure, no cross-backend fallback, planner-native timed-result bypass, malformed/native output rejection, motion-limit rejection, and no plan caching after failure. +- [x] 4.5 Update preview/execution tests to prove the accepted timed trajectory reaches visualization and robot-local coordinator dispatch without regeneration or retiming. +- [x] 4.6 Run focused test targets including `dimos/manipulation/test_generated_plan_materialization.py`, `dimos/manipulation/test_planning_factory.py`, `dimos/manipulation/test_roboplan_world.py`, `dimos/manipulation/test_plan_execution.py`, and new parametrizer tests. +- [x] 4.7 Add runtime-scale and Viser tests covering valid/invalid values, + future-plan application, Cartesian request scaling, current-plan + preservation, and active-operation disabling. ## 5. Documentation -- [ ] 5.1 Update `dimos/manipulation/planning/README.md` with the path-to-trajectory lifecycle, backend configuration examples, RoboPlan fitting modes, `RoboPlanWorld` compatibility, no cross-backend fallback, and URDF limit requirements. -- [ ] 5.2 Update `docs/capabilities/manipulation/index.md` to explain that a plan is accepted only after parametrization and that preview and execution share the stored trajectory. -- [ ] 5.3 Update `docs/capabilities/manipulation/adding_a_custom_arm.md` with RoboPlan 0.5.1 URDF velocity and extended acceleration-limit requirements and missing-limit failure behavior. -- [ ] 5.4 Reconcile `CONTEXT.md` and `docs/adr/0001` through `docs/adr/0006` with the implemented names and behavior; update `AGENTS.md` only if stable extension guidance is added. +- [x] 5.1 Update `dimos/manipulation/planning/README.md` with the path-to-trajectory lifecycle, backend configuration examples, RoboPlan fitting modes, `RoboPlanWorld` compatibility, no cross-backend fallback, and URDF limit requirements. +- [x] 5.2 Update `docs/capabilities/manipulation/index.md` to explain that a plan is accepted only after parametrization and that preview and execution share the stored trajectory. +- [x] 5.3 Update `docs/capabilities/manipulation/adding_a_custom_arm.md` with RoboPlan 0.5.1 URDF velocity and extended acceleration-limit requirements and missing-limit failure behavior. +- [x] 5.4 Reconcile `CONTEXT.md` and `docs/development/adr/0001` through `docs/development/adr/0006` with the implemented names and behavior; update `AGENTS.md` only if stable extension guidance is added. +- [x] 5.5 Document Viser next-plan speed semantics and the fact that changing + the slider requires planning again. ## 6. Verification and Manual QA -- [ ] 6.1 Run `OPENSPEC_TELEMETRY=0 openspec validate add-trajectory-parametrization`. -- [ ] 6.2 Run `uv lock --check` and verify RoboPlan resolves to exactly `0.5.1` on supported Python/platform markers. -- [ ] 6.3 Run `uv run mypy dimos/manipulation` and the repository's Ruff/pre-commit checks for changed Python files. -- [ ] 6.4 Run the focused tests from task 4.6 and the broader fast manipulation test suite. -- [ ] 6.5 Run `doclinks` and applicable `md-babel-py run` commands for changed documentation examples; run `bin/gen-diagrams` only if generated diagram sources changed. +- [x] 6.1 Run `OPENSPEC_TELEMETRY=0 openspec validate add-trajectory-parametrization`. +- [x] 6.2 Run `uv lock --check` and verify RoboPlan resolves to exactly `0.5.1` on supported Python/platform markers. +- [x] 6.3 Run `uv run mypy dimos/manipulation` and the repository's Ruff/pre-commit checks for changed Python files. +- [x] 6.4 Run the focused tests from task 4.6 and the broader fast manipulation test suite. +- [x] 6.5 Run `doclinks` and applicable `md-babel-py run` commands for changed documentation examples; run `bin/gen-diagrams` only if generated diagram sources changed. - [ ] 6.6 Manually plan, preview, and execute a nontrivial multi-waypoint path in a manipulation simulation with `simple_trapezoid`, confirming compatibility behavior and identical preview/execution timing. - [ ] 6.7 Manually repeat the simulation with `RoboPlanWorld` and `roboplan_toppra`, confirming smooth interior traversal, URDF-limit compliance, and identical preview/execution timing. - [ ] 6.8 Manually verify actionable pre-motion failures for an incompatible world/backend combination, a missing URDF acceleration limit, and a TOPP-RA parametrization failure. From be1ca833b895963b330932ef7fe5697c6526cbb1 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 29 Jul 2026 23:44:20 -0700 Subject: [PATCH 06/14] refactor(manipulation): centralize plan materialization --- CONTEXT.md | 5 + dimos/manipulation/manipulation_module.py | 359 ++---------------- dimos/manipulation/pick_and_place_module.py | 4 +- dimos/manipulation/planning/README.md | 5 + dimos/manipulation/planning/factory.py | 21 +- dimos/manipulation/planning/spec/protocols.py | 16 + .../trajectory_generator/parametrizer.py | 317 ++++++++++++++-- .../roboplan_toppra_parametrizer.py | 65 ++-- .../simple_parametrizer.py | 75 +++- .../trajectory_generator/test_parametrizer.py | 203 ++++++++++ .../test_roboplan_toppra_parametrizer.py | 112 ++++-- .../test_simple_parametrizer.py | 136 +++++-- .../test_generated_plan_materialization.py | 138 +------ .../test_manipulation_monitor_preview.py | 18 +- dimos/manipulation/test_manipulation_unit.py | 55 ++- dimos/manipulation/test_plan_execution.py | 2 +- dimos/manipulation/test_planning_factory.py | 30 +- ...-one-trajectory-parametrization-backend.md | 16 +- .../add-trajectory-parametrization/design.md | 68 ++-- .../proposal.md | 2 +- .../add-trajectory-parametrization/tasks.md | 15 +- 21 files changed, 962 insertions(+), 700 deletions(-) create mode 100644 dimos/manipulation/planning/trajectory_generator/test_parametrizer.py diff --git a/CONTEXT.md b/CONTEXT.md index 98de271185..4cecc25972 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -58,6 +58,11 @@ accepted. A failure does not switch backends and leaves no executable plan cached. Planner-native timed results skip conversion because they are already timed trajectories, not because the selected backend failed. +`TrajectoryParametrizerSpec` owns the conversion boundary, including canonical +trajectory validation and construction of the `GeneratedPlan`. +`ManipulationModule` selects the planning groups, delegates that conversion, +and atomically stores the accepted result. + `simple_trapezoid` uses the current DimOS motion-limit resolution. `roboplan_toppra` is available only with `RoboPlanWorld` and uses finite, positive URDF velocity and extended acceleration limits from the RoboPlan diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index a15ee2fc81..38cd80e1a7 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -49,7 +49,6 @@ KinematicsName, WorldBackend, create_planning_specs, - create_trajectory_parametrizer, create_world, ) from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection @@ -81,18 +80,15 @@ RobotName, WorldRobotID, ) -from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec +from dimos.manipulation.planning.spec.protocols import ( + KinematicsSpec, + PlannerSpec, + TrajectoryParametrizerSpec, +) from dimos.manipulation.planning.trajectory_generator.config import ( SimpleTrapezoidParametrizationConfig, TrajectoryParametrizationConfig, ) -from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( - JointTrajectoryGenerator, -) -from dimos.manipulation.planning.trajectory_generator.parametrizer import ( - TrajectoryParametrizationRequest, - TrajectoryParametrizer, -) from dimos.manipulation.skill_errors import ManipulationSkillError from dimos.manipulation.visualization.config import ( ManipulationVisualizationConfig, @@ -106,19 +102,13 @@ from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint from dimos.utils.logging_config import setup_logger logger = setup_logger() -_TRAJECTORY_POSITION_TOLERANCE = 1e-6 -_TRAJECTORY_LIMIT_RELATIVE_TOLERANCE = 1e-2 -_TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE = 1e-8 - # Composite type aliases for readability (using semantic IDs from planning.spec) -RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] -"""(world_robot_id, config, trajectory_generator)""" +RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig] +"""(world_robot_id, config)""" RobotRegistry: TypeAlias = dict[RobotName, RobotEntry] """Maps robot_name -> RobotEntry""" @@ -199,9 +189,9 @@ def __init__(self, **kwargs: Any) -> None: self._world_monitor: WorldMonitor | None = None self._planner: PlannerSpec | None = None self._kinematics: KinematicsSpec | None = None - self._trajectory_parametrizer: TrajectoryParametrizer | None = None + self._trajectory_parametrizer: TrajectoryParametrizerSpec | None = None - # Robot registry: maps robot_name -> (world_robot_id, config, trajectory_gen) + # Robot registry: maps robot_name -> (world_robot_id, config) self._robots: RobotRegistry = {} # Canonical generated plan for plan/preview/execute workflow. @@ -257,6 +247,7 @@ def _initialize_planning(self) -> None: self._world_monitor = planning_specs.world_monitor self._planner = planning_specs.planner self._kinematics = planning_specs.kinematics + self._trajectory_parametrizer = planning_specs.trajectory_parametrizer visualization = create_manipulation_visualization( self.config.visualization, world=world, @@ -266,20 +257,10 @@ def _initialize_planning(self) -> None: for robot_config in self.config.robots: robot_id = self._world_monitor.add_robot(robot_config) - traj_gen = JointTrajectoryGenerator( - num_joints=len(robot_config.joint_names), - max_velocity=robot_config.max_velocity, - max_acceleration=robot_config.max_acceleration, - ) - self._robots[robot_config.name] = (robot_id, robot_config, traj_gen) + self._robots[robot_config.name] = (robot_id, robot_config) operator = ManipulationOperator(self, self._world_monitor) self._world_monitor.finalize(visualization, operator=operator) - self._trajectory_parametrizer = create_trajectory_parametrizer( - self.config.trajectory_parametrization, - world=world, - world_backend=self.config.world_backend, - ) # Add floor obstacle to prevent trajectories below the table surface if self.config.floor_z is not None: @@ -298,7 +279,7 @@ def _initialize_planning(self) -> None: self._world_monitor.add_obstacle(floor_obs) logger.info(f"Floor obstacle added at z={fz:.3f}") - for _, (robot_id, _, _) in self._robots.items(): + for _, (robot_id, _) in self._robots.items(): self._world_monitor.start_state_monitor(robot_id) if self._world_monitor.visualization is not None: @@ -307,7 +288,7 @@ def _initialize_planning(self) -> None: logger.info(f"Visualization: {url}") # Start TF publishing thread if any robot has tf_extra_links - if any(c.tf_extra_links for _, c, _ in self._robots.values()): + if any(c.tf_extra_links for _, c in self._robots.values()): logger.info(f"Eager-initializing TF: {self.tf}") self._tf_stop_event.clear() self._tf_thread = threading.Thread( @@ -324,14 +305,14 @@ def _get_default_robot_name(self) -> RobotName | None: def _get_robot( self, robot_name: RobotName | None = None - ) -> tuple[RobotName, WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] | None: + ) -> tuple[RobotName, WorldRobotID, RobotModelConfig] | None: """Get robot by name or default. Args: robot_name: Robot name or None for default (if single robot) Returns: - (robot_name, robot_id, config, traj_gen) or None if not found + (robot_name, robot_id, config) or None if not found """ if not robot_name: # None or empty string (LLMs often pass "") robot_name = self._get_default_robot_name() @@ -343,8 +324,8 @@ def _get_robot( logger.error(f"Unknown robot: {robot_name}") return None - robot_id, config, traj_gen = self._robots[robot_name] - return (robot_name, robot_id, config, traj_gen) + robot_id, config = self._robots[robot_name] + return (robot_name, robot_id, config) def _on_joint_state(self, msg: JointState) -> None: """Callback when joint state received from driver. @@ -359,7 +340,7 @@ def _on_joint_state(self, msg: JointState) -> None: # 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(): + 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] if any(idx is None for idx in indices): @@ -409,7 +390,7 @@ def _tf_publish_loop(self) -> None: if self._world_monitor is None: break transforms: list[Transform] = [] - for robot_id, config, _ in self._robots.values(): + 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: @@ -557,7 +538,7 @@ def is_collision_free(self, joints: list[float], robot_name: RobotName | None = robot_name: Robot to check (required if multiple robots configured) """ if (robot := self._get_robot(robot_name)) and self._world_monitor: - _, robot_id, config, _ = robot + _, robot_id, config = robot joint_state = JointState(name=config.joint_names, position=joints) return self._world_monitor.is_state_valid(robot_id, joint_state) return False @@ -611,250 +592,6 @@ def _require_unique_pose_group_id_for_robot(self, robot_name: RobotName) -> Plan ) return group_id - @staticmethod - def _assert_finite_sequence(values: Sequence[float], label: str) -> None: - for value in values: - if not math.isfinite(value): - raise ValueError(f"{label} contains non-finite value") - - def _limits_for_global_joints( - self, joint_names: Sequence[str] - ) -> tuple[list[float], list[float]]: - velocities: list[float] = [] - accelerations: list[float] = [] - for global_name in joint_names: - if "/" not in global_name: - raise ValueError(f"Joint '{global_name}' is not globally named") - robot_name, local_name = global_name.split("/", 1) - robot = self._get_robot(robot_name) - if robot is None: - raise ValueError(f"Unknown robot for joint '{global_name}'") - _, _, config, _ = robot - if local_name not in config.joint_names: - raise ValueError(f"Unknown local joint '{global_name}'") - velocity = float(config.max_velocity) - acceleration = float(config.max_acceleration) - if not math.isfinite(velocity) or velocity <= 0.0: - raise ValueError(f"Invalid velocity limit for '{global_name}'") - if not math.isfinite(acceleration) or acceleration <= 0.0: - raise ValueError(f"Invalid acceleration limit for '{global_name}'") - velocities.append(velocity) - accelerations.append(acceleration) - return velocities, accelerations - - def _validate_selected_path( - self, path: Sequence[JointState], expected_names: Sequence[str] - ) -> list[list[float]]: - if len(path) < 2: - raise ValueError("Planner returned fewer than two waypoints") - expected = list(expected_names) - waypoints: list[list[float]] = [] - for waypoint_index, state in enumerate(path): - if list(state.name) != expected: - raise ValueError( - f"Waypoint {waypoint_index} joint names do not match selected order" - ) - positions = list(state.position) - if len(positions) != len(expected): - raise ValueError(f"Waypoint {waypoint_index} position dimension mismatch") - self._assert_finite_sequence(positions, f"Waypoint {waypoint_index} positions") - waypoints.append(positions) - return waypoints - - def _validate_generated_trajectory( - self, - trajectory: JointTrajectory, - expected_names: Sequence[str], - waypoints: Sequence[Sequence[float]], - *, - velocity_limits: Sequence[float] | None = None, - acceleration_limits: Sequence[float] | None = None, - accelerations: Sequence[Sequence[float]] | None = None, - ) -> None: - expected = list(expected_names) - if list(trajectory.joint_names) != expected: - raise ValueError("Generated trajectory joint names do not match selected order") - if not trajectory.points: - raise ValueError("Generated trajectory has no points") - previous_time: float | None = None - for point_index, point in enumerate(trajectory.points): - if len(point.positions) != len(expected) or len(point.velocities) != len(expected): - raise ValueError(f"Generated point {point_index} dimension mismatch") - self._assert_finite_sequence( - point.positions, f"Generated point {point_index} positions" - ) - self._assert_finite_sequence( - point.velocities, f"Generated point {point_index} velocities" - ) - if not math.isfinite(point.time_from_start): - raise ValueError(f"Generated point {point_index} time is non-finite") - if point_index == 0 and point.time_from_start != 0.0: - raise ValueError("Generated trajectory must start at time 0") - if previous_time is not None and point.time_from_start <= previous_time: - raise ValueError("Generated trajectory times must be strictly increasing") - previous_time = point.time_from_start - non_noop = any(list(waypoint) != list(waypoints[0]) for waypoint in waypoints[1:]) - if non_noop and trajectory.duration <= 0.0: - raise ValueError("Generated trajectory duration must be positive") - if not self._positions_close(trajectory.points[0].positions, waypoints[0]): - raise ValueError("Generated trajectory does not preserve the path start") - if not self._positions_close(trajectory.points[-1].positions, waypoints[-1]): - raise ValueError("Generated trajectory does not preserve the path goal") - if velocity_limits is not None: - self._validate_motion_limits( - trajectory, - velocity_limits, - acceleration_limits, - accelerations, - ) - - @staticmethod - def _positions_close(first: Sequence[float], second: Sequence[float]) -> bool: - return len(first) == len(second) and all( - math.isclose( - left, - right, - rel_tol=0.0, - abs_tol=_TRAJECTORY_POSITION_TOLERANCE, - ) - for left, right in zip(first, second, strict=True) - ) - - def _validate_motion_limits( - self, - trajectory: JointTrajectory, - velocity_limits: Sequence[float], - acceleration_limits: Sequence[float] | None, - accelerations: Sequence[Sequence[float]] | None, - ) -> None: - expected_dimension = len(trajectory.joint_names) - if len(velocity_limits) != expected_dimension: - raise ValueError("Velocity limits do not match selected joints") - self._assert_valid_motion_limits(velocity_limits, "velocity") - for point_index, point in enumerate(trajectory.points): - self._assert_within_limits( - point.velocities, - velocity_limits, - f"Generated point {point_index} velocity", - ) - if acceleration_limits is None: - return - if len(acceleration_limits) != expected_dimension: - raise ValueError("Acceleration limits do not match selected joints") - self._assert_valid_motion_limits(acceleration_limits, "acceleration") - if accelerations is not None: - if len(accelerations) != len(trajectory.points): - raise ValueError("Acceleration samples do not match trajectory points") - for point_index, values in enumerate(accelerations): - if len(values) != expected_dimension: - raise ValueError( - f"Generated point {point_index} acceleration dimension mismatch" - ) - self._assert_finite_sequence(values, f"Generated point {point_index} accelerations") - self._assert_within_limits( - values, - acceleration_limits, - f"Generated point {point_index} acceleration", - ) - return - for point_index in range(1, len(trajectory.points)): - previous = trajectory.points[point_index - 1] - current = trajectory.points[point_index] - dt = current.time_from_start - previous.time_from_start - derived = [ - (current_velocity - previous_velocity) / dt - for previous_velocity, current_velocity in zip( - previous.velocities, current.velocities, strict=True - ) - ] - self._assert_within_limits( - derived, - acceleration_limits, - f"Generated interval {point_index - 1}:{point_index} acceleration", - ) - - @staticmethod - def _assert_valid_motion_limits(values: Sequence[float], label: str) -> None: - if any(not math.isfinite(value) or value <= 0.0 for value in values): - raise ValueError(f"Invalid {label} limits") - - @staticmethod - def _assert_within_limits(values: Sequence[float], limits: Sequence[float], label: str) -> None: - for joint_index, (value, limit) in enumerate(zip(values, limits, strict=True)): - tolerance = max( - _TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE, - limit * _TRAJECTORY_LIMIT_RELATIVE_TOLERANCE, - ) - if abs(value) > limit + tolerance: - raise ValueError(f"{label} exceeds joint {joint_index} limit: {value} vs {limit}") - - def _materialize_generated_plan( - self, group_ids: tuple[PlanningGroupID, ...], result_path: Sequence[JointState] - ) -> tuple[list[JointState], JointTrajectory]: - assert self._world_monitor is not None - selection = self._world_monitor.planning_groups.select(group_ids) - expected_names = list(selection.joint_names) - path = [JointState(state) for state in result_path] - waypoints = self._validate_selected_path(path, expected_names) - if self._trajectory_parametrizer is None: - raise ValueError("Trajectory parametrizer is not initialized") - velocity_limits: tuple[float, ...] | None = None - acceleration_limits: tuple[float, ...] | None = None - if self._trajectory_parametrizer.uses_request_limits: - velocities, accelerations = self._limits_for_global_joints(expected_names) - velocity_limits = tuple(velocities) - acceleration_limits = tuple(accelerations) - parametrized = self._trajectory_parametrizer.parametrize( - TrajectoryParametrizationRequest( - group_ids=group_ids, - joint_names=tuple(expected_names), - path=tuple(path), - velocity_limits=velocity_limits, - acceleration_limits=acceleration_limits, - speed_scale=self.get_motion_speed(), - ) - ) - trajectory = parametrized.trajectory - self._validate_generated_trajectory( - trajectory, - expected_names, - waypoints, - velocity_limits=parametrized.velocity_limits, - acceleration_limits=parametrized.acceleration_limits, - accelerations=parametrized.accelerations, - ) - return path, trajectory - - def _materialize_timed_generated_plan( - self, - group_ids: tuple[PlanningGroupID, ...], - result: PlanningResult, - ) -> tuple[list[JointState], JointTrajectory]: - """Preserve a planner-supplied timed trajectory without reparameterizing it.""" - assert self._world_monitor is not None - selection = self._world_monitor.planning_groups.select(group_ids) - expected_names = list(selection.joint_names) - path = [JointState(state) for state in result.path] - waypoints = self._validate_selected_path(path, expected_names) - timestamps = result.timestamps - if timestamps is None or len(timestamps) != len(path): - raise ValueError("Planner must return one timestamp per waypoint") - points: list[TrajectoryPoint] = [] - for waypoint_index, (state, timestamp) in enumerate(zip(path, timestamps, strict=True)): - velocities = list(state.velocity) - if len(velocities) != len(expected_names): - raise ValueError(f"Waypoint {waypoint_index} velocity dimension mismatch") - points.append( - TrajectoryPoint( - time_from_start=float(timestamp), - positions=list(state.position), - velocities=velocities, - ) - ) - trajectory = JointTrajectory(joint_names=expected_names, points=points) - self._validate_generated_trajectory(trajectory, expected_names, waypoints) - return path, trajectory - def _resolve_group_plan_start( self, group_ids: tuple[PlanningGroupID, ...], @@ -876,28 +613,21 @@ def _store_generated_plan( group_ids: tuple[PlanningGroupID, ...], result: PlanningResult, planning_epoch: int, - *, - preserve_timing: bool = False, ) -> GeneratedPlan | None: """Validate, materialize, and atomically store a successful planning result.""" try: - if preserve_timing: - path, trajectory = self._materialize_timed_generated_plan(group_ids, result) - else: - path, trajectory = self._materialize_generated_plan(group_ids, result.path) + if self._world_monitor is None or self._trajectory_parametrizer is None: + raise ValueError("Trajectory parametrizer is not initialized") + selection = self._world_monitor.planning_groups.select(group_ids) + plan = self._trajectory_parametrizer.materialize_plan( + world=self._world_monitor.world, + selection=selection, + result=result, + speed_scale=self.get_motion_speed(), + ) except Exception as exc: self._fail_planning_epoch(planning_epoch, f"Failed to materialize plan: {exc}") return None - plan = GeneratedPlan( - group_ids=group_ids, - trajectory=trajectory, - path=path, - status=result.status, - planning_time=result.planning_time, - path_length=result.path_length, - iterations=result.iterations, - message=result.message, - ) with self._lock: if self._state != ManipulationState.PLANNING or planning_epoch != self._planning_epoch: logger.info("Discarding cancelled planning result") @@ -1059,7 +789,7 @@ def inverse_kinematics_single( robot = self._get_robot(robot_name) if robot is None: return IKResult(status=IKStatus.NO_SOLUTION, message="Robot not found") - selected_robot_name, _, _, _ = robot + selected_robot_name, _, _ = robot try: group_id = self._require_unique_pose_group_id_for_robot(selected_robot_name) except ValueError as exc: @@ -1134,7 +864,7 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: if robot is None: self._record_error("Robot not found or robot_name is required") return False - selected_robot_name, _, _, _ = robot + selected_robot_name, _, _ = robot try: group_id = self._require_unique_pose_group_id_for_robot(selected_robot_name) except ValueError as exc: @@ -1163,7 +893,7 @@ def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None robot = self._get_robot(robot_name) if robot is None: return False - selected_robot_name, _, _, _ = robot + selected_robot_name, _, _ = robot logger.info( f"Planning to joints for {selected_robot_name}: {[f'{j:.3f}' for j in joints.position]}" ) @@ -1318,12 +1048,7 @@ def generate_cartesian_plan( planning_epoch, f"Cartesian planning failed: {result.status.name}{detail}" ) return None - return self._store_generated_plan( - group_ids, - result, - planning_epoch, - preserve_timing=True, - ) + return self._store_generated_plan(group_ids, result, planning_epoch) @rpc def preview_path( @@ -1447,7 +1172,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> RobotInfoPayloa if robot is None: return None - robot_name, robot_id, config, _ = robot + robot_name, robot_id, config = robot planning_groups = ( list(self._world_monitor.planning_groups.groups_for_robot(robot_name)) if self._world_monitor is not None @@ -1477,7 +1202,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> RobotInfoPayloa def robot_items(self) -> list[tuple[RobotName, WorldRobotID, RobotModelConfig]]: """Return configured robots for in-process visualization adapters.""" - return [(name, robot_id, config) for name, (robot_id, config, _) in self._robots.items()] + return [(name, robot_id, config) for name, (robot_id, config) in self._robots.items()] def robot_id_for_name(self, robot_name: RobotName) -> WorldRobotID | None: """Return the planning-world robot id for a configured robot name.""" @@ -1486,7 +1211,7 @@ def robot_id_for_name(self, robot_name: RobotName) -> WorldRobotID | None: def robot_name_for_id(self, robot_id: WorldRobotID) -> RobotName | None: """Return the configured robot name for a planning-world robot id.""" - for robot_name, (candidate_id, _, _) in self._robots.items(): + for robot_name, (candidate_id, _) in self._robots.items(): if candidate_id == robot_id: return robot_name return None @@ -1613,7 +1338,7 @@ def set_init_joints_to_current(self, robot_name: RobotName | None = None) -> boo robot = self._get_robot(robot_name) if robot is None: return False - robot_name_resolved, robot_id, _, _ = robot + robot_name_resolved, robot_id, _ = robot if self._world_monitor is None: return False current = self._world_monitor.get_current_joint_state(robot_id) @@ -1635,7 +1360,7 @@ def _initialize_execution(self) -> None: model_joint_names=config.joint_names, coordinator_to_model=config.joint_name_mapping, ) - for _, config, _ in self._robots.values() + for _, config in self._robots.values() ] self._execution_manager = PlanExecutionManager( targets=targets, @@ -1781,7 +1506,7 @@ def _get_gripper_hardware_id(self, robot_name: RobotName | None = None) -> str | robot = self._get_robot(robot_name) if robot is None: return None - _, _, config, _ = robot + _, _, config = robot if not config.gripper_hardware_id: logger.warning(f"No gripper_hardware_id configured for '{config.name}'") return None @@ -2019,7 +1744,7 @@ def move_to_joints( robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, _, config, _ = robot + rname, _, config = robot goal = JointState(name=config.joint_names, position=joint_values) logger.info(f"Planning motion to joints [{', '.join(f'{j:.3f}' for j in joint_values)}]...") @@ -2047,7 +1772,7 @@ def go_home(self, robot_name: str | None = None) -> SkillResult[ManipulationSkil robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, _, config, _ = robot + rname, _, config = robot if config.home_joints is None: return SkillResult.fail( @@ -2083,7 +1808,7 @@ def go_init(self, robot_name: str | None = None) -> SkillResult[ManipulationSkil robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, robot_id, _, _ = robot + rname, robot_id, _ = robot init = self._init_joints.get(rname) if init is None: diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index bc59adce37..b37f6e9882 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -482,7 +482,7 @@ def pick( robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, _, config, _ = robot + rname, _, config = robot pre_grasp_offset = config.pre_grasp_offset # 1. Generate grasps (uses already-cached detections — call scan_objects first) @@ -589,7 +589,7 @@ def _place_with_orientation( robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, _, config, _ = robot + rname, _, config = robot pre_place_offset = config.pre_grasp_offset # Reduce pre-place height for far targets diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index 3acfea151b..6cf9e83910 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -97,6 +97,11 @@ validates joint ordering, dimensions, finite values, strictly increasing time, start and goal preservation, and applicable velocity and acceleration limits. A failure leaves no executable plan cached. +This boundary is exposed internally as `TrajectoryParametrizerSpec`, alongside +`PlannerSpec` and `WorldSpec`. Its implementations own conversion, validation, +and `GeneratedPlan` construction; `ManipulationModule` only supplies the world, +selected planning groups, planning result, and next-plan speed. + A planner may instead return a trajectory that already contains timestamps and velocities. That result is already on the trajectory side of the boundary, so DimOS skips parametrization, preserves its timing, and applies the same diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index 9195ad49d2..b1119e8726 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -30,15 +30,15 @@ ManipulationPlannerConfig, RoboPlanPlannerConfig, ) -from dimos.manipulation.planning.spec.protocols import PlannerSpec +from dimos.manipulation.planning.spec.protocols import ( + PlannerSpec, + TrajectoryParametrizerSpec, +) from dimos.manipulation.planning.trajectory_generator.config import ( RoboPlanTOPPRAParametrizationConfig, SimpleTrapezoidParametrizationConfig, TrajectoryParametrizationConfig, ) -from dimos.manipulation.planning.trajectory_generator.parametrizer import ( - TrajectoryParametrizer, -) from dimos.manipulation.visualization.config import ( ManipulationVisualizationConfig, NoManipulationVisualizationConfig, @@ -59,6 +59,7 @@ class PlanningSpecs: world_monitor: WorldMonitor kinematics: KinematicsSpec planner: PlannerSpec + trajectory_parametrizer: TrajectoryParametrizerSpec WorldBackend: TypeAlias = Literal["drake", "roboplan"] @@ -114,9 +115,8 @@ def validate_backend_combination( def create_trajectory_parametrizer( config: TrajectoryParametrizationConfig, *, - world: WorldSpec, world_backend: str, -) -> TrajectoryParametrizer: +) -> TrajectoryParametrizerSpec: """Construct the one startup-selected path parametrizer.""" if config.backend == "roboplan_toppra" and world_backend != "roboplan": raise ValueError( @@ -132,11 +132,8 @@ def create_trajectory_parametrizer( from dimos.manipulation.planning.trajectory_generator.roboplan_toppra_parametrizer import ( RoboPlanTOPPRAParametrizer, ) - from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld - if not isinstance(world, RoboPlanWorld): - raise ValueError("RoboPlan TOPP-RA requires a finalized RoboPlanWorld instance") - return RoboPlanTOPPRAParametrizer(world, config) + return RoboPlanTOPPRAParametrizer(config) raise TypeError(f"Unsupported trajectory parametrization config: {type(config).__name__}") @@ -244,6 +241,10 @@ def create_planning_specs( world_monitor=WorldMonitor(world=world), kinematics=create_kinematics(config=kinematics), planner=create_planner(config=planner, world=world, world_backend=world_backend), + trajectory_parametrizer=create_trajectory_parametrizer( + trajectory_parametrization, + world_backend=world_backend, + ), ) diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index ff33953025..9214879155 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -34,6 +34,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( CartesianTarget, + GeneratedPlan, IKResult, Obstacle, PlanningGroupID, @@ -332,3 +333,18 @@ def plan_cartesian_path( def get_name(self) -> str: """Get planner name.""" ... + + +@runtime_checkable +class TrajectoryParametrizerSpec(Protocol): + """Convert successful planning output into one canonical generated plan.""" + + def materialize_plan( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + result: PlanningResult, + speed_scale: float = 1.0, + ) -> GeneratedPlan: + """Preserve timed output or parametrize an untimed path, then validate it.""" + ... diff --git a/dimos/manipulation/planning/trajectory_generator/parametrizer.py b/dimos/manipulation/planning/trajectory_generator/parametrizer.py index 52f8dff539..1f255f24ce 100644 --- a/dimos/manipulation/planning/trajectory_generator/parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/parametrizer.py @@ -12,40 +12,32 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Internal path-to-trajectory parametrization boundary.""" +"""Shared implementation for the trajectory-parametrizer planning Spec.""" +from abc import ABC, abstractmethod +from collections.abc import Sequence from dataclasses import dataclass import math -from typing import Protocol -from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.planning.groups.models import PlanningGroupSelection +from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningResult +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +_TRAJECTORY_POSITION_TOLERANCE = 1e-6 +_TRAJECTORY_LIMIT_RELATIVE_TOLERANCE = 1e-2 +_TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE = 1e-8 -class TrajectoryParametrizationError(ValueError): - """A path could not be converted into a valid timed trajectory.""" - - -@dataclass(frozen=True) -class TrajectoryParametrizationRequest: - """Canonical input for an untimed selected-joint path.""" - - group_ids: tuple[PlanningGroupID, ...] - joint_names: tuple[str, ...] - path: tuple[JointState, ...] - velocity_limits: tuple[float, ...] | None = None - acceleration_limits: tuple[float, ...] | None = None - speed_scale: float = 1.0 - def __post_init__(self) -> None: - if not math.isfinite(self.speed_scale) or self.speed_scale <= 0.0 or self.speed_scale > 1.0: - raise ValueError("speed_scale must be finite, > 0, and <= 1") +class TrajectoryParametrizationError(ValueError): + """Planning output could not be converted into a valid generated plan.""" @dataclass(frozen=True) class ParametrizedTrajectory: - """Canonical output plus the limits and accelerations used to validate it.""" + """Backend output plus the limits and accelerations used to validate it.""" trajectory: JointTrajectory velocity_limits: tuple[float, ...] @@ -53,10 +45,283 @@ class ParametrizedTrajectory: accelerations: tuple[tuple[float, ...], ...] | None = None -class TrajectoryParametrizer(Protocol): - """Convert an untimed geometric path into one timed trajectory.""" +class BaseTrajectoryParametrizer(ABC): + """Own common PlanningResult-to-GeneratedPlan materialization.""" + + def materialize_plan( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + result: PlanningResult, + speed_scale: float = 1.0, + ) -> GeneratedPlan: + """Preserve timed output or parametrize an untimed path, then validate it.""" + self._validate_speed_scale(speed_scale) + if not result.is_success(): + raise TrajectoryParametrizationError( + f"Cannot materialize unsuccessful planning result: {result.status.name}" + ) + + path = [JointState(state) for state in result.path] + waypoints = self._validate_selected_path(path, selection.joint_names) + if result.timestamps is None: + parametrized = self._parametrize_path(world, selection, tuple(path), speed_scale) + trajectory = parametrized.trajectory + self._validate_generated_trajectory( + trajectory, + selection.joint_names, + waypoints, + velocity_limits=parametrized.velocity_limits, + acceleration_limits=parametrized.acceleration_limits, + accelerations=parametrized.accelerations, + ) + else: + trajectory = self._timed_trajectory(selection, path, result.timestamps) + self._validate_generated_trajectory( + trajectory, + selection.joint_names, + waypoints, + ) + + return GeneratedPlan( + group_ids=selection.group_ids, + trajectory=trajectory, + path=path, + status=result.status, + planning_time=result.planning_time, + path_length=result.path_length, + iterations=result.iterations, + message=result.message, + ) + + @abstractmethod + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> ParametrizedTrajectory: + """Convert one validated untimed path using the selected backend.""" + + @staticmethod + def _validate_speed_scale(speed_scale: float) -> None: + if not math.isfinite(speed_scale) or speed_scale <= 0.0 or speed_scale > 1.0: + raise TrajectoryParametrizationError("speed_scale must be finite, > 0, and <= 1") + + @staticmethod + def _assert_finite_sequence(values: Sequence[float], label: str) -> None: + for value in values: + if not math.isfinite(value): + raise TrajectoryParametrizationError(f"{label} contains non-finite value") + + @classmethod + def _validate_selected_path( + cls, + path: Sequence[JointState], + expected_names: Sequence[str], + ) -> list[list[float]]: + if len(path) < 2: + raise TrajectoryParametrizationError("Planner returned fewer than two waypoints") + expected = list(expected_names) + waypoints: list[list[float]] = [] + for waypoint_index, state in enumerate(path): + if list(state.name) != expected: + raise TrajectoryParametrizationError( + f"Waypoint {waypoint_index} joint names do not match selected order" + ) + positions = list(state.position) + if len(positions) != len(expected): + raise TrajectoryParametrizationError( + f"Waypoint {waypoint_index} position dimension mismatch" + ) + cls._assert_finite_sequence( + positions, + f"Waypoint {waypoint_index} positions", + ) + waypoints.append(positions) + return waypoints + + @classmethod + def _timed_trajectory( + cls, + selection: PlanningGroupSelection, + path: Sequence[JointState], + timestamps: Sequence[float], + ) -> JointTrajectory: + if len(timestamps) != len(path): + raise TrajectoryParametrizationError("Planner must return one timestamp per waypoint") + points: list[TrajectoryPoint] = [] + for waypoint_index, (state, timestamp) in enumerate(zip(path, timestamps, strict=True)): + velocities = list(state.velocity) + if len(velocities) != len(selection.joint_names): + raise TrajectoryParametrizationError( + f"Waypoint {waypoint_index} velocity dimension mismatch" + ) + points.append( + TrajectoryPoint( + time_from_start=float(timestamp), + positions=list(state.position), + velocities=velocities, + ) + ) + return JointTrajectory( + joint_names=list(selection.joint_names), + points=points, + ) + + @classmethod + def _validate_generated_trajectory( + cls, + trajectory: JointTrajectory, + expected_names: Sequence[str], + waypoints: Sequence[Sequence[float]], + *, + velocity_limits: Sequence[float] | None = None, + acceleration_limits: Sequence[float] | None = None, + accelerations: Sequence[Sequence[float]] | None = None, + ) -> None: + expected = list(expected_names) + if list(trajectory.joint_names) != expected: + raise TrajectoryParametrizationError( + "Generated trajectory joint names do not match selected order" + ) + if not trajectory.points: + raise TrajectoryParametrizationError("Generated trajectory has no points") + previous_time: float | None = None + for point_index, point in enumerate(trajectory.points): + if len(point.positions) != len(expected) or len(point.velocities) != len(expected): + raise TrajectoryParametrizationError( + f"Generated point {point_index} dimension mismatch" + ) + cls._assert_finite_sequence( + point.positions, + f"Generated point {point_index} positions", + ) + cls._assert_finite_sequence( + point.velocities, + f"Generated point {point_index} velocities", + ) + if not math.isfinite(point.time_from_start): + raise TrajectoryParametrizationError( + f"Generated point {point_index} time is non-finite" + ) + if point_index == 0 and point.time_from_start != 0.0: + raise TrajectoryParametrizationError("Generated trajectory must start at time 0") + if previous_time is not None and point.time_from_start <= previous_time: + raise TrajectoryParametrizationError( + "Generated trajectory times must be strictly increasing" + ) + previous_time = point.time_from_start + non_noop = any(list(waypoint) != list(waypoints[0]) for waypoint in waypoints[1:]) + if non_noop and trajectory.duration <= 0.0: + raise TrajectoryParametrizationError("Generated trajectory duration must be positive") + if not cls._positions_close(trajectory.points[0].positions, waypoints[0]): + raise TrajectoryParametrizationError( + "Generated trajectory does not preserve the path start" + ) + if not cls._positions_close(trajectory.points[-1].positions, waypoints[-1]): + raise TrajectoryParametrizationError( + "Generated trajectory does not preserve the path goal" + ) + if velocity_limits is not None: + cls._validate_motion_limits( + trajectory, + velocity_limits, + acceleration_limits, + accelerations, + ) + + @staticmethod + def _positions_close(first: Sequence[float], second: Sequence[float]) -> bool: + return len(first) == len(second) and all( + math.isclose( + left, + right, + rel_tol=0.0, + abs_tol=_TRAJECTORY_POSITION_TOLERANCE, + ) + for left, right in zip(first, second, strict=True) + ) + + @classmethod + def _validate_motion_limits( + cls, + trajectory: JointTrajectory, + velocity_limits: Sequence[float], + acceleration_limits: Sequence[float] | None, + accelerations: Sequence[Sequence[float]] | None, + ) -> None: + expected_dimension = len(trajectory.joint_names) + if len(velocity_limits) != expected_dimension: + raise TrajectoryParametrizationError("Velocity limits do not match selected joints") + cls._assert_valid_motion_limits(velocity_limits, "velocity") + for point_index, point in enumerate(trajectory.points): + cls._assert_within_limits( + point.velocities, + velocity_limits, + f"Generated point {point_index} velocity", + ) + if acceleration_limits is None: + return + if len(acceleration_limits) != expected_dimension: + raise TrajectoryParametrizationError("Acceleration limits do not match selected joints") + cls._assert_valid_motion_limits(acceleration_limits, "acceleration") + if accelerations is not None: + if len(accelerations) != len(trajectory.points): + raise TrajectoryParametrizationError( + "Acceleration samples do not match trajectory points" + ) + for point_index, values in enumerate(accelerations): + if len(values) != expected_dimension: + raise TrajectoryParametrizationError( + f"Generated point {point_index} acceleration dimension mismatch" + ) + cls._assert_finite_sequence( + values, + f"Generated point {point_index} accelerations", + ) + cls._assert_within_limits( + values, + acceleration_limits, + f"Generated point {point_index} acceleration", + ) + return + for point_index in range(1, len(trajectory.points)): + previous = trajectory.points[point_index - 1] + current = trajectory.points[point_index] + dt = current.time_from_start - previous.time_from_start + derived = [ + (current_velocity - previous_velocity) / dt + for previous_velocity, current_velocity in zip( + previous.velocities, + current.velocities, + strict=True, + ) + ] + cls._assert_within_limits( + derived, + acceleration_limits, + f"Generated interval {point_index - 1}:{point_index} acceleration", + ) - @property - def uses_request_limits(self) -> bool: ... + @staticmethod + def _assert_valid_motion_limits(values: Sequence[float], label: str) -> None: + if any(not math.isfinite(value) or value <= 0.0 for value in values): + raise TrajectoryParametrizationError(f"Invalid {label} limits") - def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: ... + @staticmethod + def _assert_within_limits( + values: Sequence[float], + limits: Sequence[float], + label: str, + ) -> None: + for joint_index, (value, limit) in enumerate(zip(values, limits, strict=True)): + tolerance = max( + _TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE, + limit * _TRAJECTORY_LIMIT_RELATIVE_TOLERANCE, + ) + if abs(value) > limit + tolerance: + raise TrajectoryParametrizationError( + f"{label} exceeds joint {joint_index} limit: {value} vs {limit}" + ) diff --git a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py index ebb678781f..560d88731b 100644 --- a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py @@ -23,16 +23,19 @@ import roboplan.core as roboplan_core import roboplan.toppra as roboplan_toppra +from dimos.manipulation.planning.groups.models import PlanningGroupSelection +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.trajectory_generator.config import ( RoboPlanTOPPRAParametrizationConfig, ) from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + BaseTrajectoryParametrizer, ParametrizedTrajectory, TrajectoryParametrizationError, - TrajectoryParametrizationRequest, ) from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint @@ -45,32 +48,38 @@ class _GroupParametrizer: acceleration_limits: tuple[float, ...] -class RoboPlanTOPPRAParametrizer: +class RoboPlanTOPPRAParametrizer(BaseTrajectoryParametrizer): """Convert selected-joint paths with a finalized RoboPlan scene.""" def __init__( self, - world: RoboPlanWorld, config: RoboPlanTOPPRAParametrizationConfig, ) -> None: - self._world = world self._config = config self._groups: dict[frozenset[str], _GroupParametrizer] = {} - @property - def uses_request_limits(self) -> bool: - """RoboPlan uses only limits from its authoritative scene.""" - return False - - def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> ParametrizedTrajectory: + if not isinstance(world, RoboPlanWorld): + raise TrajectoryParametrizationError("RoboPlan TOPP-RA requires RoboPlanWorld") try: - with self._world.parametrization_model() as model: - resolved = self._resolve_group(model, request) - native_path = self._native_path(resolved.group, request) + with world.parametrization_model() as model: + resolved = self._resolve_group(model, selection) + native_path = self._native_path(resolved.group, selection, path) native_trajectory = resolved.native.generate( - native_path, self._options(request.speed_scale) + native_path, self._options(speed_scale) + ) + return self._canonical_result( + resolved, + selection, + speed_scale, + native_trajectory, ) - return self._canonical_result(resolved, request, native_trajectory) except TrajectoryParametrizationError: raise except (IndexError, KeyError, RuntimeError, TypeError, ValueError) as exc: @@ -81,18 +90,18 @@ def parametrize(self, request: TrajectoryParametrizationRequest) -> Parametrized def _resolve_group( self, model: RoboPlanModel, - request: TrajectoryParametrizationRequest, + selection: PlanningGroupSelection, ) -> _GroupParametrizer: - key = frozenset(request.group_ids) + key = frozenset(selection.group_ids) cached = self._groups.get(key) if cached is not None: return cached group = model.groups.get(key) if group is None: raise TrajectoryParametrizationError( - f"RoboPlan has no generated group for {list(request.group_ids)}" + f"RoboPlan has no generated group for {list(selection.group_ids)}" ) - expected = set(request.joint_names) + expected = set(selection.joint_names) if expected != set(group.public_names): raise TrajectoryParametrizationError( f"RoboPlan group '{group.name}' does not match selected joints" @@ -144,9 +153,10 @@ def _limits( @staticmethod def _native_path( group: RoboPlanGroup, - request: TrajectoryParametrizationRequest, + selection: PlanningGroupSelection, + path_states: tuple[JointState, ...], ) -> Any: - public_index = {name: index for index, name in enumerate(request.joint_names)} + public_index = {name: index for index, name in enumerate(selection.joint_names)} path = roboplan_core.JointPath() path.joint_names = list(group.native_names) path.positions = [ @@ -154,7 +164,7 @@ def _native_path( [state.position[public_index[public_name]] for public_name in group.public_names], dtype=np.float64, ) - for state in request.path + for state in path_states ] return path @@ -177,7 +187,8 @@ def _options(self, speed_scale: float) -> Any: @staticmethod def _canonical_result( resolved: _GroupParametrizer, - request: TrajectoryParametrizationRequest, + selection: PlanningGroupSelection, + speed_scale: float, native_trajectory: Any, ) -> ParametrizedTrajectory: native_names = tuple(native_trajectory.joint_names) @@ -191,7 +202,7 @@ def _canonical_result( strict=True, ) ) - output_indices = [native_index[native_by_public[name]] for name in request.joint_names] + output_indices = [native_index[native_by_public[name]] for name in selection.joint_names] times = [float(value) for value in native_trajectory.times] positions = list(native_trajectory.positions) velocities = list(native_trajectory.velocities) @@ -228,14 +239,14 @@ def _canonical_result( ) return ParametrizedTrajectory( trajectory=JointTrajectory( - joint_names=list(request.joint_names), + joint_names=list(selection.joint_names), points=points, ), velocity_limits=tuple( - velocity_by_public[name] * request.speed_scale for name in request.joint_names + velocity_by_public[name] * speed_scale for name in selection.joint_names ), acceleration_limits=tuple( - acceleration_by_public[name] * request.speed_scale for name in request.joint_names + acceleration_by_public[name] * speed_scale for name in selection.joint_names ), accelerations=canonical_accelerations, ) diff --git a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py index e938732548..d1ec1c9e85 100644 --- a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py @@ -14,6 +14,10 @@ """Compatibility trajectory parametrizer using segmented trapezoids.""" +import math + +from dimos.manipulation.planning.groups.models import PlanningGroupSelection +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.trajectory_generator.config import ( SimpleTrapezoidParametrizationConfig, ) @@ -21,51 +25,52 @@ JointTrajectoryGenerator, ) from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + BaseTrajectoryParametrizer, ParametrizedTrajectory, TrajectoryParametrizationError, - TrajectoryParametrizationRequest, ) +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -class SimpleTrapezoidParametrizer: +class SimpleTrapezoidParametrizer(BaseTrajectoryParametrizer): """Wrap the existing trajectory generator behind the adapter protocol.""" def __init__(self, config: SimpleTrapezoidParametrizationConfig) -> None: self._config = config - @property - def uses_request_limits(self) -> bool: - """The compatibility backend uses limits resolved from DimOS config.""" - return True - - def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: - if request.velocity_limits is None or request.acceleration_limits is None: - raise TrajectoryParametrizationError( - "Simple trapezoid parametrization requires DimOS motion limits" - ) + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> ParametrizedTrajectory: + request_velocity_limits, request_acceleration_limits = self._selected_limits( + world, + selection, + ) velocity_limits = tuple( - value * self._config.velocity_scale * request.speed_scale - for value in request.velocity_limits + value * self._config.velocity_scale * speed_scale for value in request_velocity_limits ) acceleration_limits = tuple( - value * self._config.acceleration_scale * request.speed_scale - for value in request.acceleration_limits + value * self._config.acceleration_scale * speed_scale + for value in request_acceleration_limits ) try: generator = JointTrajectoryGenerator( - num_joints=len(request.joint_names), + num_joints=len(selection.joint_names), max_velocity=list(velocity_limits), max_acceleration=list(acceleration_limits), points_per_segment=self._config.points_per_segment, ) - generated = generator.generate([list(state.position) for state in request.path]) + generated = generator.generate([list(state.position) for state in path]) except (IndexError, RuntimeError, TypeError, ValueError) as exc: raise TrajectoryParametrizationError( f"Simple trapezoid parametrization failed: {exc}" ) from exc trajectory = JointTrajectory( - joint_names=list(request.joint_names), + joint_names=list(selection.joint_names), points=generated.points, timestamp=generated.timestamp, ) @@ -74,3 +79,35 @@ def parametrize(self, request: TrajectoryParametrizationRequest) -> Parametrized velocity_limits=velocity_limits, acceleration_limits=acceleration_limits, ) + + @staticmethod + def _selected_limits( + world: WorldSpec, + selection: PlanningGroupSelection, + ) -> tuple[tuple[float, ...], tuple[float, ...]]: + configs = {} + for robot_id in world.get_robot_ids(): + config = world.get_robot_config(robot_id) + configs[config.name] = config + velocities: list[float] = [] + accelerations: list[float] = [] + for global_name in selection.joint_names: + if "/" not in global_name: + raise TrajectoryParametrizationError(f"Joint '{global_name}' is not globally named") + robot_name, local_name = global_name.split("/", 1) + selected_config = configs.get(robot_name) + if selected_config is None: + raise TrajectoryParametrizationError(f"Unknown robot for joint '{global_name}'") + if local_name not in selected_config.joint_names: + raise TrajectoryParametrizationError(f"Unknown local joint '{global_name}'") + velocity = float(selected_config.max_velocity) + acceleration = float(selected_config.max_acceleration) + if not math.isfinite(velocity) or velocity <= 0.0: + raise TrajectoryParametrizationError(f"Invalid velocity limit for '{global_name}'") + if not math.isfinite(acceleration) or acceleration <= 0.0: + raise TrajectoryParametrizationError( + f"Invalid acceleration limit for '{global_name}'" + ) + velocities.append(velocity) + accelerations.append(acceleration) + return tuple(velocities), tuple(accelerations) diff --git a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py new file mode 100644 index 0000000000..07e75d3655 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py @@ -0,0 +1,203 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Contract tests for PlanningResult-to-GeneratedPlan materialization.""" + +from unittest.mock import MagicMock + +import pytest + +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupSelection, +) +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + BaseTrajectoryParametrizer, + ParametrizedTrajectory, + TrajectoryParametrizationError, +) +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint + + +class _FixedParametrizer(BaseTrajectoryParametrizer): + def __init__(self, output: ParametrizedTrajectory) -> None: + self.output = output + self.calls: list[float] = [] + + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> ParametrizedTrajectory: + self.calls.append(speed_scale) + return self.output + + +def _selection() -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups( + ( + PlanningGroup( + id="arm/group", + robot_name="arm", + group_name="group", + joint_names=("arm/a", "arm/b"), + local_joint_names=("a", "b"), + base_link="base", + ), + ) + ) + + +def _path() -> list[JointState]: + names = ["arm/a", "arm/b"] + return [ + JointState(name=names, position=[0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1]), + JointState(name=names, position=[0.4, 0.0]), + ] + + +def _output( + *, + velocities: tuple[list[float], list[float]] = ([0.0, 0.0], [0.0, 0.0]), + accelerations: tuple[tuple[float, float], tuple[float, float]] = ( + (0.0, 0.0), + (0.0, 0.0), + ), +) -> ParametrizedTrajectory: + return ParametrizedTrajectory( + trajectory=JointTrajectory( + joint_names=["arm/a", "arm/b"], + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0, 0.0], + velocities=velocities[0], + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[0.4, 0.0], + velocities=velocities[1], + ), + ], + ), + velocity_limits=(1.0, 1.0), + acceleration_limits=(2.0, 2.0), + accelerations=accelerations, + ) + + +def test_materializes_bounded_fitting_and_preserves_source_path() -> None: + parametrizer = _FixedParametrizer(_output()) + source_path = _path() + + plan = parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult( + status=PlanningStatus.SUCCESS, + path=source_path, + planning_time=0.2, + iterations=12, + ), + speed_scale=0.4, + ) + + assert [state.position for state in plan.path] == [ + [0.0, 0.0], + [0.2, 0.1], + [0.4, 0.0], + ] + assert plan.path is not source_path + assert plan.trajectory is parametrizer.output.trajectory + assert plan.planning_time == 0.2 + assert plan.iterations == 12 + assert parametrizer.calls == [0.4] + + +def test_timed_planner_result_bypasses_backend_path_conversion() -> None: + parametrizer = _FixedParametrizer(_output()) + path = [ + JointState( + name=["arm/a", "arm/b"], + position=[0.0, 0.0], + velocity=[0.0, 0.0], + ), + JointState( + name=["arm/a", "arm/b"], + position=[0.4, 0.0], + velocity=[0.3, 0.0], + ), + ] + + plan = parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + timestamps=[0.0, 0.75], + ), + ) + + assert parametrizer.calls == [] + assert [point.time_from_start for point in plan.trajectory.points] == [ + 0.0, + 0.75, + ] + assert plan.trajectory.points[-1].velocities == [0.3, 0.0] + + +@pytest.mark.parametrize( + ("velocities", "accelerations", "message"), + [ + (([0.0, 0.0], [1.1, 0.0]), ((0.0, 0.0), (0.0, 0.0)), "velocity exceeds"), + (([0.0, 0.0], [0.0, 0.0]), ((0.0, 0.0), (2.1, 0.0)), "acceleration exceeds"), + ], +) +def test_rejects_parametrized_motion_limit_violations( + velocities: tuple[list[float], list[float]], + accelerations: tuple[tuple[float, float], tuple[float, float]], + message: str, +) -> None: + parametrizer = _FixedParametrizer(_output(velocities=velocities, accelerations=accelerations)) + + with pytest.raises(TrajectoryParametrizationError, match=message): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult(status=PlanningStatus.SUCCESS, path=_path()), + ) + + +def test_rejects_malformed_path_before_invoking_backend() -> None: + parametrizer = _FixedParametrizer(_output()) + path = _path() + path[1].name = ["wrong/a", "wrong/b"] + + with pytest.raises(TrajectoryParametrizationError, match="joint names"): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult(status=PlanningStatus.SUCCESS, path=path), + ) + + assert parametrizer.calls == [] diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py index c6f476fcef..4e9adfaf9b 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -23,17 +23,24 @@ pytest.importorskip("roboplan.toppra") +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupSelection, +) +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.spec.protocols import TrajectoryParametrizerSpec from dimos.manipulation.planning.trajectory_generator.config import ( RoboPlanTOPPRAParametrizationConfig, ) from dimos.manipulation.planning.trajectory_generator.parametrizer import ( TrajectoryParametrizationError, - TrajectoryParametrizationRequest, ) from dimos.manipulation.planning.trajectory_generator.roboplan_toppra_parametrizer import ( RoboPlanTOPPRAParametrizer, ) from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel +from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld from dimos.msgs.sensor_msgs.JointState import JointState pytestmark = pytest.mark.self_hosted @@ -53,7 +60,7 @@ def getAccelerationLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.n return np.asarray([-6.0, -maximum]), np.asarray([6.0, maximum]) -class _World: +class _World(RoboPlanWorld): def __init__(self, model: RoboPlanModel) -> None: self.model = model @@ -79,19 +86,35 @@ def _model(*, missing_acceleration: bool = False) -> RoboPlanModel: ) -def _request( +def _selection_and_result( names: tuple[str, str] = ("left/a", "right/b"), - *, - speed_scale: float = 1.0, -) -> TrajectoryParametrizationRequest: +) -> tuple[PlanningGroupSelection, PlanningResult]: positions_by_name = { "left/a": (0.0, 0.3), "right/b": (0.1, 0.4), } - return TrajectoryParametrizationRequest( - group_ids=("right/arm", "left/arm"), - joint_names=names, - path=( + groups_by_name = { + "left/a": PlanningGroup( + id="left/arm", + robot_name="left", + group_name="arm", + joint_names=("left/a",), + local_joint_names=("a",), + base_link="base", + ), + "right/b": PlanningGroup( + id="right/arm", + robot_name="right", + group_name="arm", + joint_names=("right/b",), + local_joint_names=("b",), + base_link="base", + ), + } + selection = PlanningGroupSelection.from_groups(tuple(groups_by_name[name] for name in names)) + result = PlanningResult( + status=PlanningStatus.SUCCESS, + path=[ JointState( name=list(names), position=[positions_by_name[name][0] for name in names], @@ -100,11 +123,9 @@ def _request( name=list(names), position=[positions_by_name[name][1] for name in names], ), - ), - velocity_limits=(999.0, 999.0), - acceleration_limits=(999.0, 999.0), - speed_scale=speed_scale, + ], ) + return selection, result @pytest.mark.parametrize( @@ -119,8 +140,8 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( joint_names=["native_a", "native_b"], times=[0.0, 0.5], positions=[np.asarray([0.0, 0.1]), np.asarray([0.3, 0.4])], - velocities=[np.asarray([0.0, 0.0]), np.asarray([0.6, 0.2])], - accelerations=[np.asarray([0.0, 0.0]), np.asarray([1.2, 0.4])], + velocities=[np.asarray([0.0, 0.0]), np.asarray([0.2, 0.2])], + accelerations=[np.asarray([0.0, 0.0]), np.asarray([0.4, 0.4])], ) native = mocker.MagicMock() native.generate.return_value = generated @@ -130,7 +151,6 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( return_value=native, ) parametrizer = RoboPlanTOPPRAParametrizer( - _World(_model()), RoboPlanTOPPRAParametrizationConfig( fitting_mode=fitting_mode, output_period=0.02, @@ -138,11 +158,17 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( acceleration_scale=0.25, ), ) - request = _request(speed_scale=0.5) + world = _World(_model()) + selection, planning_result = _selection_and_result() - result = parametrizer.parametrize(request) + result = parametrizer.materialize_plan( + world, + selection, + planning_result, + speed_scale=0.5, + ) - assert not parametrizer.uses_request_limits + assert isinstance(parametrizer, TrajectoryParametrizerSpec) constructor.assert_called_once() native_path, options = native.generate.call_args.args assert native_path.joint_names == ["native_b", "native_a"] @@ -154,8 +180,6 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( assert options.mode.name.lower().replace("linearblend", "linear_blend") == fitting_mode assert options.velocity_scale == 0.25 assert options.acceleration_scale == 0.125 - assert result.velocity_limits == (0.25, 0.5) - assert result.acceleration_limits == (0.5, 0.75) assert result.trajectory.joint_names == ["left/a", "right/b"] assert [point.positions for point in result.trajectory.points] == [ [0.0, 0.1], @@ -163,10 +187,12 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( ] assert [point.velocities for point in result.trajectory.points] == [ [0.0, 0.0], - [0.6, 0.2], + [0.2, 0.2], + ] + assert [state.position for state in planning_result.path] == [ + [0.0, 0.1], + [0.3, 0.4], ] - assert result.accelerations == ((0.0, 0.0), (1.2, 0.4)) - assert [state.position for state in request.path] == [[0.0, 0.1], [0.3, 0.4]] def test_roboplan_parametrizer_rejects_missing_urdf_acceleration_without_fallback( @@ -177,15 +203,19 @@ def test_roboplan_parametrizer_rejects_missing_urdf_acceleration_without_fallbac "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA" ) parametrizer = RoboPlanTOPPRAParametrizer( - _World(_model(missing_acceleration=True)), RoboPlanTOPPRAParametrizationConfig(), ) + selection, result = _selection_and_result() with pytest.raises( TrajectoryParametrizationError, match="no usable URDF acceleration limit for joint 'left/a'", ): - parametrizer.parametrize(_request()) + parametrizer.materialize_plan( + _World(_model(missing_acceleration=True)), + selection, + result, + ) constructor.assert_not_called() @@ -196,9 +226,9 @@ def test_cached_group_limits_follow_each_request_joint_order( generated = SimpleNamespace( joint_names=["native_b", "native_a"], times=[0.0, 0.5], - positions=[np.asarray([0.0, 0.1]), np.asarray([0.3, 0.4])], - velocities=[np.asarray([0.0, 0.0]), np.asarray([0.6, 0.2])], - accelerations=[np.asarray([0.0, 0.0]), np.asarray([1.2, 0.4])], + positions=[np.asarray([0.1, 0.0]), np.asarray([0.4, 0.3])], + velocities=[np.asarray([0.0, 0.0]), np.asarray([0.2, 0.4])], + accelerations=[np.asarray([0.0, 0.0]), np.asarray([0.4, 0.8])], ) native = mocker.MagicMock() native.generate.return_value = generated @@ -208,18 +238,26 @@ def test_cached_group_limits_follow_each_request_joint_order( return_value=native, ) parametrizer = RoboPlanTOPPRAParametrizer( - _World(_model()), RoboPlanTOPPRAParametrizationConfig( velocity_scale=0.5, acceleration_scale=0.25, ), ) + world = _World(_model()) + canonical_selection, canonical_result = _selection_and_result() + reversed_selection, reversed_result = _selection_and_result(("right/b", "left/a")) - canonical = parametrizer.parametrize(_request()) - reversed_order = parametrizer.parametrize(_request(("right/b", "left/a"))) + canonical = parametrizer.materialize_plan( + world, + canonical_selection, + canonical_result, + ) + reversed_order = parametrizer.materialize_plan( + world, + reversed_selection, + reversed_result, + ) constructor.assert_called_once() - assert canonical.velocity_limits == (0.5, 1.0) - assert canonical.acceleration_limits == (1.0, 1.5) - assert reversed_order.velocity_limits == (1.0, 0.5) - assert reversed_order.acceleration_limits == (1.5, 1.0) + assert canonical.trajectory.joint_names == ["left/a", "right/b"] + assert reversed_order.trajectory.joint_names == ["right/b", "left/a"] diff --git a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py index e8a623c244..2e509552ba 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py @@ -12,41 +12,82 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Tests for the compatibility trajectory parametrizer.""" +"""Tests for the compatibility trajectory parametrizer Spec implementation.""" + +from pathlib import Path +from unittest.mock import MagicMock import pytest +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupSelection, +) +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.spec.protocols import ( + TrajectoryParametrizerSpec, + WorldSpec, +) from dimos.manipulation.planning.trajectory_generator.config import ( SimpleTrapezoidParametrizationConfig, ) from dimos.manipulation.planning.trajectory_generator.parametrizer import ( TrajectoryParametrizationError, - TrajectoryParametrizationRequest, ) from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( SimpleTrapezoidParametrizer, ) +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState -def _request(*, speed_scale: float = 1.0) -> TrajectoryParametrizationRequest: - names = ("arm/a", "arm/b") - return TrajectoryParametrizationRequest( - group_ids=("arm/manipulator",), - joint_names=names, - path=( - JointState(name=list(names), position=[0.0, 0.0]), - JointState(name=list(names), position=[0.2, 0.1]), - JointState(name=list(names), position=[0.4, 0.0]), - ), - velocity_limits=(2.0, 4.0), - acceleration_limits=(6.0, 8.0), - speed_scale=speed_scale, +def _selection() -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups( + ( + PlanningGroup( + id="arm/manipulator", + robot_name="arm", + group_name="manipulator", + joint_names=("arm/a", "arm/b"), + local_joint_names=("a", "b"), + base_link="base", + tip_link="tip", + ), + ) + ) + + +def _world(*, velocity: float = 2.0, acceleration: float = 6.0) -> WorldSpec: + config = RobotModelConfig( + name="arm", + model_path=Path("/robot.urdf"), + base_pose=PoseStamped(), + joint_names=["a", "b"], + base_link="base", + max_velocity=velocity, + max_acceleration=acceleration, + ) + world = MagicMock(spec=WorldSpec) + world.get_robot_ids.return_value = ["arm-id"] + world.get_robot_config.return_value = config + return world + + +def _result() -> PlanningResult: + names = ["arm/a", "arm/b"] + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=[ + JointState(name=names, position=[0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1]), + JointState(name=names, position=[0.4, 0.0]), + ], ) -def test_simple_parametrizer_preserves_segmented_trapezoid_behavior() -> None: - request = _request(speed_scale=0.5) +def test_simple_parametrizer_materializes_segmented_trapezoid_plan() -> None: parametrizer = SimpleTrapezoidParametrizer( SimpleTrapezoidParametrizationConfig( velocity_scale=0.5, @@ -54,41 +95,52 @@ def test_simple_parametrizer_preserves_segmented_trapezoid_behavior() -> None: points_per_segment=4, ) ) + result = _result() + + plan = parametrizer.materialize_plan( + _world(), + _selection(), + result, + speed_scale=0.5, + ) - result = parametrizer.parametrize(request) - - assert parametrizer.uses_request_limits - assert result.velocity_limits == (0.5, 1.0) - assert result.acceleration_limits == (0.75, 1.0) - assert result.accelerations is None - assert result.trajectory.joint_names == list(request.joint_names) - assert len(result.trajectory.points) == 9 - assert result.trajectory.points[0].positions == [0.0, 0.0] - assert result.trajectory.points[4].positions == [0.2, 0.1] - assert result.trajectory.points[-1].positions == [0.4, 0.0] - assert [state.position for state in request.path] == [ + assert isinstance(parametrizer, TrajectoryParametrizerSpec) + assert plan.group_ids == ("arm/manipulator",) + assert plan.trajectory.joint_names == ["arm/a", "arm/b"] + assert len(plan.trajectory.points) == 9 + assert plan.trajectory.points[0].positions == [0.0, 0.0] + assert plan.trajectory.points[4].positions == [0.2, 0.1] + assert plan.trajectory.points[-1].positions == [0.4, 0.0] + assert [state.position for state in result.path] == [ [0.0, 0.0], [0.2, 0.1], [0.4, 0.0], ] -def test_simple_parametrizer_requires_dimos_limits() -> None: - request = _request() - request = TrajectoryParametrizationRequest( - group_ids=request.group_ids, - joint_names=request.joint_names, - path=request.path, - ) +def test_simple_parametrizer_rejects_invalid_dimos_limits() -> None: + parametrizer = SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()) with pytest.raises( TrajectoryParametrizationError, - match="requires DimOS motion limits", + match="Invalid velocity limit for 'arm/a'", ): - SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()).parametrize(request) + parametrizer.materialize_plan( + _world(velocity=0.0), + _selection(), + _result(), + ) -@pytest.mark.parametrize("speed_scale", [0.0, -0.1, 1.01, float("inf"), float("nan")]) -def test_parametrization_request_rejects_invalid_runtime_speed(speed_scale: float) -> None: - with pytest.raises(ValueError, match="speed_scale"): - _request(speed_scale=speed_scale) +@pytest.mark.parametrize( + "speed_scale", + [0.0, -0.1, 1.01, float("inf"), float("nan")], +) +def test_parametrizer_rejects_invalid_runtime_speed(speed_scale: float) -> None: + with pytest.raises(TrajectoryParametrizationError, match="speed_scale"): + SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()).materialize_plan( + _world(), + _selection(), + _result(), + speed_scale=speed_scale, + ) diff --git a/dimos/manipulation/test_generated_plan_materialization.py b/dimos/manipulation/test_generated_plan_materialization.py index acbc7d7810..89a9ee5e97 100644 --- a/dimos/manipulation/test_generated_plan_materialization.py +++ b/dimos/manipulation/test_generated_plan_materialization.py @@ -29,10 +29,6 @@ from dimos.manipulation.planning.trajectory_generator.config import ( SimpleTrapezoidParametrizationConfig, ) -from dimos.manipulation.planning.trajectory_generator.parametrizer import ( - ParametrizedTrajectory, - TrajectoryParametrizationRequest, -) from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( SimpleTrapezoidParametrizer, ) @@ -77,18 +73,6 @@ def generate(self, waypoints: list[list[float]]) -> JointTrajectory: ) -class FixedParametrizer: - uses_request_limits = False - - def __init__(self, result: ParametrizedTrajectory) -> None: - self.result = result - self.requests: list[TrajectoryParametrizationRequest] = [] - - def parametrize(self, request: TrajectoryParametrizationRequest) -> ParametrizedTrajectory: - self.requests.append(request) - return self.result - - def _robot(name: str, joints: list[str], velocity: float, acceleration: float) -> RobotModelConfig: return RobotModelConfig( name=name, @@ -119,11 +103,16 @@ def _module(monkeypatch: pytest.MonkeyPatch, module_factory): right = _robot("right", ["c"], 3.0, 4.0) module = module_factory() module._robots = { - "left": ("left_id", left, MagicMock()), - "right": ("right_id", right, MagicMock()), + "left": ("left_id", left), + "right": ("right_id", right), } module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() + module._world_monitor.world.get_robot_ids.return_value = ["left_id", "right_id"] + module._world_monitor.world.get_robot_config.side_effect = { + "left_id": left, + "right_id": right, + }.__getitem__ module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) module._planner = MagicMock() module._trajectory_parametrizer = SimpleTrapezoidParametrizer( @@ -201,7 +190,6 @@ def test_cartesian_plan_preserves_planner_timestamps_and_velocities(monkeypatch, @pytest.mark.parametrize( ("timestamps", "velocities", "message"), [ - (None, [[0.0, 0.0], [0.1, 0.1]], "one timestamp"), ([0.0, 0.0], [[0.0, 0.0], [0.1, 0.1]], "strictly increasing"), ([0.0, 0.1], [[], [0.1, 0.1]], "velocity dimension"), ], @@ -293,115 +281,3 @@ def test_zero_generation_after_caching_for_status_and_completion(monkeypatch, mo module._wait_for_trajectory_completion(timeout=0.0) assert RecordingGenerator.calls == [] - - -def test_materialization_accepts_bounded_fitting_without_interior_waypoint( - monkeypatch, - module_factory, -): - module = _module(monkeypatch, module_factory) - names = ["left/b", "left/a"] - source_path = [ - JointState(name=names, position=[0.0, 0.0]), - JointState(name=names, position=[0.2, 0.1]), - JointState(name=names, position=[0.4, 0.0]), - ] - trajectory = JointTrajectory( - joint_names=names, - points=[ - TrajectoryPoint( - time_from_start=0.0, - positions=[0.0, 0.0], - velocities=[0.0, 0.0], - ), - TrajectoryPoint( - time_from_start=0.5, - positions=[0.4, 0.0], - velocities=[0.0, 0.0], - ), - ], - ) - parametrizer = FixedParametrizer( - ParametrizedTrajectory( - trajectory=trajectory, - velocity_limits=(1.0, 1.0), - acceleration_limits=(2.0, 2.0), - accelerations=((0.0, 0.0), (0.0, 0.0)), - ) - ) - module._trajectory_parametrizer = parametrizer - assert module.set_motion_speed(0.4) - - path, result = module._materialize_generated_plan(("left/group",), source_path) - - assert [state.position for state in path] == [ - [0.0, 0.0], - [0.2, 0.1], - [0.4, 0.0], - ] - assert result is trajectory - assert len(parametrizer.requests) == 1 - assert parametrizer.requests[0].speed_scale == pytest.approx(0.4) - - -@pytest.mark.parametrize( - ("velocities", "accelerations", "message"), - [ - ([[0.0, 0.0], [1.1, 0.0]], ((0.0, 0.0), (0.0, 0.0)), "velocity exceeds"), - ([[0.0, 0.0], [0.0, 0.0]], ((0.0, 0.0), (2.1, 0.0)), "acceleration exceeds"), - ], -) -def test_materialization_rejects_parametrized_motion_limit_violations( - monkeypatch, - module_factory, - velocities, - accelerations, - message, -): - module = _module(monkeypatch, module_factory) - names = ["left/b", "left/a"] - path = _path(names, [0.0, 0.0], [0.4, 0.0]) - module._trajectory_parametrizer = FixedParametrizer( - ParametrizedTrajectory( - trajectory=JointTrajectory( - joint_names=names, - points=[ - TrajectoryPoint( - time_from_start=0.0, - positions=[0.0, 0.0], - velocities=velocities[0], - ), - TrajectoryPoint( - time_from_start=0.5, - positions=[0.4, 0.0], - velocities=velocities[1], - ), - ], - ), - velocity_limits=(1.0, 1.0), - acceleration_limits=(2.0, 2.0), - accelerations=accelerations, - ) - ) - - with pytest.raises(ValueError, match=message): - module._materialize_generated_plan(("left/group",), path) - - -def test_materialization_validates_real_simple_backend( - monkeypatch, - module_factory, -): - module = _module(monkeypatch, module_factory) - module._trajectory_parametrizer = SimpleTrapezoidParametrizer( - SimpleTrapezoidParametrizationConfig() - ) - names = ["left/b", "left/a"] - - path, trajectory = module._materialize_generated_plan( - ("left/group",), - _path(names, [0.0, 0.0], [0.2, 0.1]), - ) - - assert path[-1].position == [0.2, 0.1] - assert trajectory.points[-1].positions == [0.2, 0.1] diff --git a/dimos/manipulation/test_manipulation_monitor_preview.py b/dimos/manipulation/test_manipulation_monitor_preview.py index 76ad85f0d0..ae69ddb25f 100644 --- a/dimos/manipulation/test_manipulation_monitor_preview.py +++ b/dimos/manipulation/test_manipulation_monitor_preview.py @@ -80,12 +80,11 @@ def _one_joint_config(name: str = "arm") -> RobotModelConfig: def _install_generated_plan( module: ManipulationModule, config: RobotModelConfig, - traj_gen: MagicMock, *points: list[float], ) -> None: """Install a generated plan and enough monitor state to derive robot paths.""" global_joint_names = [f"{config.name}/{joint}" for joint in config.joint_names] - module._robots = {config.name: ("robot_id", config, traj_gen)} + module._robots = {config.name: ("robot_id", config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([config]) module._world_monitor.get_current_joint_state.return_value = JointState( @@ -126,7 +125,7 @@ def _make_module_with_monitor( module._init_joints = {} for config in configs: robot_id = f"robot_{config.name}" - module._robots[config.name] = (robot_id, config, MagicMock()) + module._robots[config.name] = (robot_id, config) return module @@ -300,7 +299,7 @@ def test_multi_robot_splits_correctly(self, module_factory): def test_no_monitor_returns_early(self, robot_config_with_mapping, module_factory): """When world_monitor is None, _on_joint_state returns without error.""" module = module_factory() - module._robots = {"left_arm": ("id", robot_config_with_mapping, MagicMock())} + module._robots = {"left_arm": ("id", robot_config_with_mapping)} module._world_monitor = None # Should not raise @@ -397,8 +396,7 @@ def test_dismiss_preview_routes_to_monitor(self, module_factory): def test_preview_routes_one_complete_plan_with_default_duration(self, module_factory): module = module_factory() config = _one_joint_config() - traj_gen = MagicMock() - _install_generated_plan(module, config, traj_gen, [0.0], [2.0]) + _install_generated_plan(module, config, [0.0], [2.0]) assert module.preview_plan() is True @@ -410,10 +408,9 @@ def test_preview_robot_name_validates_affectedness_without_trimming(self, module module = module_factory() left = _one_joint_config("left") right = _one_joint_config("right") - traj_gen = MagicMock() module._robots = { - "left": ("left_id", left, traj_gen), - "right": ("right_id", right, traj_gen), + "left": ("left_id", left), + "right": ("right_id", right), } module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) @@ -436,8 +433,7 @@ def test_preview_robot_name_validates_affectedness_without_trimming(self, module def test_preview_rejects_unaffected_compatibility_robot(self, module_factory): module = module_factory() config = _one_joint_config() - traj_gen = MagicMock() - _install_generated_plan(module, config, traj_gen, [0.0], [1.0]) + _install_generated_plan(module, config, [0.0], [1.0]) assert module.preview_plan(robot_name="other") is False module._world_monitor.animate_trajectory.assert_not_called() diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 8ac2662b92..5e29d15410 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -136,12 +136,11 @@ def _one_joint_config(name: str = "arm") -> RobotModelConfig: def _install_generated_plan( module: ManipulationModule, config: RobotModelConfig, - traj_gen: MagicMock, *points: list[float], ) -> None: """Install a generated plan and enough monitor state to derive robot paths.""" global_joint_names = [f"{config.name}/{joint}" for joint in config.joint_names] - module._robots = {config.name: ("robot_id", config, traj_gen)} + module._robots = {config.name: ("robot_id", config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([config]) module._world_monitor.get_current_joint_state.return_value = JointState( @@ -322,7 +321,7 @@ def test_cancel_hides_active_plan_preview(self, module_factory): def test_cancel_completed_execution_cancels_coordinator_task(self, module_factory): module = module_factory() config = _one_joint_config() - _install_generated_plan(module, config, MagicMock(), [0.0], [0.1]) + _install_generated_plan(module, config, [0.0], [0.1]) coordinator = _control_coordinator(cancel_status=TrajectoryCancellationStatus.CANCELLED) module._control_coordinator = coordinator module._initialize_execution() @@ -383,7 +382,7 @@ def test_begin_planning_state_checks(self, robot_config, module_factory): """_begin_planning only allowed from IDLE or COMPLETED.""" module = module_factory() module._world_monitor = MagicMock() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} # From IDLE - OK module._state = ManipulationState.IDLE @@ -405,7 +404,7 @@ class TestRobotSelection: def test_single_robot_default(self, robot_config, module_factory): """Single robot is used by default.""" module = module_factory() - module._robots = {"arm": ("id", robot_config, MagicMock())} + module._robots = {"arm": ("id", robot_config)} result = module._get_robot() assert result is not None @@ -415,8 +414,8 @@ def test_multiple_robots_require_name(self, robot_config, module_factory): """Multiple robots require explicit name.""" module = module_factory() module._robots = { - "left": ("id1", robot_config, MagicMock()), - "right": ("id2", robot_config, MagicMock()), + "left": ("id1", robot_config), + "right": ("id2", robot_config), } # No name - fails @@ -437,6 +436,7 @@ def __init__(self, mocker: MockerFixture) -> None: world_monitor=self.mock_world_monitor, planner=MagicMock(), kinematics=MagicMock(), + trajectory_parametrizer=MagicMock(), ) self.mock_planning_specs = mocker.patch( "dimos.manipulation.manipulation_module.create_planning_specs", @@ -447,7 +447,6 @@ def __init__(self, mocker: MockerFixture) -> None: return_value=self.mock_world, ) mocker.patch("dimos.manipulation.manipulation_module.create_manipulation_visualization") - mocker.patch("dimos.manipulation.manipulation_module.JointTrajectoryGenerator") @pytest.fixture @@ -545,7 +544,7 @@ def test_nested_kinematics_config_parses_cli_override_shape(self) -> None: def test_solve_ik_rpc_calls_configured_backend(self, robot_config, module_factory): """solve_ik returns the backend IKResult without path planning.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) @@ -586,7 +585,7 @@ def test_solve_ik_rpc_calls_configured_backend(self, robot_config, module_factor def test_solve_ik_rpc_returns_failure_without_joint_state(self, robot_config, module_factory): """solve_ik reports a failed IKResult when no seed state is available.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) module._world_monitor.current_global_joint_state.return_value = JointState( @@ -607,7 +606,7 @@ def test_solve_ik_rpc_accepts_explicit_seed_without_current_state( ): """solve_ik succeeds with an explicit seed when no current state is available.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) @@ -632,7 +631,7 @@ class TestPlanningGroupApis: def test_list_planning_groups_and_robot_info_include_groups(self, robot_config, module_factory): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = registry module._init_joints = {} @@ -651,14 +650,12 @@ def test_plan_to_joint_targets_stores_generated_plan_and_legacy_caches( ): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - traj_gen = MagicMock() - traj_gen.generate.return_value = _make_trajectory( - (0.0, [0.0, 0.0, 0.0]), (1.0, [0.1, 0.2, 0.3]) - ) - module._robots = {"test_arm": ("robot_id", robot_config, traj_gen)} + module._robots = {"test_arm": ("robot_id", robot_config)} _enable_simple_parametrization(module) module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() + module._world_monitor.world.get_robot_ids.return_value = ["robot_id"] + module._world_monitor.world.get_robot_config.return_value = robot_config module._world_monitor.planning_groups = registry module._world_monitor.current_global_joint_state.return_value = JointState( name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], @@ -736,14 +733,12 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( ): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - traj_gen = MagicMock() - traj_gen.generate.return_value = _make_trajectory( - (0.0, [0.0, 0.0, 0.0]), (1.0, [0.1, 0.2, 0.3]) - ) - module._robots = {"test_arm": ("robot_id", robot_config, traj_gen)} + module._robots = {"test_arm": ("robot_id", robot_config)} _enable_simple_parametrization(module) module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() + module._world_monitor.world.get_robot_ids.return_value = ["robot_id"] + module._world_monitor.world.get_robot_config.return_value = robot_config module._world_monitor.planning_groups = registry module._world_monitor.current_global_joint_state.return_value = JointState( name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], @@ -799,7 +794,7 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( def test_failed_plan_materialization_clears_generated_plan(self, robot_config, module_factory): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = registry @@ -875,8 +870,8 @@ def test_execute_plan_dispatches_selected_subsets_once_with_shared_clock_and_map ) module = module_factory() module._robots = { - "left": ("left_id", left, MagicMock()), - "right": ("right_id", right, MagicMock()), + "left": ("left_id", left), + "right": ("right_id", right), } module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) @@ -941,7 +936,7 @@ def test_pose_wrappers_fail_safely_without_unique_pose_group( ], ) module = module_factory() - module._robots = {"test_arm": ("robot_id", no_pose_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", no_pose_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([no_pose_config]) module._world_monitor.get_ee_pose.side_effect = ValueError("no pose group") @@ -980,7 +975,7 @@ def test_pose_wrappers_fail_safely_with_multiple_pose_groups( ], ) module = module_factory() - module._robots = {"test_arm": ("robot_id", multi_pose_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", multi_pose_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([multi_pose_config]) module._world_monitor.get_ee_pose.side_effect = ValueError("multiple pose groups") @@ -997,7 +992,7 @@ def test_pose_wrappers_fail_safely_with_multiple_pose_groups( def test_solve_ik_preserves_backend_failure_detail(self, robot_config, module_factory): """IK diagnostics include the backend's human-readable failure message.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) @@ -1032,7 +1027,7 @@ def test_planner_failure_preserves_backend_detail(self, robot_config, module_fac status=PlanningStatus.TIMEOUT, message="planner timed out" ) - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} assert not module.plan_to_joints( JointState(position=[1.0, 1.0, 1.0]), robot_name="test_arm" ) @@ -1047,7 +1042,7 @@ class TestExecute: def test_execute_requires_trajectory(self, robot_config, module_factory): """Execute fails without planned trajectory.""" module = module_factory() - module._robots = {"test_arm": ("id", robot_config, MagicMock())} + module._robots = {"test_arm": ("id", robot_config)} assert module.execute() is False assert module._state == ManipulationState.IDLE diff --git a/dimos/manipulation/test_plan_execution.py b/dimos/manipulation/test_plan_execution.py index 62fbe9ed78..cc8de4296b 100644 --- a/dimos/manipulation/test_plan_execution.py +++ b/dimos/manipulation/test_plan_execution.py @@ -81,7 +81,7 @@ def _module_with_coordinator( ) ], ) - module._robots = {"arm": ("arm_id", config, MagicMock())} + module._robots = {"arm": ("arm_id", config)} module._initialize_execution() return module diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index 80b82b7416..dd98457c22 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -46,7 +46,10 @@ ) from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.protocols import PlannerSpec +from dimos.manipulation.planning.spec.protocols import ( + PlannerSpec, + TrajectoryParametrizerSpec, +) from dimos.manipulation.planning.trajectory_generator.config import ( RoboPlanTOPPRAParametrizationConfig, SimpleTrapezoidParametrizationConfig, @@ -144,28 +147,23 @@ def test_validate_backend_combination_rejects_invalid_combinations() -> None: ) -def test_create_trajectory_parametrizer_selects_simple_backend( - mocker: MockerFixture, -) -> None: +def test_create_trajectory_parametrizer_selects_simple_backend() -> None: result = create_trajectory_parametrizer( SimpleTrapezoidParametrizationConfig(), - world=mocker.MagicMock(), world_backend="drake", ) assert isinstance(result, SimpleTrapezoidParametrizer) + assert isinstance(result, TrajectoryParametrizerSpec) -def test_create_trajectory_parametrizer_rejects_toppra_with_non_roboplan_world( - mocker: MockerFixture, -) -> None: +def test_create_trajectory_parametrizer_rejects_toppra_with_non_roboplan_world() -> None: with pytest.raises( ValueError, match='trajectory_parametrization.backend="roboplan_toppra" requires', ): create_trajectory_parametrizer( RoboPlanTOPPRAParametrizationConfig(), - world=mocker.MagicMock(), world_backend="drake", ) @@ -274,6 +272,7 @@ def test_start_uses_configured_planner_and_kinematics( world_monitor=world_monitor, planner=planner, kinematics=kinematics, + trajectory_parametrizer=mocker.MagicMock(name="trajectory_parametrizer"), ) create_world_mock = mocker.patch( "dimos.manipulation.manipulation_module.create_world", return_value=world @@ -282,12 +281,6 @@ def test_start_uses_configured_planner_and_kinematics( "dimos.manipulation.manipulation_module.create_planning_specs", return_value=planning_specs, ) - parametrizer = mocker.MagicMock(name="trajectory_parametrizer") - create_parametrizer_mock = mocker.patch( - "dimos.manipulation.manipulation_module.create_trajectory_parametrizer", - return_value=parametrizer, - ) - module._initialize_planning() create_world_mock.assert_called_once_with( @@ -301,12 +294,7 @@ def test_start_uses_configured_planner_and_kinematics( kinematics=module.config.kinematics, trajectory_parametrization=module.config.trajectory_parametrization, ) - create_parametrizer_mock.assert_called_once_with( - module.config.trajectory_parametrization, - world=world, - world_backend="roboplan", - ) assert module._planner is planner assert module._kinematics is kinematics - assert module._trajectory_parametrizer is parametrizer + assert module._trajectory_parametrizer is planning_specs.trajectory_parametrizer assert module._robots["arm"][0] == "robot-id" diff --git a/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md b/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md index 3ca932ac28..4941bc5f58 100644 --- a/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md +++ b/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md @@ -1,3 +1,17 @@ # Select one trajectory parametrization backend at startup -Each manipulation deployment selects exactly one trajectory parametrization backend at startup. Every untimed geometric path uses that backend. Planner-native timed results bypass parametrization because they are already trajectories, not because a backend failed. If the selected backend cannot parametrize a geometric path, plan materialization fails explicitly; the system does not fall back to another parametrizer because doing so would silently change trajectory semantics, timing, and failure behavior. A selected backend may use its own documented safety behavior between internal curve-fitting modes, such as RoboPlan TOPP-RA falling back from a colliding linear blend to Hermite fitting. +Each manipulation deployment selects exactly one trajectory parametrization +backend at startup. `TrajectoryParametrizerSpec` owns the complete successful +`PlanningResult`-to-`GeneratedPlan` conversion: it preserves the source path, +converts untimed paths, recognizes planner-native timed results, and validates +the canonical trajectory. The manipulation module only selects planning groups +and atomically stores the accepted result. + +Every untimed geometric path uses the selected backend. Planner-native timed +results bypass parametrization because they are already trajectories, not +because a backend failed. If the selected backend cannot parametrize a +geometric path, plan materialization fails explicitly; the system does not fall +back to another parametrizer because doing so would silently change trajectory +semantics, timing, and failure behavior. A selected backend may use its own +documented safety behavior between internal curve-fitting modes, such as +RoboPlan TOPP-RA falling back from a colliding linear blend to Hermite fitting. diff --git a/openspec/changes/add-trajectory-parametrization/design.md b/openspec/changes/add-trajectory-parametrization/design.md index 89fbfbd8f5..c47d22ddc0 100644 --- a/openspec/changes/add-trajectory-parametrization/design.md +++ b/openspec/changes/add-trajectory-parametrization/design.md @@ -2,7 +2,7 @@ `ManipulationModule._materialize_generated_plan()` currently validates a planner path, resolves selected-joint limits, directly constructs `JointTrajectoryGenerator`, and stores its output beside the source path in `GeneratedPlan`. `JointTrajectoryGenerator` creates an independent trapezoidal profile for each adjacent waypoint pair, so every interior waypoint is a stop. Dense paths consequently execute much more slowly than their geometric length and robot limits imply. -The current execution architecture is intentionally atomic: a cached `GeneratedPlan` contains both its source path and executable `JointTrajectory`, and `PlanExecutionManager` dispatches that stored trajectory without regenerating it. This design preserves that contract while introducing a deep path-to-trajectory adapter seam. +The current execution architecture is intentionally atomic: a cached `GeneratedPlan` contains both its source path and executable `JointTrajectory`, and `PlanExecutionManager` dispatches that stored trajectory without regenerating it. This design preserves that contract while introducing a deep trajectory-parametrization planning seam. RoboPlan 0.5.1 provides TOPP-RA with Hermite, cubic, adaptive, and linear-blend curve-fitting modes. Its parameterizer owns collision preservation for fitted curves and obtains absolute velocity and acceleration limits from its RoboPlan scene. The DimOS `RobotModelConfig` motion-limit fields are currently informal and are not authoritative for this backend. @@ -43,7 +43,7 @@ Add a typed `TrajectoryParametrizationConfig` under manipulation planning config - simple-backend point density/minimum segment controls needed for compatibility; - RoboPlan spline-fitting mode and its adaptive/blend controls. -The configuration factory validates the complete backend combination during startup. `roboplan_toppra` requires a finalized `RoboPlanWorld`; a non-RoboPlan world is rejected before planning. The selected parametrizer is constructed once and retained by `ManipulationModule`. +The configuration factory validates the complete backend combination during startup. `roboplan_toppra` requires `RoboPlanWorld`; a non-RoboPlan world is rejected before planning. The selected parametrizer is constructed once with the other planning roles and retained by `ManipulationModule` through `TrajectoryParametrizerSpec`. The module also owns a runtime next-plan speed scale in `(0, 1]`, initially `1.0`. This is not backend selection or persistent configuration. Each new @@ -53,18 +53,34 @@ planning receives the same scale through its per-request velocity and acceleration fields before it creates authoritative timing. Changing the scale does not invalidate, reparametrize, or retime an existing `GeneratedPlan`. -### Adapter Protocol +### Trajectory parametrizer Spec -Introduce a small adapter `Protocol`, distinct from an RPC-oriented DimOS `Spec`, for path-to-trajectory conversion. Its input contains: +Add `TrajectoryParametrizerSpec` beside `PlannerSpec`, `WorldSpec`, and +`KinematicsSpec` in the planning spec package. Its single materialization +operation accepts the active `WorldSpec`, ordered `PlanningGroupSelection`, +successful `PlanningResult`, and runtime speed scale. It returns the canonical +`GeneratedPlan` or raises a typed failure that the module converts into its +existing planning error surface. -- selected planning-group IDs; -- exact global joint ordering; -- the validated source `JointState` path; -- backend configuration. +This interface owns the complete successful-result conversion: -Its output is the canonical `JointTrajectory`, or it raises/returns a typed failure that plan materialization converts into the module's existing planning error surface. The adapter must not mutate the input path. +- copy and validate the source path; +- preserve and validate planner-native timing when timestamps are present; +- otherwise invoke the startup-selected geometric-path implementation; +- validate the canonical timed trajectory and applicable motion limits; +- construct `GeneratedPlan` with the unchanged source path and result metadata. -The simple adapter wraps `JointTrajectoryGenerator` and receives its existing DimOS-resolved limits. The RoboPlan adapter owns a finalized `RoboPlanWorld`/`RoboPlanModel` reference, resolves the selected group from the model, converts global names to native RoboPlan ordering, invokes `PathParameterizerTOPPRA`, and converts the native result back to exact selected global ordering. +The backend-specific path conversion remains private implementation. The simple +implementation wraps `JointTrajectoryGenerator` and resolves its existing +DimOS limits through `WorldSpec` robot configuration. The RoboPlan +implementation requires `RoboPlanWorld` at operation time, resolves the +selected group from its finalized model, converts global names to native +RoboPlan ordering, invokes `PathParameterizerTOPPRA`, and converts the native +result back to exact selected global ordering. + +The public Spec does not expose backend limit ownership. In particular it has +no `uses_request_limits` capability flag and no caller-supplied optional limit +fields. The RoboPlan adapter may cache one native TOPP-RA parameterizer per selected group set. This is internal optimization; construction and use must remain safe under the manipulation module's existing planning concurrency rules. @@ -72,20 +88,28 @@ The RoboPlan adapter may cache one native TOPP-RA parameterizer per selected gro Retain `GeneratedPlan` as the canonical accepted aggregate. An untimed `PlanningResult.path` passes through canonical input validation, the -startup-selected `TrajectoryParametrizer`, and canonical timed-output -validation before becoming `GeneratedPlan(path + trajectory)`. +startup-selected `TrajectoryParametrizerSpec`, and canonical timed-output +validation before the same operation returns +`GeneratedPlan(path + trajectory)`. A planner-native timed result already sits on the trajectory side of this -boundary. It bypasses `TrajectoryParametrizer`, retains its planner-defined -timestamps and velocities, and passes through the same canonical timed-output -validation before becoming `GeneratedPlan(path + trajectory)`. This bypass is -not fallback: no alternative parametrization backend is selected or invoked. - -Replace the direct `JointTrajectoryGenerator` construction inside materialization with the selected adapter. A failure at either parametrization or validation leaves `_last_plan` unset and follows the existing planning-epoch failure path. No separate public `GeneratedTrajectory` lifecycle is added. - -Keep the existing planner-native timed materialization path for results such as -RoboPlan Cartesian planning. It must not invoke the selected parametrizer or -discard bounded/time-optimal TCP timing semantics. +boundary. `TrajectoryParametrizerSpec` detects timestamps in `PlanningResult`, +does not invoke its backend-specific path conversion, retains the +planner-defined timestamps and velocities, and applies the same canonical +timed-output validation before returning +`GeneratedPlan(path + trajectory)`. This bypass is not fallback: no alternative +parametrization backend is selected or invoked. + +Remove path validation, motion-limit resolution, timed-result conversion, and +trajectory validation from `ManipulationModule`. The module selects planning +groups, invokes `TrajectoryParametrizerSpec.materialize_plan()`, and atomically +stores the returned plan only if its planning epoch remains current. A failure +leaves `_last_plan` unset and follows the existing planning-epoch failure path. +No separate public `GeneratedTrajectory` lifecycle is added. + +Remove the unused per-robot `JointTrajectoryGenerator` retained by the module's +robot registry. Keep the generator implementation only behind the +`simple_trapezoid` backend and its separate coordinator-control use. Canonical validation retains the current strong invariants: exact global joint ordering, finite and dimensionally aligned positions/velocities, first time at zero, strictly increasing times, positive duration for non-noop motion, and preserved start/goal. It also checks returned motion against the applicable velocity and acceleration limits with a documented numerical tolerance. Where RoboPlan exposes native accelerations, validate them before converting to the current positions/velocities-only message; otherwise derive the acceleration check consistently from velocity samples. diff --git a/openspec/changes/add-trajectory-parametrization/proposal.md b/openspec/changes/add-trajectory-parametrization/proposal.md index ca22242e90..e843cafe54 100644 --- a/openspec/changes/add-trajectory-parametrization/proposal.md +++ b/openspec/changes/add-trajectory-parametrization/proposal.md @@ -21,7 +21,7 @@ DimOS needs an explicit path-to-trajectory parametrization boundary that can ret ## Affected DimOS Surfaces -- Modules/streams: manipulation plan materialization, planning configuration/models, a trajectory-parametrizer adapter protocol, RoboPlan world/model integration, and timed-trajectory validation; planner-native timed results bypass path parametrization and no stream contracts change. +- Modules/streams: manipulation plan materialization, planning configuration/models, a `TrajectoryParametrizerSpec` beside the existing planning Specs, RoboPlan world/model integration, and timed-trajectory validation; planner-native timed results bypass backend path conversion and no stream contracts change. - Blueprints/CLI: manipulation blueprint configuration gains a startup-selectable parametrization backend; Viser gains a next-plan speed slider; no new CLI command or blueprint name is introduced. - Skills/MCP: existing plan, preview, and execute surfaces retain their signatures; unsuccessful parametrization makes planning fail before preview or execution. - Hardware/simulation/replay: hardware and simulation preserve the accepted trajectory's time domain during robot-local joint projection; RoboPlan TOPP-RA requires URDF velocity and acceleration limits. Replay behavior is unchanged. diff --git a/openspec/changes/add-trajectory-parametrization/tasks.md b/openspec/changes/add-trajectory-parametrization/tasks.md index e176975310..f78b8c94fe 100644 --- a/openspec/changes/add-trajectory-parametrization/tasks.md +++ b/openspec/changes/add-trajectory-parametrization/tasks.md @@ -3,8 +3,12 @@ - [x] 1.1 Pin `roboplan==0.5.1` in manipulation and lint dependencies and regenerate `uv.lock`. - [x] 1.2 Add a focused RoboPlan 0.5.1 binding contract test covering TOPP-RA construction, fitting-mode names, options, native trajectory fields, and missing-limit behavior. - [x] 1.3 Add typed startup configuration for `simple_trapezoid` and `roboplan_toppra`, including validated common scales/output period and backend-specific fitting controls. -- [x] 1.4 Add the internal trajectory-parametrizer adapter Protocol and typed request/failure boundary without introducing a separate public generated-trajectory lifecycle. +- [x] 1.4 Add the typed trajectory-parametrizer implementation and failure + boundary without introducing a separate generated-trajectory lifecycle. - [x] 1.5 Extend planning factory validation so exactly one parametrizer is constructed at startup and `roboplan_toppra` with a non-RoboPlan world fails before planning. +- [x] 1.6 Move the public parametrization interface to + `TrajectoryParametrizerSpec` beside the other planning Specs, include it in + `PlanningSpecs`, and hide backend limit ownership from callers. ## 2. Parametrization Backends @@ -18,7 +22,8 @@ ## 3. Plan Materialization and Validation - [x] 3.1 Construct and retain the selected trajectory parametrizer during manipulation planning initialization. -- [x] 3.2 Route `_materialize_generated_plan()` through the selected adapter while preserving the source `JointState` path unchanged in `GeneratedPlan`. +- [x] 3.2 Route untimed successful planning output through the selected adapter + while preserving the source `JointState` path unchanged in `GeneratedPlan`. - [x] 3.2a Preserve the existing planner-native timed-result path so it bypasses parametrization, retains its timestamps and velocities, and still receives canonical timed-output validation. - [x] 3.3 Preserve planning-epoch atomicity so parametrization or output-validation failure leaves no cached executable plan. - [x] 3.4 Extend canonical timed-trajectory validation for exact global joint ordering, dimensions, finite values, zero start time, strictly increasing times, positive non-noop duration, and start/goal preservation. @@ -29,6 +34,10 @@ immutable when the setting changes. - [x] 3.8 Add the Viser `Next plan speed` slider through `ManipulationOperator`, including active-operation disabling. +- [x] 3.9 Make `TrajectoryParametrizerSpec` own successful + `PlanningResult`-to-`GeneratedPlan` materialization, infer the timed-result + bypass from timestamps, and remove materialization/validation plus the dead + per-robot generator from `ManipulationModule`. ## 4. Automated Tests @@ -41,6 +50,8 @@ - [x] 4.7 Add runtime-scale and Viser tests covering valid/invalid values, future-plan application, Cartesian request scaling, current-plan preservation, and active-operation disabling. +- [x] 4.8 Move materialization contract tests to the parametrizer Spec seam + while retaining module tests for planning-epoch failure and atomic storage. ## 5. Documentation From a8247f16435a251aa83e9fd2705466700a859f12 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 29 Jul 2026 23:47:09 -0700 Subject: [PATCH 07/14] spec: remove --- CONTEXT.md | 78 ------- docs/agents/domain.md | 60 ----- docs/agents/issue-tracker.md | 62 ----- ...-one-trajectory-parametrization-backend.md | 17 -- ...scope-roboplan-toppra-to-roboplan-world.md | 3 - ...parametrize-during-plan-materialization.md | 3 - ...-post-processing-out-of-parametrization.md | 3 - ...ust-parametrizer-collision-preservation.md | 3 - ...-urdf-motion-limits-for-roboplan-toppra.md | 3 - .../.openspec.yaml | 2 - .../add-trajectory-parametrization/design.md | 213 ------------------ .../add-trajectory-parametrization/docs.md | 39 ---- .../proposal.md | 44 ---- .../spec.md | 165 -------------- .../add-trajectory-parametrization/tasks.md | 74 ------ 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 -- 22 files changed, 1059 deletions(-) delete mode 100644 CONTEXT.md delete mode 100644 docs/agents/domain.md delete mode 100644 docs/agents/issue-tracker.md delete mode 100644 docs/development/adr/0001-select-one-trajectory-parametrization-backend.md delete mode 100644 docs/development/adr/0002-scope-roboplan-toppra-to-roboplan-world.md delete mode 100644 docs/development/adr/0003-parametrize-during-plan-materialization.md delete mode 100644 docs/development/adr/0004-keep-geometric-post-processing-out-of-parametrization.md delete mode 100644 docs/development/adr/0005-trust-parametrizer-collision-preservation.md delete mode 100644 docs/development/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md delete mode 100644 openspec/changes/add-trajectory-parametrization/.openspec.yaml delete mode 100644 openspec/changes/add-trajectory-parametrization/design.md delete mode 100644 openspec/changes/add-trajectory-parametrization/docs.md delete mode 100644 openspec/changes/add-trajectory-parametrization/proposal.md delete mode 100644 openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md delete mode 100644 openspec/changes/add-trajectory-parametrization/tasks.md delete mode 100644 openspec/config.yaml delete mode 100644 openspec/schemas/dimos-capability/schema.yaml delete mode 100644 openspec/schemas/dimos-capability/templates/design.md delete mode 100644 openspec/schemas/dimos-capability/templates/docs.md delete mode 100644 openspec/schemas/dimos-capability/templates/proposal.md delete mode 100644 openspec/schemas/dimos-capability/templates/spec.md delete mode 100644 openspec/schemas/dimos-capability/templates/tasks.md diff --git a/CONTEXT.md b/CONTEXT.md deleted file mode 100644 index 4cecc25972..0000000000 --- a/CONTEXT.md +++ /dev/null @@ -1,78 +0,0 @@ -# Manipulation Planning - -This context describes requests for planning robot motion through joint and -Cartesian spaces, assigning time to that motion, and executing it through robot -control. - -## Language - -**Cartesian Waypoint**: -One absolute TCP pose or relative rigid displacement within a Cartesian target. - -**Cartesian Target**: -An ordered, homogeneous sequence of Cartesian waypoints for one planning group, including its starting waypoint. An absolute target contains only `PoseStamped` waypoints and starts at the current TCP pose. A relative target contains only `Transform` waypoints, starts with the identity transform, and measures every waypoint from the planning-start TCP pose. -_Avoid_: Cartesian track - -**Cartesian Path Configuration**: -Per-planning-call policy that selects how Cartesian waypoints are connected and constrains that operation. It is independent of the startup configuration that selects and constructs a planner backend. - -**Standard Cartesian Planning**: -Cartesian waypoint planning through a backend's supported serializable options. For RoboPlan, this includes multi-waypoint and simultaneous multi-end-effector paths, bounded and time-optimal speed modes, tracking tolerances, and solver tuning. - -**Bounded Speed Mode**: -A Cartesian timing policy that treats configured tool speeds and accelerations as maxima and slows the motion further when required by tracking or joint limits. - -**Time-Optimal Speed Mode**: -A Cartesian timing policy that resolves the requested path into joint space and retimes it against joint limits, optionally blending intermediate corners. - -**Custom Planner Components**: -Backend-native solver tasks, constraints, and barriers injected as live objects. These are outside standard Cartesian planning and require a separate constrained-IK interface. - -**Geometric Path**: -An ordered sequence of robot configurations describing where a robot may move, without prescribing when it reaches them. -_Avoid_: Untimed trajectory, parametrized path - -**Timed Trajectory**: -A robot motion expressed on a shared time domain, including timed configurations and their motion derivatives where available. -_Avoid_: Parametrized path, timed path - -**Generated Plan**: -The accepted manipulation result pairing a geometric path with the timed trajectory prepared for preview and execution. -_Avoid_: Path, trajectory - -**Trajectory Parametrization**: -The conversion of a geometric path into a timed trajectory under motion limits, including the bounded interpolation needed to define continuous motion between waypoints. -_Avoid_: Path planning, trajectory generation - -**Planner-Native Timed Result**: -A planner result that already contains authoritative timestamps and velocities. -It bypasses trajectory parametrization, retains its time domain, and still -receives canonical timed-trajectory validation. -_Avoid_: Parametrization fallback - -## Trajectory Parametrization Boundary - -Each manipulation stack selects one parametrization backend at startup. -Untimed geometric paths use that backend before a `GeneratedPlan` can be -accepted. A failure does not switch backends and leaves no executable plan -cached. Planner-native timed results skip conversion because they are already -timed trajectories, not because the selected backend failed. - -`TrajectoryParametrizerSpec` owns the conversion boundary, including canonical -trajectory validation and construction of the `GeneratedPlan`. -`ManipulationModule` selects the planning groups, delegates that conversion, -and atomically stores the accepted result. - -`simple_trapezoid` uses the current DimOS motion-limit resolution. -`roboplan_toppra` is available only with `RoboPlanWorld` and uses finite, -positive URDF velocity and extended acceleration limits from the RoboPlan -scene. It does not substitute the current generic DimOS limit fields. Preview -and execution share the accepted trajectory time domain; execution may only -project global joints into robot-local order without regenerating or retiming. - -**Next-Plan Speed**: -A runtime reduction scale in `(0, 1]` applied when generating a future -trajectory. Changing it never mutates or retimes an accepted `GeneratedPlan`; -the operator must plan again. Viser exposes this policy through its -`Next plan speed` slider. -_Avoid_: Playback speed, execution override diff --git a/docs/agents/domain.md b/docs/agents/domain.md deleted file mode 100644 index e1de27973a..0000000000 --- a/docs/agents/domain.md +++ /dev/null @@ -1,60 +0,0 @@ -# DimOS agent domain context - -## Context loading - -Before working on a change, load the repository context in this order: - -1. Read `AGENTS.md` and follow its applicable instructions. -2. Read `openspec/config.yaml` for the OpenSpec schema, terminology, and rules. -3. Read the relevant files under `openspec/specs/`. -4. Read the root `CONTEXT.md` if it exists. -5. Read relevant records under `docs/adr/` if that directory exists. - -`CONTEXT.md` and `docs/adr/` are optional. If either is absent, continue -silently; do not report the absence as an error. Select specs and ADRs based on -the affected behavior and implementation surface rather than reading -unrelated material. - -## Two meanings of “spec” - -Keep these terms separate: - -- An **OpenSpec spec** is a behavior specification under `openspec/specs/`. - It describes observable behavior, user or developer outcomes, public - interfaces, safety constraints, and testable scenarios. -- A **DimOS Python Spec Protocol** is a code-level interface contract, usually - a `Protocol` inheriting from `dimos.spec.utils.Spec`, often found in a - `*_spec.py` file. It describes module RPCs and injected interfaces. - -An OpenSpec spec is not a Python Protocol, and a Python Protocol does not -replace an OpenSpec behavioral requirement. Keep implementation details such as -class names, module wiring, stream types, generated registries, and rollout -steps in the OpenSpec change design or tasks unless they are externally -observable. - -## Work layout - -Organize work through this chain: - -```text -Linear issue -> OpenSpec change -> implementation tasks -> pull request -``` - -Linear provides intake and tracking. The OpenSpec change is the source of truth -for the behavioral change, design, and tasks. The pull request implements and -reviews those tasks. Keep the identifiers and links aligned across all three -artifacts; any Linear link edit requires user confirmation before it is made. - -When a task affects behavior, update the relevant OpenSpec change and, where -appropriate, the corresponding spec under `openspec/specs/`. Include concrete -scenarios for behavioral requirements. Call out DimOS Python Spec Protocols, -blueprint composition, streams, skills/MCP exposure, generated files, and -hardware, simulation, or replay assumptions in design and task material when -they are relevant. - -## Conflicting guidance - -Surface conflicts between an ADR and an OpenSpec spec explicitly. Do not -silently reconcile, overwrite, or guess which decision applies. Report the -conflict, identify the affected behavior or implementation, and ask for the -decision or update the authoritative document only when instructed. diff --git a/docs/agents/issue-tracker.md b/docs/agents/issue-tracker.md deleted file mode 100644 index c0db692d0f..0000000000 --- a/docs/agents/issue-tracker.md +++ /dev/null @@ -1,62 +0,0 @@ -# Issue tracking with Linear - -## Workspace - -DimOS work is tracked in the **DIM** team in Linear: - - - -Access Linear through the configured Linear MCP. Do not assume that a local -copy, an unconfigured client, or a direct API call is an alternative source of -truth. - -## Confirmation policy - -User confirmation is required immediately before **every** Linear edit. This -includes, without limitation: - -- creating an issue; -- changing any issue field, including title, description, assignee, priority, - project, or due date; -- adding, removing, or changing labels; -- posting comments; -- changing state or making any other state transition; and -- adding, removing, or changing links. - -Reading Linear is not an edit. Before an edit, state exactly what will change -and wait for explicit user confirmation. One confirmation does not authorize -later edits, even when they concern the same issue or change. - -## Linking convention - -Keep the work chain navigable: - -```text -Linear issue <-> openspec/changes/ <-> pull request -``` - -Use the OpenSpec change ID as the stable identifier in the relationship. Link -the Linear issue to the relevant OpenSpec change and link the pull request to -both when the tools support those links. If a link must be created or changed, -it is a Linear edit and requires confirmation under the policy above. - -## Source of truth and workflow - -Linear is the intake and tracking system. It records requests, ownership, -status, discussion, and delivery progress. OpenSpec is the source of truth for -the behavioral change, its design, and its implementation tasks. The pull -request is the review and delivery vehicle. - -Use this sequence: - -1. Capture or find the Linear issue in the DIM team. -2. Create or update `openspec/changes//` for the proposed behavior, - design, and tasks. -3. Implement the tasks and keep the OpenSpec change current. -4. Open the pull request and connect it to the issue and OpenSpec change. -5. Reflect progress in Linear only after confirming each requested edit. - -Do not use a Linear description, comment, or state as a substitute for an -OpenSpec requirement, design decision, or task. If Linear and OpenSpec -disagree about behavior, treat OpenSpec as authoritative and surface the -discrepancy to the user rather than silently choosing a version. diff --git a/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md b/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md deleted file mode 100644 index 4941bc5f58..0000000000 --- a/docs/development/adr/0001-select-one-trajectory-parametrization-backend.md +++ /dev/null @@ -1,17 +0,0 @@ -# Select one trajectory parametrization backend at startup - -Each manipulation deployment selects exactly one trajectory parametrization -backend at startup. `TrajectoryParametrizerSpec` owns the complete successful -`PlanningResult`-to-`GeneratedPlan` conversion: it preserves the source path, -converts untimed paths, recognizes planner-native timed results, and validates -the canonical trajectory. The manipulation module only selects planning groups -and atomically stores the accepted result. - -Every untimed geometric path uses the selected backend. Planner-native timed -results bypass parametrization because they are already trajectories, not -because a backend failed. If the selected backend cannot parametrize a -geometric path, plan materialization fails explicitly; the system does not fall -back to another parametrizer because doing so would silently change trajectory -semantics, timing, and failure behavior. A selected backend may use its own -documented safety behavior between internal curve-fitting modes, such as -RoboPlan TOPP-RA falling back from a colliding linear blend to Hermite fitting. diff --git a/docs/development/adr/0002-scope-roboplan-toppra-to-roboplan-world.md b/docs/development/adr/0002-scope-roboplan-toppra-to-roboplan-world.md deleted file mode 100644 index b3975d2710..0000000000 --- a/docs/development/adr/0002-scope-roboplan-toppra-to-roboplan-world.md +++ /dev/null @@ -1,3 +0,0 @@ -# Scope RoboPlan TOPP-RA to RoboPlanWorld - -The RoboPlan TOPP-RA parametrization backend accepts geometric paths produced by any planner, but it is available only when the manipulation world is `RoboPlanWorld`. This preserves planner independence without introducing and synchronizing a second RoboPlan robot model for other world backends; unsupported backend combinations fail during startup. diff --git a/docs/development/adr/0003-parametrize-during-plan-materialization.md b/docs/development/adr/0003-parametrize-during-plan-materialization.md deleted file mode 100644 index 9b36b7e541..0000000000 --- a/docs/development/adr/0003-parametrize-during-plan-materialization.md +++ /dev/null @@ -1,3 +0,0 @@ -# Parametrize during plan materialization - -Trajectory parametrization runs immediately after untimed geometric planning, before a `GeneratedPlan` is accepted or cached. Planner-native timed results already sit on the trajectory side of this boundary, so they bypass parametrization while retaining canonical validation. Preview and execution therefore consume the same validated time domain; execution may project globally named joints into robot-local order but does not regenerate or retime the trajectory. A runtime next-plan speed reduction is captured while producing a new trajectory and never changes an accepted plan. Parametrization failures prevent an untimed plan from being presented as ready rather than surfacing during execution. diff --git a/docs/development/adr/0004-keep-geometric-post-processing-out-of-parametrization.md b/docs/development/adr/0004-keep-geometric-post-processing-out-of-parametrization.md deleted file mode 100644 index e15946b1a8..0000000000 --- a/docs/development/adr/0004-keep-geometric-post-processing-out-of-parametrization.md +++ /dev/null @@ -1,3 +0,0 @@ -# Keep geometric post-processing out of trajectory parametrization - -Trajectory parametrization converts an accepted geometric path into a timed trajectory under motion limits. It may perform bounded interpolation or curve fitting needed to define continuous motion between the supplied waypoints, but it does not rewrite the source path through shortcutting, waypoint simplification, or path-class-specific resampling; those operations belong to plan generation or its post-processing stage. diff --git a/docs/development/adr/0005-trust-parametrizer-collision-preservation.md b/docs/development/adr/0005-trust-parametrizer-collision-preservation.md deleted file mode 100644 index 6a1dafbcd2..0000000000 --- a/docs/development/adr/0005-trust-parametrizer-collision-preservation.md +++ /dev/null @@ -1,3 +0,0 @@ -# Trust the parametrizer to preserve collision validity - -DimOS does not independently collision-check every sample of a returned timed trajectory during plan materialization. A parametrization backend that fits a curve away from the source waypoint polyline is responsible for collision-checking that curve against its authoritative world; DimOS validates the returned trajectory's structure and motion limits without duplicating the backend's potentially expensive collision pass. diff --git a/docs/development/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md b/docs/development/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md deleted file mode 100644 index 6afaae2d5a..0000000000 --- a/docs/development/adr/0006-use-urdf-motion-limits-for-roboplan-toppra.md +++ /dev/null @@ -1,3 +0,0 @@ -# Use URDF motion limits for RoboPlan TOPP-RA - -The RoboPlan TOPP-RA backend uses velocity and acceleration limits loaded by its RoboPlan scene from the robot URDF. Missing required limits fail explicitly rather than falling back to DimOS's current generic motion-limit fields; formal, globally named per-joint DimOS overrides are deferred to a separate change and will later map into RoboPlan's supported limit-override mechanism. diff --git a/openspec/changes/add-trajectory-parametrization/.openspec.yaml b/openspec/changes/add-trajectory-parametrization/.openspec.yaml deleted file mode 100644 index d581a3210f..0000000000 --- a/openspec/changes/add-trajectory-parametrization/.openspec.yaml +++ /dev/null @@ -1,2 +0,0 @@ -schema: dimos-capability -created: 2026-07-30 diff --git a/openspec/changes/add-trajectory-parametrization/design.md b/openspec/changes/add-trajectory-parametrization/design.md deleted file mode 100644 index c47d22ddc0..0000000000 --- a/openspec/changes/add-trajectory-parametrization/design.md +++ /dev/null @@ -1,213 +0,0 @@ -## Context - -`ManipulationModule._materialize_generated_plan()` currently validates a planner path, resolves selected-joint limits, directly constructs `JointTrajectoryGenerator`, and stores its output beside the source path in `GeneratedPlan`. `JointTrajectoryGenerator` creates an independent trapezoidal profile for each adjacent waypoint pair, so every interior waypoint is a stop. Dense paths consequently execute much more slowly than their geometric length and robot limits imply. - -The current execution architecture is intentionally atomic: a cached `GeneratedPlan` contains both its source path and executable `JointTrajectory`, and `PlanExecutionManager` dispatches that stored trajectory without regenerating it. This design preserves that contract while introducing a deep trajectory-parametrization planning seam. - -RoboPlan 0.5.1 provides TOPP-RA with Hermite, cubic, adaptive, and linear-blend curve-fitting modes. Its parameterizer owns collision preservation for fitted curves and obtains absolute velocity and acceleration limits from its RoboPlan scene. The DimOS `RobotModelConfig` motion-limit fields are currently informal and are not authoritative for this backend. - -## Goals / Non-Goals - -**Goals:** - -- Select one parametrization backend at manipulation-stack startup. -- Preserve the existing simple segmented-trapezoid behavior as a compatibility backend. -- Add RoboPlan TOPP-RA for any planner path represented in `RoboPlanWorld`. -- Convert and validate a path before constructing or caching `GeneratedPlan`. -- Preserve and validate planner-native timed trajectories without parametrizing them again. -- Allow an operator to reduce the speed of future plans from Viser without - changing the selected backend or mutating an accepted plan. -- Preserve the source path while allowing bounded backend interpolation between waypoints. -- Keep preview and execution on the accepted trajectory's time domain, allowing - robot-local joint projection but no regeneration or retiming. -- Use URDF-backed RoboPlan velocity and acceleration limits and fail clearly when they are unavailable. -- Retain current public manipulation RPC, skill, MCP, stream, and execution signatures. - -**Non-Goals:** - -- Path shortcutting, waypoint simplification, or path-class-specific resampling. -- Linear-TCP constraint metadata or constraint-aware geometric post-processing. -- Reparametrizing one stored geometric path at multiple speeds. -- Runtime backend switching or cross-backend fallback. -- Formal per-joint DimOS motion-limit models or RoboPlan YAML overrides. -- Independent DimOS collision resampling of trajectories already checked by RoboPlan. -- Jerk-limited execution or changes to the trajectory message schema. - -## DimOS Architecture - -### Configuration and startup - -Add a typed `TrajectoryParametrizationConfig` under manipulation planning configuration. It selects `simple_trapezoid` or `roboplan_toppra` and carries backend-specific options: - -- common operating scales and output sample period; -- simple-backend point density/minimum segment controls needed for compatibility; -- RoboPlan spline-fitting mode and its adaptive/blend controls. - -The configuration factory validates the complete backend combination during startup. `roboplan_toppra` requires `RoboPlanWorld`; a non-RoboPlan world is rejected before planning. The selected parametrizer is constructed once with the other planning roles and retained by `ManipulationModule` through `TrajectoryParametrizerSpec`. - -The module also owns a runtime next-plan speed scale in `(0, 1]`, initially -`1.0`. This is not backend selection or persistent configuration. Each new -untimed path materialization captures the current scale and multiplies the -configured velocity and acceleration reductions. Planner-native Cartesian -planning receives the same scale through its per-request velocity and -acceleration fields before it creates authoritative timing. Changing the scale -does not invalidate, reparametrize, or retime an existing `GeneratedPlan`. - -### Trajectory parametrizer Spec - -Add `TrajectoryParametrizerSpec` beside `PlannerSpec`, `WorldSpec`, and -`KinematicsSpec` in the planning spec package. Its single materialization -operation accepts the active `WorldSpec`, ordered `PlanningGroupSelection`, -successful `PlanningResult`, and runtime speed scale. It returns the canonical -`GeneratedPlan` or raises a typed failure that the module converts into its -existing planning error surface. - -This interface owns the complete successful-result conversion: - -- copy and validate the source path; -- preserve and validate planner-native timing when timestamps are present; -- otherwise invoke the startup-selected geometric-path implementation; -- validate the canonical timed trajectory and applicable motion limits; -- construct `GeneratedPlan` with the unchanged source path and result metadata. - -The backend-specific path conversion remains private implementation. The simple -implementation wraps `JointTrajectoryGenerator` and resolves its existing -DimOS limits through `WorldSpec` robot configuration. The RoboPlan -implementation requires `RoboPlanWorld` at operation time, resolves the -selected group from its finalized model, converts global names to native -RoboPlan ordering, invokes `PathParameterizerTOPPRA`, and converts the native -result back to exact selected global ordering. - -The public Spec does not expose backend limit ownership. In particular it has -no `uses_request_limits` capability flag and no caller-supplied optional limit -fields. - -The RoboPlan adapter may cache one native TOPP-RA parameterizer per selected group set. This is internal optimization; construction and use must remain safe under the manipulation module's existing planning concurrency rules. - -### Plan materialization - -Retain `GeneratedPlan` as the canonical accepted aggregate. An untimed -`PlanningResult.path` passes through canonical input validation, the -startup-selected `TrajectoryParametrizerSpec`, and canonical timed-output -validation before the same operation returns -`GeneratedPlan(path + trajectory)`. - -A planner-native timed result already sits on the trajectory side of this -boundary. `TrajectoryParametrizerSpec` detects timestamps in `PlanningResult`, -does not invoke its backend-specific path conversion, retains the -planner-defined timestamps and velocities, and applies the same canonical -timed-output validation before returning -`GeneratedPlan(path + trajectory)`. This bypass is not fallback: no alternative -parametrization backend is selected or invoked. - -Remove path validation, motion-limit resolution, timed-result conversion, and -trajectory validation from `ManipulationModule`. The module selects planning -groups, invokes `TrajectoryParametrizerSpec.materialize_plan()`, and atomically -stores the returned plan only if its planning epoch remains current. A failure -leaves `_last_plan` unset and follows the existing planning-epoch failure path. -No separate public `GeneratedTrajectory` lifecycle is added. - -Remove the unused per-robot `JointTrajectoryGenerator` retained by the module's -robot registry. Keep the generator implementation only behind the -`simple_trapezoid` backend and its separate coordinator-control use. - -Canonical validation retains the current strong invariants: exact global joint ordering, finite and dimensionally aligned positions/velocities, first time at zero, strictly increasing times, positive duration for non-noop motion, and preserved start/goal. It also checks returned motion against the applicable velocity and acceleration limits with a documented numerical tolerance. Where RoboPlan exposes native accelerations, validate them before converting to the current positions/velocities-only message; otherwise derive the acceleration check consistently from velocity samples. - -### RoboPlan limits and fitting - -Pin RoboPlan to `0.5.1`. `RoboPlanModel.scene` remains the source of native joint order and absolute TOPP-RA limits: - -- velocity from the URDF model; -- acceleration from extended URDF joint limits. - -DimOS `max_velocity`, `velocity_limits`, and `max_acceleration` do not override RoboPlan TOPP-RA in this change. Startup or first selected-group construction fails explicitly when a required URDF limit is missing or invalid. Common TOPP-RA velocity and acceleration scales may reduce, but never increase, the scene limits. - -Default RoboPlan fitting is `LinearBlend`, subject to confirming the 0.5.1 Python binding names during implementation. Other supported modes remain startup-selectable. Curve fitting is part of path-to-trajectory conversion, but preprocessing that rewrites the source waypoint sequence is not. - -RoboPlan owns collision checking for a fitted curve against its authoritative scene. DimOS does not repeat that expensive collision pass. RoboPlan's documented internal transition to a safe fitting mode remains within the selected `roboplan_toppra` backend and is allowed; failure of the backend as a whole does not invoke `simple_trapezoid`. - -### Other DimOS surfaces - -No streams, transports, module references, blueprint composition, existing RPC signatures, skills, MCP tools, CLI commands, or generated registry inputs change. The Viser control uses additive speed-setting RPCs on the existing manipulation operator seam. Existing preview and execution flows consume the stored `GeneratedPlan.trajectory`. No `all_blueprints.py` regeneration is expected. - -Viser exposes the runtime scale as a `Next plan speed` slider from `0.05` to -`1.0` in `0.05` steps. The control is disabled during an active panel -operation. It calls the UI-neutral `ManipulationOperator`, which delegates to -the module's runtime getter/setter; Viser does not own trajectory generation. - -## Decisions - -### Keep `GeneratedPlan` as the accepted aggregate - -The feature does not introduce separately cached geometric-plan and timed-trajectory artifacts because no current caller retimes one plan multiple ways. A narrow internal adapter provides extensibility without changing the public lifecycle. - -Alternative: restore frontier's public `GeneratedPlan`/`GeneratedTrajectory`/dispatch split. Rejected because it conflicts with the newer atomic execution architecture and solves no current use case. - -### Select one backend for the run - -Backend selection is startup configuration. Every untimed geometric path that -requires path-to-trajectory conversion uses the selected backend. A selected -backend's failure fails materialization; no other backend is attempted. -Planner-native timed results skip conversion because they are already -trajectories, not because a backend failed. - -Alternative: fall back to the simple backend after TOPP-RA failure. Rejected because it silently changes timing and stop behavior. - -### Scope RoboPlan TOPP-RA to `RoboPlanWorld` - -TOPP-RA accepts paths from any planner, but it reuses the authoritative RoboPlan scene and planning groups rather than building and synchronizing a second RoboPlan model for other worlds. - -Alternative: make RoboPlan TOPP-RA work with Drake by constructing a shadow RoboPlan scene. Deferred because model, naming, group, and limit synchronization add complexity without a current deployment need. - -### Keep geometric post-processing outside this feature - -The parametrizer may fit a bounded continuous curve while producing a trajectory. It does not shortcut, simplify, resample, or replace the source waypoint sequence. - -Alternative: port frontier's adaptive uniform waypoint decimator. Rejected because RoboPlan provides path-shortcutting and path-specific resampling facilities, and those operations change planning geometry. - -### Trust backend collision preservation - -RoboPlan owns collision checking introduced by its fitting mode. DimOS validates representation and motion constraints without repeating collision checks. - -Alternative: sample and collision-check the complete returned trajectory again in DimOS. Rejected for duplicated cost and competing backend logic. - -### Use URDF limits for RoboPlan - -RoboPlan scene limits are authoritative. Generic existing DimOS defaults are not injected. - -Alternative: wire current scalar/list DimOS fields into RoboPlan. Rejected because their ordering, provenance, defaults, and test coverage are insufficient. Formal globally named per-joint overrides are future work. - -## Safety / Simulation / Replay - -- Hardware never receives a trajectory that failed canonical validation or whose selected backend failed. -- Missing or invalid URDF motion limits fail rather than selecting generic defaults. -- The TOPP-RA reduction scales are constrained to safe ranges and cannot raise URDF limits. -- Simulation uses the same materialized trajectory path as hardware and is the primary manual QA surface. -- Preview must show the stored trajectory later projected into robot-local - joint order for execution without regeneration or retiming. -- Replay behavior is unaffected because no stream or replay-data format changes. -- Manual QA should compare simple and TOPP-RA trajectories for the same RoboPlan-world path, check smooth traversal of interior waypoints, and verify explicit failures for missing limits and incompatible startup configuration before any hardware trial. - -## Risks / Trade-offs - -- Existing robot URDFs may lack acceleration attributes. Mitigation: inventory relevant manipulation models, add valid model limits where authoritative, and test the failure diagnostic. -- RoboPlan's Python binding may expose names or return shapes different from the C++ documentation. Mitigation: add a focused API contract test against pinned 0.5.1 before integrating. -- `LinearBlend` can deviate from the waypoint polyline. Mitigation: bound deviation through backend configuration and rely on RoboPlan's scene collision check/internal safe-mode behavior. -- Finite-difference acceleration validation can be sensitive to output sampling. Mitigation: prefer native accelerations when available and document a numerical tolerance. -- Pinning RoboPlan 0.5.1 upgrades Pinocchio/Coal transitive dependencies. Mitigation: retain the regenerated lockfile and run focused RoboPlan world/planning tests. -- Simple and RoboPlan backends use different absolute-limit sources. Mitigation: document this explicitly; formal unified limit overrides remain separate work. - -## Migration / Rollout - -1. Land the RoboPlan 0.5.1 pin and compatible lock update. -2. Add configuration, adapter protocol, factory validation, and the wrapped simple backend while preserving its default behavior. -3. Add the RoboPlan TOPP-RA adapter and URDF-limit validation. -4. Route plan materialization through the startup-selected adapter. -5. Update manipulation planning docs with backend compatibility, limit requirements, and configuration examples. -6. Run focused manipulation/RoboPlan tests and manual simulation preview/execute QA before enabling TOPP-RA on hardware. - -Rollback is configuration-only while the simple backend remains available. No generated blueprint registry update or persistent data migration is required. - -## Open Questions - -None. Implementation must verify the exact RoboPlan 0.5.1 Python binding surface and choose numerical validation tolerances, but the intended behavior and ownership boundaries are decided. diff --git a/openspec/changes/add-trajectory-parametrization/docs.md b/openspec/changes/add-trajectory-parametrization/docs.md deleted file mode 100644 index efd4fe1e44..0000000000 --- a/openspec/changes/add-trajectory-parametrization/docs.md +++ /dev/null @@ -1,39 +0,0 @@ -## User-Facing Docs - -- Update `docs/capabilities/manipulation/index.md` with the path-to-trajectory lifecycle and the fact that preview and execution use the same materialized trajectory. -- Update `docs/capabilities/manipulation/adding_a_custom_arm.md` to require valid URDF velocity and extended acceleration limits when RoboPlan TOPP-RA is selected. -- Update `dimos/manipulation/planning/README.md` with: - - startup backend selection and configuration examples; - - `simple_trapezoid` versus `roboplan_toppra`; - - the `RoboPlanWorld` compatibility requirement; - - supported RoboPlan fitting modes and bounded deviation; - - no cross-backend fallback; - - URDF limit ownership and explicit missing-limit failures; - - Viser next-plan speed behavior and its non-retroactive boundary. - -## Contributor Docs - -- No new standalone contributor guide is required. -- If implementation reveals a non-obvious RoboPlan packaging or URDF 1.2 limit convention, add a focused note under `docs/development/` rather than expanding user-facing architecture prose. -- Keep the architecture decisions under `docs/development/adr/` and ensure the OpenSpec design remains consistent with them. - -## Coding-Agent Docs - -- Update `AGENTS.md` only if trajectory-parametrizer extension guidance becomes a stable coding-agent workflow. If updated, document: - - the geometric-path versus timed-trajectory boundary; - - startup-only backend selection; - - RoboPlan URDF limit ownership; - - the prohibition on silent cross-backend fallback. -- No coding-agent doc update is required merely for private class or file names. - -## Doc Validation - -- Run `doclinks` for changed Markdown links. -- Run `md-babel-py run dimos/manipulation/planning/README.md` if executable Python or shell examples are added or modified. -- Run `md-babel-py run docs/capabilities/manipulation/adding_a_custom_arm.md` if executable examples are changed. -- Run `bin/gen-diagrams` only if a checked-in generated diagram source is introduced or changed. -- Run the repository's documentation build/check command applicable to changed capability pages. - -## No Docs Needed - -Not applicable. Backend selection and URDF motion-limit requirements affect robot configuration and failure behavior, so user-facing documentation is required. diff --git a/openspec/changes/add-trajectory-parametrization/proposal.md b/openspec/changes/add-trajectory-parametrization/proposal.md deleted file mode 100644 index e843cafe54..0000000000 --- a/openspec/changes/add-trajectory-parametrization/proposal.md +++ /dev/null @@ -1,44 +0,0 @@ -## Why - -Manipulation planning currently turns every pair of geometric waypoints into an independent trapezoidal segment. The robot therefore stops at every waypoint, so dense planner output produces slow and mechanically awkward motion instead of one continuous trajectory constrained by the robot's actual motion limits. - -DimOS needs an explicit path-to-trajectory parametrization boundary that can retain the current simple implementation while allowing RoboPlan TOPP-RA to generate continuous, time-optimal trajectories. The selected behavior must be deterministic at startup, fail before a plan is exposed as executable, and use authoritative robot limits. - -## What Changes - -- Add startup configuration that selects exactly one manipulation trajectory parametrization backend for the lifetime of the stack. -- Preserve the existing simple trapezoid behavior as a selectable compatibility backend. -- Add a RoboPlan TOPP-RA backend for any geometric path planned against `RoboPlanWorld`, independent of which planner produced that path. -- Parametrize immediately after geometric planning and only construct/cache a `GeneratedPlan` after trajectory generation and validation succeed. -- Preserve planner-native timed trajectories without parametrizing them again; validate and store their existing timing. -- Allow the selected backend to perform bounded interpolation or curve fitting while converting the source path into a timed trajectory. -- Use RoboPlan scene limits sourced from URDF velocity and acceleration limits; fail explicitly when required limits or the selected backend are unavailable. -- Do not switch parametrization backends after startup or fall back to another backend when parametrization fails. -- Add a Viser "Next plan speed" control that applies a bounded runtime reduction - scale to future plans without mutating an already accepted plan. -- Exclude geometric path shortcutting, waypoint simplification, path-specific resampling, and formal DimOS per-joint limit overrides from this change. -- Pin the optional RoboPlan dependency to version `0.5.1`. - -## Affected DimOS Surfaces - -- Modules/streams: manipulation plan materialization, planning configuration/models, a `TrajectoryParametrizerSpec` beside the existing planning Specs, RoboPlan world/model integration, and timed-trajectory validation; planner-native timed results bypass backend path conversion and no stream contracts change. -- Blueprints/CLI: manipulation blueprint configuration gains a startup-selectable parametrization backend; Viser gains a next-plan speed slider; no new CLI command or blueprint name is introduced. -- Skills/MCP: existing plan, preview, and execute surfaces retain their signatures; unsuccessful parametrization makes planning fail before preview or execution. -- Hardware/simulation/replay: hardware and simulation preserve the accepted trajectory's time domain during robot-local joint projection; RoboPlan TOPP-RA requires URDF velocity and acceleration limits. Replay behavior is unchanged. -- Docs/generated registries: manipulation planning documentation and dependency guidance require updates; no generated blueprint registry change is expected. - -## Capabilities - -### New Capabilities - -- `manipulation-trajectory-parametrization`: Startup backend selection and conversion of accepted geometric manipulation paths into validated timed trajectories. - -### Modified Capabilities - -None. - -## Impact - -Users may choose the existing simple backend or RoboPlan TOPP-RA at startup. RoboPlan TOPP-RA configurations become stricter: they require `RoboPlanWorld`, RoboPlan `0.5.1`, and usable URDF velocity and acceleration limits. Parametrization failures are reported as planning/materialization failures rather than being deferred to execution or hidden by fallback. - -The implementation touches manipulation planning internals and dependency resolution but does not intentionally break existing plan, preview, execute, skill, MCP, stream, or CLI signatures. Verification requires backend/configuration tests, trajectory invariant and failure tests, RoboPlan adapter tests, dependency lock validation, simulation/manual preview and execution QA, and documentation validation. diff --git a/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md b/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md deleted file mode 100644 index 3677a6f48d..0000000000 --- a/openspec/changes/add-trajectory-parametrization/specs/manipulation-trajectory-parametrization/spec.md +++ /dev/null @@ -1,165 +0,0 @@ -## ADDED Requirements - -### Requirement: A single trajectory parametrization backend is selected at startup - -The manipulation stack SHALL select exactly one trajectory parametrization backend during startup and SHALL use that backend for every untimed geometric path materialized during that run. - -#### Scenario: Simple backend is selected -- **GIVEN** a manipulation stack configured with the simple trajectory parametrization backend -- **WHEN** the stack starts successfully -- **THEN** every accepted geometric path MUST be converted by the simple backend -- **AND** the backend MUST remain unchanged for the lifetime of the running stack - -#### Scenario: RoboPlan TOPP-RA backend is selected -- **GIVEN** a manipulation stack configured with the RoboPlan TOPP-RA backend and `RoboPlanWorld` -- **WHEN** the stack starts successfully -- **THEN** every accepted geometric path MUST be converted by RoboPlan TOPP-RA -- **AND** the planner that produced the path MUST NOT be required to be RoboPlan's planner - -#### Scenario: Backend and world are incompatible -- **GIVEN** a manipulation stack configured with RoboPlan TOPP-RA and a non-RoboPlan world -- **WHEN** the stack is initialized -- **THEN** initialization MUST fail with an actionable configuration error -- **AND** planning MUST NOT begin with a backend that cannot operate against the configured world - -#### Scenario: Planner returns a timed trajectory -- **GIVEN** a planner returns a trajectory with authoritative timestamps and velocities -- **WHEN** the generated plan is materialized -- **THEN** the system MUST preserve and canonically validate the planner-provided timing -- **AND** it MUST NOT invoke a trajectory parametrization backend -- **AND** this bypass MUST NOT be treated as backend fallback - -### Requirement: Parametrization completes before a generated plan is accepted - -The manipulation stack SHALL convert an accepted geometric path into a timed trajectory before exposing or caching the corresponding generated plan. - -#### Scenario: Parametrization succeeds -- **GIVEN** a planner has returned a successful geometric path -- **WHEN** the selected backend produces a valid timed trajectory -- **THEN** the system SHALL construct and cache one generated plan containing the source path and timed trajectory -- **AND** preview and execution MUST consume that same accepted trajectory - -#### Scenario: Parametrization fails -- **GIVEN** a planner has returned a successful geometric path -- **WHEN** the selected backend cannot produce a valid timed trajectory -- **THEN** the system MUST report plan materialization failure -- **AND** it MUST NOT cache or expose that path as an executable generated plan - -#### Scenario: Existing planner timing is accepted -- **GIVEN** a planner has returned a valid timed trajectory -- **WHEN** canonical timed-output validation succeeds -- **THEN** the system SHALL construct and cache one generated plan containing the source path and planner-native trajectory -- **AND** preview and execution MUST consume that same trajectory without retiming - -### Requirement: Parametrization converts a path into continuous timed motion - -The selected backend SHALL convert the source geometric path into one trajectory with a shared time domain across every selected joint. - -#### Scenario: Multi-waypoint path is converted -- **GIVEN** a valid path containing two or more consistently ordered joint configurations -- **WHEN** the path is parametrized -- **THEN** the result MUST contain timed positions and velocities for every selected joint -- **AND** its time values MUST start at zero and increase strictly through the final trajectory duration - -#### Scenario: Backend performs bounded curve fitting -- **GIVEN** a backend mode that fits continuous motion between supplied waypoints -- **WHEN** the path is parametrized -- **THEN** the backend MAY produce samples that do not coincide with every interior waypoint -- **AND** it MUST preserve the source start and goal within the configured numerical tolerance -- **AND** it MUST honor its configured geometric-deviation and collision-preservation contract - -#### Scenario: Source geometric path remains available -- **GIVEN** a path is successfully converted with interpolation or bounded curve fitting -- **WHEN** the generated plan is inspected -- **THEN** its source geometric path MUST remain unchanged -- **AND** its timed trajectory MUST be stored as a distinct representation within the generated plan - -### Requirement: Timed trajectories satisfy canonical invariants - -The manipulation stack MUST reject timed trajectories that are malformed, non-finite, incorrectly ordered, or inconsistent with the selected joints and motion limits. - -#### Scenario: Valid trajectory is accepted -- **GIVEN** a backend returns a trajectory with the expected global joint ordering -- **WHEN** all samples have finite positions and velocities, strictly increasing finite times, preserved endpoints, and motion within applicable limits -- **THEN** the trajectory SHALL be accepted for preview and execution - -#### Scenario: Malformed trajectory is rejected -- **GIVEN** a backend returns missing samples, inconsistent dimensions, duplicate or decreasing times, non-finite values, unexpected joint names, or a mismatched endpoint -- **WHEN** the result is validated -- **THEN** plan materialization MUST fail with a diagnostic identifying the violated invariant -- **AND** the invalid trajectory MUST NOT reach execution - -#### Scenario: Motion limits are exceeded -- **GIVEN** a backend returns motion exceeding an applicable joint velocity or acceleration limit beyond numerical tolerance -- **WHEN** the result is validated -- **THEN** plan materialization MUST fail -- **AND** the generated motion MUST NOT be exposed as executable - -### Requirement: RoboPlan TOPP-RA uses authoritative URDF limits - -The RoboPlan TOPP-RA backend SHALL use the RoboPlan scene's joint velocity and acceleration limits sourced from the robot URDF. - -#### Scenario: Required URDF limits are present -- **GIVEN** every selected joint has usable velocity and acceleration limits in the URDF-backed RoboPlan scene -- **WHEN** RoboPlan TOPP-RA parametrizes a path -- **THEN** it MUST constrain the trajectory using those limits and the configured reduction scales - -#### Scenario: A required URDF limit is missing -- **GIVEN** at least one selected joint lacks a usable URDF velocity or acceleration limit -- **WHEN** RoboPlan TOPP-RA is initialized for or applied to that planning group -- **THEN** the operation MUST fail with a diagnostic naming the missing limit and affected joint -- **AND** the system MUST NOT substitute DimOS's generic motion-limit defaults - -### Requirement: Backend failures do not trigger cross-backend fallback - -The manipulation stack MUST NOT silently switch to another trajectory parametrization backend when the startup-selected backend fails. - -#### Scenario: Selected backend rejects a path -- **GIVEN** exactly one parametrization backend was selected at startup -- **WHEN** that backend rejects or fails to parametrize a path -- **THEN** plan materialization MUST fail using that backend's diagnostic -- **AND** no other parametrization backend may be invoked for the path - -#### Scenario: RoboPlan uses an internal safety fitting mode -- **GIVEN** RoboPlan TOPP-RA remains the selected backend -- **WHEN** RoboPlan applies its documented internal safety behavior between curve-fitting modes -- **THEN** the result MAY be accepted if the backend reports a valid trajectory -- **AND** this MUST NOT be treated as switching to a different parametrization backend - -### Requirement: Existing manipulation control surfaces remain compatible - -Trajectory parametrization SHALL integrate without changing the public plan, preview, execute, skill, MCP, or stream signatures. - -#### Scenario: Existing preview and execution flow -- **GIVEN** a generated plan was successfully materialized from either an untimed path or a planner-native timed trajectory -- **WHEN** a caller invokes the existing preview or execute surface -- **THEN** the caller MUST use the same public operation and argument shape as before -- **AND** execution MAY project the accepted stored trajectory into robot-local joint order -- **AND** the accepted timestamps and velocities MUST be previewed or dispatched without regeneration or retiming - -### Requirement: Viser controls the speed of future plans - -The manipulation Viser panel SHALL expose a bounded runtime speed scale for -plans generated after the setting changes. - -#### Scenario: Operator reduces next-plan speed -- **GIVEN** the Viser panel is idle and displays a fresh accepted plan -- **WHEN** the operator moves `Next plan speed` to a value in `(0, 1]` -- **THEN** the module MUST retain that value for future planning -- **AND** the existing accepted plan MUST remain unchanged and executable - -#### Scenario: A new untimed path is planned -- **GIVEN** a runtime next-plan speed below `1.0` -- **WHEN** an untimed geometric path is materialized -- **THEN** the selected parametrizer MUST multiply its configured velocity and acceleration reduction scales by the runtime scale - -#### Scenario: A new Cartesian path is planned from Viser -- **GIVEN** a runtime next-plan speed below `1.0` -- **WHEN** Viser requests planner-native Cartesian planning -- **THEN** the request MUST carry that velocity and acceleration scale -- **AND** the returned planner-native timing MUST still bypass path parametrization - -#### Scenario: Speed changes during an active panel operation -- **GIVEN** the Viser panel is planning, previewing, executing, cancelling, or clearing -- **WHEN** the speed control is rendered -- **THEN** it MUST be disabled until the operation becomes idle diff --git a/openspec/changes/add-trajectory-parametrization/tasks.md b/openspec/changes/add-trajectory-parametrization/tasks.md deleted file mode 100644 index f78b8c94fe..0000000000 --- a/openspec/changes/add-trajectory-parametrization/tasks.md +++ /dev/null @@ -1,74 +0,0 @@ -## 1. Configuration and Adapter Boundary - -- [x] 1.1 Pin `roboplan==0.5.1` in manipulation and lint dependencies and regenerate `uv.lock`. -- [x] 1.2 Add a focused RoboPlan 0.5.1 binding contract test covering TOPP-RA construction, fitting-mode names, options, native trajectory fields, and missing-limit behavior. -- [x] 1.3 Add typed startup configuration for `simple_trapezoid` and `roboplan_toppra`, including validated common scales/output period and backend-specific fitting controls. -- [x] 1.4 Add the typed trajectory-parametrizer implementation and failure - boundary without introducing a separate generated-trajectory lifecycle. -- [x] 1.5 Extend planning factory validation so exactly one parametrizer is constructed at startup and `roboplan_toppra` with a non-RoboPlan world fails before planning. -- [x] 1.6 Move the public parametrization interface to - `TrajectoryParametrizerSpec` beside the other planning Specs, include it in - `PlanningSpecs`, and hide backend limit ownership from callers. - -## 2. Parametrization Backends - -- [x] 2.1 Wrap the existing `JointTrajectoryGenerator` as the `simple_trapezoid` adapter while preserving current limit resolution, waypoint, and timing behavior. -- [x] 2.2 Implement the RoboPlan TOPP-RA adapter using the finalized `RoboPlanWorld` model, selected-group lookup, and exact global-to-native joint mapping. -- [x] 2.3 Validate that every selected RoboPlan joint has finite positive URDF-backed velocity and acceleration limits, with no fallback to generic DimOS motion-limit fields. -- [x] 2.4 Map configured TOPP-RA fitting mode, output period, velocity/acceleration reduction scales, and adaptive/blend options into the pinned 0.5.1 API. -- [x] 2.5 Convert RoboPlan native trajectory output back to exact selected global joint order and retain positions, velocities, timestamps, and native acceleration data long enough for limit validation. -- [x] 2.6 Ensure a selected backend failure returns one actionable materialization error and never invokes the other backend; retain documented RoboPlan internal safe fitting-mode behavior. - -## 3. Plan Materialization and Validation - -- [x] 3.1 Construct and retain the selected trajectory parametrizer during manipulation planning initialization. -- [x] 3.2 Route untimed successful planning output through the selected adapter - while preserving the source `JointState` path unchanged in `GeneratedPlan`. -- [x] 3.2a Preserve the existing planner-native timed-result path so it bypasses parametrization, retains its timestamps and velocities, and still receives canonical timed-output validation. -- [x] 3.3 Preserve planning-epoch atomicity so parametrization or output-validation failure leaves no cached executable plan. -- [x] 3.4 Extend canonical timed-trajectory validation for exact global joint ordering, dimensions, finite values, zero start time, strictly increasing times, positive non-noop duration, and start/goal preservation. -- [x] 3.5 Validate returned velocity and acceleration against the backend's applicable limits with documented numerical tolerances, preferring native acceleration samples when available. -- [x] 3.6 Verify preview and execution reuse the accepted stored trajectory without regeneration or retiming. -- [x] 3.7 Add a runtime next-plan speed setting, apply it to untimed - parametrization and Viser Cartesian request timing, and keep accepted plans - immutable when the setting changes. -- [x] 3.8 Add the Viser `Next plan speed` slider through - `ManipulationOperator`, including active-operation disabling. -- [x] 3.9 Make `TrajectoryParametrizerSpec` own successful - `PlanningResult`-to-`GeneratedPlan` materialization, infer the timed-result - bypass from timestamps, and remove materialization/validation plus the dead - per-robot generator from `ManipulationModule`. - -## 4. Automated Tests - -- [x] 4.1 Add adapter tests for valid simple and RoboPlan trajectories, multi-waypoint continuity, global/native reordering, composite planning groups, and configurable fitting modes. -- [x] 4.2 Add startup/configuration tests for each backend, unknown backends, invalid scales/options, incompatible world selection, and startup-only backend lifetime. -- [x] 4.3 Add RoboPlan limit tests for valid URDF velocity/acceleration limits, missing limits, non-finite or non-positive limits, reduction scales, and proof that generic DimOS defaults are not substituted. -- [x] 4.4 Add materialization tests for backend failure, no cross-backend fallback, planner-native timed-result bypass, malformed/native output rejection, motion-limit rejection, and no plan caching after failure. -- [x] 4.5 Update preview/execution tests to prove the accepted timed trajectory reaches visualization and robot-local coordinator dispatch without regeneration or retiming. -- [x] 4.6 Run focused test targets including `dimos/manipulation/test_generated_plan_materialization.py`, `dimos/manipulation/test_planning_factory.py`, `dimos/manipulation/test_roboplan_world.py`, `dimos/manipulation/test_plan_execution.py`, and new parametrizer tests. -- [x] 4.7 Add runtime-scale and Viser tests covering valid/invalid values, - future-plan application, Cartesian request scaling, current-plan - preservation, and active-operation disabling. -- [x] 4.8 Move materialization contract tests to the parametrizer Spec seam - while retaining module tests for planning-epoch failure and atomic storage. - -## 5. Documentation - -- [x] 5.1 Update `dimos/manipulation/planning/README.md` with the path-to-trajectory lifecycle, backend configuration examples, RoboPlan fitting modes, `RoboPlanWorld` compatibility, no cross-backend fallback, and URDF limit requirements. -- [x] 5.2 Update `docs/capabilities/manipulation/index.md` to explain that a plan is accepted only after parametrization and that preview and execution share the stored trajectory. -- [x] 5.3 Update `docs/capabilities/manipulation/adding_a_custom_arm.md` with RoboPlan 0.5.1 URDF velocity and extended acceleration-limit requirements and missing-limit failure behavior. -- [x] 5.4 Reconcile `CONTEXT.md` and `docs/development/adr/0001` through `docs/development/adr/0006` with the implemented names and behavior; update `AGENTS.md` only if stable extension guidance is added. -- [x] 5.5 Document Viser next-plan speed semantics and the fact that changing - the slider requires planning again. - -## 6. Verification and Manual QA - -- [x] 6.1 Run `OPENSPEC_TELEMETRY=0 openspec validate add-trajectory-parametrization`. -- [x] 6.2 Run `uv lock --check` and verify RoboPlan resolves to exactly `0.5.1` on supported Python/platform markers. -- [x] 6.3 Run `uv run mypy dimos/manipulation` and the repository's Ruff/pre-commit checks for changed Python files. -- [x] 6.4 Run the focused tests from task 4.6 and the broader fast manipulation test suite. -- [x] 6.5 Run `doclinks` and applicable `md-babel-py run` commands for changed documentation examples; run `bin/gen-diagrams` only if generated diagram sources changed. -- [ ] 6.6 Manually plan, preview, and execute a nontrivial multi-waypoint path in a manipulation simulation with `simple_trapezoid`, confirming compatibility behavior and identical preview/execution timing. -- [ ] 6.7 Manually repeat the simulation with `RoboPlanWorld` and `roboplan_toppra`, confirming smooth interior traversal, URDF-limit compliance, and identical preview/execution timing. -- [ ] 6.8 Manually verify actionable pre-motion failures for an incompatible world/backend combination, a missing URDF acceleration limit, and a TOPP-RA parametrization failure. diff --git a/openspec/config.yaml b/openspec/config.yaml deleted file mode 100644 index 62a72bba63..0000000000 --- a/openspec/config.yaml +++ /dev/null @@ -1,45 +0,0 @@ -schema: dimos-capability - -context: | - DimOS is a robotics operating system for generalist robots. Modules communicate - through typed streams (`In[T]`, `Out[T]`) over LCM, SHM, ROS, DDS, or other - transports. Blueprints compose modules into runnable robot stacks. Skills are - `@skill`-annotated RPC methods exposed to agents and MCP clients. - - Terminology boundary: - - "OpenSpec spec" means a behavior specification under `openspec/specs/`. - - "DimOS Spec" means a Python Protocol/RPC contract in `*_spec.py` files, - usually inheriting `dimos.spec.utils.Spec` and `typing.Protocol`. - Keep these separate. OpenSpec specs describe observable behavior; DimOS Specs - describe code-level module interfaces. - - OpenSpec specs should capture current behavior, user/developer-visible - outcomes, public CLI/API/tool surfaces, robot safety constraints, and testable - scenarios. Put implementation choices, class names, module wiring, generated - registry updates, and rollout details in `design.md` or `tasks.md`. - - Documentation lives in: - - `docs/usage/` for user-facing concepts and APIs. - - `docs/capabilities/` for capability and platform guides. - - `docs/development/` for contributor process. - - `docs/coding-agents/` and `AGENTS.md` for coding-agent guidance. - -rules: - proposal: - - "Identify affected DimOS surfaces: modules, streams, blueprints, CLI, skills/MCP, docs, hardware, simulation, replay, or generated registries." - - Use capability names that match behavior domains, not Python class names. - - Mark hardware safety or public API/CLI changes explicitly. - specs: - - Write behavior-first requirements; avoid implementation detail unless it is externally observable. - - Every requirement must include at least one `#### Scenario:` block with concrete observable outcomes. - - Use "OpenSpec capability spec" when prose might otherwise be confused with DimOS Python `Spec` Protocols. - design: - - Call out DimOS `Spec` Protocols, adapter Protocols, blueprint composition, stream names/types, and skill/MCP exposure when relevant. - - Mention generated files and required regeneration commands, especially `pytest dimos/robot/test_all_blueprints_generation.py` for blueprint registry changes. - - Include hardware/simulation/replay assumptions and safety constraints for robot-facing work. - docs: - - List user-facing docs, contributor docs, coding-agent docs, and AGENTS.md updates required by the change. - - Include documentation validation commands for changed docs, such as `doclinks` and `md-babel-py run ` where applicable. - tasks: - - Include verification tasks for OpenSpec validation, relevant pytest targets, type checks when needed, and manual QA through the user-facing surface. - - Add registry generation tasks when blueprint names, module classes, or generated registry inputs change. diff --git a/openspec/schemas/dimos-capability/schema.yaml b/openspec/schemas/dimos-capability/schema.yaml deleted file mode 100644 index fedb7964ee..0000000000 --- a/openspec/schemas/dimos-capability/schema.yaml +++ /dev/null @@ -1,128 +0,0 @@ -name: dimos-capability -version: 1 -description: DimOS capability workflow - proposal → specs/design/docs → tasks -artifacts: - - id: proposal - generates: proposal.md - description: DimOS change proposal covering intent, scope, capability impact, and affected robot/software surfaces - template: proposal.md - instruction: | - Create the proposal document that establishes WHY this change is needed and what DimOS behavior it affects. - - Sections: - - **Why**: 1-2 concise paragraphs on the problem or opportunity. Explain why the change matters now. - - **What Changes**: Bullet list of added, modified, or removed behavior. Mark public API/CLI or hardware-safety breaking changes with **BREAKING**. - - **Affected DimOS Surfaces**: Identify modules, streams, blueprints, CLI commands, skills/MCP tools, docs, hardware, simulation, replay, generated registries, or external protocols touched by the change. - - **Capabilities**: Identify which OpenSpec capability specs will be created or modified: - - **New Capabilities**: List behavior domains introduced by the change. Each becomes `specs//spec.md`. Use kebab-case names (for example, `agent-skills-mcp`, `blueprint-composition`, `manipulation-stack`). - - **Modified Capabilities**: List existing `openspec/specs//` entries whose requirements change. Only include spec-level behavior changes, not implementation-only refactors. - - **Impact**: Summarize user/developer impact, compatibility risks, dependency changes, documentation updates, and test/QA scope. - - Keep proposals concise. Do not include line-by-line implementation details; put architecture and rollout decisions in `design.md`. - requires: [] - - id: specs - generates: specs/**/*.md - description: Behavior-first OpenSpec capability delta specifications - template: spec.md - instruction: | - Create OpenSpec capability specs that define WHAT DimOS should do, not how it is implemented. - - Create one delta spec file per capability listed in proposal.md: - - New capabilities: use `specs//spec.md` with the exact kebab-case name from the proposal. - - Modified capabilities: use the existing folder from `openspec/specs//`. - - Use these delta sections as `##` headers: - - **ADDED Requirements**: New externally observable behavior. - - **MODIFIED Requirements**: Changed behavior. Include the full updated requirement block, not a partial patch. - - **REMOVED Requirements**: Deprecated behavior. Include **Reason** and **Migration**. - - **RENAMED Requirements**: Name-only changes. Use FROM:/TO: format. - - Requirement format: - - Use `### Requirement: `. - - Use SHALL/MUST for normative requirements. - - Include at least one `#### Scenario: ` per requirement. Scenario headings MUST use exactly four `#` characters. - - Prefer `- **GIVEN**`, `- **WHEN**`, `- **THEN**`, and `- **AND**` bullets. - - Cover happy path plus meaningful edge/error/safety cases. - - DimOS-specific guidance: - - Specify user/developer-visible behavior, robot outcomes, CLI behavior, skill/MCP tool behavior, stream contracts, safety constraints, and compatibility expectations. - - Avoid Python class names, private module internals, transport implementation choices, and generated-file details unless those details are observable API contracts. - - Use "OpenSpec capability spec" in prose when needed to avoid confusion with DimOS Python `Spec` Protocols. - - If the behavior only changes implementation and not observable requirements, do not create a spec delta. - requires: - - proposal - - id: design - generates: design.md - description: DimOS technical design and architecture decisions - template: design.md - instruction: | - Create the design document that explains HOW the change should be implemented in DimOS. - - Include design.md for cross-module changes, new robot/hardware integration, new public interfaces, new dependencies, safety-sensitive behavior, generated registry changes, or unclear architecture. - - Sections: - - **Context**: Current state, relevant modules/blueprints/docs, and constraints. - - **Goals / Non-Goals**: What the design achieves and explicitly excludes. - - **DimOS Architecture**: Modules, streams, transports, blueprints, RPC/module refs, DimOS `Spec` Protocols, adapter Protocols, skills/MCP exposure, CLI entry points, and generated registries involved. - - **Decisions**: Key choices with rationale and alternatives considered. - - **Safety / Simulation / Replay**: Hardware assumptions, sim/replay behavior, safety constraints, and manual QA surface. - - **Risks / Trade-offs**: Known risks and mitigations. - - **Migration / Rollout**: Compatibility, generated files, docs, and deployment steps. - - **Open Questions**: Outstanding decisions or unknowns. - - Reference proposal.md for intent and specs for behavior. Keep line-by-line work in tasks.md. - requires: - - proposal - - id: docs - generates: docs.md - description: Documentation impact plan for user, contributor, and coding-agent docs - template: docs.md - instruction: | - Create the documentation impact plan for the change. - - Sections: - - **User-Facing Docs**: Updates under `docs/usage/`, `docs/capabilities/`, `docs/platforms/`, or README files. - - **Contributor Docs**: Updates under `docs/development/`. - - **Coding-Agent Docs**: Updates under `docs/coding-agents/` or `AGENTS.md`. - - **Doc Validation**: Commands needed for changed docs, such as `doclinks`, `md-babel-py run `, and `bin/gen-diagrams`. - - **No Docs Needed**: If no docs are needed, explain why. - - Match `docs/development/writing_docs.md`: contributor-only docs belong in `docs/development`; user-facing behavior belongs in `docs/usage` or `docs/capabilities`. - requires: - - proposal - - id: tasks - generates: tasks.md - description: Implementation, validation, docs, and manual-QA checklist - template: tasks.md - instruction: | - Create the implementation checklist. The apply phase parses checkbox format, so every actionable task MUST use `- [ ]`. - - Guidelines: - - Group tasks under numbered `##` headings. - - Each task must be `- [ ] X.Y Task description`. - - Keep tasks small enough to complete in one focused session. - - Order tasks by dependency. - - Include docs and validation tasks from docs.md. - - Include generated registry tasks when blueprints or module registry inputs change. - - Include manual QA through the actual user surface: CLI, TUI, HTTP API, MCP tool, simulation/replay blueprint, hardware procedure, or library driver. - - Typical DimOS validation tasks: - - Run `openspec validate `. - - Run focused pytest targets for changed modules. - - Run `pytest dimos/robot/test_all_blueprints_generation.py` when blueprint registry output may change. - - Run docs validation commands for changed docs. - - Run lints/types when the touched area requires them. - - Reference specs for WHAT, design for HOW, and docs.md for documentation work. - requires: - - specs - - design - - docs -apply: - requires: - - tasks - tracks: tasks.md - instruction: | - Read proposal.md, specs, design.md, docs.md, and tasks.md before editing code. - Work through pending tasks, mark checkboxes complete as they finish, and keep artifacts current when implementation changes the plan. - Verify with OpenSpec validation, focused tests, docs checks, and manual QA through the relevant DimOS surface. diff --git a/openspec/schemas/dimos-capability/templates/design.md b/openspec/schemas/dimos-capability/templates/design.md deleted file mode 100644 index 25031ceb8b..0000000000 --- a/openspec/schemas/dimos-capability/templates/design.md +++ /dev/null @@ -1,35 +0,0 @@ -## Context - - - -## Goals / Non-Goals - -**Goals:** - - -**Non-Goals:** - - -## DimOS Architecture - - - -## Decisions - - - -## Safety / Simulation / Replay - - - -## Risks / Trade-offs - - - -## Migration / Rollout - - - -## Open Questions - - diff --git a/openspec/schemas/dimos-capability/templates/docs.md b/openspec/schemas/dimos-capability/templates/docs.md deleted file mode 100644 index d274aed653..0000000000 --- a/openspec/schemas/dimos-capability/templates/docs.md +++ /dev/null @@ -1,19 +0,0 @@ -## User-Facing Docs - - - -## Contributor Docs - - - -## Coding-Agent Docs - - - -## Doc Validation - - - -## No Docs Needed - - diff --git a/openspec/schemas/dimos-capability/templates/proposal.md b/openspec/schemas/dimos-capability/templates/proposal.md deleted file mode 100644 index 98d409e8de..0000000000 --- a/openspec/schemas/dimos-capability/templates/proposal.md +++ /dev/null @@ -1,32 +0,0 @@ -## Why - - - -## What Changes - - - -## Affected DimOS Surfaces - - -- Modules/streams: -- Blueprints/CLI: -- Skills/MCP: -- Hardware/simulation/replay: -- Docs/generated registries: - -## Capabilities - -### New Capabilities - -- ``: - -### Modified Capabilities - -- ``: - -## Impact - - diff --git a/openspec/schemas/dimos-capability/templates/spec.md b/openspec/schemas/dimos-capability/templates/spec.md deleted file mode 100644 index afc0c1ff58..0000000000 --- a/openspec/schemas/dimos-capability/templates/spec.md +++ /dev/null @@ -1,16 +0,0 @@ -## ADDED Requirements - -### Requirement: - - -#### Scenario: -- **GIVEN** -- **WHEN** -- **THEN** -- **AND** - - diff --git a/openspec/schemas/dimos-capability/templates/tasks.md b/openspec/schemas/dimos-capability/templates/tasks.md deleted file mode 100644 index b38fcdfabb..0000000000 --- a/openspec/schemas/dimos-capability/templates/tasks.md +++ /dev/null @@ -1,15 +0,0 @@ -## 1. Implementation - -- [ ] 1.1 -- [ ] 1.2 - -## 2. Documentation - -- [ ] 2.1 - -## 3. Verification - -- [ ] 3.1 Run `openspec validate ` -- [ ] 3.2 Run focused tests for changed code -- [ ] 3.3 Run docs validation commands for changed docs -- [ ] 3.4 Manually QA through the relevant DimOS surface (CLI, MCP, simulation/replay, hardware procedure, HTTP API, or library driver) From bd09967caa9ceefa74849622fd44951c9cfafabf Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 30 Jul 2026 10:01:47 -0700 Subject: [PATCH 08/14] refactor(manipulation): simplify trajectory validation --- dimos/manipulation/planning/README.md | 5 +- .../trajectory_generator/parametrizer.py | 148 ++---------------- .../roboplan_toppra_parametrizer.py | 60 ++----- .../simple_parametrizer.py | 10 +- .../trajectory_generator/test_parametrizer.py | 85 +++++----- .../test_roboplan_toppra_parametrizer.py | 11 +- 6 files changed, 70 insertions(+), 249 deletions(-) diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index 6cf9e83910..ae32e8a1c0 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -94,8 +94,9 @@ A joint-space planner normally returns an untimed geometric path. Before DimOS accepts a `GeneratedPlan`, the one trajectory-parametrization backend selected at startup converts that path into a timed `JointTrajectory`. DimOS then validates joint ordering, dimensions, finite values, strictly increasing time, -start and goal preservation, and applicable velocity and acceleration limits. -A failure leaves no executable plan cached. +and start and goal preservation. Each backend is responsible for generating +motion within the velocity and acceleration limits it receives. A failure +leaves no executable plan cached. This boundary is exposed internally as `TrajectoryParametrizerSpec`, alongside `PlannerSpec` and `WorldSpec`. Its implementations own conversion, validation, diff --git a/dimos/manipulation/planning/trajectory_generator/parametrizer.py b/dimos/manipulation/planning/trajectory_generator/parametrizer.py index 1f255f24ce..3497b29e94 100644 --- a/dimos/manipulation/planning/trajectory_generator/parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/parametrizer.py @@ -16,7 +16,6 @@ from abc import ABC, abstractmethod from collections.abc import Sequence -from dataclasses import dataclass import math from dimos.manipulation.planning.groups.models import PlanningGroupSelection @@ -27,24 +26,12 @@ from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint _TRAJECTORY_POSITION_TOLERANCE = 1e-6 -_TRAJECTORY_LIMIT_RELATIVE_TOLERANCE = 1e-2 -_TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE = 1e-8 class TrajectoryParametrizationError(ValueError): """Planning output could not be converted into a valid generated plan.""" -@dataclass(frozen=True) -class ParametrizedTrajectory: - """Backend output plus the limits and accelerations used to validate it.""" - - trajectory: JointTrajectory - velocity_limits: tuple[float, ...] - acceleration_limits: tuple[float, ...] - accelerations: tuple[tuple[float, ...], ...] | None = None - - class BaseTrajectoryParametrizer(ABC): """Own common PlanningResult-to-GeneratedPlan materialization.""" @@ -63,25 +50,17 @@ def materialize_plan( ) path = [JointState(state) for state in result.path] - waypoints = self._validate_selected_path(path, selection.joint_names) + self._validate_selected_path(path, selection.joint_names) if result.timestamps is None: - parametrized = self._parametrize_path(world, selection, tuple(path), speed_scale) - trajectory = parametrized.trajectory - self._validate_generated_trajectory( - trajectory, - selection.joint_names, - waypoints, - velocity_limits=parametrized.velocity_limits, - acceleration_limits=parametrized.acceleration_limits, - accelerations=parametrized.accelerations, - ) + trajectory = self._parametrize_path(world, selection, tuple(path), speed_scale) else: trajectory = self._timed_trajectory(selection, path, result.timestamps) - self._validate_generated_trajectory( - trajectory, - selection.joint_names, - waypoints, - ) + self._validate_trajectory( + trajectory, + selection.joint_names, + expected_start=path[0].position, + expected_goal=path[-1].position, + ) return GeneratedPlan( group_ids=selection.group_ids, @@ -101,7 +80,7 @@ def _parametrize_path( selection: PlanningGroupSelection, path: tuple[JointState, ...], speed_scale: float, - ) -> ParametrizedTrajectory: + ) -> JointTrajectory: """Convert one validated untimed path using the selected backend.""" @staticmethod @@ -120,11 +99,10 @@ def _validate_selected_path( cls, path: Sequence[JointState], expected_names: Sequence[str], - ) -> list[list[float]]: + ) -> None: if len(path) < 2: raise TrajectoryParametrizationError("Planner returned fewer than two waypoints") expected = list(expected_names) - waypoints: list[list[float]] = [] for waypoint_index, state in enumerate(path): if list(state.name) != expected: raise TrajectoryParametrizationError( @@ -139,8 +117,6 @@ def _validate_selected_path( positions, f"Waypoint {waypoint_index} positions", ) - waypoints.append(positions) - return waypoints @classmethod def _timed_trajectory( @@ -171,15 +147,13 @@ def _timed_trajectory( ) @classmethod - def _validate_generated_trajectory( + def _validate_trajectory( cls, trajectory: JointTrajectory, expected_names: Sequence[str], - waypoints: Sequence[Sequence[float]], *, - velocity_limits: Sequence[float] | None = None, - acceleration_limits: Sequence[float] | None = None, - accelerations: Sequence[Sequence[float]] | None = None, + expected_start: Sequence[float], + expected_goal: Sequence[float], ) -> None: expected = list(expected_names) if list(trajectory.joint_names) != expected: @@ -213,24 +187,14 @@ def _validate_generated_trajectory( "Generated trajectory times must be strictly increasing" ) previous_time = point.time_from_start - non_noop = any(list(waypoint) != list(waypoints[0]) for waypoint in waypoints[1:]) - if non_noop and trajectory.duration <= 0.0: - raise TrajectoryParametrizationError("Generated trajectory duration must be positive") - if not cls._positions_close(trajectory.points[0].positions, waypoints[0]): + if not cls._positions_close(trajectory.points[0].positions, expected_start): raise TrajectoryParametrizationError( "Generated trajectory does not preserve the path start" ) - if not cls._positions_close(trajectory.points[-1].positions, waypoints[-1]): + if not cls._positions_close(trajectory.points[-1].positions, expected_goal): raise TrajectoryParametrizationError( "Generated trajectory does not preserve the path goal" ) - if velocity_limits is not None: - cls._validate_motion_limits( - trajectory, - velocity_limits, - acceleration_limits, - accelerations, - ) @staticmethod def _positions_close(first: Sequence[float], second: Sequence[float]) -> bool: @@ -243,85 +207,3 @@ def _positions_close(first: Sequence[float], second: Sequence[float]) -> bool: ) for left, right in zip(first, second, strict=True) ) - - @classmethod - def _validate_motion_limits( - cls, - trajectory: JointTrajectory, - velocity_limits: Sequence[float], - acceleration_limits: Sequence[float] | None, - accelerations: Sequence[Sequence[float]] | None, - ) -> None: - expected_dimension = len(trajectory.joint_names) - if len(velocity_limits) != expected_dimension: - raise TrajectoryParametrizationError("Velocity limits do not match selected joints") - cls._assert_valid_motion_limits(velocity_limits, "velocity") - for point_index, point in enumerate(trajectory.points): - cls._assert_within_limits( - point.velocities, - velocity_limits, - f"Generated point {point_index} velocity", - ) - if acceleration_limits is None: - return - if len(acceleration_limits) != expected_dimension: - raise TrajectoryParametrizationError("Acceleration limits do not match selected joints") - cls._assert_valid_motion_limits(acceleration_limits, "acceleration") - if accelerations is not None: - if len(accelerations) != len(trajectory.points): - raise TrajectoryParametrizationError( - "Acceleration samples do not match trajectory points" - ) - for point_index, values in enumerate(accelerations): - if len(values) != expected_dimension: - raise TrajectoryParametrizationError( - f"Generated point {point_index} acceleration dimension mismatch" - ) - cls._assert_finite_sequence( - values, - f"Generated point {point_index} accelerations", - ) - cls._assert_within_limits( - values, - acceleration_limits, - f"Generated point {point_index} acceleration", - ) - return - for point_index in range(1, len(trajectory.points)): - previous = trajectory.points[point_index - 1] - current = trajectory.points[point_index] - dt = current.time_from_start - previous.time_from_start - derived = [ - (current_velocity - previous_velocity) / dt - for previous_velocity, current_velocity in zip( - previous.velocities, - current.velocities, - strict=True, - ) - ] - cls._assert_within_limits( - derived, - acceleration_limits, - f"Generated interval {point_index - 1}:{point_index} acceleration", - ) - - @staticmethod - def _assert_valid_motion_limits(values: Sequence[float], label: str) -> None: - if any(not math.isfinite(value) or value <= 0.0 for value in values): - raise TrajectoryParametrizationError(f"Invalid {label} limits") - - @staticmethod - def _assert_within_limits( - values: Sequence[float], - limits: Sequence[float], - label: str, - ) -> None: - for joint_index, (value, limit) in enumerate(zip(values, limits, strict=True)): - tolerance = max( - _TRAJECTORY_LIMIT_ABSOLUTE_TOLERANCE, - limit * _TRAJECTORY_LIMIT_RELATIVE_TOLERANCE, - ) - if abs(value) > limit + tolerance: - raise TrajectoryParametrizationError( - f"{label} exceeds joint {joint_index} limit: {value} vs {limit}" - ) diff --git a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py index 560d88731b..4004d80ceb 100644 --- a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py @@ -30,7 +30,6 @@ ) from dimos.manipulation.planning.trajectory_generator.parametrizer import ( BaseTrajectoryParametrizer, - ParametrizedTrajectory, TrajectoryParametrizationError, ) from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel @@ -44,8 +43,6 @@ class _GroupParametrizer: group: RoboPlanGroup native: Any - velocity_limits: tuple[float, ...] - acceleration_limits: tuple[float, ...] class RoboPlanTOPPRAParametrizer(BaseTrajectoryParametrizer): @@ -64,7 +61,7 @@ def _parametrize_path( selection: PlanningGroupSelection, path: tuple[JointState, ...], speed_scale: float, - ) -> ParametrizedTrajectory: + ) -> JointTrajectory: if not isinstance(world, RoboPlanWorld): raise TrajectoryParametrizationError("RoboPlan TOPP-RA requires RoboPlanWorld") try: @@ -77,7 +74,6 @@ def _parametrize_path( return self._canonical_result( resolved, selection, - speed_scale, native_trajectory, ) except TrajectoryParametrizationError: @@ -106,12 +102,12 @@ def _resolve_group( raise TrajectoryParametrizationError( f"RoboPlan group '{group.name}' does not match selected joints" ) - velocity_limits = self._limits( + self._validate_limits( model.scene.getVelocityLimitVectors(group.name), group, "velocity", ) - acceleration_limits = self._limits( + self._validate_limits( model.scene.getAccelerationLimitVectors(group.name), group, "acceleration", @@ -119,27 +115,22 @@ def _resolve_group( resolved = _GroupParametrizer( group=group, native=roboplan_toppra.PathParameterizerTOPPRA(model.scene, group.name), - velocity_limits=tuple(value * self._config.velocity_scale for value in velocity_limits), - acceleration_limits=tuple( - value * self._config.acceleration_scale for value in acceleration_limits - ), ) self._groups[key] = resolved return resolved @staticmethod - def _limits( + def _validate_limits( bounds: tuple[Any, Any], group: RoboPlanGroup, label: str, - ) -> tuple[float, ...]: + ) -> None: lower = np.asarray(bounds[0], dtype=np.float64) upper = np.asarray(bounds[1], dtype=np.float64) if lower.shape != upper.shape or len(lower) != len(group.native_names): raise TrajectoryParametrizationError( f"RoboPlan {label} limits do not match group '{group.name}'" ) - by_public: dict[str, float] = {} for public_name, low, high in zip(group.public_names, lower, upper, strict=True): magnitude = min(abs(float(low)), abs(float(high))) if not math.isfinite(magnitude) or magnitude <= 0.0 or magnitude >= sys.float_info.max: @@ -147,8 +138,6 @@ def _limits( f"RoboPlan group '{group.name}' has no usable URDF {label} " f"limit for joint '{public_name}'" ) - by_public[public_name] = magnitude - return tuple(by_public[name] for name in group.public_names) @staticmethod def _native_path( @@ -188,9 +177,8 @@ def _options(self, speed_scale: float) -> Any: def _canonical_result( resolved: _GroupParametrizer, selection: PlanningGroupSelection, - speed_scale: float, native_trajectory: Any, - ) -> ParametrizedTrajectory: + ) -> JointTrajectory: native_names = tuple(native_trajectory.joint_names) if set(native_names) != set(resolved.group.native_names): raise TrajectoryParametrizationError("RoboPlan TOPP-RA returned unexpected joint names") @@ -206,8 +194,7 @@ def _canonical_result( times = [float(value) for value in native_trajectory.times] positions = list(native_trajectory.positions) velocities = list(native_trajectory.velocities) - accelerations = list(native_trajectory.accelerations) - if not (len(times) == len(positions) == len(velocities) == len(accelerations)): + if not (len(times) == len(positions) == len(velocities)): raise TrajectoryParametrizationError( "RoboPlan TOPP-RA returned inconsistent trajectory fields" ) @@ -219,34 +206,7 @@ def _canonical_result( ) for time, position, velocity in zip(times, positions, velocities, strict=True) ] - canonical_accelerations = tuple( - tuple(float(acceleration[index]) for index in output_indices) - for acceleration in accelerations - ) - velocity_by_public = dict( - zip( - resolved.group.public_names, - resolved.velocity_limits, - strict=True, - ) - ) - acceleration_by_public = dict( - zip( - resolved.group.public_names, - resolved.acceleration_limits, - strict=True, - ) - ) - return ParametrizedTrajectory( - trajectory=JointTrajectory( - joint_names=list(selection.joint_names), - points=points, - ), - velocity_limits=tuple( - velocity_by_public[name] * speed_scale for name in selection.joint_names - ), - acceleration_limits=tuple( - acceleration_by_public[name] * speed_scale for name in selection.joint_names - ), - accelerations=canonical_accelerations, + return JointTrajectory( + joint_names=list(selection.joint_names), + points=points, ) diff --git a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py index d1ec1c9e85..f939029693 100644 --- a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py @@ -26,7 +26,6 @@ ) from dimos.manipulation.planning.trajectory_generator.parametrizer import ( BaseTrajectoryParametrizer, - ParametrizedTrajectory, TrajectoryParametrizationError, ) from dimos.msgs.sensor_msgs.JointState import JointState @@ -45,7 +44,7 @@ def _parametrize_path( selection: PlanningGroupSelection, path: tuple[JointState, ...], speed_scale: float, - ) -> ParametrizedTrajectory: + ) -> JointTrajectory: request_velocity_limits, request_acceleration_limits = self._selected_limits( world, selection, @@ -69,16 +68,11 @@ def _parametrize_path( raise TrajectoryParametrizationError( f"Simple trapezoid parametrization failed: {exc}" ) from exc - trajectory = JointTrajectory( + return JointTrajectory( joint_names=list(selection.joint_names), points=generated.points, timestamp=generated.timestamp, ) - return ParametrizedTrajectory( - trajectory=trajectory, - velocity_limits=velocity_limits, - acceleration_limits=acceleration_limits, - ) @staticmethod def _selected_limits( diff --git a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py index 07e75d3655..509fe05820 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py @@ -27,7 +27,6 @@ from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.trajectory_generator.parametrizer import ( BaseTrajectoryParametrizer, - ParametrizedTrajectory, TrajectoryParametrizationError, ) from dimos.msgs.sensor_msgs.JointState import JointState @@ -36,7 +35,7 @@ class _FixedParametrizer(BaseTrajectoryParametrizer): - def __init__(self, output: ParametrizedTrajectory) -> None: + def __init__(self, output: JointTrajectory) -> None: self.output = output self.calls: list[float] = [] @@ -46,7 +45,7 @@ def _parametrize_path( selection: PlanningGroupSelection, path: tuple[JointState, ...], speed_scale: float, - ) -> ParametrizedTrajectory: + ) -> JointTrajectory: self.calls.append(speed_scale) return self.output @@ -75,37 +74,25 @@ def _path() -> list[JointState]: ] -def _output( - *, - velocities: tuple[list[float], list[float]] = ([0.0, 0.0], [0.0, 0.0]), - accelerations: tuple[tuple[float, float], tuple[float, float]] = ( - (0.0, 0.0), - (0.0, 0.0), - ), -) -> ParametrizedTrajectory: - return ParametrizedTrajectory( - trajectory=JointTrajectory( - joint_names=["arm/a", "arm/b"], - points=[ - TrajectoryPoint( - time_from_start=0.0, - positions=[0.0, 0.0], - velocities=velocities[0], - ), - TrajectoryPoint( - time_from_start=0.5, - positions=[0.4, 0.0], - velocities=velocities[1], - ), - ], - ), - velocity_limits=(1.0, 1.0), - acceleration_limits=(2.0, 2.0), - accelerations=accelerations, +def _output() -> JointTrajectory: + return JointTrajectory( + joint_names=["arm/a", "arm/b"], + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0, 0.0], + velocities=[0.0, 0.0], + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[0.4, 0.0], + velocities=[0.0, 0.0], + ), + ], ) -def test_materializes_bounded_fitting_and_preserves_source_path() -> None: +def test_materializes_trajectory_and_preserves_source_path() -> None: parametrizer = _FixedParametrizer(_output()) source_path = _path() @@ -127,7 +114,7 @@ def test_materializes_bounded_fitting_and_preserves_source_path() -> None: [0.4, 0.0], ] assert plan.path is not source_path - assert plan.trajectory is parametrizer.output.trajectory + assert plan.trajectory is parametrizer.output assert plan.planning_time == 0.2 assert plan.iterations == 12 assert parametrizer.calls == [0.4] @@ -166,21 +153,25 @@ def test_timed_planner_result_bypasses_backend_path_conversion() -> None: assert plan.trajectory.points[-1].velocities == [0.3, 0.0] -@pytest.mark.parametrize( - ("velocities", "accelerations", "message"), - [ - (([0.0, 0.0], [1.1, 0.0]), ((0.0, 0.0), (0.0, 0.0)), "velocity exceeds"), - (([0.0, 0.0], [0.0, 0.0]), ((0.0, 0.0), (2.1, 0.0)), "acceleration exceeds"), - ], -) -def test_rejects_parametrized_motion_limit_violations( - velocities: tuple[list[float], list[float]], - accelerations: tuple[tuple[float, float], tuple[float, float]], - message: str, -) -> None: - parametrizer = _FixedParametrizer(_output(velocities=velocities, accelerations=accelerations)) - - with pytest.raises(TrajectoryParametrizationError, match=message): +def test_rejects_backend_trajectory_with_nonincreasing_time() -> None: + output = _output() + output.points[-1].time_from_start = 0.0 + parametrizer = _FixedParametrizer(output) + + with pytest.raises(TrajectoryParametrizationError, match="strictly increasing"): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult(status=PlanningStatus.SUCCESS, path=_path()), + ) + + +def test_rejects_backend_trajectory_that_changes_path_goal() -> None: + output = _output() + output.points[-1].positions = [0.3, 0.0] + parametrizer = _FixedParametrizer(output) + + with pytest.raises(TrajectoryParametrizationError, match="path goal"): parametrizer.materialize_plan( MagicMock(spec=WorldSpec), _selection(), diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py index 4e9adfaf9b..423f903f50 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -141,7 +141,6 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( times=[0.0, 0.5], positions=[np.asarray([0.0, 0.1]), np.asarray([0.3, 0.4])], velocities=[np.asarray([0.0, 0.0]), np.asarray([0.2, 0.2])], - accelerations=[np.asarray([0.0, 0.0]), np.asarray([0.4, 0.4])], ) native = mocker.MagicMock() native.generate.return_value = generated @@ -220,7 +219,7 @@ def test_roboplan_parametrizer_rejects_missing_urdf_acceleration_without_fallbac constructor.assert_not_called() -def test_cached_group_limits_follow_each_request_joint_order( +def test_cached_group_preserves_each_request_joint_order( mocker: MockerFixture, ) -> None: generated = SimpleNamespace( @@ -228,7 +227,6 @@ def test_cached_group_limits_follow_each_request_joint_order( times=[0.0, 0.5], positions=[np.asarray([0.1, 0.0]), np.asarray([0.4, 0.3])], velocities=[np.asarray([0.0, 0.0]), np.asarray([0.2, 0.4])], - accelerations=[np.asarray([0.0, 0.0]), np.asarray([0.4, 0.8])], ) native = mocker.MagicMock() native.generate.return_value = generated @@ -237,12 +235,7 @@ def test_cached_group_limits_follow_each_request_joint_order( "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", return_value=native, ) - parametrizer = RoboPlanTOPPRAParametrizer( - RoboPlanTOPPRAParametrizationConfig( - velocity_scale=0.5, - acceleration_scale=0.25, - ), - ) + parametrizer = RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()) world = _World(_model()) canonical_selection, canonical_result = _selection_and_result() reversed_selection, reversed_result = _selection_and_result(("right/b", "left/a")) From 548dfe8ea3fba36d95d6c0797804e89b79a9c53a Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 30 Jul 2026 11:38:54 -0700 Subject: [PATCH 09/14] test(manipulation): focus trajectory coverage on behavior --- .../trajectory_generator/test_config.py | 62 ------------- .../trajectory_generator/test_parametrizer.py | 51 ++++++++++- .../test_roboplan_toppra_contract.py | 15 +--- .../test_roboplan_toppra_parametrizer.py | 13 +-- .../test_simple_parametrizer.py | 24 ++++-- .../test_generated_plan_materialization.py | 86 ------------------- dimos/manipulation/test_manipulation_unit.py | 15 ++-- dimos/manipulation/test_planning_factory.py | 38 +------- .../visualization/test_operator.py | 17 ---- .../viser/test_viser_visualization.py | 47 +++++----- 10 files changed, 103 insertions(+), 265 deletions(-) delete mode 100644 dimos/manipulation/planning/trajectory_generator/test_config.py diff --git a/dimos/manipulation/planning/trajectory_generator/test_config.py b/dimos/manipulation/planning/trajectory_generator/test_config.py deleted file mode 100644 index 0a583f81f6..0000000000 --- a/dimos/manipulation/planning/trajectory_generator/test_config.py +++ /dev/null @@ -1,62 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Tests for trajectory-parametrization startup configuration.""" - -from pydantic import TypeAdapter, ValidationError -import pytest - -from dimos.manipulation.planning.trajectory_generator.config import ( - RoboPlanTOPPRAParametrizationConfig, - SimpleTrapezoidParametrizationConfig, - TrajectoryParametrizationConfig, -) - - -@pytest.mark.parametrize( - ("payload", "expected_type"), - [ - ({"backend": "simple_trapezoid"}, SimpleTrapezoidParametrizationConfig), - ({"backend": "roboplan_toppra"}, RoboPlanTOPPRAParametrizationConfig), - ], -) -def test_trajectory_parametrization_config_selects_one_backend( - payload: dict[str, str], - expected_type: type[object], -) -> None: - result = TypeAdapter(TrajectoryParametrizationConfig).validate_python(payload) - - assert isinstance(result, expected_type) - - -@pytest.mark.parametrize( - "payload", - [ - {"backend": "unknown"}, - {"backend": "simple_trapezoid", "velocity_scale": 0.0}, - {"backend": "simple_trapezoid", "acceleration_scale": 1.01}, - {"backend": "simple_trapezoid", "points_per_segment": 0}, - {"backend": "roboplan_toppra", "output_period": 0.0}, - {"backend": "roboplan_toppra", "velocity_scale": 1.01}, - {"backend": "roboplan_toppra", "acceleration_scale": -0.1}, - {"backend": "roboplan_toppra", "max_adaptive_iterations": 0}, - {"backend": "roboplan_toppra", "max_adaptive_step_size": 0.0}, - {"backend": "roboplan_toppra", "max_blend_deviation": -0.1}, - ], -) -def test_trajectory_parametrization_config_rejects_invalid_options( - payload: dict[str, object], -) -> None: - with pytest.raises(ValidationError): - TypeAdapter(TrajectoryParametrizationConfig).validate_python(payload) diff --git a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py index 509fe05820..8d49c58eec 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py @@ -153,6 +153,22 @@ def test_timed_planner_result_bypasses_backend_path_conversion() -> None: assert plan.trajectory.points[-1].velocities == [0.3, 0.0] +def test_timed_planner_result_requires_velocity_for_each_joint() -> None: + parametrizer = _FixedParametrizer(_output()) + path = _path() + + with pytest.raises(TrajectoryParametrizationError, match="velocity dimension"): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + timestamps=[0.0, 0.5, 1.0], + ), + ) + + def test_rejects_backend_trajectory_with_nonincreasing_time() -> None: output = _output() output.points[-1].time_from_start = 0.0 @@ -179,12 +195,39 @@ def test_rejects_backend_trajectory_that_changes_path_goal() -> None: ) -def test_rejects_malformed_path_before_invoking_backend() -> None: +@pytest.mark.parametrize( + ("path", "message"), + [ + ( + [ + JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), + JointState(name=["wrong/a", "wrong/b"], position=[0.4, 0.0]), + ], + "joint names", + ), + ( + [ + JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), + JointState(name=["arm/a", "arm/b"], position=[0.4]), + ], + "dimension", + ), + ( + [ + JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), + JointState(name=["arm/a", "arm/b"], position=[float("nan"), 0.0]), + ], + "non-finite", + ), + ], +) +def test_rejects_malformed_path_before_invoking_backend( + path: list[JointState], + message: str, +) -> None: parametrizer = _FixedParametrizer(_output()) - path = _path() - path[1].name = ["wrong/a", "wrong/b"] - with pytest.raises(TrajectoryParametrizationError, match="joint names"): + with pytest.raises(TrajectoryParametrizationError, match=message): parametrizer.materialize_plan( MagicMock(spec=WorldSpec), _selection(), diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py index 5f918557bf..36d6725163 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py @@ -56,7 +56,7 @@ def _scene(tmp_path: Path, *, acceleration: float | None) -> object: return roboplan_core.Scene("contract_robot", urdf, srdf, []) -def test_roboplan_051_toppra_options_and_native_trajectory_contract(tmp_path: Path) -> None: +def test_roboplan_051_generates_native_trajectory(tmp_path: Path) -> None: scene = _scene(tmp_path, acceleration=2.0) options = roboplan_toppra.TOPPRAOptions( dt=0.02, @@ -77,19 +77,6 @@ def test_roboplan_051_toppra_options_and_native_trajectory_contract(tmp_path: Pa trajectory = roboplan_toppra.PathParameterizerTOPPRA(scene, "arm").generate(path, options) - assert list(roboplan_toppra.SplineFittingMode) == [ - roboplan_toppra.SplineFittingMode.Hermite, - roboplan_toppra.SplineFittingMode.Cubic, - roboplan_toppra.SplineFittingMode.Adaptive, - roboplan_toppra.SplineFittingMode.LinearBlend, - ] - assert options.dt == 0.02 - assert options.mode is roboplan_toppra.SplineFittingMode.LinearBlend - assert options.velocity_scale == 0.5 - assert options.acceleration_scale == 0.25 - assert options.max_adaptive_iterations == 7 - assert options.max_adaptive_step_size == 0.03 - assert options.max_blend_deviation == 0.01 assert trajectory.joint_names == ["joint"] assert len(trajectory.times) == len(trajectory.positions) assert len(trajectory.velocities) == len(trajectory.positions) diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py index 423f903f50..cfd9962e1d 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -29,7 +29,6 @@ ) from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import PlanningResult -from dimos.manipulation.planning.spec.protocols import TrajectoryParametrizerSpec from dimos.manipulation.planning.trajectory_generator.config import ( RoboPlanTOPPRAParametrizationConfig, ) @@ -128,13 +127,8 @@ def _selection_and_result( return selection, result -@pytest.mark.parametrize( - "fitting_mode", - ["hermite", "cubic", "adaptive", "linear_blend"], -) -def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( +def test_roboplan_parametrizer_maps_composite_order_and_native_output( mocker: MockerFixture, - fitting_mode: str, ) -> None: generated = SimpleNamespace( joint_names=["native_a", "native_b"], @@ -151,8 +145,6 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( ) parametrizer = RoboPlanTOPPRAParametrizer( RoboPlanTOPPRAParametrizationConfig( - fitting_mode=fitting_mode, - output_period=0.02, velocity_scale=0.5, acceleration_scale=0.25, ), @@ -167,7 +159,6 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( speed_scale=0.5, ) - assert isinstance(parametrizer, TrajectoryParametrizerSpec) constructor.assert_called_once() native_path, options = native.generate.call_args.args assert native_path.joint_names == ["native_b", "native_a"] @@ -175,8 +166,6 @@ def test_roboplan_parametrizer_maps_composite_order_options_and_native_output( [0.1, 0.0], [0.4, 0.3], ] - assert options.dt == 0.02 - assert options.mode.name.lower().replace("linearblend", "linear_blend") == fitting_mode assert options.velocity_scale == 0.25 assert options.acceleration_scale == 0.125 assert result.trajectory.joint_names == ["left/a", "right/b"] diff --git a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py index 2e509552ba..37c353610b 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py @@ -18,6 +18,7 @@ from unittest.mock import MagicMock import pytest +from pytest_mock import MockerFixture from dimos.manipulation.planning.groups.models import ( PlanningGroup, @@ -26,10 +27,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import PlanningResult -from dimos.manipulation.planning.spec.protocols import ( - TrajectoryParametrizerSpec, - WorldSpec, -) +from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.trajectory_generator.config import ( SimpleTrapezoidParametrizationConfig, ) @@ -104,7 +102,6 @@ def test_simple_parametrizer_materializes_segmented_trapezoid_plan() -> None: speed_scale=0.5, ) - assert isinstance(parametrizer, TrajectoryParametrizerSpec) assert plan.group_ids == ("arm/manipulator",) assert plan.trajectory.joint_names == ["arm/a", "arm/b"] assert len(plan.trajectory.points) == 9 @@ -132,9 +129,24 @@ def test_simple_parametrizer_rejects_invalid_dimos_limits() -> None: ) +def test_simple_parametrizer_reports_generator_failure(mocker: MockerFixture) -> None: + generator = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "simple_parametrizer.JointTrajectoryGenerator" + ) + generator.return_value.generate.side_effect = RuntimeError("boom") + + with pytest.raises(TrajectoryParametrizationError, match="failed: boom"): + SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()).materialize_plan( + _world(), + _selection(), + _result(), + ) + + @pytest.mark.parametrize( "speed_scale", - [0.0, -0.1, 1.01, float("inf"), float("nan")], + [0.0, 1.01, float("nan")], ) def test_parametrizer_rejects_invalid_runtime_speed(speed_scale: float) -> None: with pytest.raises(TrajectoryParametrizationError, match="speed_scale"): diff --git a/dimos/manipulation/test_generated_plan_materialization.py b/dimos/manipulation/test_generated_plan_materialization.py index 89a9ee5e97..1e337bc16f 100644 --- a/dimos/manipulation/test_generated_plan_materialization.py +++ b/dimos/manipulation/test_generated_plan_materialization.py @@ -44,7 +44,6 @@ class RecordingGenerator: calls: list[list[list[float]]] = [] limits: tuple[list[float], list[float]] | None = None - fail = False def __init__( self, @@ -59,8 +58,6 @@ def __init__( def generate(self, waypoints: list[list[float]]) -> JointTrajectory: RecordingGenerator.calls.append(waypoints) - if RecordingGenerator.fail: - raise RuntimeError("boom") return JointTrajectory( points=[ TrajectoryPoint( @@ -93,7 +90,6 @@ def _robot(name: str, joints: list[str], velocity: float, acceleration: float) - def _module(monkeypatch: pytest.MonkeyPatch, module_factory): RecordingGenerator.calls = [] RecordingGenerator.limits = None - RecordingGenerator.fail = False monkeypatch.setattr( "dimos.manipulation.planning.trajectory_generator." "simple_parametrizer.JointTrajectoryGenerator", @@ -187,88 +183,6 @@ def test_cartesian_plan_preserves_planner_timestamps_and_velocities(monkeypatch, assert request["auxiliary_groups"] == () -@pytest.mark.parametrize( - ("timestamps", "velocities", "message"), - [ - ([0.0, 0.0], [[0.0, 0.0], [0.1, 0.1]], "strictly increasing"), - ([0.0, 0.1], [[], [0.1, 0.1]], "velocity dimension"), - ], -) -def test_cartesian_plan_rejects_malformed_timed_results( - monkeypatch, module_factory, timestamps, velocities, message -): - module = _module(monkeypatch, module_factory) - module._state = ManipulationState.IDLE - names = ["left/b", "left/a"] - start = JointState(name=names, position=[0.0, 0.0]) - path = [ - JointState(name=names, position=[0.0, 0.0], velocity=velocities[0]), - JointState(name=names, position=[0.2, 0.1], velocity=velocities[1]), - ] - module._world_monitor.current_global_joint_state.return_value = start - module._planner.plan_cartesian_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, - path=path, - timestamps=timestamps, - ) - - result = module.generate_cartesian_plan( - { - "left/group": ( - Transform.identity(), - Transform(translation=Vector3(0.01, 0.0, 0.0)), - ) - }, - RoboPlanCartesianPathConfig(), - ) - - assert result is None - assert module._last_plan is None - assert message in module._error_message - - -@pytest.mark.parametrize( - ("path", "message"), - [ - (_path(["left/a", "left/b"], [0.0, 0.0], [1.0, 1.0]), "joint names"), - (_path(["left/b", "left/a"], [0.0], [1.0]), "dimension"), - (_path(["left/b", "left/a"], [0.0, float("nan")], [1.0, 1.0]), "non-finite"), - ], -) -def test_rejects_malformed_or_nonfinite_waypoints(monkeypatch, module_factory, path, message): - module = _module(monkeypatch, module_factory) - module._planner.plan_selected_joint_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, path=path - ) - - assert not module._plan_selected_path(("left/group",), path[0], path[-1], 1) - assert module._last_plan is None - assert message in module._error_message - - -def test_rejects_invalid_limits_and_generator_failure_without_caching(monkeypatch, module_factory): - module = _module(monkeypatch, module_factory) - module._robots["left"][1].max_velocity = 0.0 - names = ["left/b", "left/a"] - path = _path(names, [0.0, 0.0], [1.0, 1.0]) - module._planner.plan_selected_joint_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, path=path - ) - - assert not module._plan_selected_path(("left/group",), path[0], path[-1], 1) - assert module._last_plan is None - assert RecordingGenerator.calls == [] - - module = _module(monkeypatch, module_factory) - RecordingGenerator.fail = True - module._planner.plan_selected_joint_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, path=path - ) - assert not module._plan_selected_path(("left/group",), path[0], path[-1], 1) - assert module._last_plan is None - assert len(RecordingGenerator.calls) == 1 - - def test_zero_generation_after_caching_for_status_and_completion(monkeypatch, module_factory): module = _module(monkeypatch, module_factory) names = ["left/b", "left/a"] diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 5e29d15410..dab99dd2b4 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -17,7 +17,7 @@ from __future__ import annotations from pathlib import Path -from unittest.mock import MagicMock +from unittest.mock import ANY, MagicMock import pytest from pytest_mock import MockerFixture @@ -359,8 +359,7 @@ def test_fail_sets_fault_state(self, module_factory): assert module._state == ManipulationState.FAULT assert module._error_message == "Test error" - @pytest.mark.parametrize("invalid", [0.0, -0.1, 1.01, float("inf"), float("nan")]) - def test_motion_speed_applies_to_future_plans_only(self, module_factory, invalid: float): + def test_motion_speed_applies_to_future_plans_only(self, module_factory): module = module_factory() accepted = GeneratedPlan( trajectory=JointTrajectory(), @@ -373,9 +372,13 @@ def test_motion_speed_applies_to_future_plans_only(self, module_factory, invalid assert module.get_motion_speed() == pytest.approx(0.5) assert module._last_plan is accepted + @pytest.mark.parametrize("invalid", [0.0, 1.01, float("nan")]) + def test_motion_speed_rejects_invalid_values(self, module_factory, invalid: float): + module = module_factory() + assert module.set_motion_speed(0.5) is True + assert module.set_motion_speed(invalid) is False assert module.get_motion_speed() == pytest.approx(0.5) - assert module._last_plan is accepted assert "motion speed scale" in module.get_error() def test_begin_planning_state_checks(self, robot_config, module_factory): @@ -498,7 +501,7 @@ def test_kinematics_config_is_passed_to_factory( planner=module.config.planner, kinematics_name=None, kinematics=kinematics, - trajectory_parametrization=module.config.trajectory_parametrization, + trajectory_parametrization=ANY, ) def test_legacy_kinematics_name_still_selects_backend( @@ -522,7 +525,7 @@ def test_legacy_kinematics_name_still_selects_backend( planner=module.config.planner, kinematics_name="pink", kinematics=module.config.kinematics, - trajectory_parametrization=module.config.trajectory_parametrization, + trajectory_parametrization=ANY, ) def test_nested_kinematics_config_parses_cli_override_shape(self) -> None: diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index dd98457c22..570e9cc45c 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -21,6 +21,7 @@ import sys from types import ModuleType from typing import Any +from unittest.mock import ANY import pytest from pytest_mock import MockerFixture @@ -30,7 +31,6 @@ create_kinematics, create_planner, create_planning_stack, - create_trajectory_parametrizer, create_world, validate_backend_combination, ) @@ -46,17 +46,7 @@ ) from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.protocols import ( - PlannerSpec, - TrajectoryParametrizerSpec, -) -from dimos.manipulation.planning.trajectory_generator.config import ( - RoboPlanTOPPRAParametrizationConfig, - SimpleTrapezoidParametrizationConfig, -) -from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( - SimpleTrapezoidParametrizer, -) +from dimos.manipulation.planning.spec.protocols import PlannerSpec from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -147,27 +137,6 @@ def test_validate_backend_combination_rejects_invalid_combinations() -> None: ) -def test_create_trajectory_parametrizer_selects_simple_backend() -> None: - result = create_trajectory_parametrizer( - SimpleTrapezoidParametrizationConfig(), - world_backend="drake", - ) - - assert isinstance(result, SimpleTrapezoidParametrizer) - assert isinstance(result, TrajectoryParametrizerSpec) - - -def test_create_trajectory_parametrizer_rejects_toppra_with_non_roboplan_world() -> None: - with pytest.raises( - ValueError, - match='trajectory_parametrization.backend="roboplan_toppra" requires', - ): - create_trajectory_parametrizer( - RoboPlanTOPPRAParametrizationConfig(), - world_backend="drake", - ) - - def test_create_planner_uses_roboplan_world_as_native_planner(mocker: MockerFixture) -> None: world = mocker.MagicMock(spec=PlannerSpec) roboplan_world_module = ModuleType("dimos.manipulation.planning.world.roboplan_world") @@ -292,9 +261,8 @@ def test_start_uses_configured_planner_and_kinematics( planner=planner_config, kinematics_name=None, kinematics=module.config.kinematics, - trajectory_parametrization=module.config.trajectory_parametrization, + trajectory_parametrization=ANY, ) assert module._planner is planner assert module._kinematics is kinematics - assert module._trajectory_parametrizer is planning_specs.trajectory_parametrizer assert module._robots["arm"][0] == "robot-id" diff --git a/dimos/manipulation/visualization/test_operator.py b/dimos/manipulation/visualization/test_operator.py index 1820848ed2..a302fe9642 100644 --- a/dimos/manipulation/visualization/test_operator.py +++ b/dimos/manipulation/visualization/test_operator.py @@ -81,7 +81,6 @@ def __init__(self) -> None: self.state = "COMPLETED" self.error = "" self.has_plan = True - self.motion_speed = 1.0 self.plan = GeneratedPlan( group_ids=("arm/manipulator",), trajectory=JointTrajectory( @@ -127,13 +126,6 @@ def get_error(self) -> str: def has_planned_path(self) -> bool: return self.has_plan - def get_motion_speed(self) -> float: - return self.motion_speed - - def set_motion_speed(self, speed_scale: float) -> bool: - self.motion_speed = speed_scale - return True - def get_robot_config(self, robot_name: RobotName) -> RobotModelConfig | None: self.topology_calls += 1 return self.robot_configs.get(robot_name) @@ -285,15 +277,6 @@ def test_status_is_compact_and_does_not_read_topology_or_telemetry() -> None: assert monitor.telemetry_calls == 0 -def test_motion_speed_delegates_to_module() -> None: - operator, module, _ = _operator() - - assert operator.get_motion_speed() == 1.0 - assert operator.set_motion_speed(0.5) is True - assert operator.get_motion_speed() == 0.5 - assert module.motion_speed == 0.5 - - def test_evaluate_joint_target_accepts_exact_global_selection_domain() -> None: operator, _, _ = _operator() diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 15b701e10e..78ba20cf22 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -531,15 +531,17 @@ def test_panel_contract_group_order_defaults_and_controls( assert server.gui.dropdowns[0].options == ["Select preset...", "Init", "Current", "Home"] assert server.gui.dropdowns[1].options == ["Joint space", "Cartesian space"] assert [ - (slider.label, slider.min, slider.max, slider.value) for slider in server.gui.sliders - ] == [ - ("Next plan speed", 0.05, 1.0, 1.0), - ("arm/manipulator/j1", -1.0, 1.0, 0.1), - ] + (slider.label, slider.min, slider.max, slider.value) + for slider in server.gui.sliders + if slider.label != "Next plan speed" + ] == [("arm/manipulator/j1", -1.0, 1.0, 0.1)] server.gui.buttons[1].callback(SimpleNamespace()) assert gui.state.selected_group_ids == ("arm/manipulator", "arm/gripper") - assert [slider.label for slider in server.gui.sliders if not slider.removed] == [ - "Next plan speed", + assert [ + slider.label + for slider in server.gui.sliders + if not slider.removed and slider.label != "Next plan speed" + ] == [ "arm/manipulator/j1", "arm/gripper/j2", ] @@ -620,7 +622,6 @@ def test_cartesian_space_mode_plans_absolute_pose_targets_with_auxiliary_groups( pose_group = group("arm", "manipulator", ("j1",), pose=True) auxiliary_group = group("arm", "gripper", ("j2",)) gui, module, server = panel([pose_group, auxiliary_group], states("arm")) - module.motion_speed = 0.4 gui._toggle_group_selected(auxiliary_group.id) gui.state.target_status = TargetStatus.FEASIBLE gui._operation_worker.stop() @@ -640,8 +641,6 @@ def test_cartesian_space_mode_plans_absolute_pose_targets_with_auxiliary_groups( assert tuple(targets) == (pose_group.id,) assert targets[pose_group.id].frame_id == "world" assert config.speed_mode == "bounded" # type: ignore[attr-defined] - assert config.velocity_scale == pytest.approx(0.4) # type: ignore[attr-defined] - assert config.acceleration_scale == pytest.approx(0.4) # type: ignore[attr-defined] assert auxiliary_ids == (auxiliary_group.id,) assert gui.state.plan_state.status == PlanStatus.FRESH assert gui.state.last_result == "plan_cartesian_space=True" @@ -728,14 +727,19 @@ def test_valid_init_preset_builds_sliders_after_incomplete_initial_telemetry( ) assert gui.state.group_joint_targets == {} - assert [slider.label for slider in server.gui.sliders] == ["Next plan speed"] + assert [ + slider.label for slider in server.gui.sliders if slider.label != "Next plan speed" + ] == [] module.configs["arm"].home_joints = [-0.5, -1.0] gui._apply_preset("Init") assert gui.state.group_joint_targets[selected.id].position == [-0.5, -1.0] - assert [slider.label for slider in server.gui.sliders if not slider.removed] == [ - "Next plan speed", + assert [ + slider.label + for slider in server.gui.sliders + if not slider.removed and slider.label != "Next plan speed" + ] == [ "arm/manipulator/j1", "arm/manipulator/j2", ] @@ -919,8 +923,8 @@ def test_panel_preset_defaults_and_joint_slider_limits( assert [ (slider.label, slider.min, slider.max, slider.step, slider.value) for slider in server.gui.sliders + if slider.label != "Next plan speed" ] == [ - ("Next plan speed", 0.05, 1.0, 0.05, 1.0), ("arm/manipulator/j1", -1.0, 1.0, 0.001, 0.1), ("arm/manipulator/j2", -2.0, 2.0, 0.001, 0.2), ] @@ -990,13 +994,6 @@ def test_panel_action_controls_are_present_in_source_order( "Manipulation Panel", "Joint Control", ] - assert len(server.gui.sliders) == 2 - speed_slider = server.gui.sliders[0] - assert speed_slider.label == "Next plan speed" - assert speed_slider.min == pytest.approx(0.05) - assert speed_slider.max == pytest.approx(1.0) - assert speed_slider.step == pytest.approx(0.05) - assert speed_slider.value == pytest.approx(1.0) def test_next_plan_speed_slider_updates_future_speed_without_staling_plan( @@ -1006,7 +1003,9 @@ def test_next_plan_speed_slider_updates_future_speed_without_staling_plan( gui, module, server = panel([selected], states("arm")) accepted = module.make_plan((selected.id,)) gui.state.plan_state = PanelPlanState(status=PlanStatus.FRESH, plan=accepted) - speed_slider = server.gui.sliders[0] + speed_slider = next( + slider for slider in server.gui.sliders if slider.label == "Next plan speed" + ) speed_slider.value = 0.5 assert speed_slider.callback is not None @@ -1023,7 +1022,9 @@ def test_next_plan_speed_slider_is_disabled_during_panel_operation( ) -> None: selected = group("arm", "manipulator", ("j1",), pose=True) gui, _module, server = panel([selected], states("arm")) - speed_slider = server.gui.sliders[0] + speed_slider = next( + slider for slider in server.gui.sliders if slider.label == "Next plan speed" + ) gui.state.action_status = ActionStatus.RUNNING gui.refresh() From bec13be959bb85ec5ce3f68e545ae90729ad5c3c Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 30 Jul 2026 11:55:28 -0700 Subject: [PATCH 10/14] fix(manipulation): match parametrizer default to world --- dimos/manipulation/manipulation_module.py | 9 ++-- dimos/manipulation/planning/README.md | 7 ++- dimos/manipulation/planning/factory.py | 12 ++++- dimos/manipulation/test_planning_factory.py | 53 +++++++++++++++++++ .../manipulation/adding_a_custom_arm.md | 6 ++- docs/capabilities/manipulation/index.md | 18 +++++-- 6 files changed, 94 insertions(+), 11 deletions(-) diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 4008c2b8ec..e4abcc8f63 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -86,7 +86,6 @@ TrajectoryParametrizerSpec, ) from dimos.manipulation.planning.trajectory_generator.config import ( - SimpleTrapezoidParametrizationConfig, TrajectoryParametrizationConfig, ) from dimos.manipulation.skill_errors import ManipulationSkillError @@ -149,8 +148,12 @@ class ManipulationModuleConfig(ModuleConfig): default_factory=NoManipulationVisualizationConfig ) planner: ManipulationPlannerConfig = Field(default_factory=RoboPlanPlannerConfig) - trajectory_parametrization: TrajectoryParametrizationConfig = Field( - default_factory=SimpleTrapezoidParametrizationConfig + trajectory_parametrization: TrajectoryParametrizationConfig | None = Field( + default=None, + description=( + "Path parametrizer selected at startup. Omit to use roboplan_toppra " + "with RoboPlanWorld or simple_trapezoid with DrakeWorld." + ), ) kinematics: ManipulationKinematicsConfig = Field(default_factory=PinkKinematicsConfig) # Deprecated: use kinematics.backend instead. diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index ae32e8a1c0..b9cfc79913 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -98,6 +98,10 @@ and start and goal preservation. Each backend is responsible for generating motion within the velocity and acceleration limits it receives. A failure leaves no executable plan cached. +When `trajectory_parametrization` is omitted, the world selects the matching +default: `RoboPlanWorld` uses `roboplan_toppra`, while `DrakeWorld` uses +`simple_trapezoid`. An explicit backend always overrides that default. + This boundary is exposed internally as `TrajectoryParametrizerSpec`, alongside `PlannerSpec` and `WorldSpec`. Its implementations own conversion, validation, and `GeneratedPlan` construction; `ManipulationModule` only supplies the world, @@ -122,8 +126,7 @@ pass it to the native planner before that planner produces timestamps. ## Trajectory Parametrization -The default compatibility backend retains the existing segmented trapezoidal -behavior: +The compatibility backend retains the existing segmented trapezoidal behavior: ```python skip ManipulationModuleConfig( diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index b1119e8726..b738f5bd33 100644 --- a/dimos/manipulation/planning/factory.py +++ b/dimos/manipulation/planning/factory.py @@ -82,9 +82,13 @@ def validate_backend_combination( world_backend: str = "roboplan", planner_backend: str = "roboplan", kinematics_name: str = DEFAULT_KINEMATICS_NAME, - trajectory_parametrization_backend: str = "simple_trapezoid", + trajectory_parametrization_backend: str | None = None, ) -> None: """Validate manipulation backend choices before constructing the stack.""" + if trajectory_parametrization_backend is None: + trajectory_parametrization_backend = ( + "roboplan_toppra" if world_backend == "roboplan" else "simple_trapezoid" + ) if world_backend not in SUPPORTED_WORLD_BACKENDS: raise ValueError( f"Unknown backend: {world_backend}. Available: {list(SUPPORTED_WORLD_BACKENDS)}" @@ -228,7 +232,11 @@ def create_planning_specs( if planner is None: planner = RoboPlanPlannerConfig() if trajectory_parametrization is None: - trajectory_parametrization = SimpleTrapezoidParametrizationConfig() + trajectory_parametrization = ( + RoboPlanTOPPRAParametrizationConfig() + if world_backend == "roboplan" + else SimpleTrapezoidParametrizationConfig() + ) validate_backend_combination( world_backend=world_backend, diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index 570e9cc45c..6c263c1819 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -30,6 +30,7 @@ from dimos.manipulation.planning.factory import ( create_kinematics, create_planner, + create_planning_specs, create_planning_stack, create_world, validate_backend_combination, @@ -47,6 +48,10 @@ from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.protocols import PlannerSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, + TrajectoryParametrizationConfig, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -137,6 +142,54 @@ def test_validate_backend_combination_rejects_invalid_combinations() -> None: ) +@pytest.mark.parametrize( + ("world_backend", "planner", "configured", "expected_backend"), + [ + ("roboplan", RoboPlanPlannerConfig(), None, "roboplan_toppra"), + ("drake", RRTConnectPlannerConfig(), None, "simple_trapezoid"), + ( + "roboplan", + RoboPlanPlannerConfig(), + SimpleTrapezoidParametrizationConfig(), + "simple_trapezoid", + ), + ], +) +def test_create_planning_specs_selects_world_default_unless_overridden( + mocker: MockerFixture, + world_backend: str, + planner: RoboPlanPlannerConfig | RRTConnectPlannerConfig, + configured: TrajectoryParametrizationConfig | None, + expected_backend: str, +) -> None: + world = mocker.MagicMock() + trajectory_parametrizer = mocker.MagicMock() + mocker.patch( + "dimos.manipulation.planning.factory.create_kinematics", + return_value=mocker.MagicMock(), + ) + mocker.patch( + "dimos.manipulation.planning.factory.create_planner", + return_value=mocker.MagicMock(), + ) + create_parametrizer = mocker.patch( + "dimos.manipulation.planning.factory.create_trajectory_parametrizer", + return_value=trajectory_parametrizer, + ) + + result = create_planning_specs( + world=world, + world_backend=world_backend, + planner=planner, + trajectory_parametrization=configured, + ) + + selected = create_parametrizer.call_args.args[0] + assert selected.backend == expected_backend + create_parametrizer.assert_called_once_with(selected, world_backend=world_backend) + assert result.trajectory_parametrizer is trajectory_parametrizer + + def test_create_planner_uses_roboplan_world_as_native_planner(mocker: MockerFixture) -> None: world = mocker.MagicMock(spec=PlannerSpec) roboplan_world_module = ModuleType("dimos.manipulation.planning.world.roboplan_world") diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index c486db9b54..b702bf8385 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -583,7 +583,11 @@ yourarm_planner = manipulation_module( # topic, so no `.transports(...)` override is needed. ``` -To use continuous TOPP-RA timing instead, select RoboPlan for the world and +You may omit `trajectory_parametrization` when the world-based default is +appropriate: `world_backend="roboplan"` selects `roboplan_toppra`, while +`world_backend="drake"` selects `simple_trapezoid`. + +To configure TOPP-RA tuning explicitly, select RoboPlan for the world and parametrizer after adding the URDF limits described above: ```python skip diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index e9b635aeef..29a0ea022a 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -120,15 +120,24 @@ normally return an untimed geometric path; DimOS accepts the plan only after the selected backend converts that path to a validated timed trajectory: ```bash -# Compatibility behavior: independent trapezoids between path waypoints +# Stock xArm compatibility test: independent trapezoids on RoboPlanWorld dimos run xarm7-planner-coordinator \ -o manipulationmodule.trajectory_parametrization.backend=simple_trapezoid -# Continuous TOPP-RA timing, available with RoboPlanWorld +# After adding finite velocity and acceleration limits to the robot URDF, +# omitting trajectory_parametrization selects TOPP-RA for RoboPlanWorld +dimos run xarm7-planner-coordinator + +# Equivalent explicit TOPP-RA selection dimos run xarm7-planner-coordinator \ -o manipulationmodule.world_backend=roboplan \ -o manipulationmodule.trajectory_parametrization.backend=roboplan_toppra \ -o manipulationmodule.trajectory_parametrization.fitting_mode=linear_blend + +# DrakeWorld selects simple_trapezoid when no parametrizer is specified +dimos run xarm7-planner-coordinator \ + -o manipulationmodule.world_backend=drake \ + -o manipulationmodule.planner.backend=rrt_connect ``` Exactly one backend is constructed for the stack lifetime. There is no @@ -137,7 +146,10 @@ RoboPlan's planner or the generic RRT planner, but it requires `world_backend=roboplan` because it reuses that world's model, groups, and URDF motion limits. A planner-native result that already has timestamps and velocities bypasses path parametrization and retains its existing timing after -canonical validation. +canonical validation. Explicit configuration overrides the world-based default. +The bundled xArm URDF currently lacks the extended acceleration attribute, so +use `simple_trapezoid` for that stock model or add the required URDF limits +before testing `roboplan_toppra`. The Viser panel's **Next plan speed** slider provides runtime speed tuning from `0.05` to `1.0`. Changing it leaves the accepted plan and any active execution From f8bdd6b6ab5e7bc6105d311fcfa2a799a78659a6 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 30 Jul 2026 12:10:49 -0700 Subject: [PATCH 11/14] fix(manipulation): add temporary acceleration fallback --- dimos/manipulation/planning/README.md | 5 +++- .../test_roboplan_toppra_parametrizer.py | 14 +++++------ .../planning/world/roboplan_model.py | 12 ++++++++++ dimos/manipulation/test_roboplan_world.py | 24 +++++++++++++++++++ .../manipulation/adding_a_custom_arm.md | 6 +++-- docs/capabilities/manipulation/index.md | 9 ++++--- 6 files changed, 55 insertions(+), 15 deletions(-) diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index b9cfc79913..05288bda3e 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -165,7 +165,10 @@ and planning groups. Selecting it with another world fails during startup. DimOS pins RoboPlan to `0.5.1` for this integration. For every selected movable joint, the RoboPlan URDF must provide a finite, -positive velocity limit and an extended acceleration limit: +positive velocity limit. DimOS uses an authored extended acceleration limit +when present; otherwise it temporarily inserts a global `2.0 rad/s²` fallback +while composing the RoboPlan model. Formal per-joint acceleration overrides +will replace this fallback. ```xml None: - self.missing_acceleration = missing_acceleration + def __init__(self, *, unbounded_acceleration: bool = False) -> None: + self.unbounded_acceleration = unbounded_acceleration def getVelocityLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarray]: assert group_name == "composite" @@ -55,7 +55,7 @@ def getVelocityLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarr def getAccelerationLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarray]: assert group_name == "composite" - maximum = np.finfo(np.float64).max if self.missing_acceleration else 4.0 + maximum = np.finfo(np.float64).max if self.unbounded_acceleration else 4.0 return np.asarray([-6.0, -maximum]), np.asarray([6.0, maximum]) @@ -68,7 +68,7 @@ def parametrization_model(self): yield self.model -def _model(*, missing_acceleration: bool = False) -> RoboPlanModel: +def _model(*, unbounded_acceleration: bool = False) -> RoboPlanModel: group = RoboPlanGroup( group_ids=("left/arm", "right/arm"), name="composite", @@ -76,7 +76,7 @@ def _model(*, missing_acceleration: bool = False) -> RoboPlanModel: public_names=("right/b", "left/a"), ) return RoboPlanModel( - scene=_Scene(missing_acceleration=missing_acceleration), + scene=_Scene(unbounded_acceleration=unbounded_acceleration), groups={frozenset(group.group_ids): group}, legacy_group_ids={}, native_joint_by_global={}, @@ -183,7 +183,7 @@ def test_roboplan_parametrizer_maps_composite_order_and_native_output( ] -def test_roboplan_parametrizer_rejects_missing_urdf_acceleration_without_fallback( +def test_roboplan_parametrizer_rejects_unbounded_scene_acceleration( mocker: MockerFixture, ) -> None: constructor = mocker.patch( @@ -200,7 +200,7 @@ def test_roboplan_parametrizer_rejects_missing_urdf_acceleration_without_fallbac match="no usable URDF acceleration limit for joint 'left/a'", ): parametrizer.materialize_plan( - _World(_model(missing_acceleration=True)), + _World(_model(unbounded_acceleration=True)), selection, result, ) diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index 75f5b1d492..3971dbfa64 100644 --- a/dimos/manipulation/planning/world/roboplan_model.py +++ b/dimos/manipulation/planning/world/roboplan_model.py @@ -37,6 +37,8 @@ _ROOT_LINK = "dimos_world" _ROOT_JOINT = "dimos_world_joint" _FREE_ROOTS = {"world", "map", _ROOT_LINK} +# TODO: Remove this global fallback when formal per-joint acceleration overrides are available. +_DEFAULT_ACCELERATION_LIMIT = 2.0 _REFERENCE_ATTRIBUTES = ( "reference", "frame", @@ -178,6 +180,7 @@ def _compose(prepared: Sequence[tuple[_BuildRobot, Path]], composite: bool) -> _ root = ET.parse(path).getroot() if _tag(root.tag) != "robot": raise ValueError(f"Prepared model for '{config.name}' is not a URDF robot") + _add_missing_acceleration_limits(root) mapping = _name_map(root, config.name, composite) mapped_names = { value @@ -234,6 +237,15 @@ def _compose(prepared: Sequence[tuple[_BuildRobot, Path]], composite: bool) -> _ ) +def _add_missing_acceleration_limits(root: ET.Element) -> None: + for joint in root.iter(): + if _tag(joint.tag) != "joint" or joint.get("type") == "fixed": + continue + limit = next((child for child in joint if _tag(child.tag) == "limit"), None) + if limit is not None and limit.get("acceleration") is None: + limit.set("acceleration", str(_DEFAULT_ACCELERATION_LIMIT)) + + def _name_map(root: ET.Element, robot_name: RobotName, prefix: bool) -> _NameMap: def names(tag: str) -> dict[str, str]: return { diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..7accf08161 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -1930,6 +1930,30 @@ def test_scene_receives_generated_model_contents_inline( assert world._scene.constructor_kwargs["package_paths"] == [] +def test_composed_model_fills_only_missing_acceleration_limits( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + tree = ET.parse(robot_config.model_path) + authored = tree.find("./joint[@name='joint1']/limit") + assert authored is not None + authored.set("acceleration", "3.5") + tree.write(robot_config.model_path) + + world, _ = _make_world(fake_roboplan, robot_config) + + urdf = ET.fromstring(world._scene.constructor_kwargs["urdf"]) + acceleration_by_joint = { + joint.get("name"): limit.get("acceleration") + for joint in urdf.findall("./joint") + if (limit := joint.find("./limit")) is not None + } + assert acceleration_by_joint == { + "joint1": "3.5", + "joint2": "2.0", + "joint3": "2.0", + } + + def test_base_pose_is_written_to_composed_model( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index b702bf8385..076546584c 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -483,8 +483,10 @@ Place your URDF/xacro files under LFS data so they can be resolved via `LfsPath` If the planning blueprint selects the RoboPlan TOPP-RA trajectory parametrizer, DimOS currently pins RoboPlan to `0.5.1`. Every movable joint in -each selected planning group must provide finite, positive velocity and -extended acceleration limits: +each selected planning group must provide finite, positive velocity limits. +Authored extended acceleration limits take precedence; when absent, DimOS +temporarily inserts a global `2.0 rad/s²` acceleration fallback during RoboPlan +model composition: ```xml diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 29a0ea022a..a671c46cdf 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -124,8 +124,7 @@ the selected backend converts that path to a validated timed trajectory: dimos run xarm7-planner-coordinator \ -o manipulationmodule.trajectory_parametrization.backend=simple_trapezoid -# After adding finite velocity and acceleration limits to the robot URDF, -# omitting trajectory_parametrization selects TOPP-RA for RoboPlanWorld +# Omitting trajectory_parametrization selects TOPP-RA for RoboPlanWorld dimos run xarm7-planner-coordinator # Equivalent explicit TOPP-RA selection @@ -147,9 +146,9 @@ RoboPlan's planner or the generic RRT planner, but it requires motion limits. A planner-native result that already has timestamps and velocities bypasses path parametrization and retains its existing timing after canonical validation. Explicit configuration overrides the world-based default. -The bundled xArm URDF currently lacks the extended acceleration attribute, so -use `simple_trapezoid` for that stock model or add the required URDF limits -before testing `roboplan_toppra`. +RoboPlan model composition preserves authored acceleration limits and inserts a +temporary global `2.0 rad/s²` fallback where they are absent. Formal per-joint +acceleration overrides will replace this fallback. The Viser panel's **Next plan speed** slider provides runtime speed tuning from `0.05` to `1.0`. Changing it leaves the accepted plan and any active execution From 9dffa15dfdc27c86896fcb52b3090d51f6046218 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 30 Jul 2026 12:20:28 -0700 Subject: [PATCH 12/14] test(manipulation): isolate optional parametrizer backend --- .../planning/monitor/test_world_monitor.py | 13 ++++++++----- dimos/manipulation/test_planning_factory.py | 4 ++++ 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index b14000496d..cb19f79389 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -347,17 +347,19 @@ def test_obstacle_monitor_routes_mutations_through_parent_world_monitor( remove_obstacle.assert_called_once_with("parent-id") -def test_create_planning_specs_wraps_existing_world(monkeypatch) -> None: +def test_create_planning_specs_wraps_existing_world(mocker: MockerFixture) -> None: fake_world = FakeWorld() fake_kinematics = object() fake_planner = object() + fake_parametrizer = object() - monkeypatch.setattr( + mocker.patch.object(planning_factory, "create_kinematics", return_value=fake_kinematics) + mocker.patch.object(planning_factory, "create_planner", return_value=fake_planner) + mocker.patch.object( planning_factory, - "create_kinematics", - lambda *args, **kwargs: fake_kinematics, + "create_trajectory_parametrizer", + return_value=fake_parametrizer, ) - monkeypatch.setattr(planning_factory, "create_planner", lambda **kwargs: fake_planner) planning_specs = planning_factory.create_planning_specs(world=fake_world) # type: ignore[arg-type] @@ -365,6 +367,7 @@ def test_create_planning_specs_wraps_existing_world(monkeypatch) -> None: assert planning_specs.world_monitor.visualization is None assert planning_specs.kinematics is fake_kinematics assert planning_specs.planner is fake_planner + assert planning_specs.trajectory_parametrizer is fake_parametrizer def test_world_monitor_exposes_planning_groups_and_duplicate_names_do_not_mutate() -> None: diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index 6c263c1819..ce8e388b85 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -242,6 +242,10 @@ def test_create_planning_stack_defaults_to_roboplan( "dimos.manipulation.planning.factory.create_planner", return_value=planner, ) + mocker.patch( + "dimos.manipulation.planning.factory.create_trajectory_parametrizer", + return_value=mocker.MagicMock(name="trajectory_parametrizer"), + ) result = create_planning_stack(robot_config) From a413e93cc56e328c6f5d3300ca94569ab666672e Mon Sep 17 00:00:00 2001 From: cc <55869557+TomCC7@users.noreply.github.com> Date: Thu, 30 Jul 2026 15:24:51 -0400 Subject: [PATCH 13/14] Delete dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py --- .../test_roboplan_toppra_contract.py | 98 ------------------- 1 file changed, 98 deletions(-) delete mode 100644 dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py deleted file mode 100644 index 36d6725163..0000000000 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_contract.py +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Contract tests for the pinned RoboPlan TOPP-RA Python binding.""" - -from pathlib import Path -import sys - -import numpy as np -import pytest - -roboplan_core = pytest.importorskip("roboplan.core") -roboplan_toppra = pytest.importorskip("roboplan.toppra") - -pytestmark = pytest.mark.self_hosted - - -def _scene(tmp_path: Path, *, acceleration: float | None) -> object: - acceleration_attribute = "" if acceleration is None else f' acceleration="{acceleration}"' - urdf = tmp_path / "robot.urdf" - urdf.write_text( - f"""\ - - - - - - - - - - -""" - ) - srdf = tmp_path / "robot.srdf" - srdf.write_text( - """\ - - - - - -""" - ) - return roboplan_core.Scene("contract_robot", urdf, srdf, []) - - -def test_roboplan_051_generates_native_trajectory(tmp_path: Path) -> None: - scene = _scene(tmp_path, acceleration=2.0) - options = roboplan_toppra.TOPPRAOptions( - dt=0.02, - mode=roboplan_toppra.SplineFittingMode.LinearBlend, - velocity_scale=0.5, - acceleration_scale=0.25, - max_adaptive_iterations=7, - max_adaptive_step_size=0.03, - max_blend_deviation=0.01, - ) - path = roboplan_core.JointPath() - path.joint_names = ["joint"] - path.positions = [ - np.asarray([0.0], dtype=np.float64), - np.asarray([0.2], dtype=np.float64), - np.asarray([0.4], dtype=np.float64), - ] - - trajectory = roboplan_toppra.PathParameterizerTOPPRA(scene, "arm").generate(path, options) - - assert trajectory.joint_names == ["joint"] - assert len(trajectory.times) == len(trajectory.positions) - assert len(trajectory.velocities) == len(trajectory.positions) - assert len(trajectory.accelerations) == len(trajectory.positions) - assert trajectory.times[0] == 0.0 - assert trajectory.times[-1] > 0.0 - assert np.allclose(trajectory.positions[0], [0.0]) - assert np.allclose(trajectory.positions[-1], [0.4]) - - -def test_roboplan_051_missing_urdf_acceleration_is_effectively_unbounded( - tmp_path: Path, -) -> None: - scene = _scene(tmp_path, acceleration=None) - - lower, upper = scene.getAccelerationLimitVectors("arm") - - assert lower.tolist() == [-sys.float_info.max] - assert upper.tolist() == [sys.float_info.max] From fe8617afae714ea5b06155d5928472a10597e2c9 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 30 Jul 2026 12:39:55 -0700 Subject: [PATCH 14/14] test(manipulation): cover parametrizer failures --- dimos/control/test_control.py | 15 +- .../test_roboplan_toppra_parametrizer.py | 133 ++++++++++++++++++ 2 files changed, 143 insertions(+), 5 deletions(-) diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index c481965070..38853afe3a 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -973,7 +973,7 @@ def test_tick_loop_calls_compute(self, mock_adapter): class TestIntegration: - def test_full_trajectory_execution(self, mock_adapter): + def test_full_trajectory_execution(self, mock_adapter, wait_until): component = HardwareComponent( hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, @@ -1017,10 +1017,15 @@ def test_full_trajectory_execution(self, mock_adapter): ) tick_loop.start() - traj_task.execute(trajectory, trajectory_start_positions(trajectory)) - - time.sleep(0.6) - tick_loop.stop() + try: + traj_task.execute(trajectory, trajectory_start_positions(trajectory)) + wait_until( + lambda: traj_task.get_state() == TrajectoryState.COMPLETED, + timeout=2.0, + interval=0.01, + ) + finally: + tick_loop.stop() assert traj_task.get_state() == TrajectoryState.COMPLETED assert mock_adapter.write_joint_positions.call_count > 0 diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py index 2182b982e1..ee339d0aa9 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -15,6 +15,7 @@ """Tests for the RoboPlan TOPP-RA trajectory parametrizer.""" from contextlib import contextmanager +from dataclasses import replace from types import SimpleNamespace import numpy as np @@ -243,3 +244,135 @@ def test_cached_group_preserves_each_request_joint_order( constructor.assert_called_once() assert canonical.trajectory.joint_names == ["left/a", "right/b"] assert reversed_order.trajectory.joint_names == ["right/b", "left/a"] + + +def test_roboplan_parametrizer_rejects_incompatible_world( + mocker: MockerFixture, +) -> None: + selection, result = _selection_and_result() + + with pytest.raises( + TrajectoryParametrizationError, + match="RoboPlan TOPP-RA requires RoboPlanWorld", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + mocker.MagicMock(), selection, result + ) + + +def test_roboplan_parametrizer_reports_missing_generated_group() -> None: + selection, result = _selection_and_result() + model = replace(_model(), groups={}) + + with pytest.raises( + TrajectoryParametrizationError, + match=r"RoboPlan has no generated group for \['left/arm', 'right/arm'\]", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(model), selection, result + ) + + +def test_roboplan_parametrizer_rejects_group_with_different_joints() -> None: + selection, result = _selection_and_result() + model = _model() + mismatched_group = replace(model.all_group, public_names=("left/a", "right/other")) + model = replace( + model, + groups={frozenset(mismatched_group.group_ids): mismatched_group}, + all_group=mismatched_group, + ) + + with pytest.raises( + TrajectoryParametrizationError, + match="does not match selected joints", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(model), selection, result + ) + + +def test_roboplan_parametrizer_rejects_limit_vector_with_wrong_size( + mocker: MockerFixture, +) -> None: + selection, result = _selection_and_result() + model = _model() + mocker.patch.object( + model.scene, + "getVelocityLimitVectors", + return_value=([-1.0], [1.0]), + ) + + with pytest.raises( + TrajectoryParametrizationError, + match="velocity limits do not match group 'composite'", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(model), selection, result + ) + + +def test_roboplan_parametrizer_wraps_native_generation_error( + mocker: MockerFixture, +) -> None: + native = mocker.MagicMock() + native.generate.side_effect = RuntimeError("native failure") + mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + selection, result = _selection_and_result() + + with pytest.raises( + TrajectoryParametrizationError, + match="RoboPlan TOPP-RA parametrization failed: native failure", + ) as error: + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(_model()), selection, result + ) + + assert isinstance(error.value.__cause__, RuntimeError) + + +@pytest.mark.parametrize( + ("generated", "message"), + [ + ( + SimpleNamespace( + joint_names=["native_a", "unexpected"], + times=[0.0], + positions=[np.asarray([0.0, 0.1])], + velocities=[np.asarray([0.0, 0.0])], + ), + "returned unexpected joint names", + ), + ( + SimpleNamespace( + joint_names=["native_a", "native_b"], + times=[0.0], + positions=[np.asarray([0.0, 0.1])], + velocities=[], + ), + "returned inconsistent trajectory fields", + ), + ], +) +def test_roboplan_parametrizer_rejects_malformed_native_trajectory( + mocker: MockerFixture, + generated: SimpleNamespace, + message: str, +) -> None: + native = mocker.MagicMock() + native.generate.return_value = generated + mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + selection, result = _selection_and_result() + + with pytest.raises(TrajectoryParametrizationError, match=message): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(_model()), selection, result + )