From fe7a7de5874ffbc4ba290e97aae6a863205dd4f0 Mon Sep 17 00:00:00 2001 From: cc Date: Thu, 4 Jun 2026 13:41:32 -0700 Subject: [PATCH 01/19] 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/19] 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 00e47a9f5d767ee8d273ba6ec3d799b98608905c Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 15 Jul 2026 18:32:26 -0700 Subject: [PATCH 03/19] feat: default control IK to Pink --- .../cartesian_ik_task/cartesian_ik_task.py | 174 ++++--- .../cartesian_ik_task/pink_control_ik.py | 424 ++++++++++++++++++ .../cartesian_ik_task/test_pink_control_ik.py | 410 +++++++++++++++++ .../tasks/eef_twist_task/eef_twist_task.py | 199 +++----- .../eef_twist_task/test_eef_twist_task.py | 49 +- .../manipulators/a1z/blueprints/teleop.py | 13 +- .../manipulators/a750/blueprints/teleop.py | 13 +- dimos/robot/manipulators/common/blueprints.py | 92 +++- .../manipulators/openarm/blueprints/teleop.py | 18 +- .../manipulators/piper/blueprints/teleop.py | 29 +- dimos/robot/manipulators/test_blueprints.py | 72 ++- .../manipulators/xarm/blueprints/teleop.py | 23 +- .../manipulation/adding_a_custom_arm.md | 69 +++ docs/capabilities/manipulation/index.md | 61 ++- .../.openspec.yaml | 2 + .../design.md | 123 +++++ .../docs.md | 30 ++ .../proposal.md | 54 +++ .../specs/pink-control-ik/spec.md | 76 ++++ .../tasks.md | 72 +++ pyproject.toml | 2 + 21 files changed, 1783 insertions(+), 222 deletions(-) create mode 100644 dimos/control/tasks/cartesian_ik_task/pink_control_ik.py create mode 100644 dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py create mode 100644 openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml create mode 100644 openspec/changes/add-pink-control-ik-self-collision/design.md create mode 100644 openspec/changes/add-pink-control-ik-self-collision/docs.md create mode 100644 openspec/changes/add-pink-control-ik-self-collision/proposal.md create mode 100644 openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md create mode 100644 openspec/changes/add-pink-control-ik-self-collision/tasks.md diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index fcf58b833a..79f92d0801 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Cartesian control task with internal Pinocchio IK solver. +"""Cartesian control task with Pink differential IK by default. Accepts streaming cartesian poses (e.g., from teleoperation, visual servoing) and computes inverse kinematics internally to output joint commands. @@ -21,13 +21,16 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field from pathlib import Path import threading -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING import numpy as np +import pinocchio +from pydantic import Field +from dimos.control.coordinator import TaskConfig from dimos.control.task import ( BaseControlTask, ControlMode, @@ -35,18 +38,19 @@ JointCommandOutput, ResourceClaim, ) +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( + PinkControlIK, + PinkControlIKConfig, +) from dimos.manipulation.planning.kinematics.pinocchio_ik import ( - PinocchioIK, check_joint_delta, get_worst_joint_delta, - pose_to_se3, ) from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from numpy.typing import NDArray - import pinocchio from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -60,8 +64,8 @@ class CartesianIKTaskConfig: Attributes: joint_names: List of joint names this task controls (must match model DOF) - model_path: Path to URDF or MJCF file for IK solver - ee_joint_id: End-effector joint ID in the kinematic chain + model_path: Path to the direct Pink or legacy Pinocchio model + ee_joint_id: Legacy Pinocchio end-effector joint ID, when selected priority: Priority for arbitration (higher wins) timeout: If no command received for this many seconds, go inactive (0 = never) max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) @@ -69,31 +73,36 @@ class CartesianIKTaskConfig: joint_names: list[str] model_path: str | Path - ee_joint_id: int + ee_joint_id: int | None = None priority: int = 10 timeout: float = 0.5 max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz + control_ik: PinkControlIKConfig = field(default_factory=PinkControlIKConfig) class CartesianIKTask(BaseControlTask): - """Cartesian control task with internal Pinocchio IK solver. + """Cartesian control task with selectable Pink or legacy Pinocchio IK. Accepts streaming cartesian poses via on_cartesian_command() and computes IK - internally to output joint commands. Uses current joint state from - CoordinatorState as IK warm-start for fast convergence. + internally to output joint commands. Pink re-anchors each solve to the + current joint state from CoordinatorState. Unlike CartesianServoTask (which bypasses joint arbitration), this task outputs JointCommandOutput and participates in joint-level arbitration. Example: - >>> from dimos.utils.data import get_data - >>> piper_path = get_data("piper_description") + >>> from dimos.robot.manipulators.piper.config import ( + ... PIPER_MODEL_PATH, + ... make_piper_model_config, + ... ) >>> task = CartesianIKTask( ... name="cartesian_arm", ... config=CartesianIKTaskConfig( ... joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], - ... model_path=piper_path / "mujoco_model" / "piper_no_gripper_description.xml", - ... ee_joint_id=6, + ... model_path=PIPER_MODEL_PATH, + ... control_ik=PinkControlIKConfig( + ... robot_model=make_piper_model_config(), + ... ), ... priority=10, ... timeout=0.5, ... ), @@ -112,10 +121,14 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: name: Unique task name config: Task configuration """ - if not config.joint_names: + if not config.joint_names or len(set(config.joint_names)) != len(config.joint_names): raise ValueError(f"CartesianIKTask '{name}' requires at least one joint") if not config.model_path: raise ValueError(f"CartesianIKTask '{name}' requires model_path for IK solver") + if not np.isfinite(config.timeout) or config.timeout < 0.0: + raise ValueError("CartesianIKTask timeout must be finite and non-negative") + if not np.isfinite(config.max_joint_delta_deg) or config.max_joint_delta_deg <= 0.0: + raise ValueError("CartesianIKTask max_joint_delta_deg must be positive and finite") self._name = name self._config = config @@ -124,11 +137,16 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._num_joints = len(config.joint_names) # Create IK solver from model - self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id) + self._ik = PinkControlIK( + config.model_path, + config.ee_joint_id, + self._joint_names_list, + config.control_ik, + ) # Validate DOF matches joint names if self._ik.nq != self._num_joints: - logger.warning( + raise ValueError( f"CartesianIKTask {name}: model DOF ({self._ik.nq}) != " f"joint_names count ({self._num_joints})" ) @@ -139,9 +157,6 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._last_update_time: float = 0.0 self._active = False - # Cache last successful IK solution for warm-starting - self._last_q_solution: NDArray[np.floating[Any]] | None = None - logger.info( f"CartesianIKTask {name} initialized with model: {config.model_path}, " f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" @@ -169,13 +184,14 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: """Compute IK and output joint positions. Args: - state: Current coordinator state (contains joint positions for IK warm-start) + state: Current coordinator state (contains measured joint positions) Returns: - JointCommandOutput with positions, or None if inactive/timed out/IK failed + JointCommandOutput with positions or a measured-state hold after an + expected runtime failure; None if inactive or timed out. """ with self._lock: - if not self._active or self._target_pose is None: + if not self._active or (self._target_pose is None and not self._uses_prepared_target()): return None # Check timeout if self._config.timeout > 0: @@ -186,25 +202,38 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: f"(no update for {time_since_update:.3f}s)" ) self._active = False + self._target_pose = None + self._on_timeout() return None - raw_pose = self._target_pose - # Convert to SE3 right before use - target_pose = pose_to_se3(raw_pose) - # Get current joint positions for IK warm-start q_current = self._get_current_joints(state) if q_current is None: logger.debug(f"CartesianIKTask {self._name}: missing joint state for IK warm-start") return None + if not np.all(np.isfinite(q_current)): + logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name) + return None + dt = self._clamped_dt(state.dt) + if dt is None: + return self._hold(q_current) + try: + target_pose = self._prepare_target(state, q_current, dt) + except (FloatingPointError, RuntimeError, ValueError) as exc: + logger.warning("CartesianIKTask %s: target preparation failed: %s", self._name, exc) + return self._hold(q_current) + if target_pose is None: + return self._hold(q_current) # Compute IK - q_solution, converged, final_error = self._ik.solve(target_pose, q_current) - # Use the solution even if it didn't fully converge - if not converged: - logger.debug( - f"CartesianIKTask {self._name}: IK did not converge " - f"(error={final_error:.4f}), using partial solution" - ) + try: + result = self._ik.solve(target_pose, q_current, dt) + except (FloatingPointError, RuntimeError, ValueError) as exc: + logger.warning("CartesianIKTask %s: IK solve failed: %s", self._name, exc) + return self._hold(q_current) + q_solution = np.asarray(result.positions, dtype=np.float64).reshape(-1) + if not np.all(np.isfinite(q_solution)) or q_solution.shape != q_current.shape: + logger.warning("CartesianIKTask %s: rejecting invalid IK output", self._name) + return self._hold(q_current) # Safety check: reject if any joint delta exceeds limit if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg): @@ -214,34 +243,74 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: f"joint {self._joint_names_list[worst_idx]} delta " f"{worst_deg:.1f}° exceeds limit {self._config.max_joint_delta_deg}°" ) - return None + return self._hold(q_current) - # Cache solution for next warm-start - with self._lock: - self._last_q_solution = q_solution.copy() return JointCommandOutput( joint_names=self._joint_names_list, positions=q_solution.flatten().tolist(), mode=ControlMode.SERVO_POSITION, ) - def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None: - """Get current joint positions from coordinator state. + def _hold(self, q_current: NDArray[np.float64]) -> JointCommandOutput: + """Keep the measured configuration under the task's servo contract.""" + return JointCommandOutput( + joint_names=self._joint_names_list, + positions=q_current.tolist(), + mode=ControlMode.SERVO_POSITION, + ) - Falls back to last IK solution if joint state unavailable. - """ + def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.float64] | None: + """Get the measured coordinator joint snapshot (never a command cache).""" positions = [] for joint_name in self._joint_names_list: pos = state.joints.get_position(joint_name) if pos is None: - # Fallback to last solution - if self._last_q_solution is not None: - result: NDArray[np.floating[Any]] = self._last_q_solution.copy() - return result return None positions.append(pos) return np.array(positions, dtype=np.float64) + def _prepare_target( + self, + state: CoordinatorState, + q_current: NDArray[np.float64], + dt: float, + ) -> pinocchio.SE3 | None: + """Prepare one normalized target for the measured-state solve.""" + with self._lock: + pose = self._target_pose + if pose is None: + return None + quaternion = np.array( + [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w], + dtype=np.float64, + ) + quaternion_norm = float(np.linalg.norm(quaternion)) + if not np.isfinite(quaternion_norm) or quaternion_norm <= 1e-12: + return None + normalized = quaternion / quaternion_norm + target = pinocchio.SE3( + pinocchio.Quaternion( + normalized[3], normalized[0], normalized[1], normalized[2] + ).toRotationMatrix(), + np.array([pose.x, pose.y, pose.z], dtype=np.float64), + ) + values = np.concatenate((target.translation, target.rotation.reshape(-1))) + if not np.all(np.isfinite(values)): + return None + return target + + def _clamped_dt(self, dt: float) -> float | None: + if not np.isfinite(dt) or dt <= 0.0: + return None + bounds = self._config.control_ik + return min(max(dt, bounds.min_dt), bounds.max_dt) + + def _on_timeout(self) -> None: + """Hook for target sources with state outside the Cartesian pose cache.""" + + def _uses_prepared_target(self) -> bool: + return False + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: """Handle preemption by higher-priority task. @@ -281,6 +350,7 @@ def stop(self) -> None: """Deactivate the task (stop outputting commands).""" with self._lock: self._active = False + self._target_pose = None logger.info(f"CartesianIKTask {self._name} stopped") def clear(self) -> None: @@ -312,7 +382,7 @@ def get_current_ee_pose(self, state: CoordinatorState) -> pinocchio.SE3 | None: return self._ik.forward_kinematics(q_current) - def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3: + def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio.SE3: """Compute end-effector pose from joint positions. Args: @@ -326,10 +396,11 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino class CartesianIKTaskParams(BaseConfig): model_path: str | Path - ee_joint_id: int = 6 + ee_joint_id: int | None = None + control_ik: PinkControlIKConfig = Field(default_factory=PinkControlIKConfig) -def create_task(cfg: Any, hardware: Any) -> CartesianIKTask: +def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: params = CartesianIKTaskParams.model_validate(cfg.params) return CartesianIKTask( cfg.name, @@ -338,5 +409,6 @@ def create_task(cfg: Any, hardware: Any) -> CartesianIKTask: model_path=params.model_path, ee_joint_id=params.ee_joint_id, priority=cfg.priority, + control_ik=params.control_ik, ), ) diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py new file mode 100644 index 0000000000..4797dc9dbd --- /dev/null +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -0,0 +1,424 @@ +# 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. + +"""Pink and legacy Pinocchio backends for coordinator Cartesian control.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +from pathlib import Path +from typing import Literal + +import numpy as np +from numpy.typing import NDArray +import pink +from pink.limits import ConfigurationLimit, Limit, VelocityLimit +import pinocchio +from pydantic import Field, field_validator + +from dimos.manipulation.planning.kinematics.pinocchio_ik import PinocchioIK +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.protocol.service.spec import BaseConfig + + +class PinkControlIKConfig(BaseConfig): + """Typed configuration for the control IK backend.""" + + backend: Literal["pink", "pinocchio"] = "pink" + robot_model: RobotModelConfig | None = None + solver: str = "proxqp" + max_velocity: float = 10.0 + lm_damping: float = 1e-4 + task_gain: float = 1.0 + position_cost: float = 1.0 + orientation_cost: float = 1.0 + min_dt: float = 1e-4 + max_dt: float = 0.05 + reference_q: list[float] | None = None + qpsolver_options: dict[str, float] = Field(default_factory=dict) + + @field_validator("robot_model", mode="before") + @classmethod + def _rebuild_robot_model(cls, value: object) -> RobotModelConfig | None: + if value is None or isinstance(value, RobotModelConfig): + return value + if not isinstance(value, Mapping): + raise ValueError("Pink robot_model must be a serialized RobotModelConfig") + payload = dict(value) + base_pose = payload.get("base_pose") + if isinstance(base_pose, Mapping): + position = base_pose.get("position") + orientation = base_pose.get("orientation") + if not isinstance(position, list) or not isinstance(orientation, list): + raise ValueError("serialized RobotModelConfig base_pose is invalid") + payload["base_pose"] = PoseStamped( + ts=float(base_pose.get("ts", 0.0)), + frame_id=str(base_pose.get("frame_id", "")), + position=position, + orientation=orientation, + ) + return RobotModelConfig.model_validate(payload) + + def validate_settings( + self, + joint_count: int, + ee_joint_id: int | None, + model_path: str | Path | None = None, + ) -> None: + numeric = ( + self.max_velocity, + self.lm_damping, + self.task_gain, + self.position_cost, + self.orientation_cost, + self.min_dt, + self.max_dt, + ) + if not all(np.isfinite(value) for value in numeric): + raise ValueError("control IK numeric settings must be finite") + if self.max_velocity <= 0.0 or self.lm_damping <= 0.0 or self.task_gain <= 0.0: + raise ValueError("control IK velocity, damping, and gain must be positive") + if self.position_cost < 0.0 or self.orientation_cost < 0.0: + raise ValueError("control IK task costs must not be negative") + if self.min_dt <= 0.0 or self.max_dt < self.min_dt: + raise ValueError("control IK dt bounds must be positive and ordered") + if any(not np.isfinite(value) for value in self.qpsolver_options.values()): + raise ValueError("control IK QP options must be finite") + if self.backend == "pink": + if self.robot_model is None: + raise ValueError("Pink control requires a RobotModelConfig") + if not self.robot_model.end_effector_link: + raise ValueError("Pink control requires a named end-effector frame") + if len(self.robot_model.joint_names) != joint_count: + raise ValueError("RobotModelConfig and control task joint counts differ") + if ( + model_path is not None + and Path(self.robot_model.model_path).resolve() != Path(model_path).resolve() + ): + raise ValueError("Pink RobotModelConfig must use the authoritative model path") + elif not isinstance(ee_joint_id, int) or isinstance(ee_joint_id, bool): + raise ValueError("Pinocchio control requires a numeric ee_joint_id") + + +@dataclass(frozen=True) +class ControlIKResult: + positions: NDArray[np.float64] + velocity: NDArray[np.float64] + + +class PinkControlRuntimeError(RuntimeError): + """A runtime solver/model failure that should produce a bounded hold.""" + + +class PinkControlIK: + """One-step Pink control IK with explicit legacy Pinocchio compatibility.""" + + def __init__( + self, + model_path: str | Path, + ee_joint_id: int | None, + joint_names: list[str], + config: PinkControlIKConfig, + ) -> None: + self._config = config + self._joint_names = list(joint_names) + self._config.validate_settings(len(self._joint_names), ee_joint_id, model_path) + self._is_pinocchio = config.backend == "pinocchio" + self._legacy_ik: PinocchioIK | None = None + + if self._is_pinocchio: + if ee_joint_id is None: + raise ValueError("Pinocchio control requires an explicit ee_joint_id") + self._legacy_ik = PinocchioIK.from_model_path(model_path, ee_joint_id) + self._model = self._legacy_ik.model + self._data = self._model.createData() + self._q_indices: list[int] = [] + self._v_indices: list[int] = [] + self._configuration = None + self._frame_task = None + self._limits: list[Limit] = [] + return + + robot = config.robot_model + if robot is None: # guarded by validate_settings; retained for narrowing + raise ValueError("Pink control requires a RobotModelConfig") + prepared_path = Path( + prepare_urdf_for_drake( + robot.model_path, + package_paths=robot.package_paths, + xacro_args=robot.xacro_args, + convert_meshes=False, + ) + ) + if not prepared_path.exists(): + raise FileNotFoundError(f"prepared Pink control URDF not found: {prepared_path}") + + self._model = pinocchio.buildModelFromUrdf(str(prepared_path)) + self._data = self._model.createData() + self._q_indices, self._v_indices = self._build_mapping(robot) + self._ee_frame_id = self._validate_frame(robot.end_effector_link) + self._apply_limits(robot) + full_reference_q = self._build_reference_q() + controlled_joint_ids = set(self._controlled_joint_ids) + locked_joint_ids = [ + joint_id + for joint_id in range(1, len(self._model.joints)) + if joint_id not in controlled_joint_ids + ] + if locked_joint_ids: + if self._config.reference_q is None and self._uncontrolled_ee_chain( + self._ee_frame_id, controlled_joint_ids + ): + raise ValueError( + "Pink requires reference_q for an uncontrolled joint on the end-effector chain" + ) + self._model = pinocchio.buildReducedModel( + self._model, locked_joint_ids, full_reference_q + ) + self._data = self._model.createData() + self._q_indices, self._v_indices = self._build_mapping(robot) + self._ee_frame_id = self._validate_frame(robot.end_effector_link) + self._apply_limits(robot) + self._reference_q = self._build_reference_q(use_config_reference=False) + self._configuration = pink.Configuration( + self._model, + self._data, + self._reference_q.copy(), + ) + self._frame_task = pink.tasks.FrameTask( + robot.end_effector_link, + position_cost=config.position_cost, + orientation_cost=config.orientation_cost, + lm_damping=config.lm_damping, + gain=config.task_gain, + ) + + @property + def nq(self) -> int: + """Number of controlled coordinates, matching the task contract.""" + if self._is_pinocchio: + if self._legacy_ik is None: + raise PinkControlRuntimeError("Pinocchio control backend is unavailable") + return self._legacy_ik.nq + return len(self._joint_names) + + def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3: + if self._is_pinocchio: + if self._legacy_ik is None: + raise PinkControlRuntimeError("Pinocchio control backend is unavailable") + return self._legacy_ik.forward_kinematics(q) + full_q = self._full_q(q) + pinocchio.forwardKinematics(self._model, self._data, full_q) + pinocchio.updateFramePlacements(self._model, self._data) + return self._data.oMf[self._ee_frame_id].copy() + + def solve( + self, + target: pinocchio.SE3, + measured: NDArray[np.float64], + dt: float, + ) -> ControlIKResult: + measured = np.asarray(measured, dtype=np.float64).reshape(-1) + if measured.size != len(self._joint_names) or not np.all(np.isfinite(measured)): + raise ValueError("measured joint state is invalid") + if not np.isfinite(dt) or dt <= 0.0: + raise ValueError("control IK dt must be finite and positive") + dt = min(max(dt, self._config.min_dt), self._config.max_dt) + if self._is_pinocchio: + if self._legacy_ik is None: + raise PinkControlRuntimeError("Pinocchio control backend is unavailable") + positions, _, _ = self._legacy_ik.solve(target, measured) + return ControlIKResult(np.asarray(positions, dtype=np.float64), positions - measured) + + configuration = self._configuration + frame_task = self._frame_task + if configuration is None or frame_task is None: + raise PinkControlRuntimeError("Pink control backend is unavailable") + try: + configuration.update(self._full_q(measured)) + frame_task.set_target(target) + velocity = pink.solve_ik( + configuration, + [frame_task], + dt, + solver=self._config.solver, + damping=self._config.lm_damping, + limits=self._limits, + **self._config.qpsolver_options, + ) + velocity = np.asarray(velocity, dtype=np.float64).reshape(-1) + if velocity.size != self._model.nv or not np.all(np.isfinite(velocity)): + raise PinkControlRuntimeError("Pink produced an invalid velocity") + configuration.integrate_inplace(velocity, dt) + candidate = self._controlled_q(configuration.q, measured) + if candidate.size != measured.size or not np.all(np.isfinite(candidate)): + raise PinkControlRuntimeError("Pink produced an invalid joint candidate") + return ControlIKResult(candidate, self._controlled_velocity(velocity)) + except PinkControlRuntimeError: + raise + except Exception as exc: + raise PinkControlRuntimeError(f"Pink control solve failed: {exc}") from exc + + def _full_q(self, controlled: NDArray[np.float64]) -> NDArray[np.float64]: + q = self._reference_q.copy() + for value, index, width in zip(controlled, self._q_indices, self._q_widths, strict=True): + if width == 2: + q[index] = np.cos(value) + q[index + 1] = np.sin(value) + else: + q[index] = value + return q + + def _controlled_q( + self, full_q: NDArray[np.float64], reference: NDArray[np.float64] | None = None + ) -> NDArray[np.float64]: + positions = np.array( + [ + np.arctan2(full_q[index + 1], full_q[index]) if width == 2 else full_q[index] + for index, width in zip(self._q_indices, self._q_widths, strict=True) + ], + dtype=np.float64, + ) + if reference is not None: + for index, width in enumerate(self._q_widths): + if width == 2: + positions[index] = reference[index] + float( + (positions[index] - reference[index] + np.pi) % (2.0 * np.pi) - np.pi + ) + return positions + + def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]: + return np.array([velocity[index] for index in self._v_indices], dtype=np.float64) + + def _build_mapping(self, robot: RobotModelConfig) -> tuple[list[int], list[int]]: + coordinator_names = robot.get_coordinator_joint_names() + if coordinator_names != self._joint_names or len(set(coordinator_names)) != len( + coordinator_names + ): + raise ValueError( + "control task joints must exactly match ordered RobotModelConfig joints" + ) + indices: list[int] = [] + velocity_indices: list[int] = [] + self._q_widths: list[int] = [] + self._controlled_joint_ids: list[int] = [] + for urdf_name in (robot.get_urdf_joint_name(name) for name in coordinator_names): + if not self._model.existJointName(urdf_name): + raise ValueError(f"control joint mapping references unknown joint: {urdf_name}") + joint_id = self._model.getJointId(urdf_name) + if joint_id <= 0 or joint_id >= len(self._model.joints): + raise ValueError(f"invalid control joint index for {urdf_name}") + joint = self._model.joints[joint_id] + if int(joint.nv) != 1 or int(joint.nq) not in (1, 2): + raise ValueError(f"control joint must be one-DoF: {urdf_name}") + indices.append(int(joint.idx_q)) + velocity_indices.append(int(joint.idx_v)) + self._q_widths.append(int(joint.nq)) + self._controlled_joint_ids.append(joint_id) + return indices, velocity_indices + + def _build_reference_q(self, use_config_reference: bool = True) -> NDArray[np.float64]: + if use_config_reference and self._config.reference_q is not None: + q = np.asarray(self._config.reference_q, dtype=np.float64).reshape(-1) + if q.size != self._model.nq or not np.all(np.isfinite(q)): + raise ValueError("Pink reference_q must match model nq and be finite") + else: + q = np.asarray(pinocchio.neutral(self._model), dtype=np.float64) + if not (use_config_reference and self._config.reference_q is not None): + for joint_id in range(1, len(self._model.joints)): + joint = self._model.joints[joint_id] + start = int(joint.idx_q) + width = int(joint.nq) + if width == 2 and int(joint.nv) == 1: + q[start : start + 2] = (1.0, 0.0) + continue + if width != 1: + continue + for index in range(start, start + width): + lower = self._model.lowerPositionLimit[index] + upper = self._model.upperPositionLimit[index] + if np.isfinite(lower) and np.isfinite(upper): + q[index] = (lower + upper) / 2.0 + elif np.isfinite(lower): + q[index] = max(0.0, lower) + elif np.isfinite(upper): + q[index] = min(0.0, upper) + else: + q[index] = 0.0 + if not np.all(np.isfinite(q)): + raise ValueError("Pink reference configuration is not finite") + bounded = np.isfinite(self._model.lowerPositionLimit) & np.isfinite( + self._model.upperPositionLimit + ) + if np.any(q[bounded] < self._model.lowerPositionLimit[bounded]) or np.any( + q[bounded] > self._model.upperPositionLimit[bounded] + ): + raise ValueError("Pink reference configuration violates model limits") + return q + + def _uncontrolled_ee_chain(self, frame_id: int, controlled_joint_ids: set[int]) -> bool: + joint_id = int(self._model.frames[frame_id].parentJoint) + while joint_id > 0: + if joint_id not in controlled_joint_ids: + return True + joint_id = int(self._model.parents[joint_id]) + return False + + def _validate_frame(self, frame_name: str) -> int: + if not self._model.existFrame(frame_name): + raise ValueError(f"unknown control end-effector frame: {frame_name}") + frame_id = int(self._model.getFrameId(frame_name)) + if frame_id < 0 or frame_id >= len(self._model.frames): + raise ValueError(f"invalid control end-effector frame: {frame_name}") + return frame_id + + def _apply_limits(self, robot: RobotModelConfig) -> None: + if robot.joint_limits_lower is not None or robot.joint_limits_upper is not None: + if robot.joint_limits_lower is None or robot.joint_limits_upper is None: + raise ValueError("both configured joint limit bounds are required") + if len(robot.joint_limits_lower) != len(self._joint_names) or len( + robot.joint_limits_upper + ) != len(self._joint_names): + raise ValueError("configured joint limits do not match control joints") + for index, width, lower, upper in zip( + self._q_indices, + self._q_widths, + robot.joint_limits_lower, + robot.joint_limits_upper, + strict=True, + ): + if not np.isfinite(lower) or not np.isfinite(upper) or lower >= upper: + raise ValueError("configured joint limits must be finite and ordered") + if width == 2: + raise ValueError( + "configured position limits for continuous joints require " + "tangent-space angular limit handling" + ) + self._model.lowerPositionLimit[index] = lower + self._model.upperPositionLimit[index] = upper + if robot.velocity_limits is not None: + if len(robot.velocity_limits) != len(self._joint_names) or any( + not np.isfinite(value) or value <= 0.0 for value in robot.velocity_limits + ): + raise ValueError("configured velocity limits are invalid") + for index, limit in zip(self._v_indices, robot.velocity_limits, strict=True): + self._model.velocityLimit[index] = limit + for index in self._v_indices: + self._model.velocityLimit[index] = min( + self._model.velocityLimit[index], self._config.max_velocity + ) + self._limits = [ConfigurationLimit(self._model), VelocityLimit(self._model)] diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py new file mode 100644 index 0000000000..6e20d24e15 --- /dev/null +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -0,0 +1,410 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path + +import numpy as np +import pinocchio +import pytest + +from dimos.control.coordinator import TaskConfig +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import ( + CartesianIKTask, + CartesianIKTaskConfig, +) +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( + ControlIKResult, + PinkControlIK, + PinkControlIKConfig, + PinocchioIK, +) +from dimos.control.tasks.registry import control_task_registry +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + +_URDF = """\ + + + + + + + + + + + + + +""" + +_CONTINUOUS_URDF = """\ + + + + + + + +""" + +_UNCONTROLLED_URDF = """\ + + + + + + + + + + + + + + + +""" + + +def _robot( + path: Path, + *, + frame: str = "tool", + joints: list[str] | None = None, +) -> RobotModelConfig: + joint_names = joints or ["joint1", "joint2"] + joint_count = len(joint_names) + return RobotModelConfig( + name="tiny", + model_path=path, + base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + joint_names=joint_names, + end_effector_link=frame, + home_joints=[0.4] * joint_count, + joint_limits_lower=[-2.0] * joint_count, + joint_limits_upper=[2.0] * joint_count, + velocity_limits=[1.0] * joint_count, + ) + + +def _write_urdf(tmp_path: Path, name: str = "tiny.urdf", content: str = _URDF) -> Path: + path = tmp_path / name + path.write_text(content) + return path + + +def test_pink_is_default_and_requires_robot_model() -> None: + config = PinkControlIKConfig() + + assert config.backend == "pink" + with pytest.raises(ValueError, match="RobotModelConfig"): + config.validate_settings(2, None) + + +def test_pink_prepares_xacro_with_package_paths_and_arguments( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + package_path = tmp_path / "description" + package_path.mkdir() + robot = _robot(model_path).model_copy( + update={ + "model_path": tmp_path / "robot.xacro", + "package_paths": {"description": package_path}, + "xacro_args": {"dof": "2"}, + } + ) + prepared: dict[str, object] = {} + + def prepare( + path: Path, + package_paths: dict[str, Path], + xacro_args: dict[str, str], + convert_meshes: bool, + ) -> str: + prepared.update( + path=path, + package_paths=package_paths, + xacro_args=xacro_args, + convert_meshes=convert_meshes, + ) + return str(model_path) + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.prepare_urdf_for_drake", + prepare, + ) + + PinkControlIK( + tmp_path / "robot.xacro", + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=robot), + ) + + assert prepared == { + "path": tmp_path / "robot.xacro", + "package_paths": {"description": package_path}, + "xacro_args": {"dof": "2"}, + "convert_meshes": False, + } + + +def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> None: + model_path = _write_urdf(tmp_path) + + with pytest.raises(ValueError, match="end-effector frame"): + PinkControlIK( + model_path, + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=_robot(model_path, frame="missing")), + ) + + mismatched = _robot(model_path).model_copy( + update={"joint_name_mapping": {"arm/joint1": "joint1", "arm/joint2": "joint2"}} + ) + with pytest.raises(ValueError, match="exactly match"): + PinkControlIK( + model_path, + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=mismatched), + ) + + +def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + backend = PinkControlIK( + model_path, + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=_robot(model_path)), + ) + measured = np.array([0.3, 0.1]) + target = backend.forward_kinematics(measured) + calls: list[tuple[object, list[object], float]] = [] + + def solve( + configuration: object, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + calls.append((configuration, tasks, dt)) + return np.zeros(backend._model.nv) + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve + ) + result = backend.solve(target, measured, 0.01) + + assert np.array_equal(result.positions, measured) + assert len(calls) == 1 + assert len(calls[0][1]) == 1 + assert calls[0][2] == 0.01 + + +def test_pink_backend_clamps_dt_from_backend_configuration( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + backend = PinkControlIK( + model_path, + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=_robot(model_path), min_dt=0.01, max_dt=0.02), + ) + calls: list[float] = [] + + def solve( + configuration: object, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + calls.append(dt) + return np.zeros(backend._model.nv) + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve + ) + measured = np.array([0.3, 0.1]) + backend.solve(backend.forward_kinematics(measured), measured, 1.0) + + assert calls == [0.02] + + +def test_pink_rejects_uncontrolled_end_effector_chain_without_reference( + tmp_path: Path, +) -> None: + model_path = _write_urdf(tmp_path, "uncontrolled.urdf", _UNCONTROLLED_URDF) + with pytest.raises(ValueError, match="reference_q.*uncontrolled joint"): + PinkControlIK( + model_path, + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=_robot(model_path)), + ) + + +def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path: Path) -> None: + model_path = _write_urdf(tmp_path, "continuous.urdf", _CONTINUOUS_URDF) + robot = _robot(model_path, joints=["joint1"]) + + with pytest.raises(ValueError, match="continuous joints.*tangent-space"): + PinkControlIK( + model_path, + None, + ["joint1"], + PinkControlIKConfig(robot_model=robot), + ) + + roundtrip_robot = robot.model_copy( + update={"joint_limits_lower": None, "joint_limits_upper": None} + ) + backend = PinkControlIK( + model_path, + None, + ["joint1"], + PinkControlIKConfig(robot_model=roundtrip_robot), + ) + angle = np.array([3.0]) + + assert backend._q_widths == [2] + assert np.allclose(backend._controlled_q(backend._full_q(angle), angle), angle) + + +def test_pink_applies_position_velocity_limits_and_finite_output(tmp_path: Path) -> None: + model_path = _write_urdf(tmp_path) + robot = _robot(model_path).model_copy( + update={"joint_limits_lower": [-0.5, -0.25], "joint_limits_upper": [0.5, 0.25]} + ) + backend = PinkControlIK( + model_path, + None, + ["joint1", "joint2"], + PinkControlIKConfig(robot_model=robot, max_velocity=0.2), + ) + + assert np.array_equal(backend._model.lowerPositionLimit[:2], np.array([-0.5, -0.25])) + assert np.all(backend._model.velocityLimit[backend._v_indices] <= 0.2) + result = backend.solve( + backend.forward_kinematics(np.array([0.1, 0.1])), np.array([0.1, 0.1]), 0.01 + ) + assert result.positions.shape == (2,) + assert np.all(np.isfinite(result.positions)) + + +def test_cartesian_pipeline_bounds_dt_and_holds_on_expected_error( + monkeypatch: pytest.MonkeyPatch, +) -> None: + backend = _FakeControlIK() + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", + lambda *args, **kwargs: backend, + ) + task = CartesianIKTask( + "cartesian", + CartesianIKTaskConfig( + joint_names=["j1", "j2"], + model_path="unused.urdf", + timeout=0.2, + ), + ) + pose = PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + assert task.on_cartesian_command(pose, 1.0) + assert task.compute(_cartesian_state(1.01, dt=1.0)) is not None + assert backend.dt_calls == [task._config.control_ik.max_dt] + + invalid_dt_hold = task.compute(_cartesian_state(1.02, dt=0.0)) + assert invalid_dt_hold is not None + assert invalid_dt_hold.positions == [0.0, 0.0] + + backend.raise_runtime = True + assert task.on_cartesian_command(pose, 2.0) + hold = task.compute(_cartesian_state(2.01)) + assert hold is not None + assert hold.positions == [0.0, 0.0] + assert hold.mode.value == "servo_position" + + +def test_factory_rejects_invalid_default_pink_configuration() -> None: + config = TaskConfig( + name="cartesian", + type="cartesian_ik", + joint_names=["j1", "j2"], + priority=10, + params={"model_path": "unused.urdf"}, + ) + + with pytest.raises(ValueError, match="RobotModelConfig"): + control_task_registry.create("cartesian_ik", config, hardware={}) + + +def test_explicit_pinocchio_selection_does_not_fallback_from_pink( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + legacy = _FakeLegacyIK(pinocchio.buildModelFromUrdf(str(model_path))) + + def load(path: Path, ee_joint_id: int) -> _FakeLegacyIK: + legacy.calls.append((path, ee_joint_id)) + return legacy + + monkeypatch.setattr(PinocchioIK, "from_model_path", staticmethod(load)) + + backend = PinkControlIK( + model_path, + 2, + ["joint1", "joint2"], + PinkControlIKConfig(backend="pinocchio"), + ) + + assert backend._is_pinocchio + assert legacy.calls == [(model_path, 2)] + with pytest.raises(ValueError, match="RobotModelConfig"): + PinkControlIK(model_path, None, ["joint1", "joint2"], PinkControlIKConfig()) + + +class _FakeLegacyIK: + nq = 2 + + def __init__(self, model: pinocchio.Model) -> None: + self.model = model + self.calls: list[tuple[Path, int]] = [] + + def forward_kinematics(self, q: np.ndarray) -> pinocchio.SE3: + return pinocchio.SE3.Identity() + + +class _FakeControlIK: + nq = 2 + + def __init__(self) -> None: + self.result = np.array([0.1, 0.2]) + self.raise_runtime = False + self.dt_calls: list[float] = [] + + def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult: + self.dt_calls.append(dt) + if self.raise_runtime: + raise RuntimeError("synthetic control failure") + return ControlIKResult(self.result.copy(), self.result - measured) + + +def _cartesian_state(t_now: float, dt: float = 0.01) -> CoordinatorState: + return CoordinatorState( + joints=JointStateSnapshot(joint_positions={"j1": 0.0, "j2": 0.0}), + t_now=t_now, + dt=dt, + ) diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py index c4c8a71bac..fae665f7ab 100644 --- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py @@ -12,29 +12,25 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Measured-state end-effector twist control.""" + from __future__ import annotations from dataclasses import dataclass from pathlib import Path import threading -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING import numpy as np -from numpy.typing import NDArray import pinocchio -from dimos.control.task import ( - BaseControlTask, - ControlMode, - CoordinatorState, - JointCommandOutput, - ResourceClaim, -) -from dimos.manipulation.planning.kinematics.pinocchio_ik import ( - PinocchioIK, - check_joint_delta, - get_worst_joint_delta, +from dimos.control.coordinator import TaskConfig +from dimos.control.task import CoordinatorState +from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import ( + CartesianIKTask, + CartesianIKTaskConfig, ) +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import twist_to_numpy @@ -44,149 +40,101 @@ logger = setup_logger() -_MAX_DT = 0.05 - @dataclass -class EEFTwistTaskConfig: - joint_names: list[str] - model_path: str | Path - ee_joint_id: int - timeout: float - max_joint_delta_deg: float - priority: int = 10 +class EEFTwistTaskConfig(CartesianIKTaskConfig): + """Configuration for measured-FK-relative EEF twist control.""" -class EEFTwistTask(BaseControlTask): - """Spatial EEF twist task using twist-integrated pose IK.""" +class EEFTwistTask(CartesianIKTask): + """Cartesian task specialization whose target is prepared from a twist.""" def __init__(self, name: str, config: EEFTwistTaskConfig) -> None: - if not config.joint_names: - raise ValueError(f"EEFTwistTask '{name}' requires at least one joint") - if not config.model_path: - raise ValueError(f"EEFTwistTask '{name}' requires model_path for IK solver") - self._name = name - self._config = config - self._joint_names = frozenset(config.joint_names) - self._joint_names_list = list(config.joint_names) - self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id) - if self._ik.nq != len(config.joint_names): - raise ValueError( - f"EEFTwistTask {name}: model DOF ({self._ik.nq}) != " - f"joint_names count ({len(config.joint_names)})" - ) - self._lock = threading.Lock() + super().__init__(name, config) + self._twist_lock = threading.Lock() self._latest_twist: TwistStamped | None = None - self._last_update_time = 0.0 - - @property - def name(self) -> str: - return self._name - - def claim(self) -> ResourceClaim: - return ResourceClaim(self._joint_names, self._config.priority, ControlMode.SERVO_POSITION) def is_active(self) -> bool: + with self._twist_lock: + has_twist = self._latest_twist is not None with self._lock: - return self._latest_twist is not None + return has_twist and self._active + + def is_tracking(self) -> bool: + return self.is_active() + + def _uses_prepared_target(self) -> bool: + return True + + def on_cartesian_command(self, pose: object, t_now: float) -> bool: + """Reject Cartesian stream commands; twist is this task's only input.""" + logger.warning("EEFTwistTask rejects Cartesian commands", task=self.name) + return False def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool: values = twist_to_numpy(twist) - if not np.all(np.isfinite(values)): - logger.warning("EEFTwistTask rejecting non-finite twist", task=self._name) + if values.shape != (6,) or not np.all(np.isfinite(values)): + logger.warning("EEFTwistTask rejecting invalid twist", task=self.name) return False - with self._lock: + with self._twist_lock: if np.allclose(values, 0.0): - self._clear_locked() - self._last_update_time = t_now - return True - self._latest_twist = twist + self._latest_twist = None + cleared = True + else: + self._latest_twist = twist + cleared = False + if cleared: + super().clear() + return True + with self._lock: self._last_update_time = t_now + self._active = True return True - def compute(self, state: CoordinatorState) -> JointCommandOutput | None: - with self._lock: + def _prepare_target( + self, + state: CoordinatorState, + q_current: np.ndarray, + dt: float, + ) -> pinocchio.SE3 | None: + with self._twist_lock: twist = self._latest_twist - if twist is None: - return None - if ( - self._config.timeout > 0 - and state.t_now - self._last_update_time > self._config.timeout - ): - self._clear_locked() - return None - - q_current = self._get_current_joints(state) - if q_current is None or not np.all(np.isfinite(q_current)): - return None - target_pose = self._ik.forward_kinematics(q_current) - dt = min(max(state.dt, 0.0), _MAX_DT) - candidate = self._integrate_twist(target_pose, twist, dt) - - q_solution, converged, final_error = self._ik.solve(candidate, q_current) - if not np.all(np.isfinite(q_solution)): - return None - if not converged: - logger.debug( - "EEFTwistTask IK did not converge, using partial solution", - task=self._name, - error=final_error, - ) - if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg): - worst_idx, worst_deg = get_worst_joint_delta(q_solution, q_current) - logger.warning( - "EEFTwistTask rejecting solution: joint delta exceeds limit", - task=self._name, - joint=self._joint_names_list[worst_idx], - delta_deg=worst_deg, - max_delta_deg=self._config.max_joint_delta_deg, - ) + if twist is None: return None - - return JointCommandOutput( - joint_names=self._joint_names_list, - positions=q_solution.flatten().tolist(), - mode=ControlMode.SERVO_POSITION, - ) - - def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: - if joints & self._joint_names: - logger.warning( - "EEFTwistTask preempted", task=self._name, by_task=by_task, joints=joints - ) - - def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None: - positions = [] - for joint_name in self._joint_names_list: - pos = state.joints.get_position(joint_name) - if pos is None: - return None - positions.append(pos) - return np.array(positions, dtype=np.float64) - - def _clear_locked(self) -> None: - self._latest_twist = None - - def _integrate_twist( - self, pose: pinocchio.SE3, twist: TwistStamped, dt: float - ) -> pinocchio.SE3: - candidate = pose.copy() + pose = self.forward_kinematics(q_current) values = twist_to_numpy(twist) - candidate.translation = candidate.translation + values[:3] * dt + pose.translation = pose.translation + values[:3] * dt angular_step = values[3:] * dt if np.linalg.norm(angular_step) > 0.0: - candidate.rotation = pinocchio.exp3(angular_step) @ candidate.rotation - return candidate + pose.rotation = pinocchio.exp3(angular_step) @ pose.rotation + if not np.all(np.isfinite(pose.translation)) or not np.all(np.isfinite(pose.rotation)): + return None + return pose + + def stop(self) -> None: + with self._twist_lock: + self._latest_twist = None + super().stop() + + def _on_timeout(self) -> None: + with self._twist_lock: + self._latest_twist = None + + def clear(self) -> None: + with self._twist_lock: + self._latest_twist = None + super().clear() class EEFTwistTaskParams(BaseConfig): model_path: str | Path - ee_joint_id: int = 6 + ee_joint_id: int | None = None timeout: float = 0.3 max_joint_delta_deg: float = 15.0 + control_ik: PinkControlIKConfig = PinkControlIKConfig() -def create_task(cfg: Any, hardware: Any) -> EEFTwistTask: +def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask: params = EEFTwistTaskParams.model_validate(cfg.params) return EEFTwistTask( cfg.name, @@ -197,5 +145,6 @@ def create_task(cfg: Any, hardware: Any) -> EEFTwistTask: priority=cfg.priority, timeout=params.timeout, max_joint_delta_deg=params.max_joint_delta_deg, + control_ik=params.control_ik, ), ) diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index c937c2044d..78cc57ac5c 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -21,6 +21,9 @@ import pytest from dimos.control.task import ControlMode, CoordinatorState, JointStateSnapshot +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( + ControlIKResult, +) from dimos.control.tasks.eef_twist_task.eef_twist_task import EEFTwistTask, EEFTwistTaskConfig from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped @@ -39,26 +42,29 @@ def __init__(self) -> None: self.nq = 3 self.fk_calls: list[np.ndarray] = [] self.solve_calls: list[FakePose] = [] + self.dt_calls: list[float] = [] self.solution = np.array([0.01, 0.02, 0.03], dtype=np.float64) self.converged = True self.final_error = 0.0 + self.raise_runtime = False def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose: self.fk_calls.append(q_current.copy()) return FakePose(q_current.copy(), np.eye(3, dtype=np.float64)) - def solve( - self, pose: FakePose, q_current: NDArray[np.float64] - ) -> tuple[NDArray[np.float64], bool, float]: + def solve(self, pose: FakePose, q_current: NDArray[np.float64], dt: float) -> ControlIKResult: + if self.raise_runtime: + raise RuntimeError("synthetic solver failure") self.solve_calls.append(pose.copy()) - return self.solution.copy(), self.converged, self.final_error + self.dt_calls.append(dt) + return ControlIKResult(self.solution.copy(), self.solution - q_current) @pytest.fixture def fake_ik(mocker) -> FakeIK: ik = FakeIK() mocker.patch( - "dimos.control.tasks.eef_twist_task.eef_twist_task.PinocchioIK.from_model_path", + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", return_value=ik, ) return ik @@ -110,6 +116,29 @@ def test_first_nonzero_command_activates_seeds_from_fk_and_outputs_servo_positio assert fake_ik.solve_calls[0].translation[0] > 0.0 +def test_twist_task_rejects_cartesian_commands_and_holds_on_runtime_failure( + task: EEFTwistTask, fake_ik: FakeIK +) -> None: + assert not task.on_cartesian_command(object(), t_now=1.0) + assert task.on_ee_twist_command(_twist(), t_now=1.0) + fake_ik.raise_runtime = True + hold = task.compute(_state(1.01)) + assert hold is not None + assert hold.mode == ControlMode.SERVO_POSITION + assert hold.positions == [0.0, 0.0, 0.0] + + +def test_expected_runtime_twist_error_is_a_bounded_hold( + task: EEFTwistTask, fake_ik: FakeIK +) -> None: + assert task.on_ee_twist_command(_twist(), t_now=1.0) + fake_ik.raise_runtime = True + hold = task.compute(_state(1.01)) + assert hold is not None + assert hold.mode == ControlMode.SERVO_POSITION + assert hold.positions == [0.0, 0.0, 0.0] + + def test_integration_uses_current_fk_and_coordinator_dt( task: EEFTwistTask, fake_ik: FakeIK ) -> None: @@ -143,7 +172,9 @@ def test_non_finite_ik_solution_is_rejected(task: EEFTwistTask, fake_ik: FakeIK) assert task.on_ee_twist_command(_twist(), t_now=1.0) output = task.compute(_state(1.01)) - assert output is None + assert output is not None + assert output.mode == ControlMode.SERVO_POSITION + assert output.positions == [0.0, 0.0, 0.0] def test_non_finite_twist_is_rejected_without_activating_task(task: EEFTwistTask) -> None: @@ -166,13 +197,15 @@ def test_missing_joint_state_skips_fk_and_ik(task: EEFTwistTask, fake_ik: FakeIK assert fake_ik.solve_calls == [] -def test_joint_delta_rejection_returns_none(task: EEFTwistTask, fake_ik: FakeIK) -> None: +def test_joint_delta_rejection_returns_a_hold(task: EEFTwistTask, fake_ik: FakeIK) -> None: assert task.on_ee_twist_command(_twist(), t_now=1.0) fake_ik.solution = np.array([10.0, 0.0, 0.0], dtype=np.float64) rejected = task.compute(_state(1.01)) - assert rejected is None + assert rejected is not None + assert rejected.mode == ControlMode.SERVO_POSITION + assert rejected.positions == [0.0, 0.0, 0.0] def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds( diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py index f869b637e2..6dd0c98c17 100644 --- a/dimos/robot/manipulators/a1z/blueprints/teleop.py +++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py @@ -20,8 +20,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule from dimos.robot.manipulators.a1z.config import ( - A1Z_DOF, - A1Z_FK_MODEL, make_a1z_hardware, make_a1z_model_config, ) @@ -29,15 +27,22 @@ from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule _a1z_keyboard_hw = make_a1z_hardware("arm") +_a1z_model = make_a1z_model_config() keyboard_teleop_a1z = autoconnect( KeyboardTeleopModule.blueprint(), ControlCoordinator.blueprint( hardware=[_a1z_keyboard_hw], - tasks=[eef_twist_task(_a1z_keyboard_hw, model_path=A1Z_FK_MODEL, ee_joint_id=A1Z_DOF)], + tasks=[ + eef_twist_task( + _a1z_keyboard_hw, + model_path=_a1z_model.model_path, + robot_model=_a1z_model, + ) + ], ), ManipulationModule.blueprint( - robots=[make_a1z_model_config()], + robots=[_a1z_model], visualization={"backend": "viser"}, ), ) diff --git a/dimos/robot/manipulators/a750/blueprints/teleop.py b/dimos/robot/manipulators/a750/blueprints/teleop.py index 062c869cab..0786b2bd0d 100644 --- a/dimos/robot/manipulators/a750/blueprints/teleop.py +++ b/dimos/robot/manipulators/a750/blueprints/teleop.py @@ -20,7 +20,7 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule from dimos.robot.manipulators.a750.config import ( - A750_FK_MODEL, + A750_MODEL_PATH, a750_hardware, make_a750_model_config, ) @@ -28,6 +28,7 @@ from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule _a750_hw = a750_hardware("arm", mock_without_address=True) +_a750_model = make_a750_model_config() keyboard_teleop_a750 = autoconnect( KeyboardTeleopModule.blueprint(), @@ -36,10 +37,16 @@ publish_joint_state=True, joint_state_frame_id="coordinator", hardware=[_a750_hw], - tasks=[eef_twist_task(_a750_hw, model_path=A750_FK_MODEL, ee_joint_id=6)], + tasks=[ + eef_twist_task( + _a750_hw, + model_path=A750_MODEL_PATH, + robot_model=_a750_model, + ) + ], ), ManipulationModule.blueprint( - robots=[make_a750_model_config()], + robots=[_a750_model], visualization={"backend": "meshcat"}, ), ) diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index 5bcdc32b89..b5e4fb2ded 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -22,6 +22,7 @@ from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.core.coordination.blueprints import Blueprint from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -48,20 +49,94 @@ def trajectory_task( ) +def _resolve_control_ik( + hardware: HardwareComponent, + model_path: Path, + ee_joint_id: int | None, + control_ik: PinkControlIKConfig | None, + robot_model: RobotModelConfig | None, +) -> PinkControlIKConfig: + resolved = control_ik or PinkControlIKConfig(robot_model=robot_model) + if resolved.backend == "pink": + if robot_model is not None and resolved.robot_model is None: + resolved = resolved.model_copy(update={"robot_model": robot_model}) + elif robot_model is not None and resolved.robot_model is not None: + if resolved.robot_model != robot_model: + raise ValueError("conflicting Pink RobotModelConfig values") + elif resolved.robot_model is None: + raise ValueError("Pink helper requires an authoritative RobotModelConfig") + elif not isinstance(ee_joint_id, int) or isinstance(ee_joint_id, bool): + raise ValueError("Pinocchio helper requires a numeric ee_joint_id") + resolved.validate_settings(len(hardware.joints), ee_joint_id, model_path) + return resolved + + +def _serialize_control_ik(config: PinkControlIKConfig) -> dict[str, object]: + """Serialize solver settings without runtime-only module transport objects.""" + payload: dict[str, object] = config.model_dump(mode="json", exclude={"robot_model"}) + robot_model = config.robot_model + if robot_model is not None: + base_pose = robot_model.base_pose + robot_payload: dict[str, object] = { + "name": robot_model.name, + "model_path": str(robot_model.model_path), + "base_pose": { + "ts": float(base_pose.ts), + "frame_id": base_pose.frame_id, + "position": [base_pose.position.x, base_pose.position.y, base_pose.position.z], + "orientation": [ + base_pose.orientation.x, + base_pose.orientation.y, + base_pose.orientation.z, + base_pose.orientation.w, + ], + }, + "joint_names": list(robot_model.joint_names), + "end_effector_link": robot_model.end_effector_link, + "base_link": robot_model.base_link, + "package_paths": {name: str(path) for name, path in robot_model.package_paths.items()}, + "joint_limits_lower": robot_model.joint_limits_lower, + "joint_limits_upper": robot_model.joint_limits_upper, + "velocity_limits": robot_model.velocity_limits, + "auto_convert_meshes": robot_model.auto_convert_meshes, + "xacro_args": dict(robot_model.xacro_args), + "collision_exclusion_pairs": list(robot_model.collision_exclusion_pairs), + "max_velocity": robot_model.max_velocity, + "max_acceleration": robot_model.max_acceleration, + "joint_name_mapping": dict(robot_model.joint_name_mapping), + "coordinator_task_name": robot_model.coordinator_task_name, + "gripper_hardware_id": robot_model.gripper_hardware_id, + "tf_extra_links": list(robot_model.tf_extra_links), + "home_joints": robot_model.home_joints, + "pre_grasp_offset": robot_model.pre_grasp_offset, + } + payload["robot_model"] = robot_payload + return payload + + def cartesian_ik_task( hardware: HardwareComponent, *, model_path: Path, - ee_joint_id: int, + ee_joint_id: int | None = None, name: str = CARTESIAN_IK_TASK_NAME, priority: int = 10, + control_ik: PinkControlIKConfig | None = None, + robot_model: RobotModelConfig | None = None, ) -> TaskConfig: + resolved_control_ik = _resolve_control_ik( + hardware, model_path, ee_joint_id, control_ik, robot_model + ) return TaskConfig( name=name, type="cartesian_ik", joint_names=hardware.joints, priority=priority, - params={"model_path": model_path, "ee_joint_id": ee_joint_id}, + params={ + "model_path": model_path, + "ee_joint_id": ee_joint_id, + **({"control_ik": _serialize_control_ik(resolved_control_ik)}), + }, ) @@ -69,16 +144,25 @@ def eef_twist_task( hardware: HardwareComponent, *, model_path: Path, - ee_joint_id: int, + ee_joint_id: int | None = None, name: str = EEF_TWIST_TASK_NAME, priority: int = 10, + control_ik: PinkControlIKConfig | None = None, + robot_model: RobotModelConfig | None = None, ) -> TaskConfig: + resolved_control_ik = _resolve_control_ik( + hardware, model_path, ee_joint_id, control_ik, robot_model + ) return TaskConfig( name=name, type="eef_twist", joint_names=hardware.joints, priority=priority, - params={"model_path": model_path, "ee_joint_id": ee_joint_id}, + params={ + "model_path": model_path, + "ee_joint_id": ee_joint_id, + **({"control_ik": _serialize_control_ik(resolved_control_ik)}), + }, ) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index 33e8b27f98..e3cc4ca90e 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -22,22 +22,28 @@ from dimos.robot.manipulators.common.blueprints import eef_twist_task from dimos.robot.manipulators.openarm.config import ( LEFT_CAN, - OPENARM_V10_FK_MODEL, openarm_single_hardware, openarm_single_model_config, ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule _teleop_hw = openarm_single_hardware() +_openarm_model = openarm_single_model_config() keyboard_teleop_openarm_mock = autoconnect( KeyboardTeleopModule.blueprint(), ControlCoordinator.blueprint( hardware=[_teleop_hw], - tasks=[eef_twist_task(_teleop_hw, model_path=OPENARM_V10_FK_MODEL, ee_joint_id=7)], + tasks=[ + eef_twist_task( + _teleop_hw, + model_path=_openarm_model.model_path, + robot_model=_openarm_model, + ) + ], ), ManipulationModule.blueprint( - robots=[openarm_single_model_config()], + robots=[_openarm_model], visualization={"backend": "meshcat"}, ), ) @@ -51,13 +57,13 @@ tasks=[ eef_twist_task( _teleop_real_hw, - model_path=OPENARM_V10_FK_MODEL, - ee_joint_id=7, + model_path=_openarm_model.model_path, + robot_model=_openarm_model, ) ], ), ManipulationModule.blueprint( - robots=[openarm_single_model_config()], + robots=[_openarm_model], visualization={"backend": "meshcat"}, ), ) diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py index 86135ec4bb..6035ad5878 100644 --- a/dimos/robot/manipulators/piper/blueprints/teleop.py +++ b/dimos/robot/manipulators/piper/blueprints/teleop.py @@ -29,6 +29,7 @@ from dimos.robot.manipulators.common.sim import mujoco_if_sim from dimos.robot.manipulators.piper.config import ( PIPER_FK_MODEL, + PIPER_MODEL_PATH, PIPER_SIM_PATH, make_piper_hardware, make_piper_model_config, @@ -36,6 +37,8 @@ ) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule +_piper_model = make_piper_model_config() + _piper_keyboard_hw = make_piper_hardware( "arm", adapter_type="piper" if global_config.can_port else "mock", @@ -50,10 +53,16 @@ publish_joint_state=True, joint_state_frame_id="coordinator", hardware=[_piper_keyboard_hw], - tasks=[eef_twist_task(_piper_keyboard_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], + tasks=[ + eef_twist_task( + _piper_keyboard_hw, + model_path=PIPER_MODEL_PATH, + robot_model=_piper_model, + ) + ], ), ManipulationModule.blueprint( - robots=[make_piper_model_config()], + robots=[_piper_model], visualization={"backend": "meshcat"}, ), ) @@ -65,7 +74,13 @@ coordinator_cartesian_ik_mock = ControlCoordinator.blueprint( hardware=[_piper_mock_cartesian_hw], - tasks=[cartesian_ik_task(_piper_mock_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], + tasks=[ + cartesian_ik_task( + _piper_mock_cartesian_hw, + model_path=PIPER_MODEL_PATH, + robot_model=_piper_model, + ) + ], ) _piper_teleop_hw = piper_hardware("arm") @@ -100,5 +115,11 @@ coordinator_cartesian_ik_piper = ControlCoordinator.blueprint( hardware=[_piper_cartesian_hw], - tasks=[cartesian_ik_task(_piper_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)], + tasks=[ + cartesian_ik_task( + _piper_cartesian_hw, + model_path=PIPER_MODEL_PATH, + robot_model=_piper_model, + ) + ], ) diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 3e7848beb1..3a80d065f9 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -13,11 +13,12 @@ # limitations under the License. from pathlib import Path -from typing import Any +from typing import cast import pytest from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.core.coordination.blueprints import Blueprint from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig from dimos.manipulation.visualization.config import NoManipulationVisualizationConfig @@ -29,7 +30,12 @@ keyboard_teleop_openarm, keyboard_teleop_openarm_mock, ) -from dimos.robot.manipulators.piper.blueprints.teleop import keyboard_teleop_piper +from dimos.robot.manipulators.piper.blueprints.teleop import ( + coordinator_cartesian_ik_mock, + coordinator_cartesian_ik_piper, + keyboard_teleop_piper, +) +from dimos.robot.manipulators.piper.config import PIPER_MODEL_PATH from dimos.robot.manipulators.xarm.blueprints.basic import ( dual_xarm6_planner, xarm6_planner_only, @@ -43,11 +49,11 @@ from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -def _module_kwargs(blueprint: Blueprint, module_type: type) -> dict[str, Any]: +def _module_kwargs(blueprint: Blueprint, module_type: type) -> dict[str, object]: return next(atom.kwargs for atom in blueprint.blueprints if atom.module is module_type) -def _manipulation_kwargs(blueprint: Blueprint) -> dict[str, Any]: +def _manipulation_kwargs(blueprint: Blueprint) -> dict[str, object]: return _module_kwargs(blueprint, ManipulationModule) @@ -56,7 +62,7 @@ def _manipulation_config(blueprint: Blueprint) -> ManipulationModuleConfig: def _coordinator_tasks(blueprint: Blueprint) -> list[TaskConfig]: - return _module_kwargs(blueprint, ControlCoordinator)["tasks"] + return cast("list[TaskConfig]", _module_kwargs(blueprint, ControlCoordinator)["tasks"]) def test_planner_helper_defaults_to_no_visualization() -> None: @@ -85,15 +91,11 @@ def test_xarm_planner_blueprints_default_to_no_visualization() -> None: assert isinstance(config.visualization, NoManipulationVisualizationConfig) -def test_eef_twist_task_helper_uses_hardware_joints_and_default_name() -> None: +def test_eef_twist_task_helper_requires_pink_robot_model() -> None: hardware = make_xarm_hardware("arm", 6, adapter_type="mock") - task = eef_twist_task(hardware, model_path=Path("fake.urdf"), ee_joint_id=6) - - assert task.name == EEF_TWIST_TASK_NAME - assert task.type == "eef_twist" - assert task.joint_names == hardware.joints - assert task.params == {"model_path": Path("fake.urdf"), "ee_joint_id": 6} + with pytest.raises(ValueError, match="authoritative RobotModelConfig"): + eef_twist_task(hardware, model_path=Path("fake.urdf"), ee_joint_id=6) @pytest.mark.parametrize( @@ -118,3 +120,49 @@ def test_manipulator_keyboard_blueprint_uses_eef_twist_and_light_keyboard_kwargs assert keyboard_kwargs == {} assert [task.name for task in eef_twist_tasks] == [EEF_TWIST_TASK_NAME] assert all(task.type != "cartesian_ik" for task in coordinator_tasks) + + +@pytest.mark.parametrize( + "blueprint", + [ + pytest.param(keyboard_teleop_xarm6, id="xarm6"), + pytest.param(keyboard_teleop_xarm7, id="xarm7"), + pytest.param(keyboard_teleop_piper, id="piper"), + pytest.param(keyboard_teleop_openarm_mock, id="openarm-mock"), + pytest.param(keyboard_teleop_openarm, id="openarm"), + pytest.param(keyboard_teleop_a750, id="a750"), + pytest.param(keyboard_teleop_a1z, id="a1z"), + ], +) +def test_shipped_eef_twist_blueprints_use_pink_with_named_models( + blueprint: Blueprint, +) -> None: + task = next(task for task in _coordinator_tasks(blueprint) if task.type == "eef_twist") + control_ik = task.params["control_ik"] + + assert control_ik["backend"] == "pink" + assert control_ik["robot_model"]["end_effector_link"] + assert task.params["ee_joint_id"] is None + assert not str(task.params["model_path"]).endswith((".xml", ".mjcf")) + + +def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: + blueprints = (keyboard_teleop_piper, coordinator_cartesian_ik_mock, coordinator_cartesian_ik_piper) + for blueprint in blueprints: + task = next( + task + for task in _coordinator_tasks(blueprint) + if task.type in ("eef_twist", "cartesian_ik") + ) + control_ik = task.params["control_ik"] + assert task.params["model_path"] == PIPER_MODEL_PATH + assert control_ik["backend"] == "pink" + assert control_ik["robot_model"]["model_path"] == str(PIPER_MODEL_PATH) + assert control_ik["robot_model"]["end_effector_link"] == "gripper_base" + assert task.params["ee_joint_id"] is None + assert "self_collision_enabled" not in control_ik + + reconstructed = PinkControlIKConfig.model_validate(control_ik) + assert reconstructed.robot_model is not None + assert reconstructed.robot_model.model_path == PIPER_MODEL_PATH + assert reconstructed.robot_model.end_effector_link == "gripper_base" diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py index cc9e7c1880..0f0f85c0b4 100644 --- a/dimos/robot/manipulators/xarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -52,6 +52,9 @@ address=global_config.xarm7_ip, ) +_xarm6_model = make_xarm6_model_config(add_gripper=False) +_xarm7_model = make_xarm7_model_config(add_gripper=False) + keyboard_teleop_xarm6 = autoconnect( KeyboardTeleopModule.blueprint(), ControlCoordinator.blueprint( @@ -59,10 +62,16 @@ publish_joint_state=True, joint_state_frame_id="coordinator", hardware=[_xarm6_hw], - tasks=[eef_twist_task(_xarm6_hw, model_path=XARM6_FK_MODEL, ee_joint_id=6)], + tasks=[ + eef_twist_task( + _xarm6_hw, + model_path=_xarm6_model.model_path, + robot_model=_xarm6_model, + ) + ], ), ManipulationModule.blueprint( - robots=[make_xarm6_model_config(add_gripper=False)], + robots=[_xarm6_model], visualization={"backend": "meshcat"}, ), ) @@ -74,10 +83,16 @@ publish_joint_state=True, joint_state_frame_id="coordinator", hardware=[_xarm7_hw], - tasks=[eef_twist_task(_xarm7_hw, model_path=XARM7_FK_MODEL, ee_joint_id=7)], + tasks=[ + eef_twist_task( + _xarm7_hw, + model_path=_xarm7_model.model_path, + robot_model=_xarm7_model, + ) + ], ), ManipulationModule.blueprint( - robots=[make_xarm7_model_config(add_gripper=False)], + robots=[_xarm7_model], visualization={"backend": "meshcat"}, ), ) diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index df1299981d..baf6cf020a 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -569,6 +569,75 @@ yourarm_planner = manipulation_module( | `coordinator_task_name` | Must match the `TaskConfig.name` in your coordinator blueprint | | `collision_exclusion_pairs` | List of `(link_a, link_b)` tuples for links that may legitimately touch (e.g., gripper fingers) | +### 4d. Configure Cartesian and EEF-twist control IK + +Pink is the default backend for Cartesian and EEF-twist control. The legacy +Pinocchio backend is available only through an explicit +`backend="pinocchio"` setting. Pink control and manipulation planning are +separate: planning uses `WorldSpec` and its selected planning backend, while +control performs one local differential-IK step and does not use `WorldSpec` as +a control input. + +Use the same `RobotModelConfig` for the control model and planning robot +metadata. Its `model_path` points to the direct URDF or Xacro, `package_paths` +and `xacro_args` describe model preparation, `end_effector_link` names the EEF +frame, and `joint_name_mapping` maps coordinator joints to URDF joints. Pink +validates the prepared model, named frame, and exact ordered joint mapping at +startup. + +The common helper passes that typed configuration to Pink: + +```python skip +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, eef_twist_task + +control_ik = PinkControlIKConfig(robot_model=robot_model) + +cartesian_task = cartesian_ik_task( + hardware, + model_path=robot_model.model_path, + robot_model=robot_model, + control_ik=control_ik, +) +twist_task = eef_twist_task( + hardware, + model_path=robot_model.model_path, + robot_model=robot_model, + control_ik=control_ik, +) +``` + +Do not provide `ee_joint_id` for Pink tasks. To retain the legacy path during +migration, select it explicitly and provide its numeric EEF ID: + +```python skip +legacy_control_ik = PinkControlIKConfig(backend="pinocchio") +legacy_task = cartesian_ik_task( + hardware, + model_path=legacy_model_path, + ee_joint_id=6, + control_ik=legacy_control_ik, +) +``` + +At every coordinator tick, Pink re-anchors to measured joints, derives the EEF +target from measured FK for twist input, clamps `dt`, updates one `FrameTask`, +integrates one step, and applies position and velocity limits. The shared task +pipeline validates finite bounded output and uses a safe hold for expected +runtime solve errors. Invalid models, frames, mappings, or model-preparation +inputs fail startup; Pink is never silently replaced by Pinocchio. + +Piper follows the same path as other arms. Its Cartesian and EEF-twist tasks use +the matching existing Xacro/URDF model, `make_piper_model_config()`, and the +named `gripper_base` frame. They do not use the previous MJCF model or numeric +EEF ID on the Pink path. + +Before hardware, validate the configuration in simulation or replay at the +coordinator rate. Benchmark end-to-end latency, exercise Cartesian and twist +commands, verify startup diagnostics and runtime safe holds, and confirm +emergency-stop readiness. Hardware validation is future work and must be +supervised and low speed; this guide does not claim that it has occurred. + ## Step 5: Register Blueprints The blueprint registry in `dimos/robot/all_blueprints.py` is **auto-generated** by scanning the codebase for blueprint declarations. After adding your blueprints: diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 9851c0e8fe..3c79a97e99 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -118,6 +118,65 @@ request. For example, `planner_name=roboplan` requires `world_backend=roboplan`, and `kinematics.backend=drake_optimization` requires `world_backend=drake`. +### Cartesian control IK + +Cartesian and keyboard EEF-twist tasks use generic Pink control IK by default. +Select the legacy Pinocchio backend only explicitly with +`backend="pinocchio"`; a failed Pink setup does not silently select Pinocchio. + +Pink control uses the direct URDF/Xacro model from `RobotModelConfig`. Package +paths and Xacro arguments are prepared before startup. The configuration names +the end-effector frame and maps coordinator joints to model joints; missing +frames, mismatched mappings, or an invalid prepared model fail initialization. + +Each control tick starts from measured joints, clamps `dt`, updates one Pink +`FrameTask`, solves and integrates one local differential-IK step, and applies +position and velocity limits. Non-finite or unsafe output is rejected. Expected +runtime solve errors produce a bounded safe hold instead of an invalid command. + +The control backend is separate from manipulation planning. It does not use +`WorldSpec` to control the robot and makes no planning-world or dynamic-obstacle +avoidance claim. `WorldSpec` and its Pink/Drake backends remain responsible for +planning behavior. + +For a custom robot, the current helper API passes the typed model configuration +to Pink without a numeric EEF ID: + +```python skip +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task + +control_ik = PinkControlIKConfig(robot_model=robot_model) +task = cartesian_ik_task( + hardware, + model_path=robot_model.model_path, + robot_model=robot_model, + control_ik=control_ik, +) +``` + +The compatibility path remains explicit and uses the legacy numeric EEF ID: + +```python skip +legacy_task = cartesian_ik_task( + hardware, + model_path=legacy_model_path, + ee_joint_id=6, + control_ik=PinkControlIKConfig(backend="pinocchio"), +) +``` + +Piper's Cartesian and EEF-twist blueprints use the matching Xacro/URDF +`PIPER_MODEL_PATH`, `make_piper_model_config()`, and named `gripper_base` frame. +Piper's Pink configuration does not use its previous MJCF model or numeric EEF +ID. + +Validate a rollout in simulation or replay first: exercise Cartesian and twist +commands at the coordinator rate, benchmark end-to-end control latency, verify +model/frame diagnostics and safe holds, and confirm emergency-stop readiness. +Hardware validation remains future work and must be supervised and low speed; +no hardware validation is claimed here. + Install the manipulation dependencies: ```bash @@ -214,7 +273,7 @@ KeyboardTeleopModule ──→ ControlCoordinator ──→ ManipulationModule (pygame UI) (100Hz tick loop) (WorldSpec backend) │ │ │ TwistStamped EEFTwistTask RRT planner - spatial EEF twist (Pinocchio FK/IK) JacobianIK + spatial EEF twist (Pink control IK) JacobianIK │ DrakeWorld JointState ────────────→ (visualization) ``` diff --git a/openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml b/openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml new file mode 100644 index 0000000000..2800d4ba85 --- /dev/null +++ b/openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-07-14 diff --git a/openspec/changes/add-pink-control-ik-self-collision/design.md b/openspec/changes/add-pink-control-ik-self-collision/design.md new file mode 100644 index 0000000000..e488d625d5 --- /dev/null +++ b/openspec/changes/add-pink-control-ik-self-collision/design.md @@ -0,0 +1,123 @@ +## Context + +`CartesianIKTask` and `EEFTwistTask` currently rely on the legacy +`PinocchioIK` implementation. Cartesian control receives `PoseStamped` targets; +keyboard teleoperation sends `TwistStamped` commands to the EEF-twist task. +The two tasks need one control pipeline and one default backend while retaining +an explicit compatibility route for existing Pinocchio behavior. + +## Goals / Non-Goals + +**Goals:** + +- Make generic Pink the default control IK backend for Cartesian and EEF-twist + tasks in `ControlCoordinator`. +- Keep legacy PinocchioIK available only through explicit + `backend="pinocchio"` selection. +- Validate named EEF frames, controlled joints, model mappings, and prepared + URDF/Xacro assets before task startup. +- Re-anchor every control tick to the coordinator's measured joint state. +- Clamp `dt`, update a Pink frame task, solve one differential step, integrate + once, apply joint and velocity limits, and emit a finite bounded position + command. +- Hold safely on expected runtime solver errors or invalid solver output. +- Migrate common helpers and every shipped Cartesian/EEF-twist blueprint to + the Pink default without a Piper backend exception. +- Migrate Piper from its MJCF/numeric EEF path to the matching existing + Xacro/URDF model and named `gripper_base` frame. + +**Non-Goals:** + +- Self-collision or world-obstacle avoidance. +- Planning-world or dynamic-obstacle avoidance in live control. +- Replacing `WorldSpec` or manipulation planning Pink/Drake behavior. +- New streams, RPCs, skills, MCP tools, CLI commands, or generated registries. + +## Control Architecture + +`CartesianIKTask` owns a shared target-to-command pipeline. It validates the +task model and named EEF frame, prepares a target, reads measured joints, +clamps the tick duration, performs one backend solve, validates the finite +bounded result, and builds the servo-position output. Expected runtime solver +errors produce a bounded hold rather than an invalid command. + +`EEFTwistTask` subclasses `CartesianIKTask`. It prepares a short-horizon pose +target by applying the latest twist to forward kinematics computed from the +current measured joints, then delegates the solve and output path to the base +task. Pose and twist streams remain independently routed by the coordinator. + +The Pink backend loads the prepared direct URDF/Xacro model, resolves the +configured joint mapping and named EEF frame, owns a Pink configuration and +frame task, applies joint/velocity limits, solves one step, and integrates the +finite velocity using the clamped `dt`. The legacy backend uses the existing +PinocchioIK path only when the typed backend setting is explicitly +`"pinocchio"`. + +Model preparation is shared and deterministic: Xacro arguments and package +paths are resolved before backend construction, the resulting URDF is used by +Pink, and frame/joint mismatches fail startup with diagnostics. + +## Backend and Blueprint Decisions + +### Pink is the default + +The typed control configuration defaults to Pink. `backend="pinocchio"` is an +explicit escape hatch for compatibility and testing; no task helper or shipped +blueprint silently selects it. + +Common Cartesian and EEF-twist helpers pass the backend selection through task +configuration. All shipped task blueprints use the default Pink path. Piper is +not special-cased by backend. + +### Piper model and frame migration + +Piper Cartesian and EEF-twist tasks stop using the MJCF/numeric EEF path. They +use the matching existing Xacro/URDF model and the named `gripper_base` frame, +with the same model/joint mapping validation as other robots. + +### Planning/control separation + +Control Pink is a local differential-IK backend. Manipulation planning retains +its separate `WorldSpec` and planning Pink/Drake integration. Neither layer is +changed to provide collision behavior by this proposal. + +## Runtime Safety and Rollout + +Measured-state anchoring prevents command lag from accumulating a virtual +configuration. The `dt` clamp, joint and velocity limits, finite-value checks, +bounded joint-delta checks, and hold-on-error behavior remain in the shared +pipeline. + +Validate the default Pink path in simulation or replay at the coordinator rate +before hardware use. Benchmark end-to-end control latency, exercise Cartesian +and twist targets across normal workspace motion, verify model/frame mapping and +runtime error holds, and confirm emergency-stop readiness. Any hardware check +must be supervised and low speed. + +## Risks / Trade-offs + +- Pink may add control-loop latency; benchmark the complete coordinator path and + retain explicit Pinocchio selection for compatibility. +- URDF/Xacro frame or joint mismatches can prevent startup; validate them before + backend construction and provide actionable diagnostics. +- Differential IK can fail near singularities or conflicting limits; preserve + finite-output validation and bounded holds for expected runtime failures. +- Sharing the target-preparation boundary through inheritance requires focused + Cartesian and twist lifecycle tests, including timeout and clear behavior. + +## Migration / Rollout + +Implement the backend seam and shared pipeline first, then switch common helpers +and shipped task blueprints to Pink by default. Migrate Piper's model path and +EEF frame to the existing Xacro/URDF and `gripper_base`. Keep Pinocchio +available only when explicitly configured. Run focused tests and simulation or +replay latency validation before supervised low-speed hardware validation. + +## Open Questions + +- Confirm the exact existing Piper Xacro/URDF asset and package arguments for + each hardware and simulation blueprint. +- Select the coordinator-rate latency budget and benchmark thresholds for Pink + versus the explicit Pinocchio compatibility path. +- Define coordinator-visible diagnostics for startup model errors and bounded + runtime holds without changing stream contracts. diff --git a/openspec/changes/add-pink-control-ik-self-collision/docs.md b/openspec/changes/add-pink-control-ik-self-collision/docs.md new file mode 100644 index 0000000000..b2864a7180 --- /dev/null +++ b/openspec/changes/add-pink-control-ik-self-collision/docs.md @@ -0,0 +1,30 @@ +## Documentation Updates + +- Update `docs/capabilities/manipulation/index.md` to describe Pink as the + default Cartesian and EEF-twist control IK backend, with explicit + `backend="pinocchio"` compatibility selection. +- Update the same manipulation capability documentation to explain named EEF + frames, URDF/Xacro preparation, model/joint mapping validation, measured-state + anchoring, bounded one-step control, runtime holds, and the distinction from + planning `WorldSpec`. +- Update the existing Piper-related sections in the manipulation capability + documentation to state that Piper uses the matching Xacro/URDF model and + named `gripper_base` frame. Do not describe collision protection or create a + new Piper document. +- Update `docs/capabilities/manipulation/adding_a_custom_arm.md` with the + generic Pink control configuration, explicit legacy Pinocchio selection, + direct model preparation, frame/joint validation, and task-helper usage. +- Document simulation/replay latency benchmarking and supervised low-speed + hardware rollout checks, without claiming that validation has occurred. + +## Out of Scope + +Do not document self-collision or planning-world obstacle avoidance as control +features. This change does not add collision behavior. + +## Doc Validation + +- Run the repository documentation link checker if the changed documentation + participates in it. +- Run `md-babel-py run ` for changed executable examples when the + tool is available. diff --git a/openspec/changes/add-pink-control-ik-self-collision/proposal.md b/openspec/changes/add-pink-control-ik-self-collision/proposal.md new file mode 100644 index 0000000000..584a3d1208 --- /dev/null +++ b/openspec/changes/add-pink-control-ik-self-collision/proposal.md @@ -0,0 +1,54 @@ +## Why + +Cartesian and end-effector twist control currently use the legacy +`PinocchioIK` path. The control stack needs one generic, bounded differential-IK +backend so Cartesian pose and keyboard twist tasks share the same measured-state +and command-safety behavior. + +## What Changes + +- Add generic Pink control IK as the default `ControlCoordinator` backend for + Cartesian and EEF-twist tasks. +- Retain `PinocchioIK` only through an explicit `backend="pinocchio"` option for + compatibility. +- Validate named end-effector frames, model/joint mappings, and prepared + URDF/Xacro models before control starts. +- Re-anchor every solve to measured joints, use bounded `dt`, apply Pink frame + tasks with one-step integration, enforce joint/velocity limits, and hold on + expected runtime solve errors or invalid output. +- Update common task helpers and all shipped task blueprints to use Pink by + default without a Piper-specific backend exception. +- Migrate Piper Cartesian and EEF-twist control from its MJCF/numeric EEF path + to the matching existing Xacro/URDF model and named `gripper_base` frame. + +Self-collision and world-obstacle avoidance are out of scope for this change. + +## Affected DimOS Surfaces + +- `CartesianIKTask`, `EEFTwistTask`, `ControlCoordinator` task configuration, + `PoseStamped`, `TwistStamped`, and `JointCommandOutput` behavior. +- Common Cartesian and EEF-twist task helpers and shipped manipulator + blueprints, including Piper. +- Pink and legacy Pinocchio control-IK backend selection and model preparation. +- Simulation/replay and hardware control latency validation. + +## Capabilities + +### New Capabilities + +- `pink-control-ik`: Generic Pink differential control IK for Cartesian and EEF + twist tasks, with explicit legacy Pinocchio compatibility. + +### Modified Capabilities + +- None. No baseline OpenSpec capability specification exists for this control + path. + +## Impact + +Pink becomes the default control behavior for all shipped Cartesian and EEF-twist +task blueprints. Existing users can select `backend="pinocchio"` explicitly +during migration. Piper uses its existing Xacro/URDF model and +`gripper_base` frame instead of its MJCF/numeric EEF path. The rollout requires +focused backend, task, blueprint, and model-preparation tests plus +simulation/replay latency validation; it does not add self-collision behavior. diff --git a/openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md b/openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md new file mode 100644 index 0000000000..beb7e1688c --- /dev/null +++ b/openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md @@ -0,0 +1,76 @@ +## ADDED Requirements + +### Requirement: Pink is the default control IK backend +The system SHALL use generic Pink control IK by default for Cartesian and EEF-twist tasks created by `ControlCoordinator`. The typed backend configuration SHALL retain `backend="pinocchio"` as an explicit legacy compatibility option. The system SHALL NOT silently select Pinocchio when Pink is the configured backend or when Pink initialization fails. + +#### Scenario: Shipped Cartesian task uses the default backend +- **GIVEN** a shipped Cartesian task without an explicit backend override +- **WHEN** the task is constructed +- **THEN** it SHALL construct the Pink control IK backend + +#### Scenario: Legacy backend is explicitly selected +- **GIVEN** a Cartesian or EEF-twist task configured with `backend="pinocchio"` +- **WHEN** the task is constructed +- **THEN** it SHALL use the legacy PinocchioIK backend +- **AND** no implicit backend migration SHALL occur for that task + +### Requirement: Model and named frame validation +The system SHALL prepare the configured URDF/Xacro model and validate the named end-effector frame, controlled-joint mapping, and model/task joint correspondence before control starts. Invalid or missing model, frame, or mapping configuration SHALL fail initialization with a diagnostic error. + +#### Scenario: Valid Xacro model and named frame +- **GIVEN** a task with resolvable Xacro package paths and arguments, a valid URDF result, a named EEF frame, and matching mapped joints +- **WHEN** the Pink backend initializes +- **THEN** it SHALL construct the frame task from that model and frame + +#### Scenario: Frame or joint mapping is invalid +- **GIVEN** a task whose named EEF frame or mapped controlled joint is absent from the prepared model +- **WHEN** the backend initializes +- **THEN** initialization SHALL fail with a diagnostic error + +### Requirement: Measured-state one-step control +The Pink backend SHALL re-anchor its configuration to the coordinator's current measured joint state on every control tick. It SHALL update a named frame task, solve one differential-IK step, clamp the tick duration to the configured safe bounds, integrate the finite velocity once, and return a bounded joint-position candidate. + +#### Scenario: Robot state lags the previous command +- **GIVEN** measured joints differ from the previously emitted command +- **WHEN** the next Cartesian or EEF-twist tick runs +- **THEN** the backend SHALL start from the measured joints +- **AND** EEF-twist target preparation SHALL use FK from those measured joints + +#### Scenario: Tick duration exceeds the safe bound +- **GIVEN** a control tick with an elapsed duration outside the configured safe range +- **WHEN** one-step integration runs +- **THEN** the backend SHALL use the bounded duration +- **AND** it SHALL emit a finite bounded position candidate + +### Requirement: Control limits and runtime failure behavior +The Pink control solve SHALL enforce configured joint-position and velocity limits. The shared task pipeline SHALL reject non-finite or unbounded output and SHALL produce a bounded hold for expected runtime solve errors rather than emitting an invalid command. + +#### Scenario: Solver returns a valid limited step +- **GIVEN** a valid target and measured state +- **WHEN** Pink solves one step +- **THEN** the emitted position command SHALL respect joint and velocity limits +- **AND** the command SHALL pass finite-value and joint-delta safety checks + +#### Scenario: Expected runtime solve error +- **GIVEN** Pink raises an expected runtime solve error during a control tick +- **WHEN** the task handles the backend result +- **THEN** it SHALL emit a bounded hold or equivalent safe command +- **AND** it SHALL NOT emit non-finite or unvalidated joint positions + +### Requirement: Shared task and blueprint migration +Common Cartesian and EEF-twist task helpers SHALL expose the same backend configuration, default to Pink, and preserve independent coordinator routing and lifecycle behavior. All shipped Cartesian and EEF-twist task blueprints SHALL use that default without a Piper-specific backend exception. Piper Cartesian and EEF-twist tasks SHALL use the matching existing Xacro/URDF model and named `gripper_base` frame instead of the MJCF/numeric EEF path. + +#### Scenario: Piper task is constructed +- **GIVEN** a shipped Piper Cartesian or EEF-twist blueprint without an explicit legacy override +- **WHEN** its task configuration is built +- **THEN** it SHALL select Pink +- **AND** it SHALL use the matching Xacro/URDF model and `gripper_base` frame + +### Requirement: Control and planning remain separate +The control Pink backend SHALL provide local Cartesian differential IK only. It SHALL NOT claim, load, or enforce planning-world or dynamic-obstacle avoidance. Manipulation planning SHALL remain responsible for its separate `WorldSpec` and planning backend behavior. + +#### Scenario: Planning knows about a world obstacle +- **GIVEN** an obstacle represented only in the planning world +- **WHEN** a control IK command is generated +- **THEN** control SHALL apply its configured kinematic and command-safety behavior only +- **AND** world-obstacle handling SHALL remain the responsibility of planning diff --git a/openspec/changes/add-pink-control-ik-self-collision/tasks.md b/openspec/changes/add-pink-control-ik-self-collision/tasks.md new file mode 100644 index 0000000000..52ee70a656 --- /dev/null +++ b/openspec/changes/add-pink-control-ik-self-collision/tasks.md @@ -0,0 +1,72 @@ +## 1. Pink control backend + +- [x] 1.1 Define the typed control backend configuration with Pink as the + default and `backend="pinocchio"` as the only explicit legacy option. +- [x] 1.2 Implement shared URDF/Xacro model preparation, including package and + Xacro argument resolution, named EEF frame validation, controlled-joint + mapping validation, and actionable startup diagnostics. +- [x] 1.3 Implement the generic Pink one-step backend: measured-state reset, + named frame task update, bounded `dt`, joint/velocity limits, finite velocity + integration, and normalized joint-position result. +- [x] 1.4 Preserve the existing PinocchioIK implementation behind explicit + `backend="pinocchio"` selection; do not silently fall back from Pink. +- [x] 1.5 Add expected runtime solve-error handling that emits a bounded hold + and rejects non-finite or otherwise invalid backend output. + +## 2. Shared tasks and blueprint migration + +- [x] 2.1 Refactor `CartesianIKTask` so target preparation, measured-state + extraction, backend solve, output validation, timeout, and hold behavior form + one reusable pipeline. +- [x] 2.2 Make `EEFTwistTask` a Cartesian task specialization that derives its + short-horizon target from measured FK and the bounded twist increment. +- [x] 2.3 Preserve independent Cartesian and EEF-twist coordinator routing, + lifecycle, timeout, zero-input, and clear semantics. +- [x] 2.4 Update common task helpers so Pink is the default backend and + Pinocchio requires explicit `backend="pinocchio"`. +- [x] 2.5 Update every shipped Cartesian and EEF-twist task blueprint to use + the Pink default without backend special cases. +- [x] 2.6 Migrate Piper Cartesian and EEF-twist tasks from the MJCF/numeric EEF + path to the matching existing Xacro/URDF model and named `gripper_base` + frame. + +## 3. Tests + +- [x] 3.1 Test Pink initialization, URDF/Xacro preparation, named EEF frame + validation, model/joint mapping validation, and startup diagnostics. +- [x] 3.2 Test measured-state re-anchoring, bounded `dt`, frame-task one-step + integration, joint/velocity limits, finite output, and bounded holds on + expected runtime solve errors. +- [x] 3.3 Test explicit legacy `backend="pinocchio"` compatibility and prove + that invalid Pink setup is not silently converted to Pinocchio. +- [x] 3.4 Test the shared Cartesian and EEF-twist pipeline, including measured FK + target preparation, timeout, zero input, clear behavior, and output guards. +- [x] 3.5 Test common helper defaults and all shipped blueprint backend settings. +- [x] 3.6 Test Piper's Xacro/URDF model selection, named `gripper_base` frame, + and removal of its MJCF/numeric EEF configuration. + +## 4. Documentation + +- [x] 4.1 Update manipulation capability documentation with the default Pink + backend, explicit Pinocchio compatibility, model/frame validation, runtime + holds, planning/control separation, and non-collision rollout guidance. +- [x] 4.2 Update the custom-arm integration guide with generic Pink task + configuration, direct URDF/Xacro preparation, mapping validation, explicit + legacy backend selection, and Piper's model/frame migration reference. + +## 5. Verification and rollout + +- [x] 5.1 Run `openspec validate add-pink-control-ik-self-collision`. +- [x] 5.2 Run focused tests for Pink control IK, Cartesian IK, EEF twist, + common helpers, Piper blueprints, and model preparation. +- [x] 5.3 Run the blueprint registry generation test if blueprint discovery + inputs change. +- [x] 5.4 Run the relevant documentation link checker and executable-example + validation when available. +- [x] 5.5 Run type and lint checks for changed control/manipulation modules. +- [ ] 5.6 Validate Pink in simulation or replay at the coordinator rate, + benchmark end-to-end control latency, exercise Cartesian and twist commands, + and verify bounded holds and emergency-stop readiness. +- [ ] 5.7 Perform supervised low-speed hardware validation only after + simulation/replay checks pass; record latency and runtime error behavior + without claiming validation before it occurs. diff --git a/pyproject.toml b/pyproject.toml index f75f4ba326..db5de5a512 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -568,6 +568,8 @@ module = [ "nav_msgs.*", "open_clip", "pinocchio", + "pink", + "pink.*", "piper_sdk.*", "plotext", "plum.*", From 5a5560ab8b82f44aa00dab71e4262d26862f1b41 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Thu, 16 Jul 2026 01:34:02 +0000 Subject: [PATCH 04/19] [autofix.ci] apply automated fixes --- dimos/robot/manipulators/test_blueprints.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 3a80d065f9..b1599887c1 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -147,7 +147,11 @@ def test_shipped_eef_twist_blueprints_use_pink_with_named_models( def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: - blueprints = (keyboard_teleop_piper, coordinator_cartesian_ik_mock, coordinator_cartesian_ik_piper) + blueprints = ( + keyboard_teleop_piper, + coordinator_cartesian_ik_mock, + coordinator_cartesian_ik_piper, + ) for blueprint in blueprints: task = next( task From f82f30266175daa4bd4dc33a7617ea81936abf2c Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 15 Jul 2026 18:39:52 -0700 Subject: [PATCH 05/19] spec: remove --- .../.openspec.yaml | 2 - .../design.md | 123 ----------------- .../docs.md | 30 ---- .../proposal.md | 54 -------- .../specs/pink-control-ik/spec.md | 76 ----------- .../tasks.md | 72 ---------- openspec/config.yaml | 45 ------ openspec/schemas/dimos-capability/schema.yaml | 128 ------------------ .../dimos-capability/templates/design.md | 35 ----- .../dimos-capability/templates/docs.md | 19 --- .../dimos-capability/templates/proposal.md | 32 ----- .../dimos-capability/templates/spec.md | 16 --- .../dimos-capability/templates/tasks.md | 15 -- 13 files changed, 647 deletions(-) delete mode 100644 openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml delete mode 100644 openspec/changes/add-pink-control-ik-self-collision/design.md delete mode 100644 openspec/changes/add-pink-control-ik-self-collision/docs.md delete mode 100644 openspec/changes/add-pink-control-ik-self-collision/proposal.md delete mode 100644 openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md delete mode 100644 openspec/changes/add-pink-control-ik-self-collision/tasks.md delete mode 100644 openspec/config.yaml delete mode 100644 openspec/schemas/dimos-capability/schema.yaml delete mode 100644 openspec/schemas/dimos-capability/templates/design.md delete mode 100644 openspec/schemas/dimos-capability/templates/docs.md delete mode 100644 openspec/schemas/dimos-capability/templates/proposal.md delete mode 100644 openspec/schemas/dimos-capability/templates/spec.md delete mode 100644 openspec/schemas/dimos-capability/templates/tasks.md diff --git a/openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml b/openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml deleted file mode 100644 index 2800d4ba85..0000000000 --- a/openspec/changes/add-pink-control-ik-self-collision/.openspec.yaml +++ /dev/null @@ -1,2 +0,0 @@ -schema: dimos-capability -created: 2026-07-14 diff --git a/openspec/changes/add-pink-control-ik-self-collision/design.md b/openspec/changes/add-pink-control-ik-self-collision/design.md deleted file mode 100644 index e488d625d5..0000000000 --- a/openspec/changes/add-pink-control-ik-self-collision/design.md +++ /dev/null @@ -1,123 +0,0 @@ -## Context - -`CartesianIKTask` and `EEFTwistTask` currently rely on the legacy -`PinocchioIK` implementation. Cartesian control receives `PoseStamped` targets; -keyboard teleoperation sends `TwistStamped` commands to the EEF-twist task. -The two tasks need one control pipeline and one default backend while retaining -an explicit compatibility route for existing Pinocchio behavior. - -## Goals / Non-Goals - -**Goals:** - -- Make generic Pink the default control IK backend for Cartesian and EEF-twist - tasks in `ControlCoordinator`. -- Keep legacy PinocchioIK available only through explicit - `backend="pinocchio"` selection. -- Validate named EEF frames, controlled joints, model mappings, and prepared - URDF/Xacro assets before task startup. -- Re-anchor every control tick to the coordinator's measured joint state. -- Clamp `dt`, update a Pink frame task, solve one differential step, integrate - once, apply joint and velocity limits, and emit a finite bounded position - command. -- Hold safely on expected runtime solver errors or invalid solver output. -- Migrate common helpers and every shipped Cartesian/EEF-twist blueprint to - the Pink default without a Piper backend exception. -- Migrate Piper from its MJCF/numeric EEF path to the matching existing - Xacro/URDF model and named `gripper_base` frame. - -**Non-Goals:** - -- Self-collision or world-obstacle avoidance. -- Planning-world or dynamic-obstacle avoidance in live control. -- Replacing `WorldSpec` or manipulation planning Pink/Drake behavior. -- New streams, RPCs, skills, MCP tools, CLI commands, or generated registries. - -## Control Architecture - -`CartesianIKTask` owns a shared target-to-command pipeline. It validates the -task model and named EEF frame, prepares a target, reads measured joints, -clamps the tick duration, performs one backend solve, validates the finite -bounded result, and builds the servo-position output. Expected runtime solver -errors produce a bounded hold rather than an invalid command. - -`EEFTwistTask` subclasses `CartesianIKTask`. It prepares a short-horizon pose -target by applying the latest twist to forward kinematics computed from the -current measured joints, then delegates the solve and output path to the base -task. Pose and twist streams remain independently routed by the coordinator. - -The Pink backend loads the prepared direct URDF/Xacro model, resolves the -configured joint mapping and named EEF frame, owns a Pink configuration and -frame task, applies joint/velocity limits, solves one step, and integrates the -finite velocity using the clamped `dt`. The legacy backend uses the existing -PinocchioIK path only when the typed backend setting is explicitly -`"pinocchio"`. - -Model preparation is shared and deterministic: Xacro arguments and package -paths are resolved before backend construction, the resulting URDF is used by -Pink, and frame/joint mismatches fail startup with diagnostics. - -## Backend and Blueprint Decisions - -### Pink is the default - -The typed control configuration defaults to Pink. `backend="pinocchio"` is an -explicit escape hatch for compatibility and testing; no task helper or shipped -blueprint silently selects it. - -Common Cartesian and EEF-twist helpers pass the backend selection through task -configuration. All shipped task blueprints use the default Pink path. Piper is -not special-cased by backend. - -### Piper model and frame migration - -Piper Cartesian and EEF-twist tasks stop using the MJCF/numeric EEF path. They -use the matching existing Xacro/URDF model and the named `gripper_base` frame, -with the same model/joint mapping validation as other robots. - -### Planning/control separation - -Control Pink is a local differential-IK backend. Manipulation planning retains -its separate `WorldSpec` and planning Pink/Drake integration. Neither layer is -changed to provide collision behavior by this proposal. - -## Runtime Safety and Rollout - -Measured-state anchoring prevents command lag from accumulating a virtual -configuration. The `dt` clamp, joint and velocity limits, finite-value checks, -bounded joint-delta checks, and hold-on-error behavior remain in the shared -pipeline. - -Validate the default Pink path in simulation or replay at the coordinator rate -before hardware use. Benchmark end-to-end control latency, exercise Cartesian -and twist targets across normal workspace motion, verify model/frame mapping and -runtime error holds, and confirm emergency-stop readiness. Any hardware check -must be supervised and low speed. - -## Risks / Trade-offs - -- Pink may add control-loop latency; benchmark the complete coordinator path and - retain explicit Pinocchio selection for compatibility. -- URDF/Xacro frame or joint mismatches can prevent startup; validate them before - backend construction and provide actionable diagnostics. -- Differential IK can fail near singularities or conflicting limits; preserve - finite-output validation and bounded holds for expected runtime failures. -- Sharing the target-preparation boundary through inheritance requires focused - Cartesian and twist lifecycle tests, including timeout and clear behavior. - -## Migration / Rollout - -Implement the backend seam and shared pipeline first, then switch common helpers -and shipped task blueprints to Pink by default. Migrate Piper's model path and -EEF frame to the existing Xacro/URDF and `gripper_base`. Keep Pinocchio -available only when explicitly configured. Run focused tests and simulation or -replay latency validation before supervised low-speed hardware validation. - -## Open Questions - -- Confirm the exact existing Piper Xacro/URDF asset and package arguments for - each hardware and simulation blueprint. -- Select the coordinator-rate latency budget and benchmark thresholds for Pink - versus the explicit Pinocchio compatibility path. -- Define coordinator-visible diagnostics for startup model errors and bounded - runtime holds without changing stream contracts. diff --git a/openspec/changes/add-pink-control-ik-self-collision/docs.md b/openspec/changes/add-pink-control-ik-self-collision/docs.md deleted file mode 100644 index b2864a7180..0000000000 --- a/openspec/changes/add-pink-control-ik-self-collision/docs.md +++ /dev/null @@ -1,30 +0,0 @@ -## Documentation Updates - -- Update `docs/capabilities/manipulation/index.md` to describe Pink as the - default Cartesian and EEF-twist control IK backend, with explicit - `backend="pinocchio"` compatibility selection. -- Update the same manipulation capability documentation to explain named EEF - frames, URDF/Xacro preparation, model/joint mapping validation, measured-state - anchoring, bounded one-step control, runtime holds, and the distinction from - planning `WorldSpec`. -- Update the existing Piper-related sections in the manipulation capability - documentation to state that Piper uses the matching Xacro/URDF model and - named `gripper_base` frame. Do not describe collision protection or create a - new Piper document. -- Update `docs/capabilities/manipulation/adding_a_custom_arm.md` with the - generic Pink control configuration, explicit legacy Pinocchio selection, - direct model preparation, frame/joint validation, and task-helper usage. -- Document simulation/replay latency benchmarking and supervised low-speed - hardware rollout checks, without claiming that validation has occurred. - -## Out of Scope - -Do not document self-collision or planning-world obstacle avoidance as control -features. This change does not add collision behavior. - -## Doc Validation - -- Run the repository documentation link checker if the changed documentation - participates in it. -- Run `md-babel-py run ` for changed executable examples when the - tool is available. diff --git a/openspec/changes/add-pink-control-ik-self-collision/proposal.md b/openspec/changes/add-pink-control-ik-self-collision/proposal.md deleted file mode 100644 index 584a3d1208..0000000000 --- a/openspec/changes/add-pink-control-ik-self-collision/proposal.md +++ /dev/null @@ -1,54 +0,0 @@ -## Why - -Cartesian and end-effector twist control currently use the legacy -`PinocchioIK` path. The control stack needs one generic, bounded differential-IK -backend so Cartesian pose and keyboard twist tasks share the same measured-state -and command-safety behavior. - -## What Changes - -- Add generic Pink control IK as the default `ControlCoordinator` backend for - Cartesian and EEF-twist tasks. -- Retain `PinocchioIK` only through an explicit `backend="pinocchio"` option for - compatibility. -- Validate named end-effector frames, model/joint mappings, and prepared - URDF/Xacro models before control starts. -- Re-anchor every solve to measured joints, use bounded `dt`, apply Pink frame - tasks with one-step integration, enforce joint/velocity limits, and hold on - expected runtime solve errors or invalid output. -- Update common task helpers and all shipped task blueprints to use Pink by - default without a Piper-specific backend exception. -- Migrate Piper Cartesian and EEF-twist control from its MJCF/numeric EEF path - to the matching existing Xacro/URDF model and named `gripper_base` frame. - -Self-collision and world-obstacle avoidance are out of scope for this change. - -## Affected DimOS Surfaces - -- `CartesianIKTask`, `EEFTwistTask`, `ControlCoordinator` task configuration, - `PoseStamped`, `TwistStamped`, and `JointCommandOutput` behavior. -- Common Cartesian and EEF-twist task helpers and shipped manipulator - blueprints, including Piper. -- Pink and legacy Pinocchio control-IK backend selection and model preparation. -- Simulation/replay and hardware control latency validation. - -## Capabilities - -### New Capabilities - -- `pink-control-ik`: Generic Pink differential control IK for Cartesian and EEF - twist tasks, with explicit legacy Pinocchio compatibility. - -### Modified Capabilities - -- None. No baseline OpenSpec capability specification exists for this control - path. - -## Impact - -Pink becomes the default control behavior for all shipped Cartesian and EEF-twist -task blueprints. Existing users can select `backend="pinocchio"` explicitly -during migration. Piper uses its existing Xacro/URDF model and -`gripper_base` frame instead of its MJCF/numeric EEF path. The rollout requires -focused backend, task, blueprint, and model-preparation tests plus -simulation/replay latency validation; it does not add self-collision behavior. diff --git a/openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md b/openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md deleted file mode 100644 index beb7e1688c..0000000000 --- a/openspec/changes/add-pink-control-ik-self-collision/specs/pink-control-ik/spec.md +++ /dev/null @@ -1,76 +0,0 @@ -## ADDED Requirements - -### Requirement: Pink is the default control IK backend -The system SHALL use generic Pink control IK by default for Cartesian and EEF-twist tasks created by `ControlCoordinator`. The typed backend configuration SHALL retain `backend="pinocchio"` as an explicit legacy compatibility option. The system SHALL NOT silently select Pinocchio when Pink is the configured backend or when Pink initialization fails. - -#### Scenario: Shipped Cartesian task uses the default backend -- **GIVEN** a shipped Cartesian task without an explicit backend override -- **WHEN** the task is constructed -- **THEN** it SHALL construct the Pink control IK backend - -#### Scenario: Legacy backend is explicitly selected -- **GIVEN** a Cartesian or EEF-twist task configured with `backend="pinocchio"` -- **WHEN** the task is constructed -- **THEN** it SHALL use the legacy PinocchioIK backend -- **AND** no implicit backend migration SHALL occur for that task - -### Requirement: Model and named frame validation -The system SHALL prepare the configured URDF/Xacro model and validate the named end-effector frame, controlled-joint mapping, and model/task joint correspondence before control starts. Invalid or missing model, frame, or mapping configuration SHALL fail initialization with a diagnostic error. - -#### Scenario: Valid Xacro model and named frame -- **GIVEN** a task with resolvable Xacro package paths and arguments, a valid URDF result, a named EEF frame, and matching mapped joints -- **WHEN** the Pink backend initializes -- **THEN** it SHALL construct the frame task from that model and frame - -#### Scenario: Frame or joint mapping is invalid -- **GIVEN** a task whose named EEF frame or mapped controlled joint is absent from the prepared model -- **WHEN** the backend initializes -- **THEN** initialization SHALL fail with a diagnostic error - -### Requirement: Measured-state one-step control -The Pink backend SHALL re-anchor its configuration to the coordinator's current measured joint state on every control tick. It SHALL update a named frame task, solve one differential-IK step, clamp the tick duration to the configured safe bounds, integrate the finite velocity once, and return a bounded joint-position candidate. - -#### Scenario: Robot state lags the previous command -- **GIVEN** measured joints differ from the previously emitted command -- **WHEN** the next Cartesian or EEF-twist tick runs -- **THEN** the backend SHALL start from the measured joints -- **AND** EEF-twist target preparation SHALL use FK from those measured joints - -#### Scenario: Tick duration exceeds the safe bound -- **GIVEN** a control tick with an elapsed duration outside the configured safe range -- **WHEN** one-step integration runs -- **THEN** the backend SHALL use the bounded duration -- **AND** it SHALL emit a finite bounded position candidate - -### Requirement: Control limits and runtime failure behavior -The Pink control solve SHALL enforce configured joint-position and velocity limits. The shared task pipeline SHALL reject non-finite or unbounded output and SHALL produce a bounded hold for expected runtime solve errors rather than emitting an invalid command. - -#### Scenario: Solver returns a valid limited step -- **GIVEN** a valid target and measured state -- **WHEN** Pink solves one step -- **THEN** the emitted position command SHALL respect joint and velocity limits -- **AND** the command SHALL pass finite-value and joint-delta safety checks - -#### Scenario: Expected runtime solve error -- **GIVEN** Pink raises an expected runtime solve error during a control tick -- **WHEN** the task handles the backend result -- **THEN** it SHALL emit a bounded hold or equivalent safe command -- **AND** it SHALL NOT emit non-finite or unvalidated joint positions - -### Requirement: Shared task and blueprint migration -Common Cartesian and EEF-twist task helpers SHALL expose the same backend configuration, default to Pink, and preserve independent coordinator routing and lifecycle behavior. All shipped Cartesian and EEF-twist task blueprints SHALL use that default without a Piper-specific backend exception. Piper Cartesian and EEF-twist tasks SHALL use the matching existing Xacro/URDF model and named `gripper_base` frame instead of the MJCF/numeric EEF path. - -#### Scenario: Piper task is constructed -- **GIVEN** a shipped Piper Cartesian or EEF-twist blueprint without an explicit legacy override -- **WHEN** its task configuration is built -- **THEN** it SHALL select Pink -- **AND** it SHALL use the matching Xacro/URDF model and `gripper_base` frame - -### Requirement: Control and planning remain separate -The control Pink backend SHALL provide local Cartesian differential IK only. It SHALL NOT claim, load, or enforce planning-world or dynamic-obstacle avoidance. Manipulation planning SHALL remain responsible for its separate `WorldSpec` and planning backend behavior. - -#### Scenario: Planning knows about a world obstacle -- **GIVEN** an obstacle represented only in the planning world -- **WHEN** a control IK command is generated -- **THEN** control SHALL apply its configured kinematic and command-safety behavior only -- **AND** world-obstacle handling SHALL remain the responsibility of planning diff --git a/openspec/changes/add-pink-control-ik-self-collision/tasks.md b/openspec/changes/add-pink-control-ik-self-collision/tasks.md deleted file mode 100644 index 52ee70a656..0000000000 --- a/openspec/changes/add-pink-control-ik-self-collision/tasks.md +++ /dev/null @@ -1,72 +0,0 @@ -## 1. Pink control backend - -- [x] 1.1 Define the typed control backend configuration with Pink as the - default and `backend="pinocchio"` as the only explicit legacy option. -- [x] 1.2 Implement shared URDF/Xacro model preparation, including package and - Xacro argument resolution, named EEF frame validation, controlled-joint - mapping validation, and actionable startup diagnostics. -- [x] 1.3 Implement the generic Pink one-step backend: measured-state reset, - named frame task update, bounded `dt`, joint/velocity limits, finite velocity - integration, and normalized joint-position result. -- [x] 1.4 Preserve the existing PinocchioIK implementation behind explicit - `backend="pinocchio"` selection; do not silently fall back from Pink. -- [x] 1.5 Add expected runtime solve-error handling that emits a bounded hold - and rejects non-finite or otherwise invalid backend output. - -## 2. Shared tasks and blueprint migration - -- [x] 2.1 Refactor `CartesianIKTask` so target preparation, measured-state - extraction, backend solve, output validation, timeout, and hold behavior form - one reusable pipeline. -- [x] 2.2 Make `EEFTwistTask` a Cartesian task specialization that derives its - short-horizon target from measured FK and the bounded twist increment. -- [x] 2.3 Preserve independent Cartesian and EEF-twist coordinator routing, - lifecycle, timeout, zero-input, and clear semantics. -- [x] 2.4 Update common task helpers so Pink is the default backend and - Pinocchio requires explicit `backend="pinocchio"`. -- [x] 2.5 Update every shipped Cartesian and EEF-twist task blueprint to use - the Pink default without backend special cases. -- [x] 2.6 Migrate Piper Cartesian and EEF-twist tasks from the MJCF/numeric EEF - path to the matching existing Xacro/URDF model and named `gripper_base` - frame. - -## 3. Tests - -- [x] 3.1 Test Pink initialization, URDF/Xacro preparation, named EEF frame - validation, model/joint mapping validation, and startup diagnostics. -- [x] 3.2 Test measured-state re-anchoring, bounded `dt`, frame-task one-step - integration, joint/velocity limits, finite output, and bounded holds on - expected runtime solve errors. -- [x] 3.3 Test explicit legacy `backend="pinocchio"` compatibility and prove - that invalid Pink setup is not silently converted to Pinocchio. -- [x] 3.4 Test the shared Cartesian and EEF-twist pipeline, including measured FK - target preparation, timeout, zero input, clear behavior, and output guards. -- [x] 3.5 Test common helper defaults and all shipped blueprint backend settings. -- [x] 3.6 Test Piper's Xacro/URDF model selection, named `gripper_base` frame, - and removal of its MJCF/numeric EEF configuration. - -## 4. Documentation - -- [x] 4.1 Update manipulation capability documentation with the default Pink - backend, explicit Pinocchio compatibility, model/frame validation, runtime - holds, planning/control separation, and non-collision rollout guidance. -- [x] 4.2 Update the custom-arm integration guide with generic Pink task - configuration, direct URDF/Xacro preparation, mapping validation, explicit - legacy backend selection, and Piper's model/frame migration reference. - -## 5. Verification and rollout - -- [x] 5.1 Run `openspec validate add-pink-control-ik-self-collision`. -- [x] 5.2 Run focused tests for Pink control IK, Cartesian IK, EEF twist, - common helpers, Piper blueprints, and model preparation. -- [x] 5.3 Run the blueprint registry generation test if blueprint discovery - inputs change. -- [x] 5.4 Run the relevant documentation link checker and executable-example - validation when available. -- [x] 5.5 Run type and lint checks for changed control/manipulation modules. -- [ ] 5.6 Validate Pink in simulation or replay at the coordinator rate, - benchmark end-to-end control latency, exercise Cartesian and twist commands, - and verify bounded holds and emergency-stop readiness. -- [ ] 5.7 Perform supervised low-speed hardware validation only after - simulation/replay checks pass; record latency and runtime error behavior - without claiming validation before it occurs. 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 e4ee4f010feca6e2489c5a5264e5d456d7d08307 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 10:38:39 -0700 Subject: [PATCH 06/19] fix: clamp solution within joint limit --- .../cartesian_ik_task/pink_control_ik.py | 25 +++++++++ .../cartesian_ik_task/test_pink_control_ik.py | 51 +++++++++++++++++++ 2 files changed, 76 insertions(+) diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index 4797dc9dbd..cee91f078d 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -34,6 +34,9 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.protocol.service.spec import BaseConfig +# Pink's integration/QP boundary tolerance is small but larger than machine epsilon. +_POSITION_LIMIT_EPSILON_RAD = 1e-5 + class PinkControlIKConfig(BaseConfig): """Typed configuration for the control IK backend.""" @@ -267,6 +270,7 @@ def solve( candidate = self._controlled_q(configuration.q, measured) if candidate.size != measured.size or not np.all(np.isfinite(candidate)): raise PinkControlRuntimeError("Pink produced an invalid joint candidate") + candidate = self._clamp_position_limits(candidate) return ControlIKResult(candidate, self._controlled_velocity(velocity)) except PinkControlRuntimeError: raise @@ -304,6 +308,27 @@ def _controlled_q( def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]: return np.array([velocity[index] for index in self._v_indices], dtype=np.float64) + def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]: + bounded = candidate.copy() + for index, width in enumerate(self._q_widths): + if width != 1: + continue + q_index = self._q_indices[index] + lower = self._model.lowerPositionLimit[q_index] + upper = self._model.upperPositionLimit[q_index] + value = bounded[index] + if np.isfinite(lower) and value < lower: + if lower - value <= _POSITION_LIMIT_EPSILON_RAD: + bounded[index] = lower + else: + raise PinkControlRuntimeError("Pink produced an out-of-bounds joint candidate") + elif np.isfinite(upper) and value > upper: + if value - upper <= _POSITION_LIMIT_EPSILON_RAD: + bounded[index] = upper + else: + raise PinkControlRuntimeError("Pink produced an out-of-bounds joint candidate") + return bounded + def _build_mapping(self, robot: RobotModelConfig) -> tuple[list[int], list[int]]: coordinator_names = robot.get_coordinator_joint_names() if coordinator_names != self._joint_names or len(set(coordinator_names)) != len( diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 6e20d24e15..12241696a9 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -28,6 +28,7 @@ ControlIKResult, PinkControlIK, PinkControlIKConfig, + PinkControlRuntimeError, PinocchioIK, ) from dimos.control.tasks.registry import control_task_registry @@ -305,6 +306,56 @@ def test_pink_applies_position_velocity_limits_and_finite_output(tmp_path: Path) assert np.all(np.isfinite(result.positions)) +def test_pink_clamps_tiny_position_limit_overshoot( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + robot = _robot(model_path).model_copy( + update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} + ) + backend = PinkControlIK( + model_path, None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) + ) + measured = np.array([1.22, 0.1]) + + def solve( + configuration: object, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + return np.array([0.00013784674535, -0.2]) + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve + ) + result = backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + assert np.array_equal(result.positions, np.array([1.22, 0.098])) + assert np.array_equal(result.velocity, np.array([0.00013784674535, -0.2])) + + +def test_pink_rejects_material_position_limit_violation( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + robot = _robot(model_path).model_copy( + update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} + ) + backend = PinkControlIK( + model_path, None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) + ) + measured = np.array([1.22, 0.1]) + + def solve( + configuration: object, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + return np.array([0.01, -0.2]) + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve + ) + with pytest.raises(PinkControlRuntimeError, match="out-of-bounds"): + backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + def test_cartesian_pipeline_bounds_dt_and_holds_on_expected_error( monkeypatch: pytest.MonkeyPatch, ) -> None: From c576b7e7de7d65878a708ebe72d550521aa10b85 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 11:40:05 -0700 Subject: [PATCH 07/19] refactor: remove legacy control IK --- .../cartesian_ik_task/cartesian_ik_task.py | 18 ++--- .../cartesian_ik_task/pink_control_ik.py | 70 +++++-------------- .../cartesian_ik_task/test_pink_control_ik.py | 66 ++++------------- .../tasks/eef_twist_task/eef_twist_task.py | 4 +- .../eef_twist_task/test_eef_twist_task.py | 14 +++- dimos/control/test_control.py | 2 +- dimos/robot/manipulators/common/blueprints.py | 27 ++----- dimos/robot/manipulators/test_blueprints.py | 10 ++- .../manipulation/adding_a_custom_arm.md | 24 ++----- docs/capabilities/manipulation/index.md | 15 +--- 10 files changed, 67 insertions(+), 183 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index 79f92d0801..3bcb3b4386 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -21,14 +21,13 @@ from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass from pathlib import Path import threading from typing import TYPE_CHECKING import numpy as np import pinocchio -from pydantic import Field from dimos.control.coordinator import TaskConfig from dimos.control.task import ( @@ -64,8 +63,7 @@ class CartesianIKTaskConfig: Attributes: joint_names: List of joint names this task controls (must match model DOF) - model_path: Path to the direct Pink or legacy Pinocchio model - ee_joint_id: Legacy Pinocchio end-effector joint ID, when selected + model_path: Path to the direct Pink URDF or Xacro model priority: Priority for arbitration (higher wins) timeout: If no command received for this many seconds, go inactive (0 = never) max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) @@ -73,15 +71,14 @@ class CartesianIKTaskConfig: joint_names: list[str] model_path: str | Path - ee_joint_id: int | None = None + control_ik: PinkControlIKConfig priority: int = 10 timeout: float = 0.5 max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz - control_ik: PinkControlIKConfig = field(default_factory=PinkControlIKConfig) class CartesianIKTask(BaseControlTask): - """Cartesian control task with selectable Pink or legacy Pinocchio IK. + """Cartesian control task with Pink differential IK. Accepts streaming cartesian poses via on_cartesian_command() and computes IK internally to output joint commands. Pink re-anchors each solve to the @@ -139,7 +136,6 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: # Create IK solver from model self._ik = PinkControlIK( config.model_path, - config.ee_joint_id, self._joint_names_list, config.control_ik, ) @@ -159,7 +155,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: logger.info( f"CartesianIKTask {name} initialized with model: {config.model_path}, " - f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" + f"joints={config.joint_names}" ) @property @@ -396,8 +392,7 @@ def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio. class CartesianIKTaskParams(BaseConfig): model_path: str | Path - ee_joint_id: int | None = None - control_ik: PinkControlIKConfig = Field(default_factory=PinkControlIKConfig) + control_ik: PinkControlIKConfig def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: @@ -407,7 +402,6 @@ def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: CartesianIKTaskConfig( joint_names=cfg.joint_names, model_path=params.model_path, - ee_joint_id=params.ee_joint_id, priority=cfg.priority, control_ik=params.control_ik, ), diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index cee91f078d..1341b39adc 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -12,23 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Pink and legacy Pinocchio backends for coordinator Cartesian control.""" +"""Pink differential IK for coordinator Cartesian control.""" from __future__ import annotations from collections.abc import Mapping from dataclasses import dataclass from pathlib import Path -from typing import Literal import numpy as np from numpy.typing import NDArray import pink -from pink.limits import ConfigurationLimit, Limit, VelocityLimit +from pink.limits import ConfigurationLimit, VelocityLimit import pinocchio from pydantic import Field, field_validator -from dimos.manipulation.planning.kinematics.pinocchio_ik import PinocchioIK from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -41,8 +39,7 @@ class PinkControlIKConfig(BaseConfig): """Typed configuration for the control IK backend.""" - backend: Literal["pink", "pinocchio"] = "pink" - robot_model: RobotModelConfig | None = None + robot_model: RobotModelConfig solver: str = "proxqp" max_velocity: float = 10.0 lm_damping: float = 1e-4 @@ -56,8 +53,8 @@ class PinkControlIKConfig(BaseConfig): @field_validator("robot_model", mode="before") @classmethod - def _rebuild_robot_model(cls, value: object) -> RobotModelConfig | None: - if value is None or isinstance(value, RobotModelConfig): + def _rebuild_robot_model(cls, value: object) -> RobotModelConfig: + if isinstance(value, RobotModelConfig): return value if not isinstance(value, Mapping): raise ValueError("Pink robot_model must be a serialized RobotModelConfig") @@ -79,7 +76,6 @@ def _rebuild_robot_model(cls, value: object) -> RobotModelConfig | None: def validate_settings( self, joint_count: int, - ee_joint_id: int | None, model_path: str | Path | None = None, ) -> None: numeric = ( @@ -101,20 +97,15 @@ def validate_settings( raise ValueError("control IK dt bounds must be positive and ordered") if any(not np.isfinite(value) for value in self.qpsolver_options.values()): raise ValueError("control IK QP options must be finite") - if self.backend == "pink": - if self.robot_model is None: - raise ValueError("Pink control requires a RobotModelConfig") - if not self.robot_model.end_effector_link: - raise ValueError("Pink control requires a named end-effector frame") - if len(self.robot_model.joint_names) != joint_count: - raise ValueError("RobotModelConfig and control task joint counts differ") - if ( - model_path is not None - and Path(self.robot_model.model_path).resolve() != Path(model_path).resolve() - ): - raise ValueError("Pink RobotModelConfig must use the authoritative model path") - elif not isinstance(ee_joint_id, int) or isinstance(ee_joint_id, bool): - raise ValueError("Pinocchio control requires a numeric ee_joint_id") + if not self.robot_model.end_effector_link: + raise ValueError("Pink control requires a named end-effector frame") + if len(self.robot_model.joint_names) != joint_count: + raise ValueError("RobotModelConfig and control task joint counts differ") + if ( + model_path is not None + and Path(self.robot_model.model_path).resolve() != Path(model_path).resolve() + ): + raise ValueError("Pink RobotModelConfig must use the authoritative model path") @dataclass(frozen=True) @@ -128,33 +119,17 @@ class PinkControlRuntimeError(RuntimeError): class PinkControlIK: - """One-step Pink control IK with explicit legacy Pinocchio compatibility.""" + """One-step Pink control IK for Cartesian control.""" def __init__( self, model_path: str | Path, - ee_joint_id: int | None, joint_names: list[str], config: PinkControlIKConfig, ) -> None: self._config = config self._joint_names = list(joint_names) - self._config.validate_settings(len(self._joint_names), ee_joint_id, model_path) - self._is_pinocchio = config.backend == "pinocchio" - self._legacy_ik: PinocchioIK | None = None - - if self._is_pinocchio: - if ee_joint_id is None: - raise ValueError("Pinocchio control requires an explicit ee_joint_id") - self._legacy_ik = PinocchioIK.from_model_path(model_path, ee_joint_id) - self._model = self._legacy_ik.model - self._data = self._model.createData() - self._q_indices: list[int] = [] - self._v_indices: list[int] = [] - self._configuration = None - self._frame_task = None - self._limits: list[Limit] = [] - return + self._config.validate_settings(len(self._joint_names), model_path) robot = config.robot_model if robot is None: # guarded by validate_settings; retained for narrowing @@ -213,17 +188,9 @@ def __init__( @property def nq(self) -> int: """Number of controlled coordinates, matching the task contract.""" - if self._is_pinocchio: - if self._legacy_ik is None: - raise PinkControlRuntimeError("Pinocchio control backend is unavailable") - return self._legacy_ik.nq return len(self._joint_names) def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3: - if self._is_pinocchio: - if self._legacy_ik is None: - raise PinkControlRuntimeError("Pinocchio control backend is unavailable") - return self._legacy_ik.forward_kinematics(q) full_q = self._full_q(q) pinocchio.forwardKinematics(self._model, self._data, full_q) pinocchio.updateFramePlacements(self._model, self._data) @@ -241,11 +208,6 @@ def solve( if not np.isfinite(dt) or dt <= 0.0: raise ValueError("control IK dt must be finite and positive") dt = min(max(dt, self._config.min_dt), self._config.max_dt) - if self._is_pinocchio: - if self._legacy_ik is None: - raise PinkControlRuntimeError("Pinocchio control backend is unavailable") - positions, _, _ = self._legacy_ik.solve(target, measured) - return ControlIKResult(np.asarray(positions, dtype=np.float64), positions - measured) configuration = self._configuration frame_task = self._frame_task diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 12241696a9..6d35c12ed3 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -15,7 +15,6 @@ from pathlib import Path import numpy as np -import pinocchio import pytest from dimos.control.coordinator import TaskConfig @@ -29,7 +28,6 @@ PinkControlIK, PinkControlIKConfig, PinkControlRuntimeError, - PinocchioIK, ) from dimos.control.tasks.registry import control_task_registry from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -107,12 +105,9 @@ def _write_urdf(tmp_path: Path, name: str = "tiny.urdf", content: str = _URDF) - return path -def test_pink_is_default_and_requires_robot_model() -> None: - config = PinkControlIKConfig() - - assert config.backend == "pink" - with pytest.raises(ValueError, match="RobotModelConfig"): - config.validate_settings(2, None) +def test_pink_requires_robot_model() -> None: + with pytest.raises(ValueError, match="robot_model"): + PinkControlIKConfig() def test_pink_prepares_xacro_with_package_paths_and_arguments( @@ -151,7 +146,6 @@ def prepare( PinkControlIK( tmp_path / "robot.xacro", - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot), ) @@ -170,7 +164,6 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N with pytest.raises(ValueError, match="end-effector frame"): PinkControlIK( model_path, - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path, frame="missing")), ) @@ -181,7 +174,6 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N with pytest.raises(ValueError, match="exactly match"): PinkControlIK( model_path, - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=mismatched), ) @@ -193,7 +185,6 @@ def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( model_path = _write_urdf(tmp_path) backend = PinkControlIK( model_path, - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path)), ) @@ -224,7 +215,6 @@ def test_pink_backend_clamps_dt_from_backend_configuration( model_path = _write_urdf(tmp_path) backend = PinkControlIK( model_path, - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path), min_dt=0.01, max_dt=0.02), ) @@ -252,7 +242,6 @@ def test_pink_rejects_uncontrolled_end_effector_chain_without_reference( with pytest.raises(ValueError, match="reference_q.*uncontrolled joint"): PinkControlIK( model_path, - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path)), ) @@ -265,7 +254,6 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path with pytest.raises(ValueError, match="continuous joints.*tangent-space"): PinkControlIK( model_path, - None, ["joint1"], PinkControlIKConfig(robot_model=robot), ) @@ -275,7 +263,6 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path ) backend = PinkControlIK( model_path, - None, ["joint1"], PinkControlIKConfig(robot_model=roundtrip_robot), ) @@ -292,7 +279,6 @@ def test_pink_applies_position_velocity_limits_and_finite_output(tmp_path: Path) ) backend = PinkControlIK( model_path, - None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot, max_velocity=0.2), ) @@ -314,7 +300,7 @@ def test_pink_clamps_tiny_position_limit_overshoot( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) backend = PinkControlIK( - model_path, None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) + model_path, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) ) measured = np.array([1.22, 0.1]) @@ -340,7 +326,7 @@ def test_pink_rejects_material_position_limit_violation( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) backend = PinkControlIK( - model_path, None, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) + model_path, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) ) measured = np.array([1.22, 0.1]) @@ -369,6 +355,7 @@ def test_cartesian_pipeline_bounds_dt_and_holds_on_expected_error( CartesianIKTaskConfig( joint_names=["j1", "j2"], model_path="unused.urdf", + control_ik=PinkControlIKConfig(robot_model=_robot(Path("unused.urdf"))), timeout=0.2, ), ) @@ -398,44 +385,17 @@ def test_factory_rejects_invalid_default_pink_configuration() -> None: params={"model_path": "unused.urdf"}, ) - with pytest.raises(ValueError, match="RobotModelConfig"): + with pytest.raises(ValueError, match="control_ik"): control_task_registry.create("cartesian_ik", config, hardware={}) -def test_explicit_pinocchio_selection_does_not_fallback_from_pink( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch -) -> None: +@pytest.mark.parametrize("legacy_field", ["backend", "ee_joint_id"]) +def test_pink_rejects_legacy_configuration_fields(tmp_path: Path, legacy_field: str) -> None: model_path = _write_urdf(tmp_path) - legacy = _FakeLegacyIK(pinocchio.buildModelFromUrdf(str(model_path))) - - def load(path: Path, ee_joint_id: int) -> _FakeLegacyIK: - legacy.calls.append((path, ee_joint_id)) - return legacy - - monkeypatch.setattr(PinocchioIK, "from_model_path", staticmethod(load)) - - backend = PinkControlIK( - model_path, - 2, - ["joint1", "joint2"], - PinkControlIKConfig(backend="pinocchio"), - ) - - assert backend._is_pinocchio - assert legacy.calls == [(model_path, 2)] - with pytest.raises(ValueError, match="RobotModelConfig"): - PinkControlIK(model_path, None, ["joint1", "joint2"], PinkControlIKConfig()) - - -class _FakeLegacyIK: - nq = 2 - - def __init__(self, model: pinocchio.Model) -> None: - self.model = model - self.calls: list[tuple[Path, int]] = [] - - def forward_kinematics(self, q: np.ndarray) -> pinocchio.SE3: - return pinocchio.SE3.Identity() + with pytest.raises(ValueError, match=legacy_field): + PinkControlIKConfig.model_validate( + {"robot_model": _robot(model_path), legacy_field: "pinocchio"} + ) class _FakeControlIK: diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py index fae665f7ab..02de831462 100644 --- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py @@ -128,10 +128,9 @@ def clear(self) -> None: class EEFTwistTaskParams(BaseConfig): model_path: str | Path - ee_joint_id: int | None = None timeout: float = 0.3 max_joint_delta_deg: float = 15.0 - control_ik: PinkControlIKConfig = PinkControlIKConfig() + control_ik: PinkControlIKConfig def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask: @@ -141,7 +140,6 @@ def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask: EEFTwistTaskConfig( joint_names=cfg.joint_names, model_path=params.model_path, - ee_joint_id=params.ee_joint_id, priority=cfg.priority, timeout=params.timeout, max_joint_delta_deg=params.max_joint_delta_deg, diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index 78cc57ac5c..56db4958be 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -23,8 +23,11 @@ from dimos.control.task import ControlMode, CoordinatorState, JointStateSnapshot from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( ControlIKResult, + PinkControlIKConfig, ) from dimos.control.tasks.eef_twist_task.eef_twist_task import EEFTwistTask, EEFTwistTaskConfig +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped @@ -77,7 +80,16 @@ def task(fake_ik: FakeIK) -> EEFTwistTask: EEFTwistTaskConfig( joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], model_path="fake.urdf", - ee_joint_id=3, + control_ik=PinkControlIKConfig( + robot_model=RobotModelConfig( + name="fake", + model_path="fake.urdf", + base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], + end_effector_link="tool", + home_joints=[0.0, 0.0, 0.0], + ) + ), timeout=0.3, max_joint_delta_deg=15.0, ), diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index d6f42ec3f1..6965d9d4b8 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -258,7 +258,7 @@ def start_coordinator(tasks): name="eef", type="eef_twist", joint_names=["arm/joint1"], - params={"model_path": "fake", "ee_joint_id": 1}, + params={"model_path": "fake"}, ) ] ) diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index b5e4fb2ded..765a1103ea 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -52,22 +52,13 @@ def trajectory_task( def _resolve_control_ik( hardware: HardwareComponent, model_path: Path, - ee_joint_id: int | None, control_ik: PinkControlIKConfig | None, robot_model: RobotModelConfig | None, ) -> PinkControlIKConfig: resolved = control_ik or PinkControlIKConfig(robot_model=robot_model) - if resolved.backend == "pink": - if robot_model is not None and resolved.robot_model is None: - resolved = resolved.model_copy(update={"robot_model": robot_model}) - elif robot_model is not None and resolved.robot_model is not None: - if resolved.robot_model != robot_model: - raise ValueError("conflicting Pink RobotModelConfig values") - elif resolved.robot_model is None: - raise ValueError("Pink helper requires an authoritative RobotModelConfig") - elif not isinstance(ee_joint_id, int) or isinstance(ee_joint_id, bool): - raise ValueError("Pinocchio helper requires a numeric ee_joint_id") - resolved.validate_settings(len(hardware.joints), ee_joint_id, model_path) + if robot_model is not None and resolved.robot_model != robot_model: + raise ValueError("conflicting Pink RobotModelConfig values") + resolved.validate_settings(len(hardware.joints), model_path) return resolved @@ -118,15 +109,12 @@ def cartesian_ik_task( hardware: HardwareComponent, *, model_path: Path, - ee_joint_id: int | None = None, name: str = CARTESIAN_IK_TASK_NAME, priority: int = 10, control_ik: PinkControlIKConfig | None = None, robot_model: RobotModelConfig | None = None, ) -> TaskConfig: - resolved_control_ik = _resolve_control_ik( - hardware, model_path, ee_joint_id, control_ik, robot_model - ) + resolved_control_ik = _resolve_control_ik(hardware, model_path, control_ik, robot_model) return TaskConfig( name=name, type="cartesian_ik", @@ -134,7 +122,6 @@ def cartesian_ik_task( priority=priority, params={ "model_path": model_path, - "ee_joint_id": ee_joint_id, **({"control_ik": _serialize_control_ik(resolved_control_ik)}), }, ) @@ -144,15 +131,12 @@ def eef_twist_task( hardware: HardwareComponent, *, model_path: Path, - ee_joint_id: int | None = None, name: str = EEF_TWIST_TASK_NAME, priority: int = 10, control_ik: PinkControlIKConfig | None = None, robot_model: RobotModelConfig | None = None, ) -> TaskConfig: - resolved_control_ik = _resolve_control_ik( - hardware, model_path, ee_joint_id, control_ik, robot_model - ) + resolved_control_ik = _resolve_control_ik(hardware, model_path, control_ik, robot_model) return TaskConfig( name=name, type="eef_twist", @@ -160,7 +144,6 @@ def eef_twist_task( priority=priority, params={ "model_path": model_path, - "ee_joint_id": ee_joint_id, **({"control_ik": _serialize_control_ik(resolved_control_ik)}), }, ) diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index b1599887c1..6e2b0cabe7 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -94,8 +94,8 @@ def test_xarm_planner_blueprints_default_to_no_visualization() -> None: def test_eef_twist_task_helper_requires_pink_robot_model() -> None: hardware = make_xarm_hardware("arm", 6, adapter_type="mock") - with pytest.raises(ValueError, match="authoritative RobotModelConfig"): - eef_twist_task(hardware, model_path=Path("fake.urdf"), ee_joint_id=6) + with pytest.raises(ValueError, match="robot_model"): + eef_twist_task(hardware, model_path=Path("fake.urdf")) @pytest.mark.parametrize( @@ -140,9 +140,8 @@ def test_shipped_eef_twist_blueprints_use_pink_with_named_models( task = next(task for task in _coordinator_tasks(blueprint) if task.type == "eef_twist") control_ik = task.params["control_ik"] - assert control_ik["backend"] == "pink" assert control_ik["robot_model"]["end_effector_link"] - assert task.params["ee_joint_id"] is None + assert "ee_joint_id" not in task.params assert not str(task.params["model_path"]).endswith((".xml", ".mjcf")) @@ -160,10 +159,9 @@ def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: ) control_ik = task.params["control_ik"] assert task.params["model_path"] == PIPER_MODEL_PATH - assert control_ik["backend"] == "pink" assert control_ik["robot_model"]["model_path"] == str(PIPER_MODEL_PATH) assert control_ik["robot_model"]["end_effector_link"] == "gripper_base" - assert task.params["ee_joint_id"] is None + assert "ee_joint_id" not in task.params assert "self_collision_enabled" not in control_ik reconstructed = PinkControlIKConfig.model_validate(control_ik) diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index baf6cf020a..39515ea35d 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -571,12 +571,10 @@ yourarm_planner = manipulation_module( ### 4d. Configure Cartesian and EEF-twist control IK -Pink is the default backend for Cartesian and EEF-twist control. The legacy -Pinocchio backend is available only through an explicit -`backend="pinocchio"` setting. Pink control and manipulation planning are -separate: planning uses `WorldSpec` and its selected planning backend, while -control performs one local differential-IK step and does not use `WorldSpec` as -a control input. +Pink is the only backend for Cartesian and EEF-twist control. Pink control and +manipulation planning are separate: planning uses `WorldSpec` and its selected +planning backend, while control performs one local differential-IK step and +does not use `WorldSpec` as a control input. Use the same `RobotModelConfig` for the control model and planning robot metadata. Its `model_path` points to the direct URDF or Xacro, `package_paths` @@ -607,18 +605,8 @@ twist_task = eef_twist_task( ) ``` -Do not provide `ee_joint_id` for Pink tasks. To retain the legacy path during -migration, select it explicitly and provide its numeric EEF ID: - -```python skip -legacy_control_ik = PinkControlIKConfig(backend="pinocchio") -legacy_task = cartesian_ik_task( - hardware, - model_path=legacy_model_path, - ee_joint_id=6, - control_ik=legacy_control_ik, -) -``` +Pink control tasks use the named `RobotModelConfig.end_effector_link`; they do +not accept a numeric `ee_joint_id` or a legacy backend selector. At every coordinator tick, Pink re-anchors to measured joints, derives the EEF target from measured FK for twist input, clamps `dt`, updates one `FrameTask`, diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 3c79a97e99..e572f1d2db 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -121,8 +121,8 @@ request. For example, `planner_name=roboplan` requires ### Cartesian control IK Cartesian and keyboard EEF-twist tasks use generic Pink control IK by default. -Select the legacy Pinocchio backend only explicitly with -`backend="pinocchio"`; a failed Pink setup does not silently select Pinocchio. +Pink is the only control IK backend; a failed Pink setup does not silently select +another solver. Pink control uses the direct URDF/Xacro model from `RobotModelConfig`. Package paths and Xacro arguments are prepared before startup. The configuration names @@ -155,17 +155,6 @@ task = cartesian_ik_task( ) ``` -The compatibility path remains explicit and uses the legacy numeric EEF ID: - -```python skip -legacy_task = cartesian_ik_task( - hardware, - model_path=legacy_model_path, - ee_joint_id=6, - control_ik=PinkControlIKConfig(backend="pinocchio"), -) -``` - Piper's Cartesian and EEF-twist blueprints use the matching Xacro/URDF `PIPER_MODEL_PATH`, `make_piper_model_config()`, and named `gripper_base` frame. Piper's Pink configuration does not use its previous MJCF model or numeric EEF From 815e988e027b3b2f21058ae87e685f31e0d40bff Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 13:18:40 -0700 Subject: [PATCH 08/19] fix: address Pink control IK review --- .../cartesian_ik_task/cartesian_ik_task.py | 27 ++-- .../cartesian_ik_task/pink_control_ik.py | 114 ++++++++++------- .../cartesian_ik_task/test_pink_control_ik.py | 62 ++++----- .../tasks/eef_twist_task/eef_twist_task.py | 3 - .../eef_twist_task/test_eef_twist_task.py | 1 - .../manipulators/a1z/blueprints/teleop.py | 1 - .../manipulators/a750/blueprints/teleop.py | 2 - dimos/robot/manipulators/common/blueprints.py | 120 ++++++++---------- .../manipulators/openarm/blueprints/teleop.py | 2 - .../manipulators/piper/blueprints/teleop.py | 4 - dimos/robot/manipulators/test_blueprints.py | 19 ++- .../manipulators/xarm/blueprints/teleop.py | 2 - .../manipulation/adding_a_custom_arm.md | 10 +- docs/capabilities/manipulation/index.md | 12 +- 14 files changed, 181 insertions(+), 198 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index 3bcb3b4386..e6c280d982 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -22,7 +22,6 @@ from __future__ import annotations from dataclasses import dataclass -from pathlib import Path import threading from typing import TYPE_CHECKING @@ -63,14 +62,12 @@ class CartesianIKTaskConfig: Attributes: joint_names: List of joint names this task controls (must match model DOF) - model_path: Path to the direct Pink URDF or Xacro model priority: Priority for arbitration (higher wins) timeout: If no command received for this many seconds, go inactive (0 = never) max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) """ joint_names: list[str] - model_path: str | Path control_ik: PinkControlIKConfig priority: int = 10 timeout: float = 0.5 @@ -88,15 +85,11 @@ class CartesianIKTask(BaseControlTask): outputs JointCommandOutput and participates in joint-level arbitration. Example: - >>> from dimos.robot.manipulators.piper.config import ( - ... PIPER_MODEL_PATH, - ... make_piper_model_config, - ... ) + >>> from dimos.robot.manipulators.piper.config import make_piper_model_config >>> task = CartesianIKTask( ... name="cartesian_arm", ... config=CartesianIKTaskConfig( ... joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], - ... model_path=PIPER_MODEL_PATH, ... control_ik=PinkControlIKConfig( ... robot_model=make_piper_model_config(), ... ), @@ -120,8 +113,6 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: """ if not config.joint_names or len(set(config.joint_names)) != len(config.joint_names): raise ValueError(f"CartesianIKTask '{name}' requires at least one joint") - if not config.model_path: - raise ValueError(f"CartesianIKTask '{name}' requires model_path for IK solver") if not np.isfinite(config.timeout) or config.timeout < 0.0: raise ValueError("CartesianIKTask timeout must be finite and non-negative") if not np.isfinite(config.max_joint_delta_deg) or config.max_joint_delta_deg <= 0.0: @@ -132,13 +123,14 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._joint_names = frozenset(config.joint_names) self._joint_names_list = list(config.joint_names) self._num_joints = len(config.joint_names) + expected_joints = config.control_ik.robot_model.get_coordinator_joint_names() + if config.joint_names != expected_joints: + raise ValueError( + f"CartesianIKTask {name}: task joints must match RobotModelConfig coordinator joints" + ) # Create IK solver from model - self._ik = PinkControlIK( - config.model_path, - self._joint_names_list, - config.control_ik, - ) + self._ik = PinkControlIK(config.control_ik) # Validate DOF matches joint names if self._ik.nq != self._num_joints: @@ -154,7 +146,8 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._active = False logger.info( - f"CartesianIKTask {name} initialized with model: {config.model_path}, " + f"CartesianIKTask {name} initialized with model: " + f"{config.control_ik.robot_model.model_path}, " f"joints={config.joint_names}" ) @@ -391,7 +384,6 @@ def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio. class CartesianIKTaskParams(BaseConfig): - model_path: str | Path control_ik: PinkControlIKConfig @@ -401,7 +393,6 @@ def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: cfg.name, CartesianIKTaskConfig( joint_names=cfg.joint_names, - model_path=params.model_path, priority=cfg.priority, control_ik=params.control_ik, ), diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index 1341b39adc..1c439c27f0 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -25,7 +25,7 @@ import pink from pink.limits import ConfigurationLimit, VelocityLimit import pinocchio -from pydantic import Field, field_validator +from pydantic import Field, field_validator, model_validator from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake @@ -41,13 +41,14 @@ class PinkControlIKConfig(BaseConfig): robot_model: RobotModelConfig solver: str = "proxqp" - max_velocity: float = 10.0 - lm_damping: float = 1e-4 - task_gain: float = 1.0 - position_cost: float = 1.0 - orientation_cost: float = 1.0 - min_dt: float = 1e-4 - max_dt: float = 0.05 + max_velocity: float = Field(10.0, gt=0.0) + lm_damping: float = Field(1e-4, gt=0.0) + task_gain: float = Field(1.0, gt=0.0) + position_cost: float = Field(1.0, ge=0.0) + orientation_cost: float = Field(1.0, ge=0.0) + posture_cost: float = Field(1e-3, ge=0.0) + min_dt: float = Field(1e-4, gt=0.0) + max_dt: float = Field(0.05, gt=0.0) reference_q: list[float] | None = None qpsolver_options: dict[str, float] = Field(default_factory=dict) @@ -73,39 +74,55 @@ def _rebuild_robot_model(cls, value: object) -> RobotModelConfig: ) return RobotModelConfig.model_validate(payload) - def validate_settings( - self, - joint_count: int, - model_path: str | Path | None = None, - ) -> None: - numeric = ( - self.max_velocity, - self.lm_damping, - self.task_gain, - self.position_cost, - self.orientation_cost, - self.min_dt, - self.max_dt, - ) - if not all(np.isfinite(value) for value in numeric): + @field_validator( + "max_velocity", + "lm_damping", + "task_gain", + "position_cost", + "orientation_cost", + "posture_cost", + "min_dt", + "max_dt", + ) + @classmethod + def _finite_numeric_setting(cls, value: float) -> float: + if not np.isfinite(value): raise ValueError("control IK numeric settings must be finite") - if self.max_velocity <= 0.0 or self.lm_damping <= 0.0 or self.task_gain <= 0.0: - raise ValueError("control IK velocity, damping, and gain must be positive") - if self.position_cost < 0.0 or self.orientation_cost < 0.0: - raise ValueError("control IK task costs must not be negative") - if self.min_dt <= 0.0 or self.max_dt < self.min_dt: - raise ValueError("control IK dt bounds must be positive and ordered") - if any(not np.isfinite(value) for value in self.qpsolver_options.values()): + return value + + @field_validator("qpsolver_options") + @classmethod + def _finite_qpsolver_options(cls, value: dict[str, float]) -> dict[str, float]: + if any(not np.isfinite(option) for option in value.values()): raise ValueError("control IK QP options must be finite") - if not self.robot_model.end_effector_link: + return value + + @model_validator(mode="after") + def _validate_robot_settings(self) -> PinkControlIKConfig: + robot = self.robot_model + if not robot.end_effector_link: raise ValueError("Pink control requires a named end-effector frame") - if len(self.robot_model.joint_names) != joint_count: - raise ValueError("RobotModelConfig and control task joint counts differ") - if ( - model_path is not None - and Path(self.robot_model.model_path).resolve() != Path(model_path).resolve() + if self.max_dt < self.min_dt: + raise ValueError("control IK dt bounds must be ordered") + joint_count = len(robot.get_coordinator_joint_names()) + if (robot.joint_limits_lower is None) != (robot.joint_limits_upper is None): + raise ValueError("both configured joint limit bounds are required") + for bounds in (robot.joint_limits_lower, robot.joint_limits_upper, robot.velocity_limits): + if bounds is not None and len(bounds) != joint_count: + raise ValueError("RobotModelConfig limits must match coordinator joints") + if robot.joint_limits_lower is not None and robot.joint_limits_upper is not None: + if any( + not np.isfinite(lower) or not np.isfinite(upper) or lower >= upper + for lower, upper in zip( + robot.joint_limits_lower, robot.joint_limits_upper, strict=True + ) + ): + raise ValueError("configured joint limits must be finite and ordered") + if robot.velocity_limits is not None and any( + not np.isfinite(limit) or limit <= 0.0 for limit in robot.velocity_limits ): - raise ValueError("Pink RobotModelConfig must use the authoritative model path") + raise ValueError("configured velocity limits are invalid") + return self @dataclass(frozen=True) @@ -123,17 +140,11 @@ class PinkControlIK: def __init__( self, - model_path: str | Path, - joint_names: list[str], config: PinkControlIKConfig, ) -> None: self._config = config - self._joint_names = list(joint_names) - self._config.validate_settings(len(self._joint_names), model_path) - robot = config.robot_model - if robot is None: # guarded by validate_settings; retained for narrowing - raise ValueError("Pink control requires a RobotModelConfig") + self._joint_names = robot.get_coordinator_joint_names() prepared_path = Path( prepare_urdf_for_drake( robot.model_path, @@ -151,7 +162,7 @@ def __init__( self._ee_frame_id = self._validate_frame(robot.end_effector_link) self._apply_limits(robot) full_reference_q = self._build_reference_q() - controlled_joint_ids = set(self._controlled_joint_ids) + controlled_joint_ids = self._controlled_joint_ids locked_joint_ids = [ joint_id for joint_id in range(1, len(self._model.joints)) @@ -184,6 +195,9 @@ def __init__( lm_damping=config.lm_damping, gain=config.task_gain, ) + self._posture_task = ( + pink.tasks.PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None + ) @property def nq(self) -> int: @@ -216,9 +230,13 @@ def solve( try: configuration.update(self._full_q(measured)) frame_task.set_target(target) + tasks: list[object] = [frame_task] + if self._posture_task is not None: + self._posture_task.set_target(configuration.q.copy()) + tasks.append(self._posture_task) velocity = pink.solve_ik( configuration, - [frame_task], + tasks, dt, solver=self._config.solver, damping=self._config.lm_damping, @@ -302,7 +320,7 @@ def _build_mapping(self, robot: RobotModelConfig) -> tuple[list[int], list[int]] indices: list[int] = [] velocity_indices: list[int] = [] self._q_widths: list[int] = [] - self._controlled_joint_ids: list[int] = [] + self._controlled_joint_ids: set[int] = set() for urdf_name in (robot.get_urdf_joint_name(name) for name in coordinator_names): if not self._model.existJointName(urdf_name): raise ValueError(f"control joint mapping references unknown joint: {urdf_name}") @@ -315,7 +333,7 @@ def _build_mapping(self, robot: RobotModelConfig) -> tuple[list[int], list[int]] indices.append(int(joint.idx_q)) velocity_indices.append(int(joint.idx_v)) self._q_widths.append(int(joint.nq)) - self._controlled_joint_ids.append(joint_id) + self._controlled_joint_ids.add(joint_id) return indices, velocity_indices def _build_reference_q(self, use_config_reference: bool = True) -> NDArray[np.float64]: diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 6d35c12ed3..470162d731 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -145,8 +145,6 @@ def prepare( ) PinkControlIK( - tmp_path / "robot.xacro", - ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot), ) @@ -163,18 +161,14 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N with pytest.raises(ValueError, match="end-effector frame"): PinkControlIK( - model_path, - ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path, frame="missing")), ) mismatched = _robot(model_path).model_copy( - update={"joint_name_mapping": {"arm/joint1": "joint1", "arm/joint2": "joint2"}} + update={"joint_name_mapping": {"joint1": "missing", "joint2": "joint2"}} ) - with pytest.raises(ValueError, match="exactly match"): + with pytest.raises(ValueError, match="unknown joint"): PinkControlIK( - model_path, - ["joint1", "joint2"], PinkControlIKConfig(robot_model=mismatched), ) @@ -184,8 +178,6 @@ def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( ) -> None: model_path = _write_urdf(tmp_path) backend = PinkControlIK( - model_path, - ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path)), ) measured = np.array([0.3, 0.1]) @@ -205,7 +197,8 @@ def solve( assert np.array_equal(result.positions, measured) assert len(calls) == 1 - assert len(calls[0][1]) == 1 + assert len(calls[0][1]) == 2 + assert np.array_equal(calls[0][1][1].target_q, backend._full_q(measured)) assert calls[0][2] == 0.01 @@ -214,8 +207,6 @@ def test_pink_backend_clamps_dt_from_backend_configuration( ) -> None: model_path = _write_urdf(tmp_path) backend = PinkControlIK( - model_path, - ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path), min_dt=0.01, max_dt=0.02), ) calls: list[float] = [] @@ -235,14 +226,32 @@ def solve( assert calls == [0.02] +def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None: + model_path = _write_urdf(tmp_path) + backend = PinkControlIK(PinkControlIKConfig(robot_model=_robot(model_path), posture_cost=0.0)) + calls: list[list[object]] = [] + + def solve( + configuration: object, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + calls.append(tasks) + return np.zeros(backend._model.nv) + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve + ) + measured = np.array([0.3, 0.1]) + backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + assert calls and len(calls[0]) == 1 + + def test_pink_rejects_uncontrolled_end_effector_chain_without_reference( tmp_path: Path, ) -> None: model_path = _write_urdf(tmp_path, "uncontrolled.urdf", _UNCONTROLLED_URDF) with pytest.raises(ValueError, match="reference_q.*uncontrolled joint"): PinkControlIK( - model_path, - ["joint1", "joint2"], PinkControlIKConfig(robot_model=_robot(model_path)), ) @@ -253,8 +262,6 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path with pytest.raises(ValueError, match="continuous joints.*tangent-space"): PinkControlIK( - model_path, - ["joint1"], PinkControlIKConfig(robot_model=robot), ) @@ -262,8 +269,6 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path update={"joint_limits_lower": None, "joint_limits_upper": None} ) backend = PinkControlIK( - model_path, - ["joint1"], PinkControlIKConfig(robot_model=roundtrip_robot), ) angle = np.array([3.0]) @@ -278,8 +283,6 @@ def test_pink_applies_position_velocity_limits_and_finite_output(tmp_path: Path) update={"joint_limits_lower": [-0.5, -0.25], "joint_limits_upper": [0.5, 0.25]} ) backend = PinkControlIK( - model_path, - ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot, max_velocity=0.2), ) @@ -299,9 +302,7 @@ def test_pink_clamps_tiny_position_limit_overshoot( robot = _robot(model_path).model_copy( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) - backend = PinkControlIK( - model_path, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) - ) + backend = PinkControlIK(PinkControlIKConfig(robot_model=robot)) measured = np.array([1.22, 0.1]) def solve( @@ -325,9 +326,7 @@ def test_pink_rejects_material_position_limit_violation( robot = _robot(model_path).model_copy( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) - backend = PinkControlIK( - model_path, ["joint1", "joint2"], PinkControlIKConfig(robot_model=robot) - ) + backend = PinkControlIK(PinkControlIKConfig(robot_model=robot)) measured = np.array([1.22, 0.1]) def solve( @@ -354,8 +353,11 @@ def test_cartesian_pipeline_bounds_dt_and_holds_on_expected_error( "cartesian", CartesianIKTaskConfig( joint_names=["j1", "j2"], - model_path="unused.urdf", - control_ik=PinkControlIKConfig(robot_model=_robot(Path("unused.urdf"))), + control_ik=PinkControlIKConfig( + robot_model=_robot(Path("unused.urdf")).model_copy( + update={"joint_names": ["j1", "j2"]} + ) + ), timeout=0.2, ), ) @@ -382,7 +384,7 @@ def test_factory_rejects_invalid_default_pink_configuration() -> None: type="cartesian_ik", joint_names=["j1", "j2"], priority=10, - params={"model_path": "unused.urdf"}, + params={}, ) with pytest.raises(ValueError, match="control_ik"): diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py index 02de831462..ebfae2358f 100644 --- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py @@ -17,7 +17,6 @@ from __future__ import annotations from dataclasses import dataclass -from pathlib import Path import threading from typing import TYPE_CHECKING @@ -127,7 +126,6 @@ def clear(self) -> None: class EEFTwistTaskParams(BaseConfig): - model_path: str | Path timeout: float = 0.3 max_joint_delta_deg: float = 15.0 control_ik: PinkControlIKConfig @@ -139,7 +137,6 @@ def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask: cfg.name, EEFTwistTaskConfig( joint_names=cfg.joint_names, - model_path=params.model_path, priority=cfg.priority, timeout=params.timeout, max_joint_delta_deg=params.max_joint_delta_deg, diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index 56db4958be..bd61d71999 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -79,7 +79,6 @@ def task(fake_ik: FakeIK) -> EEFTwistTask: "eef", EEFTwistTaskConfig( joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], - model_path="fake.urdf", control_ik=PinkControlIKConfig( robot_model=RobotModelConfig( name="fake", diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py index 6dd0c98c17..38ef418ac0 100644 --- a/dimos/robot/manipulators/a1z/blueprints/teleop.py +++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py @@ -36,7 +36,6 @@ tasks=[ eef_twist_task( _a1z_keyboard_hw, - model_path=_a1z_model.model_path, robot_model=_a1z_model, ) ], diff --git a/dimos/robot/manipulators/a750/blueprints/teleop.py b/dimos/robot/manipulators/a750/blueprints/teleop.py index 0786b2bd0d..90435cb267 100644 --- a/dimos/robot/manipulators/a750/blueprints/teleop.py +++ b/dimos/robot/manipulators/a750/blueprints/teleop.py @@ -20,7 +20,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule from dimos.robot.manipulators.a750.config import ( - A750_MODEL_PATH, a750_hardware, make_a750_model_config, ) @@ -40,7 +39,6 @@ tasks=[ eef_twist_task( _a750_hw, - model_path=A750_MODEL_PATH, robot_model=_a750_model, ) ], diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index 765a1103ea..3401ab9cca 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -16,13 +16,12 @@ from __future__ import annotations -from collections.abc import Sequence +from collections.abc import Mapping, Sequence from pathlib import Path from typing import Any from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig -from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.core.coordination.blueprints import Blueprint from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -51,78 +50,71 @@ def trajectory_task( def _resolve_control_ik( hardware: HardwareComponent, - model_path: Path, - control_ik: PinkControlIKConfig | None, - robot_model: RobotModelConfig | None, -) -> PinkControlIKConfig: - resolved = control_ik or PinkControlIKConfig(robot_model=robot_model) - if robot_model is not None and resolved.robot_model != robot_model: - raise ValueError("conflicting Pink RobotModelConfig values") - resolved.validate_settings(len(hardware.joints), model_path) - return resolved - - -def _serialize_control_ik(config: PinkControlIKConfig) -> dict[str, object]: - """Serialize solver settings without runtime-only module transport objects.""" - payload: dict[str, object] = config.model_dump(mode="json", exclude={"robot_model"}) - robot_model = config.robot_model - if robot_model is not None: - base_pose = robot_model.base_pose - robot_payload: dict[str, object] = { - "name": robot_model.name, - "model_path": str(robot_model.model_path), - "base_pose": { - "ts": float(base_pose.ts), - "frame_id": base_pose.frame_id, - "position": [base_pose.position.x, base_pose.position.y, base_pose.position.z], - "orientation": [ - base_pose.orientation.x, - base_pose.orientation.y, - base_pose.orientation.z, - base_pose.orientation.w, - ], - }, - "joint_names": list(robot_model.joint_names), - "end_effector_link": robot_model.end_effector_link, - "base_link": robot_model.base_link, - "package_paths": {name: str(path) for name, path in robot_model.package_paths.items()}, - "joint_limits_lower": robot_model.joint_limits_lower, - "joint_limits_upper": robot_model.joint_limits_upper, - "velocity_limits": robot_model.velocity_limits, - "auto_convert_meshes": robot_model.auto_convert_meshes, - "xacro_args": dict(robot_model.xacro_args), - "collision_exclusion_pairs": list(robot_model.collision_exclusion_pairs), - "max_velocity": robot_model.max_velocity, - "max_acceleration": robot_model.max_acceleration, - "joint_name_mapping": dict(robot_model.joint_name_mapping), - "coordinator_task_name": robot_model.coordinator_task_name, - "gripper_hardware_id": robot_model.gripper_hardware_id, - "tf_extra_links": list(robot_model.tf_extra_links), - "home_joints": robot_model.home_joints, - "pre_grasp_offset": robot_model.pre_grasp_offset, - } - payload["robot_model"] = robot_payload + robot_model: RobotModelConfig, + control_ik: Mapping[str, object] | None, +) -> dict[str, object]: + coordinator_joints = robot_model.get_coordinator_joint_names() + if hardware.joints != coordinator_joints: + raise ValueError("hardware joints must match RobotModelConfig coordinator joints") + payload = dict(control_ik or {}) + payload["robot_model"] = _serialize_robot_model(robot_model) return payload +def _serialize_robot_model(robot_model: RobotModelConfig) -> dict[str, object]: + """Serialize the authoritative robot model without runtime-only objects.""" + base_pose = robot_model.base_pose + return { + "name": robot_model.name, + "model_path": str(robot_model.model_path), + "base_pose": { + "ts": float(base_pose.ts), + "frame_id": base_pose.frame_id, + "position": [base_pose.position.x, base_pose.position.y, base_pose.position.z], + "orientation": [ + base_pose.orientation.x, + base_pose.orientation.y, + base_pose.orientation.z, + base_pose.orientation.w, + ], + }, + "joint_names": list(robot_model.joint_names), + "end_effector_link": robot_model.end_effector_link, + "base_link": robot_model.base_link, + "package_paths": {name: str(path) for name, path in robot_model.package_paths.items()}, + "joint_limits_lower": robot_model.joint_limits_lower, + "joint_limits_upper": robot_model.joint_limits_upper, + "velocity_limits": robot_model.velocity_limits, + "auto_convert_meshes": robot_model.auto_convert_meshes, + "xacro_args": dict(robot_model.xacro_args), + "collision_exclusion_pairs": list(robot_model.collision_exclusion_pairs), + "max_velocity": robot_model.max_velocity, + "max_acceleration": robot_model.max_acceleration, + "joint_name_mapping": dict(robot_model.joint_name_mapping), + "coordinator_task_name": robot_model.coordinator_task_name, + "gripper_hardware_id": robot_model.gripper_hardware_id, + "tf_extra_links": list(robot_model.tf_extra_links), + "home_joints": robot_model.home_joints, + "pre_grasp_offset": robot_model.pre_grasp_offset, + } + + def cartesian_ik_task( hardware: HardwareComponent, *, - model_path: Path, name: str = CARTESIAN_IK_TASK_NAME, priority: int = 10, - control_ik: PinkControlIKConfig | None = None, - robot_model: RobotModelConfig | None = None, + control_ik: Mapping[str, object] | None = None, + robot_model: RobotModelConfig, ) -> TaskConfig: - resolved_control_ik = _resolve_control_ik(hardware, model_path, control_ik, robot_model) + resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik) return TaskConfig( name=name, type="cartesian_ik", joint_names=hardware.joints, priority=priority, params={ - "model_path": model_path, - **({"control_ik": _serialize_control_ik(resolved_control_ik)}), + "control_ik": resolved_control_ik, }, ) @@ -130,21 +122,19 @@ def cartesian_ik_task( def eef_twist_task( hardware: HardwareComponent, *, - model_path: Path, name: str = EEF_TWIST_TASK_NAME, priority: int = 10, - control_ik: PinkControlIKConfig | None = None, - robot_model: RobotModelConfig | None = None, + control_ik: Mapping[str, object] | None = None, + robot_model: RobotModelConfig, ) -> TaskConfig: - resolved_control_ik = _resolve_control_ik(hardware, model_path, control_ik, robot_model) + resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik) return TaskConfig( name=name, type="eef_twist", joint_names=hardware.joints, priority=priority, params={ - "model_path": model_path, - **({"control_ik": _serialize_control_ik(resolved_control_ik)}), + "control_ik": resolved_control_ik, }, ) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index e3cc4ca90e..eb6baae066 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -37,7 +37,6 @@ tasks=[ eef_twist_task( _teleop_hw, - model_path=_openarm_model.model_path, robot_model=_openarm_model, ) ], @@ -57,7 +56,6 @@ tasks=[ eef_twist_task( _teleop_real_hw, - model_path=_openarm_model.model_path, robot_model=_openarm_model, ) ], diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py index 6035ad5878..5027115dd5 100644 --- a/dimos/robot/manipulators/piper/blueprints/teleop.py +++ b/dimos/robot/manipulators/piper/blueprints/teleop.py @@ -29,7 +29,6 @@ from dimos.robot.manipulators.common.sim import mujoco_if_sim from dimos.robot.manipulators.piper.config import ( PIPER_FK_MODEL, - PIPER_MODEL_PATH, PIPER_SIM_PATH, make_piper_hardware, make_piper_model_config, @@ -56,7 +55,6 @@ tasks=[ eef_twist_task( _piper_keyboard_hw, - model_path=PIPER_MODEL_PATH, robot_model=_piper_model, ) ], @@ -77,7 +75,6 @@ tasks=[ cartesian_ik_task( _piper_mock_cartesian_hw, - model_path=PIPER_MODEL_PATH, robot_model=_piper_model, ) ], @@ -118,7 +115,6 @@ tasks=[ cartesian_ik_task( _piper_cartesian_hw, - model_path=PIPER_MODEL_PATH, robot_model=_piper_model, ) ], diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 6e2b0cabe7..9162ee75de 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from pathlib import Path from typing import cast import pytest @@ -45,7 +44,11 @@ keyboard_teleop_xarm6, keyboard_teleop_xarm7, ) -from dimos.robot.manipulators.xarm.config import make_xarm7_model_config, make_xarm_hardware +from dimos.robot.manipulators.xarm.config import ( + make_xarm6_model_config, + make_xarm7_model_config, + make_xarm_hardware, +) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule @@ -91,11 +94,13 @@ def test_xarm_planner_blueprints_default_to_no_visualization() -> None: assert isinstance(config.visualization, NoManipulationVisualizationConfig) -def test_eef_twist_task_helper_requires_pink_robot_model() -> None: +def test_eef_twist_task_helper_serializes_authoritative_robot_model() -> None: hardware = make_xarm_hardware("arm", 6, adapter_type="mock") - with pytest.raises(ValueError, match="robot_model"): - eef_twist_task(hardware, model_path=Path("fake.urdf")) + task = eef_twist_task(hardware, robot_model=make_xarm6_model_config(add_gripper=False)) + + assert "model_path" not in task.params + assert task.params["control_ik"]["robot_model"]["model_path"] @pytest.mark.parametrize( @@ -142,7 +147,7 @@ def test_shipped_eef_twist_blueprints_use_pink_with_named_models( assert control_ik["robot_model"]["end_effector_link"] assert "ee_joint_id" not in task.params - assert not str(task.params["model_path"]).endswith((".xml", ".mjcf")) + assert not str(control_ik["robot_model"]["model_path"]).endswith((".xml", ".mjcf")) def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: @@ -158,7 +163,7 @@ def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: if task.type in ("eef_twist", "cartesian_ik") ) control_ik = task.params["control_ik"] - assert task.params["model_path"] == PIPER_MODEL_PATH + assert control_ik["robot_model"]["model_path"] == str(PIPER_MODEL_PATH) assert control_ik["robot_model"]["model_path"] == str(PIPER_MODEL_PATH) assert control_ik["robot_model"]["end_effector_link"] == "gripper_base" assert "ee_joint_id" not in task.params diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py index 0f0f85c0b4..f24c474085 100644 --- a/dimos/robot/manipulators/xarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -65,7 +65,6 @@ tasks=[ eef_twist_task( _xarm6_hw, - model_path=_xarm6_model.model_path, robot_model=_xarm6_model, ) ], @@ -86,7 +85,6 @@ tasks=[ eef_twist_task( _xarm7_hw, - model_path=_xarm7_model.model_path, robot_model=_xarm7_model, ) ], diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 39515ea35d..cdaa460d4e 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -583,25 +583,19 @@ frame, and `joint_name_mapping` maps coordinator joints to URDF joints. Pink validates the prepared model, named frame, and exact ordered joint mapping at startup. -The common helper passes that typed configuration to Pink: +The common helper passes that typed configuration to Pink and derives the model +path and coordinator joint order from it: ```python skip -from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, eef_twist_task -control_ik = PinkControlIKConfig(robot_model=robot_model) - cartesian_task = cartesian_ik_task( hardware, - model_path=robot_model.model_path, robot_model=robot_model, - control_ik=control_ik, ) twist_task = eef_twist_task( hardware, - model_path=robot_model.model_path, robot_model=robot_model, - control_ik=control_ik, ) ``` diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index e572f1d2db..bc6343aa77 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -131,7 +131,9 @@ frames, mismatched mappings, or an invalid prepared model fail initialization. Each control tick starts from measured joints, clamps `dt`, updates one Pink `FrameTask`, solves and integrates one local differential-IK step, and applies -position and velocity limits. Non-finite or unsafe output is rejected. Expected +position and velocity limits. A configurable `posture_cost` (default `1e-3`) +regularizes only the null space toward the current measured configuration and +can be disabled with zero. Non-finite or unsafe output is rejected. Expected runtime solve errors produce a bounded safe hold instead of an invalid command. The control backend is separate from manipulation planning. It does not use @@ -139,19 +141,15 @@ The control backend is separate from manipulation planning. It does not use avoidance claim. `WorldSpec` and its Pink/Drake backends remain responsible for planning behavior. -For a custom robot, the current helper API passes the typed model configuration -to Pink without a numeric EEF ID: +For a custom robot, the helper API derives the Pink model and joint mapping from +the typed model configuration: ```python skip -from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.robot.manipulators.common.blueprints import cartesian_ik_task -control_ik = PinkControlIKConfig(robot_model=robot_model) task = cartesian_ik_task( hardware, - model_path=robot_model.model_path, robot_model=robot_model, - control_ik=control_ik, ) ``` From 4d3adc26662d811b9ad565fa5baaac46d4d5345d Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 15:03:32 -0700 Subject: [PATCH 09/19] fix: simplify Pink task configuration --- .../cartesian_ik_task/cartesian_ik_task.py | 29 ++-- .../cartesian_ik_task/pink_control_ik.py | 124 ++++-------------- .../cartesian_ik_task/test_pink_control_ik.py | 29 +++- .../tasks/eef_twist_task/eef_twist_task.py | 5 + .../eef_twist_task/test_eef_twist_task.py | 3 + dimos/robot/manipulators/common/blueprints.py | 48 ++----- dimos/robot/manipulators/test_blueprints.py | 23 ++-- 7 files changed, 101 insertions(+), 160 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index e6c280d982..2ab9a7c8fd 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -27,6 +27,7 @@ import numpy as np import pinocchio +from pydantic import FiniteFloat from dimos.control.coordinator import TaskConfig from dimos.control.task import ( @@ -72,6 +73,19 @@ class CartesianIKTaskConfig: priority: int = 10 timeout: float = 0.5 max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz + min_dt: FiniteFloat = 1e-4 + max_dt: FiniteFloat = 0.05 + + def __post_init__(self) -> None: + if ( + not np.isfinite(self.min_dt) + or not np.isfinite(self.max_dt) + or self.min_dt <= 0.0 + or self.max_dt <= 0.0 + ): + raise ValueError("CartesianIKTask dt bounds must be finite and positive") + if self.max_dt < self.min_dt: + raise ValueError("CartesianIKTask dt bounds must be ordered") class CartesianIKTask(BaseControlTask): @@ -202,9 +216,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if not np.all(np.isfinite(q_current)): logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name) return None - dt = self._clamped_dt(state.dt) - if dt is None: + raw_dt = state.dt + if not np.isfinite(raw_dt) or raw_dt <= 0.0: return self._hold(q_current) + dt = min(max(raw_dt, self._config.min_dt), self._config.max_dt) try: target_pose = self._prepare_target(state, q_current, dt) except (FloatingPointError, RuntimeError, ValueError) as exc: @@ -288,12 +303,6 @@ def _prepare_target( return None return target - def _clamped_dt(self, dt: float) -> float | None: - if not np.isfinite(dt) or dt <= 0.0: - return None - bounds = self._config.control_ik - return min(max(dt, bounds.min_dt), bounds.max_dt) - def _on_timeout(self) -> None: """Hook for target sources with state outside the Cartesian pose cache.""" @@ -385,6 +394,8 @@ def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio. class CartesianIKTaskParams(BaseConfig): control_ik: PinkControlIKConfig + min_dt: FiniteFloat = 1e-4 + max_dt: FiniteFloat = 0.05 def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: @@ -394,6 +405,8 @@ def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: CartesianIKTaskConfig( joint_names=cfg.joint_names, priority=cfg.priority, + min_dt=params.min_dt, + max_dt=params.max_dt, control_ik=params.control_ik, ), ) diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index 1c439c27f0..05abebd191 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -16,7 +16,6 @@ from __future__ import annotations -from collections.abc import Mapping from dataclasses import dataclass from pathlib import Path @@ -25,11 +24,10 @@ import pink from pink.limits import ConfigurationLimit, VelocityLimit import pinocchio -from pydantic import Field, field_validator, model_validator +from pydantic import Field, FiniteFloat, field_validator from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.protocol.service.spec import BaseConfig # Pink's integration/QP boundary tolerance is small but larger than machine epsilon. @@ -41,89 +39,22 @@ class PinkControlIKConfig(BaseConfig): robot_model: RobotModelConfig solver: str = "proxqp" - max_velocity: float = Field(10.0, gt=0.0) - lm_damping: float = Field(1e-4, gt=0.0) - task_gain: float = Field(1.0, gt=0.0) - position_cost: float = Field(1.0, ge=0.0) - orientation_cost: float = Field(1.0, ge=0.0) - posture_cost: float = Field(1e-3, ge=0.0) - min_dt: float = Field(1e-4, gt=0.0) - max_dt: float = Field(0.05, gt=0.0) + max_velocity: FiniteFloat = Field(10.0, gt=0.0) + lm_damping: FiniteFloat = Field(1e-4, gt=0.0) + task_gain: FiniteFloat = Field(1.0, gt=0.0) + position_cost: FiniteFloat = Field(1.0, ge=0.0) + orientation_cost: FiniteFloat = Field(1.0, ge=0.0) + posture_cost: FiniteFloat = Field(1e-3, ge=0.0) reference_q: list[float] | None = None - qpsolver_options: dict[str, float] = Field(default_factory=dict) + qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict) @field_validator("robot_model", mode="before") @classmethod - def _rebuild_robot_model(cls, value: object) -> RobotModelConfig: - if isinstance(value, RobotModelConfig): - return value - if not isinstance(value, Mapping): - raise ValueError("Pink robot_model must be a serialized RobotModelConfig") - payload = dict(value) - base_pose = payload.get("base_pose") - if isinstance(base_pose, Mapping): - position = base_pose.get("position") - orientation = base_pose.get("orientation") - if not isinstance(position, list) or not isinstance(orientation, list): - raise ValueError("serialized RobotModelConfig base_pose is invalid") - payload["base_pose"] = PoseStamped( - ts=float(base_pose.get("ts", 0.0)), - frame_id=str(base_pose.get("frame_id", "")), - position=position, - orientation=orientation, - ) - return RobotModelConfig.model_validate(payload) - - @field_validator( - "max_velocity", - "lm_damping", - "task_gain", - "position_cost", - "orientation_cost", - "posture_cost", - "min_dt", - "max_dt", - ) - @classmethod - def _finite_numeric_setting(cls, value: float) -> float: - if not np.isfinite(value): - raise ValueError("control IK numeric settings must be finite") - return value - - @field_validator("qpsolver_options") - @classmethod - def _finite_qpsolver_options(cls, value: dict[str, float]) -> dict[str, float]: - if any(not np.isfinite(option) for option in value.values()): - raise ValueError("control IK QP options must be finite") + def _accept_robot_model(cls, value: object) -> RobotModelConfig: + if not isinstance(value, RobotModelConfig): + raise TypeError("Pink robot_model must be a RobotModelConfig instance") return value - @model_validator(mode="after") - def _validate_robot_settings(self) -> PinkControlIKConfig: - robot = self.robot_model - if not robot.end_effector_link: - raise ValueError("Pink control requires a named end-effector frame") - if self.max_dt < self.min_dt: - raise ValueError("control IK dt bounds must be ordered") - joint_count = len(robot.get_coordinator_joint_names()) - if (robot.joint_limits_lower is None) != (robot.joint_limits_upper is None): - raise ValueError("both configured joint limit bounds are required") - for bounds in (robot.joint_limits_lower, robot.joint_limits_upper, robot.velocity_limits): - if bounds is not None and len(bounds) != joint_count: - raise ValueError("RobotModelConfig limits must match coordinator joints") - if robot.joint_limits_lower is not None and robot.joint_limits_upper is not None: - if any( - not np.isfinite(lower) or not np.isfinite(upper) or lower >= upper - for lower, upper in zip( - robot.joint_limits_lower, robot.joint_limits_upper, strict=True - ) - ): - raise ValueError("configured joint limits must be finite and ordered") - if robot.velocity_limits is not None and any( - not np.isfinite(limit) or limit <= 0.0 for limit in robot.velocity_limits - ): - raise ValueError("configured velocity limits are invalid") - return self - @dataclass(frozen=True) class ControlIKResult: @@ -131,7 +62,7 @@ class ControlIKResult: velocity: NDArray[np.float64] -class PinkControlRuntimeError(RuntimeError): +class IKControlRuntimeError(RuntimeError): """A runtime solver/model failure that should produce a bounded hold.""" @@ -198,6 +129,9 @@ def __init__( self._posture_task = ( pink.tasks.PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None ) + self._tasks: list[object] = [self._frame_task] + if self._posture_task is not None: + self._tasks.append(self._posture_task) @property def nq(self) -> int: @@ -221,22 +155,19 @@ def solve( raise ValueError("measured joint state is invalid") if not np.isfinite(dt) or dt <= 0.0: raise ValueError("control IK dt must be finite and positive") - dt = min(max(dt, self._config.min_dt), self._config.max_dt) configuration = self._configuration frame_task = self._frame_task if configuration is None or frame_task is None: - raise PinkControlRuntimeError("Pink control backend is unavailable") + raise IKControlRuntimeError("Pink control backend is unavailable") try: configuration.update(self._full_q(measured)) frame_task.set_target(target) - tasks: list[object] = [frame_task] if self._posture_task is not None: self._posture_task.set_target(configuration.q.copy()) - tasks.append(self._posture_task) velocity = pink.solve_ik( configuration, - tasks, + self._tasks, dt, solver=self._config.solver, damping=self._config.lm_damping, @@ -245,17 +176,17 @@ def solve( ) velocity = np.asarray(velocity, dtype=np.float64).reshape(-1) if velocity.size != self._model.nv or not np.all(np.isfinite(velocity)): - raise PinkControlRuntimeError("Pink produced an invalid velocity") + raise IKControlRuntimeError("Pink produced an invalid velocity") configuration.integrate_inplace(velocity, dt) - candidate = self._controlled_q(configuration.q, measured) + candidate = self._project_controlled_positions(configuration.q, measured) if candidate.size != measured.size or not np.all(np.isfinite(candidate)): - raise PinkControlRuntimeError("Pink produced an invalid joint candidate") + raise IKControlRuntimeError("Pink produced an invalid joint candidate") candidate = self._clamp_position_limits(candidate) return ControlIKResult(candidate, self._controlled_velocity(velocity)) - except PinkControlRuntimeError: + except IKControlRuntimeError: raise except Exception as exc: - raise PinkControlRuntimeError(f"Pink control solve failed: {exc}") from exc + raise IKControlRuntimeError(f"Pink control solve failed: {exc}") from exc def _full_q(self, controlled: NDArray[np.float64]) -> NDArray[np.float64]: q = self._reference_q.copy() @@ -267,9 +198,10 @@ def _full_q(self, controlled: NDArray[np.float64]) -> NDArray[np.float64]: q[index] = value return q - def _controlled_q( + def _project_controlled_positions( self, full_q: NDArray[np.float64], reference: NDArray[np.float64] | None = None ) -> NDArray[np.float64]: + """Project model coordinates to coordinator joints and unwrap continuous angles.""" positions = np.array( [ np.arctan2(full_q[index + 1], full_q[index]) if width == 2 else full_q[index] @@ -297,16 +229,16 @@ def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.f lower = self._model.lowerPositionLimit[q_index] upper = self._model.upperPositionLimit[q_index] value = bounded[index] - if np.isfinite(lower) and value < lower: + if value < lower: if lower - value <= _POSITION_LIMIT_EPSILON_RAD: bounded[index] = lower else: - raise PinkControlRuntimeError("Pink produced an out-of-bounds joint candidate") - elif np.isfinite(upper) and value > upper: + raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate") + elif value > upper: if value - upper <= _POSITION_LIMIT_EPSILON_RAD: bounded[index] = upper else: - raise PinkControlRuntimeError("Pink produced an out-of-bounds joint candidate") + raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate") return bounded def _build_mapping(self, robot: RobotModelConfig) -> tuple[list[int], list[int]]: diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 470162d731..da1f3395d9 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -25,9 +25,9 @@ ) from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( ControlIKResult, + IKControlRuntimeError, PinkControlIK, PinkControlIKConfig, - PinkControlRuntimeError, ) from dimos.control.tasks.registry import control_task_registry from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -110,6 +110,22 @@ def test_pink_requires_robot_model() -> None: PinkControlIKConfig() +def test_pink_settings_use_finite_declarative_validation(tmp_path: Path) -> None: + robot = _robot(_write_urdf(tmp_path)) + + with pytest.raises(ValueError, match="finite"): + PinkControlIKConfig(robot_model=robot, max_velocity=np.inf) + with pytest.raises(ValueError, match="finite"): + PinkControlIKConfig(robot_model=robot, qpsolver_options={"eps": np.nan}) + with pytest.raises(ValueError, match="ordered"): + CartesianIKTaskConfig( + joint_names=["joint1", "joint2"], + control_ik=PinkControlIKConfig(robot_model=robot), + min_dt=0.1, + max_dt=0.01, + ) + + def test_pink_prepares_xacro_with_package_paths_and_arguments( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: @@ -198,6 +214,7 @@ def solve( assert np.array_equal(result.positions, measured) assert len(calls) == 1 assert len(calls[0][1]) == 2 + assert calls[0][1] is backend._tasks assert np.array_equal(calls[0][1][1].target_q, backend._full_q(measured)) assert calls[0][2] == 0.01 @@ -207,7 +224,7 @@ def test_pink_backend_clamps_dt_from_backend_configuration( ) -> None: model_path = _write_urdf(tmp_path) backend = PinkControlIK( - PinkControlIKConfig(robot_model=_robot(model_path), min_dt=0.01, max_dt=0.02), + PinkControlIKConfig(robot_model=_robot(model_path)), ) calls: list[float] = [] @@ -223,7 +240,7 @@ def solve( measured = np.array([0.3, 0.1]) backend.solve(backend.forward_kinematics(measured), measured, 1.0) - assert calls == [0.02] + assert calls == [1.0] def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None: @@ -274,7 +291,7 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path angle = np.array([3.0]) assert backend._q_widths == [2] - assert np.allclose(backend._controlled_q(backend._full_q(angle), angle), angle) + assert np.allclose(backend._project_controlled_positions(backend._full_q(angle), angle), angle) def test_pink_applies_position_velocity_limits_and_finite_output(tmp_path: Path) -> None: @@ -337,7 +354,7 @@ def solve( monkeypatch.setattr( "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve ) - with pytest.raises(PinkControlRuntimeError, match="out-of-bounds"): + with pytest.raises(IKControlRuntimeError, match="out-of-bounds"): backend.solve(backend.forward_kinematics(measured), measured, 0.01) @@ -364,7 +381,7 @@ def test_cartesian_pipeline_bounds_dt_and_holds_on_expected_error( pose = PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) assert task.on_cartesian_command(pose, 1.0) assert task.compute(_cartesian_state(1.01, dt=1.0)) is not None - assert backend.dt_calls == [task._config.control_ik.max_dt] + assert backend.dt_calls == [task._config.max_dt] invalid_dt_hold = task.compute(_cartesian_state(1.02, dt=0.0)) assert invalid_dt_hold is not None diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py index ebfae2358f..ea921772a8 100644 --- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py @@ -22,6 +22,7 @@ import numpy as np import pinocchio +from pydantic import FiniteFloat from dimos.control.coordinator import TaskConfig from dimos.control.task import CoordinatorState @@ -128,6 +129,8 @@ def clear(self) -> None: class EEFTwistTaskParams(BaseConfig): timeout: float = 0.3 max_joint_delta_deg: float = 15.0 + min_dt: FiniteFloat = 1e-4 + max_dt: FiniteFloat = 0.05 control_ik: PinkControlIKConfig @@ -140,6 +143,8 @@ def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask: priority=cfg.priority, timeout=params.timeout, max_joint_delta_deg=params.max_joint_delta_deg, + min_dt=params.min_dt, + max_dt=params.max_dt, control_ik=params.control_ik, ), ) diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index bd61d71999..1ae5366fde 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -91,6 +91,8 @@ def task(fake_ik: FakeIK) -> EEFTwistTask: ), timeout=0.3, max_joint_delta_deg=15.0, + min_dt=0.02, + max_dt=0.03, ), ) @@ -161,6 +163,7 @@ def test_integration_uses_current_fk_and_coordinator_dt( assert first is not None assert second is not None + assert fake_ik.dt_calls == [0.02, 0.02] assert fake_ik.solve_calls[1].translation[0] > fake_ik.solve_calls[0].translation[0] diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index 3401ab9cca..994334b977 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -57,53 +57,17 @@ def _resolve_control_ik( if hardware.joints != coordinator_joints: raise ValueError("hardware joints must match RobotModelConfig coordinator joints") payload = dict(control_ik or {}) - payload["robot_model"] = _serialize_robot_model(robot_model) + payload["robot_model"] = robot_model return payload -def _serialize_robot_model(robot_model: RobotModelConfig) -> dict[str, object]: - """Serialize the authoritative robot model without runtime-only objects.""" - base_pose = robot_model.base_pose - return { - "name": robot_model.name, - "model_path": str(robot_model.model_path), - "base_pose": { - "ts": float(base_pose.ts), - "frame_id": base_pose.frame_id, - "position": [base_pose.position.x, base_pose.position.y, base_pose.position.z], - "orientation": [ - base_pose.orientation.x, - base_pose.orientation.y, - base_pose.orientation.z, - base_pose.orientation.w, - ], - }, - "joint_names": list(robot_model.joint_names), - "end_effector_link": robot_model.end_effector_link, - "base_link": robot_model.base_link, - "package_paths": {name: str(path) for name, path in robot_model.package_paths.items()}, - "joint_limits_lower": robot_model.joint_limits_lower, - "joint_limits_upper": robot_model.joint_limits_upper, - "velocity_limits": robot_model.velocity_limits, - "auto_convert_meshes": robot_model.auto_convert_meshes, - "xacro_args": dict(robot_model.xacro_args), - "collision_exclusion_pairs": list(robot_model.collision_exclusion_pairs), - "max_velocity": robot_model.max_velocity, - "max_acceleration": robot_model.max_acceleration, - "joint_name_mapping": dict(robot_model.joint_name_mapping), - "coordinator_task_name": robot_model.coordinator_task_name, - "gripper_hardware_id": robot_model.gripper_hardware_id, - "tf_extra_links": list(robot_model.tf_extra_links), - "home_joints": robot_model.home_joints, - "pre_grasp_offset": robot_model.pre_grasp_offset, - } - - def cartesian_ik_task( hardware: HardwareComponent, *, name: str = CARTESIAN_IK_TASK_NAME, priority: int = 10, + min_dt: float = 1e-4, + max_dt: float = 0.05, control_ik: Mapping[str, object] | None = None, robot_model: RobotModelConfig, ) -> TaskConfig: @@ -115,6 +79,8 @@ def cartesian_ik_task( priority=priority, params={ "control_ik": resolved_control_ik, + "min_dt": min_dt, + "max_dt": max_dt, }, ) @@ -124,6 +90,8 @@ def eef_twist_task( *, name: str = EEF_TWIST_TASK_NAME, priority: int = 10, + min_dt: float = 1e-4, + max_dt: float = 0.05, control_ik: Mapping[str, object] | None = None, robot_model: RobotModelConfig, ) -> TaskConfig: @@ -135,6 +103,8 @@ def eef_twist_task( priority=priority, params={ "control_ik": resolved_control_ik, + "min_dt": min_dt, + "max_dt": max_dt, }, ) diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 9162ee75de..100d93090e 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -61,7 +61,7 @@ def _manipulation_kwargs(blueprint: Blueprint) -> dict[str, object]: def _manipulation_config(blueprint: Blueprint) -> ManipulationModuleConfig: - return ManipulationModuleConfig(**_manipulation_kwargs(blueprint)) + return ManipulationModuleConfig.model_validate(_manipulation_kwargs(blueprint)) def _coordinator_tasks(blueprint: Blueprint) -> list[TaskConfig]: @@ -72,7 +72,7 @@ def test_planner_helper_defaults_to_no_visualization() -> None: blueprint = planner(robots=[make_xarm7_model_config(name="arm", add_gripper=True)]) kwargs = _manipulation_kwargs(blueprint) - config = ManipulationModuleConfig(**kwargs) + config = _manipulation_config(blueprint) assert "visualization" not in kwargs assert isinstance(config.visualization, NoManipulationVisualizationConfig) @@ -94,13 +94,14 @@ def test_xarm_planner_blueprints_default_to_no_visualization() -> None: assert isinstance(config.visualization, NoManipulationVisualizationConfig) -def test_eef_twist_task_helper_serializes_authoritative_robot_model() -> None: +def test_eef_twist_task_helper_passes_authoritative_robot_model() -> None: hardware = make_xarm_hardware("arm", 6, adapter_type="mock") + robot_model = make_xarm6_model_config(add_gripper=False) - task = eef_twist_task(hardware, robot_model=make_xarm6_model_config(add_gripper=False)) + task = eef_twist_task(hardware, robot_model=robot_model) assert "model_path" not in task.params - assert task.params["control_ik"]["robot_model"]["model_path"] + assert task.params["control_ik"]["robot_model"] is robot_model @pytest.mark.parametrize( @@ -145,9 +146,9 @@ def test_shipped_eef_twist_blueprints_use_pink_with_named_models( task = next(task for task in _coordinator_tasks(blueprint) if task.type == "eef_twist") control_ik = task.params["control_ik"] - assert control_ik["robot_model"]["end_effector_link"] + assert control_ik["robot_model"].end_effector_link assert "ee_joint_id" not in task.params - assert not str(control_ik["robot_model"]["model_path"]).endswith((".xml", ".mjcf")) + assert not str(control_ik["robot_model"].model_path).endswith((".xml", ".mjcf")) def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: @@ -163,13 +164,13 @@ def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: if task.type in ("eef_twist", "cartesian_ik") ) control_ik = task.params["control_ik"] - assert control_ik["robot_model"]["model_path"] == str(PIPER_MODEL_PATH) - assert control_ik["robot_model"]["model_path"] == str(PIPER_MODEL_PATH) - assert control_ik["robot_model"]["end_effector_link"] == "gripper_base" + assert control_ik["robot_model"].model_path == PIPER_MODEL_PATH + assert control_ik["robot_model"].model_path == PIPER_MODEL_PATH + assert control_ik["robot_model"].end_effector_link == "gripper_base" assert "ee_joint_id" not in task.params assert "self_collision_enabled" not in control_ik reconstructed = PinkControlIKConfig.model_validate(control_ik) - assert reconstructed.robot_model is not None + assert reconstructed.robot_model is control_ik["robot_model"] assert reconstructed.robot_model.model_path == PIPER_MODEL_PATH assert reconstructed.robot_model.end_effector_link == "gripper_base" From 1afe8f58300e6658219ce80586e943c2183d3f88 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 15:43:44 -0700 Subject: [PATCH 10/19] test: strengthen Pink control coverage --- .../test_cartesian_ik_task.py | 152 ++++++++++++++++++ .../cartesian_ik_task/test_pink_control_ik.py | 125 ++++---------- .../eef_twist_task/test_eef_twist_task.py | 25 +-- dimos/robot/manipulators/test_blueprints.py | 33 +++- 4 files changed, 223 insertions(+), 112 deletions(-) create mode 100644 dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py new file mode 100644 index 0000000000..a30e02a8ad --- /dev/null +++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py @@ -0,0 +1,152 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +from typing import cast + +import numpy as np +import pinocchio +import pytest + +from dimos.control.coordinator import TaskConfig +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import ( + CartesianIKTask, + CartesianIKTaskConfig, +) +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( + ControlIKResult, + IKControlRuntimeError, + PinkControlIKConfig, +) +from dimos.control.tasks.registry import control_task_registry +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + +def _robot(path: Path) -> RobotModelConfig: + return RobotModelConfig( + name="tiny", + model_path=path, + base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + joint_names=["joint1"], + end_effector_link="tool", + home_joints=[0.0], + ) + + +def _state(t_now: float, dt: float = 0.01) -> CoordinatorState: + return CoordinatorState( + joints=JointStateSnapshot(joint_positions={"joint1": 0.0}), t_now=t_now, dt=dt + ) + + +class _FakeControlIK: + nq = 1 + + def __init__(self) -> None: + self.target: object | None = None + self.dt: float | None = None + + def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult: + self.target = target + self.dt = dt + return ControlIKResult(measured.copy(), np.zeros(1)) + + +def test_cartesian_pipeline_passes_se3_target_and_bounded_dt( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + backend = _FakeControlIK() + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", + lambda *args, **kwargs: backend, + ) + task = CartesianIKTask( + "cartesian", + CartesianIKTaskConfig( + joint_names=["joint1"], + control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")), + min_dt=0.01, + max_dt=0.05, + ), + ) + assert task.on_cartesian_command( + PoseStamped(position=[0.2, -0.3, 0.4], orientation=[0, 0, 0, 2]), 1.0 + ) + + assert task.compute(_state(1.01, dt=1.0)) is not None + target = cast("pinocchio.SE3", backend.target) + assert isinstance(target, pinocchio.SE3) + assert np.allclose(target.translation, [0.2, -0.3, 0.4]) + assert np.allclose(target.rotation, np.eye(3)) + assert backend.dt == 0.05 + + +def test_cartesian_pipeline_rejects_invalid_quaternion_with_hold( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + backend = _FakeControlIK() + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", + lambda *args, **kwargs: backend, + ) + task = CartesianIKTask( + "cartesian", + CartesianIKTaskConfig( + joint_names=["joint1"], + control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")), + ), + ) + assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 0]), 1.0) + + hold = task.compute(_state(1.01)) + assert hold is not None + assert hold.positions == [0.0] + assert backend.target is None + + +def test_factory_rejects_invalid_default_pink_configuration() -> None: + config = TaskConfig( + name="cartesian", type="cartesian_ik", joint_names=["j1"], priority=10, params={} + ) + with pytest.raises(ValueError, match="control_ik"): + control_task_registry.create("cartesian_ik", config, hardware={}) + + +def test_cartesian_runtime_error_is_a_measured_state_hold( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + backend = _FakeControlIK() + + def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult: + raise IKControlRuntimeError("solver failed") + + monkeypatch.setattr(backend, "solve", fail) + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", + lambda *args, **kwargs: backend, + ) + task = CartesianIKTask( + "cartesian", + CartesianIKTaskConfig( + joint_names=["joint1"], + control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")), + ), + ) + assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0) + + hold = task.compute(_state(1.01)) + assert hold is not None + assert hold.positions == [0.0] diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index da1f3395d9..9607a468d9 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -17,19 +17,12 @@ import numpy as np import pytest -from dimos.control.coordinator import TaskConfig -from dimos.control.task import CoordinatorState, JointStateSnapshot -from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import ( - CartesianIKTask, - CartesianIKTaskConfig, -) +from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( - ControlIKResult, IKControlRuntimeError, PinkControlIK, PinkControlIKConfig, ) -from dimos.control.tasks.registry import control_task_registry from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -194,16 +187,16 @@ def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( ) -> None: model_path = _write_urdf(tmp_path) backend = PinkControlIK( - PinkControlIKConfig(robot_model=_robot(model_path)), + PinkControlIKConfig(robot_model=_robot(model_path), qpsolver_options={"eps": 1e-6}), ) measured = np.array([0.3, 0.1]) target = backend.forward_kinematics(measured) - calls: list[tuple[object, list[object], float]] = [] + calls: list[tuple[object, list[object], float, dict[str, object]]] = [] def solve( configuration: object, tasks: list[object], dt: float, **kwargs: object ) -> np.ndarray: - calls.append((configuration, tasks, dt)) + calls.append((configuration, tasks, dt, kwargs)) return np.zeros(backend._model.nv) monkeypatch.setattr( @@ -214,12 +207,37 @@ def solve( assert np.array_equal(result.positions, measured) assert len(calls) == 1 assert len(calls[0][1]) == 2 - assert calls[0][1] is backend._tasks - assert np.array_equal(calls[0][1][1].target_q, backend._full_q(measured)) + assert calls[0][1] == [backend._frame_task, backend._posture_task] + assert backend._posture_task is not None + assert np.array_equal(backend._posture_task.target_q, backend._full_q(measured)) assert calls[0][2] == 0.01 + assert calls[0][3] == { + "solver": "proxqp", + "damping": 1e-4, + "limits": backend._limits, + "eps": 1e-6, + } + + +def test_pink_solver_dependency_failure_is_translated_to_runtime_error( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + backend = PinkControlIK(PinkControlIKConfig(robot_model=_robot(_write_urdf(tmp_path)))) + + def solve( + configuration: object, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + raise RuntimeError("solver dependency failed") + + monkeypatch.setattr( + "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve + ) + measured = np.array([0.3, 0.1]) + with pytest.raises(IKControlRuntimeError, match="solver dependency failed"): + backend.solve(backend.forward_kinematics(measured), measured, 0.01) -def test_pink_backend_clamps_dt_from_backend_configuration( +def test_pink_receives_pre_bounded_dt_unchanged( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: model_path = _write_urdf(tmp_path) @@ -238,9 +256,9 @@ def solve( "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve ) measured = np.array([0.3, 0.1]) - backend.solve(backend.forward_kinematics(measured), measured, 1.0) + backend.solve(backend.forward_kinematics(measured), measured, 0.05) - assert calls == [1.0] + assert calls == [0.05] def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None: @@ -358,83 +376,10 @@ def solve( backend.solve(backend.forward_kinematics(measured), measured, 0.01) -def test_cartesian_pipeline_bounds_dt_and_holds_on_expected_error( - monkeypatch: pytest.MonkeyPatch, -) -> None: - backend = _FakeControlIK() - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", - lambda *args, **kwargs: backend, - ) - task = CartesianIKTask( - "cartesian", - CartesianIKTaskConfig( - joint_names=["j1", "j2"], - control_ik=PinkControlIKConfig( - robot_model=_robot(Path("unused.urdf")).model_copy( - update={"joint_names": ["j1", "j2"]} - ) - ), - timeout=0.2, - ), - ) - pose = PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) - assert task.on_cartesian_command(pose, 1.0) - assert task.compute(_cartesian_state(1.01, dt=1.0)) is not None - assert backend.dt_calls == [task._config.max_dt] - - invalid_dt_hold = task.compute(_cartesian_state(1.02, dt=0.0)) - assert invalid_dt_hold is not None - assert invalid_dt_hold.positions == [0.0, 0.0] - - backend.raise_runtime = True - assert task.on_cartesian_command(pose, 2.0) - hold = task.compute(_cartesian_state(2.01)) - assert hold is not None - assert hold.positions == [0.0, 0.0] - assert hold.mode.value == "servo_position" - - -def test_factory_rejects_invalid_default_pink_configuration() -> None: - config = TaskConfig( - name="cartesian", - type="cartesian_ik", - joint_names=["j1", "j2"], - priority=10, - params={}, - ) - - with pytest.raises(ValueError, match="control_ik"): - control_task_registry.create("cartesian_ik", config, hardware={}) - - -@pytest.mark.parametrize("legacy_field", ["backend", "ee_joint_id"]) +@pytest.mark.parametrize("legacy_field", ["backend", "ee_joint_id", "self_collision_enabled"]) def test_pink_rejects_legacy_configuration_fields(tmp_path: Path, legacy_field: str) -> None: model_path = _write_urdf(tmp_path) with pytest.raises(ValueError, match=legacy_field): PinkControlIKConfig.model_validate( {"robot_model": _robot(model_path), legacy_field: "pinocchio"} ) - - -class _FakeControlIK: - nq = 2 - - def __init__(self) -> None: - self.result = np.array([0.1, 0.2]) - self.raise_runtime = False - self.dt_calls: list[float] = [] - - def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult: - self.dt_calls.append(dt) - if self.raise_runtime: - raise RuntimeError("synthetic control failure") - return ControlIKResult(self.result.copy(), self.result - measured) - - -def _cartesian_state(t_now: float, dt: float = 0.01) -> CoordinatorState: - return CoordinatorState( - joints=JointStateSnapshot(joint_positions={"j1": 0.0, "j2": 0.0}), - t_now=t_now, - dt=dt, - ) diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index 1ae5366fde..e6d7c2d023 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -23,6 +23,7 @@ from dimos.control.task import ControlMode, CoordinatorState, JointStateSnapshot from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( ControlIKResult, + IKControlRuntimeError, PinkControlIKConfig, ) from dimos.control.tasks.eef_twist_task.eef_twist_task import EEFTwistTask, EEFTwistTaskConfig @@ -47,8 +48,6 @@ def __init__(self) -> None: self.solve_calls: list[FakePose] = [] self.dt_calls: list[float] = [] self.solution = np.array([0.01, 0.02, 0.03], dtype=np.float64) - self.converged = True - self.final_error = 0.0 self.raise_runtime = False def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose: @@ -57,7 +56,7 @@ def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose: def solve(self, pose: FakePose, q_current: NDArray[np.float64], dt: float) -> ControlIKResult: if self.raise_runtime: - raise RuntimeError("synthetic solver failure") + raise IKControlRuntimeError("synthetic solver failure") self.solve_calls.append(pose.copy()) self.dt_calls.append(dt) return ControlIKResult(self.solution.copy(), self.solution - q_current) @@ -129,21 +128,12 @@ def test_first_nonzero_command_activates_seeds_from_fk_and_outputs_servo_positio assert fake_ik.solve_calls[0].translation[0] > 0.0 -def test_twist_task_rejects_cartesian_commands_and_holds_on_runtime_failure( - task: EEFTwistTask, fake_ik: FakeIK -) -> None: +def test_twist_task_rejects_cartesian_commands_without_activation(task: EEFTwistTask) -> None: assert not task.on_cartesian_command(object(), t_now=1.0) - assert task.on_ee_twist_command(_twist(), t_now=1.0) - fake_ik.raise_runtime = True - hold = task.compute(_state(1.01)) - assert hold is not None - assert hold.mode == ControlMode.SERVO_POSITION - assert hold.positions == [0.0, 0.0, 0.0] + assert not task.is_active() -def test_expected_runtime_twist_error_is_a_bounded_hold( - task: EEFTwistTask, fake_ik: FakeIK -) -> None: +def test_ik_runtime_error_is_a_bounded_hold(task: EEFTwistTask, fake_ik: FakeIK) -> None: assert task.on_ee_twist_command(_twist(), t_now=1.0) fake_ik.raise_runtime = True hold = task.compute(_state(1.01)) @@ -167,12 +157,9 @@ def test_integration_uses_current_fk_and_coordinator_dt( assert fake_ik.solve_calls[1].translation[0] > fake_ik.solve_calls[0].translation[0] -def test_non_converged_ik_solution_is_accepted_when_joint_delta_is_safe( +def test_control_ik_result_positions_are_used_when_shape_is_valid( task: EEFTwistTask, fake_ik: FakeIK ) -> None: - fake_ik.converged = False - fake_ik.final_error = 1.0 - assert task.on_ee_twist_command(_twist(), t_now=1.0) output = task.compute(_state(1.01)) diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 100d93090e..77d6e7bdc4 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -14,27 +14,33 @@ from typing import cast +import pinocchio import pytest from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.core.coordination.blueprints import Blueprint from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.manipulation.visualization.config import NoManipulationVisualizationConfig from dimos.robot.manipulators.a1z.blueprints.teleop import keyboard_teleop_a1z +from dimos.robot.manipulators.a1z.config import make_a1z_model_config from dimos.robot.manipulators.a750.blueprints.teleop import keyboard_teleop_a750 +from dimos.robot.manipulators.a750.config import make_a750_model_config from dimos.robot.manipulators.common.blueprints import eef_twist_task, planner from dimos.robot.manipulators.common.topics import EEF_TWIST_TASK_NAME from dimos.robot.manipulators.openarm.blueprints.teleop import ( keyboard_teleop_openarm, keyboard_teleop_openarm_mock, ) +from dimos.robot.manipulators.openarm.config import openarm_model_config from dimos.robot.manipulators.piper.blueprints.teleop import ( coordinator_cartesian_ik_mock, coordinator_cartesian_ik_piper, keyboard_teleop_piper, ) -from dimos.robot.manipulators.piper.config import PIPER_MODEL_PATH +from dimos.robot.manipulators.piper.config import PIPER_MODEL_PATH, make_piper_model_config from dimos.robot.manipulators.xarm.blueprints.basic import ( dual_xarm6_planner, xarm6_planner_only, @@ -165,12 +171,33 @@ def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: ) control_ik = task.params["control_ik"] assert control_ik["robot_model"].model_path == PIPER_MODEL_PATH - assert control_ik["robot_model"].model_path == PIPER_MODEL_PATH assert control_ik["robot_model"].end_effector_link == "gripper_base" assert "ee_joint_id" not in task.params - assert "self_collision_enabled" not in control_ik reconstructed = PinkControlIKConfig.model_validate(control_ik) assert reconstructed.robot_model is control_ik["robot_model"] assert reconstructed.robot_model.model_path == PIPER_MODEL_PATH assert reconstructed.robot_model.end_effector_link == "gripper_base" + + +@pytest.mark.parametrize( + "robot_model", + [ + pytest.param(make_xarm6_model_config(add_gripper=False), id="xarm6"), + pytest.param(make_xarm7_model_config(add_gripper=False), id="xarm7"), + pytest.param(make_piper_model_config(), id="piper"), + pytest.param(openarm_model_config("left"), id="openarm"), + pytest.param(make_a750_model_config(), id="a750"), + pytest.param(make_a1z_model_config(has_gripper=True), id="a1z"), + ], +) +def test_shipped_model_family_has_named_eef_frame(robot_model: RobotModelConfig) -> None: + assert robot_model.model_path.is_file() + prepared = prepare_urdf_for_drake( + robot_model.model_path, + package_paths=robot_model.package_paths, + xacro_args=robot_model.xacro_args, + convert_meshes=False, + ) + model = pinocchio.buildModelFromUrdf(str(prepared)) + assert model.existFrame(robot_model.end_effector_link) From 0440f6f28f820892e9afa34bd03f5936036e80ad Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 16:41:41 -0700 Subject: [PATCH 11/19] fix: lazy load Pink control dependency --- .../cartesian_ik_task/pink_control_ik.py | 55 ++++++++++++++++--- .../test_cartesian_ik_task.py | 38 +++++++++++++ 2 files changed, 86 insertions(+), 7 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index 05abebd191..13dd10d980 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -16,22 +16,57 @@ from __future__ import annotations +from collections.abc import Callable from dataclasses import dataclass from pathlib import Path +from types import ModuleType +from typing import cast import numpy as np from numpy.typing import NDArray -import pink -from pink.limits import ConfigurationLimit, VelocityLimit import pinocchio from pydantic import Field, FiniteFloat, field_validator +pink: ModuleType | None = None +_configuration_limit: Callable[[object], object] | None = None +_velocity_limit: Callable[[object], object] | None = None + +try: + import pink as _pink_module + from pink.limits import ( + ConfigurationLimit as _ConfigurationLimit, + VelocityLimit as _VelocityLimit, + ) +except ModuleNotFoundError as exc: + if exc.name != "pink": + raise +else: + pink = cast("ModuleType", _pink_module) + _configuration_limit = cast("Callable[[object], object]", _ConfigurationLimit) + _velocity_limit = cast("Callable[[object], object]", _VelocityLimit) + from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.protocol.service.spec import BaseConfig # Pink's integration/QP boundary tolerance is small but larger than machine epsilon. _POSITION_LIMIT_EPSILON_RAD = 1e-5 +_PINK_INSTALL_ERROR = ( + "Pink control tasks require the optional 'pink' dependency. " + "Install it with `uv sync --extra manipulation`." +) + + +def _require_pink() -> ModuleType: + if pink is None: + raise ModuleNotFoundError(_PINK_INSTALL_ERROR, name="pink") from None + return pink + + +def _require_pink_limits() -> tuple[Callable[[object], object], Callable[[object], object]]: + if _configuration_limit is None or _velocity_limit is None: + raise ModuleNotFoundError(_PINK_INSTALL_ERROR, name="pink") from None + return _configuration_limit, _velocity_limit class PinkControlIKConfig(BaseConfig): @@ -73,6 +108,8 @@ def __init__( self, config: PinkControlIKConfig, ) -> None: + pink_module = _require_pink() + _require_pink_limits() self._config = config robot = config.robot_model self._joint_names = robot.get_coordinator_joint_names() @@ -114,12 +151,12 @@ def __init__( self._ee_frame_id = self._validate_frame(robot.end_effector_link) self._apply_limits(robot) self._reference_q = self._build_reference_q(use_config_reference=False) - self._configuration = pink.Configuration( + self._configuration = pink_module.Configuration( # type: ignore[attr-defined] self._model, self._data, self._reference_q.copy(), ) - self._frame_task = pink.tasks.FrameTask( + self._frame_task = pink_module.tasks.FrameTask( # type: ignore[attr-defined] robot.end_effector_link, position_cost=config.position_cost, orientation_cost=config.orientation_cost, @@ -127,7 +164,9 @@ def __init__( gain=config.task_gain, ) self._posture_task = ( - pink.tasks.PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None + pink_module.tasks.PostureTask(cost=config.posture_cost) # type: ignore[attr-defined] + if config.posture_cost > 0.0 + else None ) self._tasks: list[object] = [self._frame_task] if self._posture_task is not None: @@ -150,6 +189,7 @@ def solve( measured: NDArray[np.float64], dt: float, ) -> ControlIKResult: + pink_module = _require_pink() measured = np.asarray(measured, dtype=np.float64).reshape(-1) if measured.size != len(self._joint_names) or not np.all(np.isfinite(measured)): raise ValueError("measured joint state is invalid") @@ -165,7 +205,7 @@ def solve( frame_task.set_target(target) if self._posture_task is not None: self._posture_task.set_target(configuration.q.copy()) - velocity = pink.solve_ik( + velocity = pink_module.solve_ik( # type: ignore[attr-defined] configuration, self._tasks, dt, @@ -324,6 +364,7 @@ def _validate_frame(self, frame_name: str) -> int: return frame_id def _apply_limits(self, robot: RobotModelConfig) -> None: + configuration_limit, velocity_limit = _require_pink_limits() if robot.joint_limits_lower is not None or robot.joint_limits_upper is not None: if robot.joint_limits_lower is None or robot.joint_limits_upper is None: raise ValueError("both configured joint limit bounds are required") @@ -358,4 +399,4 @@ def _apply_limits(self, robot: RobotModelConfig) -> None: self._model.velocityLimit[index] = min( self._model.velocityLimit[index], self._config.max_velocity ) - self._limits = [ConfigurationLimit(self._model), VelocityLimit(self._model)] + self._limits = [configuration_limit(self._model), velocity_limit(self._model)] diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py index a30e02a8ad..ac37c756fe 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py @@ -13,6 +13,8 @@ # limitations under the License. from pathlib import Path +import subprocess +import sys from typing import cast import numpy as np @@ -30,6 +32,7 @@ IKControlRuntimeError, PinkControlIKConfig, ) +from dimos.control.tasks.eef_twist_task.eef_twist_task import create_task as _eef_create_task from dimos.control.tasks.registry import control_task_registry from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -125,6 +128,41 @@ def test_factory_rejects_invalid_default_pink_configuration() -> None: control_task_registry.create("cartesian_ik", config, hardware={}) +def test_cartesian_and_eef_modules_import_without_pink() -> None: + script = """ +import sys + +class BlockPink: + def find_spec(self, fullname, path=None, target=None): + if fullname == "pink": + raise ModuleNotFoundError("No module named 'pink'", name="pink") + return None + +sys.meta_path.insert(0, BlockPink()) +import dimos.control.tasks.cartesian_ik_task.cartesian_ik_task +import dimos.control.tasks.eef_twist_task.eef_twist_task +""" + subprocess.run([sys.executable, "-c", script], check=True, capture_output=True, text=True) + + +def test_pink_factories_fail_actionably_when_pink_is_absent( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + import dimos.control.tasks.cartesian_ik_task.pink_control_ik as pink_control_ik + + robot = _robot(tmp_path / "unused.urdf") + params = {"control_ik": {"robot_model": robot}} + monkeypatch.setattr(pink_control_ik, "pink", None) + + for task_type in ("cartesian_ik", "eef_twist"): + config = TaskConfig(name=task_type, type=task_type, joint_names=["joint1"], params=params) + with pytest.raises(ModuleNotFoundError, match="uv sync --extra manipulation"): + if task_type == "cartesian_ik": + control_task_registry.create("cartesian_ik", config, hardware={}) + else: + _eef_create_task(config, {}) + + def test_cartesian_runtime_error_is_a_measured_state_hold( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: From 15c1d4988216d1b358b364c6bfa54a6b2d3ea440 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 18 Jul 2026 17:38:12 -0700 Subject: [PATCH 12/19] test: avoid resolving LFS paths in blueprint checks --- dimos/robot/manipulators/test_blueprints.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 77d6e7bdc4..580e859bb3 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -74,6 +74,10 @@ def _coordinator_tasks(blueprint: Blueprint) -> list[TaskConfig]: return cast("list[TaskConfig]", _module_kwargs(blueprint, ControlCoordinator)["tasks"]) +def _declared_lfs_filename(path: object) -> object: + return object.__getattribute__(path, "_lfs_filename") + + def test_planner_helper_defaults_to_no_visualization() -> None: blueprint = planner(robots=[make_xarm7_model_config(name="arm", add_gripper=True)]) @@ -154,7 +158,9 @@ def test_shipped_eef_twist_blueprints_use_pink_with_named_models( assert control_ik["robot_model"].end_effector_link assert "ee_joint_id" not in task.params - assert not str(control_ik["robot_model"].model_path).endswith((".xml", ".mjcf")) + declared_filename = _declared_lfs_filename(control_ik["robot_model"].model_path) + assert isinstance(declared_filename, str) + assert not declared_filename.endswith((".xml", ".mjcf")) def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: @@ -170,13 +176,17 @@ def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: if task.type in ("eef_twist", "cartesian_ik") ) control_ik = task.params["control_ik"] - assert control_ik["robot_model"].model_path == PIPER_MODEL_PATH + assert _declared_lfs_filename(control_ik["robot_model"].model_path) == ( + _declared_lfs_filename(PIPER_MODEL_PATH) + ) assert control_ik["robot_model"].end_effector_link == "gripper_base" assert "ee_joint_id" not in task.params reconstructed = PinkControlIKConfig.model_validate(control_ik) assert reconstructed.robot_model is control_ik["robot_model"] - assert reconstructed.robot_model.model_path == PIPER_MODEL_PATH + assert _declared_lfs_filename(reconstructed.robot_model.model_path) == ( + _declared_lfs_filename(PIPER_MODEL_PATH) + ) assert reconstructed.robot_model.end_effector_link == "gripper_base" From c9dc5fb4ddbe4d8492409e82860c3e4359a8921d Mon Sep 17 00:00:00 2001 From: cc Date: Sun, 19 Jul 2026 22:05:05 -0700 Subject: [PATCH 13/19] docs: simplify control IK guidance --- .../manipulation/adding_a_custom_arm.md | 43 ++++------------- docs/capabilities/manipulation/index.md | 48 ++++++------------- 2 files changed, 24 insertions(+), 67 deletions(-) diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index cdaa460d4e..93e9eb6c3f 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -571,20 +571,12 @@ yourarm_planner = manipulation_module( ### 4d. Configure Cartesian and EEF-twist control IK -Pink is the only backend for Cartesian and EEF-twist control. Pink control and -manipulation planning are separate: planning uses `WorldSpec` and its selected -planning backend, while control performs one local differential-IK step and -does not use `WorldSpec` as a control input. +Cartesian and EEF-twist tasks use the direct URDF or Xacro in +`RobotModelConfig`. Set `package_paths` and `xacro_args` when needed, name the +end-effector link, and map coordinator joints to model joints. The task validates +the prepared model, frame, and joint mapping at startup. -Use the same `RobotModelConfig` for the control model and planning robot -metadata. Its `model_path` points to the direct URDF or Xacro, `package_paths` -and `xacro_args` describe model preparation, `end_effector_link` names the EEF -frame, and `joint_name_mapping` maps coordinator joints to URDF joints. Pink -validates the prepared model, named frame, and exact ordered joint mapping at -startup. - -The common helper passes that typed configuration to Pink and derives the model -path and coordinator joint order from it: +Pass the same model configuration to the common helpers: ```python skip from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, eef_twist_task @@ -599,26 +591,11 @@ twist_task = eef_twist_task( ) ``` -Pink control tasks use the named `RobotModelConfig.end_effector_link`; they do -not accept a numeric `ee_joint_id` or a legacy backend selector. - -At every coordinator tick, Pink re-anchors to measured joints, derives the EEF -target from measured FK for twist input, clamps `dt`, updates one `FrameTask`, -integrates one step, and applies position and velocity limits. The shared task -pipeline validates finite bounded output and uses a safe hold for expected -runtime solve errors. Invalid models, frames, mappings, or model-preparation -inputs fail startup; Pink is never silently replaced by Pinocchio. - -Piper follows the same path as other arms. Its Cartesian and EEF-twist tasks use -the matching existing Xacro/URDF model, `make_piper_model_config()`, and the -named `gripper_base` frame. They do not use the previous MJCF model or numeric -EEF ID on the Pink path. - -Before hardware, validate the configuration in simulation or replay at the -coordinator rate. Benchmark end-to-end latency, exercise Cartesian and twist -commands, verify startup diagnostics and runtime safe holds, and confirm -emergency-stop readiness. Hardware validation is future work and must be -supervised and low speed; this guide does not claim that it has occurred. +Each tick starts from measured joints and applies model position and velocity +limits. Twist targets are derived from measured forward kinematics. Invalid +models or mappings fail at startup; invalid runtime output holds the measured +position. Validate Cartesian and twist behavior in simulation or replay before +hardware use. ## Step 5: Register Blueprints diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index bc6343aa77..b3193d987b 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -120,29 +120,17 @@ request. For example, `planner_name=roboplan` requires ### Cartesian control IK -Cartesian and keyboard EEF-twist tasks use generic Pink control IK by default. -Pink is the only control IK backend; a failed Pink setup does not silently select -another solver. - -Pink control uses the direct URDF/Xacro model from `RobotModelConfig`. Package -paths and Xacro arguments are prepared before startup. The configuration names -the end-effector frame and maps coordinator joints to model joints; missing -frames, mismatched mappings, or an invalid prepared model fail initialization. - -Each control tick starts from measured joints, clamps `dt`, updates one Pink -`FrameTask`, solves and integrates one local differential-IK step, and applies -position and velocity limits. A configurable `posture_cost` (default `1e-3`) -regularizes only the null space toward the current measured configuration and -can be disabled with zero. Non-finite or unsafe output is rejected. Expected -runtime solve errors produce a bounded safe hold instead of an invalid command. - -The control backend is separate from manipulation planning. It does not use -`WorldSpec` to control the robot and makes no planning-world or dynamic-obstacle -avoidance claim. `WorldSpec` and its Pink/Drake backends remain responsible for -planning behavior. - -For a custom robot, the helper API derives the Pink model and joint mapping from -the typed model configuration: +Cartesian and keyboard EEF-twist tasks use the direct URDF/Xacro model from +`RobotModelConfig`. The configuration supplies package paths, Xacro arguments, +the named end-effector frame, and coordinator-to-model joint mapping. Invalid +models, frames, or mappings fail at startup. + +Each control tick starts from measured joints, applies model position and +velocity limits, and holds the measured position when a solve cannot produce a +safe command. This local control path is separate from manipulation planning and +does not use `WorldSpec` or provide world-obstacle avoidance. + +For a custom robot, pass the typed model configuration to the helper: ```python skip from dimos.robot.manipulators.common.blueprints import cartesian_ik_task @@ -153,16 +141,8 @@ task = cartesian_ik_task( ) ``` -Piper's Cartesian and EEF-twist blueprints use the matching Xacro/URDF -`PIPER_MODEL_PATH`, `make_piper_model_config()`, and named `gripper_base` frame. -Piper's Pink configuration does not use its previous MJCF model or numeric EEF -ID. - -Validate a rollout in simulation or replay first: exercise Cartesian and twist -commands at the coordinator rate, benchmark end-to-end control latency, verify -model/frame diagnostics and safe holds, and confirm emergency-stop readiness. -Hardware validation remains future work and must be supervised and low speed; -no hardware validation is claimed here. +Validate Cartesian and twist behavior in simulation or replay before hardware +use. Install the manipulation dependencies: @@ -260,7 +240,7 @@ KeyboardTeleopModule ──→ ControlCoordinator ──→ ManipulationModule (pygame UI) (100Hz tick loop) (WorldSpec backend) │ │ │ TwistStamped EEFTwistTask RRT planner - spatial EEF twist (Pink control IK) JacobianIK + spatial EEF twist (control IK) JacobianIK │ DrakeWorld JointState ────────────→ (visualization) ``` From 4bb0cb840c486bcf7d69f7454e4291abe1eff055 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 20 Jul 2026 11:25:35 -0700 Subject: [PATCH 14/19] test: isolate optional control IK coverage --- dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py | 2 ++ dimos/robot/manipulators/test_blueprints.py | 1 + 2 files changed, 3 insertions(+) diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 9607a468d9..8ad3ade7c1 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -17,6 +17,8 @@ import numpy as np import pytest +pytest.importorskip("pink") + from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( IKControlRuntimeError, diff --git a/dimos/robot/manipulators/test_blueprints.py b/dimos/robot/manipulators/test_blueprints.py index 580e859bb3..a18b60cfda 100644 --- a/dimos/robot/manipulators/test_blueprints.py +++ b/dimos/robot/manipulators/test_blueprints.py @@ -201,6 +201,7 @@ def test_piper_pink_task_uses_xacro_and_gripper_base() -> None: pytest.param(make_a1z_model_config(has_gripper=True), id="a1z"), ], ) +@pytest.mark.self_hosted def test_shipped_model_family_has_named_eef_frame(robot_model: RobotModelConfig) -> None: assert robot_model.model_path.is_file() prepared = prepare_urdf_for_drake( From c6c86c6b8e0bf4af31b197950131391065f6665e Mon Sep 17 00:00:00 2001 From: cc Date: Tue, 28 Jul 2026 14:47:31 -0700 Subject: [PATCH 15/19] test: migrate Cartesian IK fixture to planning groups --- .../tasks/cartesian_ik_task/test_cartesian_ik_task.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py index ac37c756fe..9981ed1e13 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py @@ -34,6 +34,7 @@ ) from dimos.control.tasks.eef_twist_task.eef_twist_task import create_task as _eef_create_task from dimos.control.tasks.registry import control_task_registry +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -44,7 +45,14 @@ def _robot(path: Path) -> RobotModelConfig: model_path=path, base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), joint_names=["joint1"], - end_effector_link="tool", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1",), + base_link="base", + tip_link="tool", + ) + ], home_joints=[0.0], ) From ea887c59d819d563225fda3a18933e07d5209c98 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 3 Aug 2026 23:31:06 -0700 Subject: [PATCH 16/19] refactor(control): make Pink IK a core dependency --- .../cartesian_ik_task/cartesian_ik_task.py | 3 +- .../cartesian_ik_task/pink_control_ik.py | 64 +++++------------ .../test_cartesian_ik_task.py | 69 ++++++++----------- .../cartesian_ik_task/test_pink_control_ik.py | 36 ++++------ .../eef_twist_task/test_eef_twist_task.py | 2 +- pyproject.toml | 4 +- uv.lock | 17 +++-- 7 files changed, 76 insertions(+), 119 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index 850b70f8d6..fbaa8e7360 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -40,6 +40,7 @@ from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( PinkControlIK, PinkControlIKConfig, + create_pink_control_ik, ) from dimos.manipulation.planning.kinematics.pinocchio_ik import ( check_joint_delta, @@ -144,7 +145,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: ) # Create IK solver from model - self._ik = PinkControlIK(config.control_ik) + self._ik: PinkControlIK = create_pink_control_ik(config.control_ik) # Validate DOF matches joint names if self._ik.nq != self._num_joints: diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index 13dd10d980..cc0e9c76ee 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -16,34 +16,25 @@ from __future__ import annotations -from collections.abc import Callable from dataclasses import dataclass from pathlib import Path -from types import ModuleType -from typing import cast import numpy as np from numpy.typing import NDArray import pinocchio from pydantic import Field, FiniteFloat, field_validator -pink: ModuleType | None = None -_configuration_limit: Callable[[object], object] | None = None -_velocity_limit: Callable[[object], object] | None = None +_PINK_INSTALL_ERROR = "Pink control tasks require the 'pink' dependency. Install it with `uv sync`." try: - import pink as _pink_module - from pink.limits import ( - ConfigurationLimit as _ConfigurationLimit, - VelocityLimit as _VelocityLimit, - ) + from pink import Configuration, solve_ik + from pink.limits import ConfigurationLimit, VelocityLimit + from pink.tasks import FrameTask, PostureTask except ModuleNotFoundError as exc: - if exc.name != "pink": - raise -else: - pink = cast("ModuleType", _pink_module) - _configuration_limit = cast("Callable[[object], object]", _ConfigurationLimit) - _velocity_limit = cast("Callable[[object], object]", _VelocityLimit) + raise ModuleNotFoundError( + f"{_PINK_INSTALL_ERROR} Missing module: {exc.name}", + name=exc.name, + ) from exc from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake @@ -51,22 +42,6 @@ # Pink's integration/QP boundary tolerance is small but larger than machine epsilon. _POSITION_LIMIT_EPSILON_RAD = 1e-5 -_PINK_INSTALL_ERROR = ( - "Pink control tasks require the optional 'pink' dependency. " - "Install it with `uv sync --extra manipulation`." -) - - -def _require_pink() -> ModuleType: - if pink is None: - raise ModuleNotFoundError(_PINK_INSTALL_ERROR, name="pink") from None - return pink - - -def _require_pink_limits() -> tuple[Callable[[object], object], Callable[[object], object]]: - if _configuration_limit is None or _velocity_limit is None: - raise ModuleNotFoundError(_PINK_INSTALL_ERROR, name="pink") from None - return _configuration_limit, _velocity_limit class PinkControlIKConfig(BaseConfig): @@ -108,8 +83,6 @@ def __init__( self, config: PinkControlIKConfig, ) -> None: - pink_module = _require_pink() - _require_pink_limits() self._config = config robot = config.robot_model self._joint_names = robot.get_coordinator_joint_names() @@ -151,12 +124,12 @@ def __init__( self._ee_frame_id = self._validate_frame(robot.end_effector_link) self._apply_limits(robot) self._reference_q = self._build_reference_q(use_config_reference=False) - self._configuration = pink_module.Configuration( # type: ignore[attr-defined] + self._configuration = Configuration( self._model, self._data, self._reference_q.copy(), ) - self._frame_task = pink_module.tasks.FrameTask( # type: ignore[attr-defined] + self._frame_task = FrameTask( robot.end_effector_link, position_cost=config.position_cost, orientation_cost=config.orientation_cost, @@ -164,9 +137,7 @@ def __init__( gain=config.task_gain, ) self._posture_task = ( - pink_module.tasks.PostureTask(cost=config.posture_cost) # type: ignore[attr-defined] - if config.posture_cost > 0.0 - else None + PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None ) self._tasks: list[object] = [self._frame_task] if self._posture_task is not None: @@ -189,7 +160,6 @@ def solve( measured: NDArray[np.float64], dt: float, ) -> ControlIKResult: - pink_module = _require_pink() measured = np.asarray(measured, dtype=np.float64).reshape(-1) if measured.size != len(self._joint_names) or not np.all(np.isfinite(measured)): raise ValueError("measured joint state is invalid") @@ -198,14 +168,12 @@ def solve( configuration = self._configuration frame_task = self._frame_task - if configuration is None or frame_task is None: - raise IKControlRuntimeError("Pink control backend is unavailable") try: configuration.update(self._full_q(measured)) frame_task.set_target(target) if self._posture_task is not None: self._posture_task.set_target(configuration.q.copy()) - velocity = pink_module.solve_ik( # type: ignore[attr-defined] + velocity = solve_ik( configuration, self._tasks, dt, @@ -364,7 +332,6 @@ def _validate_frame(self, frame_name: str) -> int: return frame_id def _apply_limits(self, robot: RobotModelConfig) -> None: - configuration_limit, velocity_limit = _require_pink_limits() if robot.joint_limits_lower is not None or robot.joint_limits_upper is not None: if robot.joint_limits_lower is None or robot.joint_limits_upper is None: raise ValueError("both configured joint limit bounds are required") @@ -399,4 +366,9 @@ def _apply_limits(self, robot: RobotModelConfig) -> None: self._model.velocityLimit[index] = min( self._model.velocityLimit[index], self._config.max_velocity ) - self._limits = [configuration_limit(self._model), velocity_limit(self._model)] + self._limits = [ConfigurationLimit(self._model), VelocityLimit(self._model)] + + +def create_pink_control_ik(config: PinkControlIKConfig) -> PinkControlIK: + """Construct the default Cartesian control IK backend.""" + return PinkControlIK(config) diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py index 9981ed1e13..de7adada09 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py @@ -32,7 +32,6 @@ IKControlRuntimeError, PinkControlIKConfig, ) -from dimos.control.tasks.eef_twist_task.eef_twist_task import create_task as _eef_create_task from dimos.control.tasks.registry import control_task_registry from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -76,13 +75,11 @@ def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKRes return ControlIKResult(measured.copy(), np.zeros(1)) -def test_cartesian_pipeline_passes_se3_target_and_bounded_dt( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch -) -> None: +def test_cartesian_pipeline_passes_se3_target_and_bounded_dt(tmp_path: Path, mocker) -> None: backend = _FakeControlIK() - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", - lambda *args, **kwargs: backend, + mocker.patch( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", + return_value=backend, ) task = CartesianIKTask( "cartesian", @@ -105,13 +102,11 @@ def test_cartesian_pipeline_passes_se3_target_and_bounded_dt( assert backend.dt == 0.05 -def test_cartesian_pipeline_rejects_invalid_quaternion_with_hold( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch -) -> None: +def test_cartesian_pipeline_rejects_invalid_quaternion_with_hold(tmp_path: Path, mocker) -> None: backend = _FakeControlIK() - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", - lambda *args, **kwargs: backend, + mocker.patch( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", + return_value=backend, ) task = CartesianIKTask( "cartesian", @@ -136,8 +131,15 @@ def test_factory_rejects_invalid_default_pink_configuration() -> None: control_task_registry.create("cartesian_ik", config, hardware={}) -def test_cartesian_and_eef_modules_import_without_pink() -> None: - script = """ +@pytest.mark.parametrize( + "module_name", + [ + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task", + "dimos.control.tasks.eef_twist_task.eef_twist_task", + ], +) +def test_control_task_import_fails_actionably_without_pink(module_name: str) -> None: + script = f""" import sys class BlockPink: @@ -147,32 +149,21 @@ def find_spec(self, fullname, path=None, target=None): return None sys.meta_path.insert(0, BlockPink()) -import dimos.control.tasks.cartesian_ik_task.cartesian_ik_task -import dimos.control.tasks.eef_twist_task.eef_twist_task +import {module_name} """ - subprocess.run([sys.executable, "-c", script], check=True, capture_output=True, text=True) - - -def test_pink_factories_fail_actionably_when_pink_is_absent( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch -) -> None: - import dimos.control.tasks.cartesian_ik_task.pink_control_ik as pink_control_ik - - robot = _robot(tmp_path / "unused.urdf") - params = {"control_ik": {"robot_model": robot}} - monkeypatch.setattr(pink_control_ik, "pink", None) + result = subprocess.run( + [sys.executable, "-c", script], + check=False, + capture_output=True, + text=True, + ) - for task_type in ("cartesian_ik", "eef_twist"): - config = TaskConfig(name=task_type, type=task_type, joint_names=["joint1"], params=params) - with pytest.raises(ModuleNotFoundError, match="uv sync --extra manipulation"): - if task_type == "cartesian_ik": - control_task_registry.create("cartesian_ik", config, hardware={}) - else: - _eef_create_task(config, {}) + assert result.returncode != 0 + assert "Install it with `uv sync`" in result.stderr def test_cartesian_runtime_error_is_a_measured_state_hold( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, mocker ) -> None: backend = _FakeControlIK() @@ -180,9 +171,9 @@ def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult: raise IKControlRuntimeError("solver failed") monkeypatch.setattr(backend, "solve", fail) - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", - lambda *args, **kwargs: backend, + mocker.patch( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", + return_value=backend, ) task = CartesianIKTask( "cartesian", diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 8ad3ade7c1..4663931c70 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -17,14 +17,13 @@ import numpy as np import pytest -pytest.importorskip("pink") - from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( IKControlRuntimeError, PinkControlIK, PinkControlIKConfig, ) +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -86,7 +85,14 @@ def _robot( model_path=path, base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), joint_names=joint_names, - end_effector_link=frame, + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(joint_names), + base_link="base", + tip_link=frame, + ) + ], home_joints=[0.4] * joint_count, joint_limits_lower=[-2.0] * joint_count, joint_limits_upper=[2.0] * joint_count, @@ -201,9 +207,7 @@ def solve( calls.append((configuration, tasks, dt, kwargs)) return np.zeros(backend._model.nv) - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve - ) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) result = backend.solve(target, measured, 0.01) assert np.array_equal(result.positions, measured) @@ -231,9 +235,7 @@ def solve( ) -> np.ndarray: raise RuntimeError("solver dependency failed") - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve - ) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) measured = np.array([0.3, 0.1]) with pytest.raises(IKControlRuntimeError, match="solver dependency failed"): backend.solve(backend.forward_kinematics(measured), measured, 0.01) @@ -254,9 +256,7 @@ def solve( calls.append(dt) return np.zeros(backend._model.nv) - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve - ) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) measured = np.array([0.3, 0.1]) backend.solve(backend.forward_kinematics(measured), measured, 0.05) @@ -274,9 +274,7 @@ def solve( calls.append(tasks) return np.zeros(backend._model.nv) - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve - ) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) measured = np.array([0.3, 0.1]) backend.solve(backend.forward_kinematics(measured), measured, 0.01) @@ -347,9 +345,7 @@ def solve( ) -> np.ndarray: return np.array([0.00013784674535, -0.2]) - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve - ) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) result = backend.solve(backend.forward_kinematics(measured), measured, 0.01) assert np.array_equal(result.positions, np.array([1.22, 0.098])) @@ -371,9 +367,7 @@ def solve( ) -> np.ndarray: return np.array([0.01, -0.2]) - monkeypatch.setattr( - "dimos.control.tasks.cartesian_ik_task.pink_control_ik.pink.solve_ik", solve - ) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) with pytest.raises(IKControlRuntimeError, match="out-of-bounds"): backend.solve(backend.forward_kinematics(measured), measured, 0.01) diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index 39620fdc5b..051b01d65d 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -68,7 +68,7 @@ def solve(self, pose: FakePose, q_current: NDArray[np.float64], dt: float) -> Co def fake_ik(mocker) -> FakeIK: ik = FakeIK() mocker.patch( - "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK", + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", return_value=ik, ) return ik diff --git a/pyproject.toml b/pyproject.toml index f847517ce7..37bc540ae5 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -108,6 +108,8 @@ dependencies = [ "numpy>=1.26.4", "scipy>=1.15.1", "pin>=3.3.0", # Pinocchio IK library + "pin-pink>=4.3.0", + "qpsolvers[proxqp]>=4.12.0", "cmeel-tinyxml2>=11,<12", # Pinocchio 4.1 requires the v11 ABI. "reactivex", "sortedcontainers==2.4.0", @@ -271,8 +273,6 @@ manipulation = [ # Planning (Drake) "drake==1.45.0; sys_platform == 'darwin' and platform_machine != 'aarch64'", "drake>=1.40.0; sys_platform != 'darwin' and platform_machine != 'aarch64'", - "pin-pink>=4.2.0", - "qpsolvers[proxqp]>=4.12.0", # Hardware SDKs "piper-sdk", diff --git a/uv.lock b/uv.lock index 799831a286..2292f5837e 100644 --- a/uv.lock +++ b/uv.lock @@ -1561,6 +1561,7 @@ dependencies = [ { name = "opencv-contrib-python" }, { name = "packaging" }, { name = "pin" }, + { name = "pin-pink" }, { name = "plotext" }, { name = "plum-dispatch" }, { name = "protobuf" }, @@ -1569,6 +1570,7 @@ dependencies = [ { name = "pydantic-settings" }, { name = "python-dotenv" }, { name = "pyturbojpeg" }, + { name = "qpsolvers", extra = ["proxqp"] }, { name = "reactivex" }, { name = "rerun-sdk" }, { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, @@ -1636,7 +1638,6 @@ all = [ { name = "open-clip-torch" }, { name = "openai" }, { name = "pillow" }, - { name = "pin-pink" }, { name = "piper-sdk" }, { name = "playground" }, { name = "portal" }, @@ -1646,7 +1647,6 @@ all = [ { name = "pyrealsense2-extended", marker = "sys_platform != 'darwin'" }, { name = "python-multipart" }, { name = "pyyaml" }, - { name = "qpsolvers", extra = ["proxqp"] }, { name = "reportlab" }, { name = "rerun-sdk" }, { name = "roboplan" }, @@ -1726,12 +1726,10 @@ manipulation = [ { name = "drake", version = "1.45.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, { name = "drake", version = "1.49.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, { name = "matplotlib" }, - { name = "pin-pink" }, { name = "piper-sdk" }, { name = "pycollada" }, { name = "pyrealsense2-extended", marker = "sys_platform != 'darwin'" }, { name = "pyyaml" }, - { name = "qpsolvers", extra = ["proxqp"] }, { name = "roboplan" }, { name = "trimesh" }, { name = "viser", extra = ["urdf"] }, @@ -2098,7 +2096,7 @@ requires-dist = [ { name = "pandas", marker = "extra == 'learning'" }, { name = "pillow", marker = "extra == 'perception'" }, { name = "pin", specifier = ">=3.3.0" }, - { name = "pin-pink", marker = "extra == 'manipulation'", specifier = ">=4.2.0" }, + { name = "pin-pink", specifier = ">=4.3.0" }, { name = "piper-sdk", marker = "extra == 'manipulation'" }, { name = "playground", marker = "extra == 'sim'", specifier = ">=0.0.5" }, { name = "plotext", specifier = "==5.3.2" }, @@ -2117,7 +2115,7 @@ requires-dist = [ { name = "python-multipart", marker = "extra == 'misc'", specifier = ">=0.0.27" }, { name = "pyturbojpeg", specifier = "==1.8.2" }, { name = "pyyaml", marker = "extra == 'manipulation'", specifier = ">=6.0" }, - { name = "qpsolvers", extras = ["proxqp"], marker = "extra == 'manipulation'", specifier = ">=4.12.0" }, + { name = "qpsolvers", extras = ["proxqp"], specifier = ">=4.12.0" }, { name = "reactivex" }, { name = "reportlab", marker = "extra == 'apriltag'", specifier = ">=4.5.0" }, { name = "rerun-sdk", specifier = "==0.32.0" }, @@ -6246,7 +6244,7 @@ wheels = [ [[package]] name = "pin-pink" -version = "4.2.0" +version = "4.3.0" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "loop-rate-limiters" }, @@ -6254,10 +6252,11 @@ dependencies = [ { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, { name = "pin" }, { name = "qpsolvers" }, + { name = "typing-extensions" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/1a/aa/52c817dea0240f41c6b2b0e0872561a63942c1cc5f26fe367f7da60fe183/pin_pink-4.2.0.tar.gz", hash = "sha256:21ffbb4624377d74036c4c4d1d9ba252a0c912cd7a2469117e768f36179c17e1", size = 284729, upload-time = "2026-04-20T09:47:38.34Z" } +sdist = { url = "https://files.pythonhosted.org/packages/7f/5b/0bfab6a426c051215753995c65f0e2859e038e49e75510b64ce60a9712c1/pin_pink-4.3.0.tar.gz", hash = "sha256:65964e4a2e125d9f5f927f37ee8b85dcf57e125b4845fe6be034fa64c6ed3379", size = 52854, upload-time = "2026-07-15T18:58:20.819Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/d9/de/9c1f8e4fe703ac917fa0bb161a928704b31c5fafce90a055dcabd5674ae4/pin_pink-4.2.0-py3-none-any.whl", hash = "sha256:8c405607eb94c92540a7b28147b5d638ba1e5dbfda8ef307e3e51572e3dd4477", size = 64871, upload-time = "2026-04-20T09:47:34.899Z" }, + { url = "https://files.pythonhosted.org/packages/01/dc/863a1cbc36fcb269ebdaa0e42c4e31f1c9d28f6aa0bdbacbd923cf6b16cc/pin_pink-4.3.0-py3-none-any.whl", hash = "sha256:6a38c07e0f01a754f827166242acb2b9f0b03e726712a078d2e243fdbdbf6f6a", size = 64305, upload-time = "2026-07-15T18:58:19.173Z" }, ] [[package]] From db8b17053e41cc0aaa2c787c5e93b9a5dcf51943 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 3 Aug 2026 23:41:46 -0700 Subject: [PATCH 17/19] refactor(control): move Pink setup into factory --- .../cartesian_ik_task/pink_control_ik.py | 427 ++++++++++-------- .../cartesian_ik_task/test_pink_control_ik.py | 100 ++-- 2 files changed, 309 insertions(+), 218 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index cc0e9c76ee..de676edac1 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -76,16 +76,39 @@ class IKControlRuntimeError(RuntimeError): """A runtime solver/model failure that should produce a bounded hold.""" -class PinkControlIK: - """One-step Pink control IK for Cartesian control.""" +@dataclass(frozen=True) +class _CoordinateMapping: + joint_names: tuple[str, ...] + q_indices: tuple[int, ...] + v_indices: tuple[int, ...] + q_widths: tuple[int, ...] + joint_ids: frozenset[int] - def __init__( - self, - config: PinkControlIKConfig, - ) -> None: + +@dataclass(frozen=True) +class _PinkRuntime: + config: PinkControlIKConfig + model: pinocchio.Model + data: pinocchio.Data + mapping: _CoordinateMapping + ee_frame_id: int + reference_q: NDArray[np.float64] + configuration: Configuration + frame_task: FrameTask + posture_task: PostureTask | None + tasks: list[object] + limits: list[object] + + +class _PinkControlIKBuilder: + """Assemble model and Pink state before creating the runtime solver.""" + + def __init__(self, config: PinkControlIKConfig) -> None: self._config = config + + def build(self) -> _PinkRuntime: + config = self._config robot = config.robot_model - self._joint_names = robot.get_coordinator_joint_names() prepared_path = Path( prepare_urdf_for_drake( robot.model_path, @@ -97,62 +120,215 @@ def __init__( if not prepared_path.exists(): raise FileNotFoundError(f"prepared Pink control URDF not found: {prepared_path}") - self._model = pinocchio.buildModelFromUrdf(str(prepared_path)) - self._data = self._model.createData() - self._q_indices, self._v_indices = self._build_mapping(robot) - self._ee_frame_id = self._validate_frame(robot.end_effector_link) - self._apply_limits(robot) - full_reference_q = self._build_reference_q() - controlled_joint_ids = self._controlled_joint_ids + model = pinocchio.buildModelFromUrdf(str(prepared_path)) + mapping = self._build_mapping(model, robot) + ee_frame_id = self._validate_frame(model, robot.end_effector_link) + limits = self._apply_limits(model, mapping, robot) + full_reference_q = self._build_reference_q(model, config.reference_q) locked_joint_ids = [ joint_id - for joint_id in range(1, len(self._model.joints)) - if joint_id not in controlled_joint_ids + for joint_id in range(1, len(model.joints)) + if joint_id not in mapping.joint_ids ] if locked_joint_ids: - if self._config.reference_q is None and self._uncontrolled_ee_chain( - self._ee_frame_id, controlled_joint_ids + if config.reference_q is None and self._uncontrolled_ee_chain( + model, ee_frame_id, mapping.joint_ids ): raise ValueError( "Pink requires reference_q for an uncontrolled joint on the end-effector chain" ) - self._model = pinocchio.buildReducedModel( - self._model, locked_joint_ids, full_reference_q - ) - self._data = self._model.createData() - self._q_indices, self._v_indices = self._build_mapping(robot) - self._ee_frame_id = self._validate_frame(robot.end_effector_link) - self._apply_limits(robot) - self._reference_q = self._build_reference_q(use_config_reference=False) - self._configuration = Configuration( - self._model, - self._data, - self._reference_q.copy(), + model = pinocchio.buildReducedModel(model, locked_joint_ids, full_reference_q) + mapping = self._build_mapping(model, robot) + ee_frame_id = self._validate_frame(model, robot.end_effector_link) + limits = self._apply_limits(model, mapping, robot) + + data = model.createData() + reference_q = self._build_reference_q(model, None) + configuration = Configuration( + model, + data, + reference_q.copy(), ) - self._frame_task = FrameTask( + frame_task = FrameTask( robot.end_effector_link, position_cost=config.position_cost, orientation_cost=config.orientation_cost, lm_damping=config.lm_damping, gain=config.task_gain, ) - self._posture_task = ( - PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None + posture_task = PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None + tasks: list[object] = [frame_task] + if posture_task is not None: + tasks.append(posture_task) + + return _PinkRuntime( + config=config, + model=model, + data=data, + mapping=mapping, + ee_frame_id=ee_frame_id, + reference_q=reference_q, + configuration=configuration, + frame_task=frame_task, + posture_task=posture_task, + tasks=tasks, + limits=limits, + ) + + @staticmethod + def _build_mapping( + model: pinocchio.Model, + robot: RobotModelConfig, + ) -> _CoordinateMapping: + joint_names = tuple(robot.get_coordinator_joint_names()) + if not joint_names or len(set(joint_names)) != len(joint_names): + raise ValueError("control task joints must be unique and non-empty") + + q_indices: list[int] = [] + v_indices: list[int] = [] + q_widths: list[int] = [] + joint_ids: set[int] = set() + for urdf_name in (robot.get_urdf_joint_name(name) for name in joint_names): + if not model.existJointName(urdf_name): + raise ValueError(f"control joint mapping references unknown joint: {urdf_name}") + joint_id = int(model.getJointId(urdf_name)) + if joint_id <= 0 or joint_id >= len(model.joints): + raise ValueError(f"invalid control joint index for {urdf_name}") + joint = model.joints[joint_id] + if int(joint.nv) != 1 or int(joint.nq) not in (1, 2): + raise ValueError(f"control joint must be one-DoF: {urdf_name}") + q_indices.append(int(joint.idx_q)) + v_indices.append(int(joint.idx_v)) + q_widths.append(int(joint.nq)) + joint_ids.add(joint_id) + + return _CoordinateMapping( + joint_names=joint_names, + q_indices=tuple(q_indices), + v_indices=tuple(v_indices), + q_widths=tuple(q_widths), + joint_ids=frozenset(joint_ids), ) - self._tasks: list[object] = [self._frame_task] - if self._posture_task is not None: - self._tasks.append(self._posture_task) + + @staticmethod + def _build_reference_q( + model: pinocchio.Model, + configured_reference_q: list[float] | None, + ) -> NDArray[np.float64]: + if configured_reference_q is not None: + q = np.asarray(configured_reference_q, dtype=np.float64).reshape(-1) + if q.size != model.nq or not np.all(np.isfinite(q)): + raise ValueError("Pink reference_q must match model nq and be finite") + else: + q = np.asarray(pinocchio.neutral(model), dtype=np.float64) + for joint_id in range(1, len(model.joints)): + joint = model.joints[joint_id] + start = int(joint.idx_q) + width = int(joint.nq) + if width == 2 and int(joint.nv) == 1: + q[start : start + 2] = (1.0, 0.0) + continue + if width != 1: + continue + lower = model.lowerPositionLimit[start] + upper = model.upperPositionLimit[start] + if np.isfinite(lower) and np.isfinite(upper): + q[start] = (lower + upper) / 2.0 + elif np.isfinite(lower): + q[start] = max(0.0, lower) + elif np.isfinite(upper): + q[start] = min(0.0, upper) + else: + q[start] = 0.0 + if not np.all(np.isfinite(q)): + raise ValueError("Pink reference configuration is not finite") + bounded = np.isfinite(model.lowerPositionLimit) & np.isfinite(model.upperPositionLimit) + if np.any(q[bounded] < model.lowerPositionLimit[bounded]) or np.any( + q[bounded] > model.upperPositionLimit[bounded] + ): + raise ValueError("Pink reference configuration violates model limits") + return q + + @staticmethod + def _uncontrolled_ee_chain( + model: pinocchio.Model, + frame_id: int, + controlled_joint_ids: frozenset[int], + ) -> bool: + joint_id = int(model.frames[frame_id].parentJoint) + while joint_id > 0: + if joint_id not in controlled_joint_ids: + return True + joint_id = int(model.parents[joint_id]) + return False + + @staticmethod + def _validate_frame(model: pinocchio.Model, frame_name: str) -> int: + if not model.existFrame(frame_name): + raise ValueError(f"unknown control end-effector frame: {frame_name}") + frame_id = int(model.getFrameId(frame_name)) + if frame_id < 0 or frame_id >= len(model.frames): + raise ValueError(f"invalid control end-effector frame: {frame_name}") + return frame_id + + def _apply_limits( + self, + model: pinocchio.Model, + mapping: _CoordinateMapping, + robot: RobotModelConfig, + ) -> list[object]: + if robot.joint_limits_lower is not None or robot.joint_limits_upper is not None: + if robot.joint_limits_lower is None or robot.joint_limits_upper is None: + raise ValueError("both configured joint limit bounds are required") + if len(robot.joint_limits_lower) != len(mapping.joint_names) or len( + robot.joint_limits_upper + ) != len(mapping.joint_names): + raise ValueError("configured joint limits do not match control joints") + for index, width, lower, upper in zip( + mapping.q_indices, + mapping.q_widths, + robot.joint_limits_lower, + robot.joint_limits_upper, + strict=True, + ): + if not np.isfinite(lower) or not np.isfinite(upper) or lower >= upper: + raise ValueError("configured joint limits must be finite and ordered") + if width == 2: + raise ValueError( + "configured position limits for continuous joints require " + "tangent-space angular limit handling" + ) + model.lowerPositionLimit[index] = lower + model.upperPositionLimit[index] = upper + if robot.velocity_limits is not None: + if len(robot.velocity_limits) != len(mapping.joint_names) or any( + not np.isfinite(value) or value <= 0.0 for value in robot.velocity_limits + ): + raise ValueError("configured velocity limits are invalid") + for index, limit in zip(mapping.v_indices, robot.velocity_limits, strict=True): + model.velocityLimit[index] = limit + for index in mapping.v_indices: + model.velocityLimit[index] = min(model.velocityLimit[index], self._config.max_velocity) + return [ConfigurationLimit(model), VelocityLimit(model)] + + +class PinkControlIK: + """One-step Pink control IK assembled by :func:`create_pink_control_ik`.""" + + def __init__(self, runtime: _PinkRuntime) -> None: + self._runtime = runtime @property def nq(self) -> int: """Number of controlled coordinates, matching the task contract.""" - return len(self._joint_names) + return len(self._runtime.mapping.joint_names) def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3: + runtime = self._runtime full_q = self._full_q(q) - pinocchio.forwardKinematics(self._model, self._data, full_q) - pinocchio.updateFramePlacements(self._model, self._data) - return self._data.oMf[self._ee_frame_id].copy() + pinocchio.forwardKinematics(runtime.model, runtime.data, full_q) + pinocchio.updateFramePlacements(runtime.model, runtime.data) + return runtime.data.oMf[runtime.ee_frame_id].copy() def solve( self, @@ -160,30 +336,31 @@ def solve( measured: NDArray[np.float64], dt: float, ) -> ControlIKResult: + runtime = self._runtime measured = np.asarray(measured, dtype=np.float64).reshape(-1) - if measured.size != len(self._joint_names) or not np.all(np.isfinite(measured)): + if measured.size != self.nq or not np.all(np.isfinite(measured)): raise ValueError("measured joint state is invalid") if not np.isfinite(dt) or dt <= 0.0: raise ValueError("control IK dt must be finite and positive") - configuration = self._configuration - frame_task = self._frame_task + configuration = runtime.configuration + frame_task = runtime.frame_task try: configuration.update(self._full_q(measured)) frame_task.set_target(target) - if self._posture_task is not None: - self._posture_task.set_target(configuration.q.copy()) + if runtime.posture_task is not None: + runtime.posture_task.set_target(configuration.q.copy()) velocity = solve_ik( configuration, - self._tasks, + runtime.tasks, dt, - solver=self._config.solver, - damping=self._config.lm_damping, - limits=self._limits, - **self._config.qpsolver_options, + solver=runtime.config.solver, + damping=runtime.config.lm_damping, + limits=runtime.limits, + **runtime.config.qpsolver_options, ) velocity = np.asarray(velocity, dtype=np.float64).reshape(-1) - if velocity.size != self._model.nv or not np.all(np.isfinite(velocity)): + if velocity.size != runtime.model.nv or not np.all(np.isfinite(velocity)): raise IKControlRuntimeError("Pink produced an invalid velocity") configuration.integrate_inplace(velocity, dt) candidate = self._project_controlled_positions(configuration.q, measured) @@ -197,8 +374,12 @@ def solve( raise IKControlRuntimeError(f"Pink control solve failed: {exc}") from exc def _full_q(self, controlled: NDArray[np.float64]) -> NDArray[np.float64]: - q = self._reference_q.copy() - for value, index, width in zip(controlled, self._q_indices, self._q_widths, strict=True): + runtime = self._runtime + mapping = runtime.mapping + q = runtime.reference_q.copy() + for value, index, width in zip( + controlled, mapping.q_indices, mapping.q_widths, strict=True + ): if width == 2: q[index] = np.cos(value) q[index + 1] = np.sin(value) @@ -210,15 +391,16 @@ def _project_controlled_positions( self, full_q: NDArray[np.float64], reference: NDArray[np.float64] | None = None ) -> NDArray[np.float64]: """Project model coordinates to coordinator joints and unwrap continuous angles.""" + mapping = self._runtime.mapping positions = np.array( [ np.arctan2(full_q[index + 1], full_q[index]) if width == 2 else full_q[index] - for index, width in zip(self._q_indices, self._q_widths, strict=True) + for index, width in zip(mapping.q_indices, mapping.q_widths, strict=True) ], dtype=np.float64, ) if reference is not None: - for index, width in enumerate(self._q_widths): + for index, width in enumerate(mapping.q_widths): if width == 2: positions[index] = reference[index] + float( (positions[index] - reference[index] + np.pi) % (2.0 * np.pi) - np.pi @@ -226,16 +408,20 @@ def _project_controlled_positions( return positions def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]: - return np.array([velocity[index] for index in self._v_indices], dtype=np.float64) + return np.array( + [velocity[index] for index in self._runtime.mapping.v_indices], dtype=np.float64 + ) def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]: + runtime = self._runtime + mapping = runtime.mapping bounded = candidate.copy() - for index, width in enumerate(self._q_widths): + for index, width in enumerate(mapping.q_widths): if width != 1: continue - q_index = self._q_indices[index] - lower = self._model.lowerPositionLimit[q_index] - upper = self._model.upperPositionLimit[q_index] + q_index = mapping.q_indices[index] + lower = runtime.model.lowerPositionLimit[q_index] + upper = runtime.model.upperPositionLimit[q_index] value = bounded[index] if value < lower: if lower - value <= _POSITION_LIMIT_EPSILON_RAD: @@ -249,126 +435,7 @@ def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.f raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate") return bounded - def _build_mapping(self, robot: RobotModelConfig) -> tuple[list[int], list[int]]: - coordinator_names = robot.get_coordinator_joint_names() - if coordinator_names != self._joint_names or len(set(coordinator_names)) != len( - coordinator_names - ): - raise ValueError( - "control task joints must exactly match ordered RobotModelConfig joints" - ) - indices: list[int] = [] - velocity_indices: list[int] = [] - self._q_widths: list[int] = [] - self._controlled_joint_ids: set[int] = set() - for urdf_name in (robot.get_urdf_joint_name(name) for name in coordinator_names): - if not self._model.existJointName(urdf_name): - raise ValueError(f"control joint mapping references unknown joint: {urdf_name}") - joint_id = self._model.getJointId(urdf_name) - if joint_id <= 0 or joint_id >= len(self._model.joints): - raise ValueError(f"invalid control joint index for {urdf_name}") - joint = self._model.joints[joint_id] - if int(joint.nv) != 1 or int(joint.nq) not in (1, 2): - raise ValueError(f"control joint must be one-DoF: {urdf_name}") - indices.append(int(joint.idx_q)) - velocity_indices.append(int(joint.idx_v)) - self._q_widths.append(int(joint.nq)) - self._controlled_joint_ids.add(joint_id) - return indices, velocity_indices - - def _build_reference_q(self, use_config_reference: bool = True) -> NDArray[np.float64]: - if use_config_reference and self._config.reference_q is not None: - q = np.asarray(self._config.reference_q, dtype=np.float64).reshape(-1) - if q.size != self._model.nq or not np.all(np.isfinite(q)): - raise ValueError("Pink reference_q must match model nq and be finite") - else: - q = np.asarray(pinocchio.neutral(self._model), dtype=np.float64) - if not (use_config_reference and self._config.reference_q is not None): - for joint_id in range(1, len(self._model.joints)): - joint = self._model.joints[joint_id] - start = int(joint.idx_q) - width = int(joint.nq) - if width == 2 and int(joint.nv) == 1: - q[start : start + 2] = (1.0, 0.0) - continue - if width != 1: - continue - for index in range(start, start + width): - lower = self._model.lowerPositionLimit[index] - upper = self._model.upperPositionLimit[index] - if np.isfinite(lower) and np.isfinite(upper): - q[index] = (lower + upper) / 2.0 - elif np.isfinite(lower): - q[index] = max(0.0, lower) - elif np.isfinite(upper): - q[index] = min(0.0, upper) - else: - q[index] = 0.0 - if not np.all(np.isfinite(q)): - raise ValueError("Pink reference configuration is not finite") - bounded = np.isfinite(self._model.lowerPositionLimit) & np.isfinite( - self._model.upperPositionLimit - ) - if np.any(q[bounded] < self._model.lowerPositionLimit[bounded]) or np.any( - q[bounded] > self._model.upperPositionLimit[bounded] - ): - raise ValueError("Pink reference configuration violates model limits") - return q - - def _uncontrolled_ee_chain(self, frame_id: int, controlled_joint_ids: set[int]) -> bool: - joint_id = int(self._model.frames[frame_id].parentJoint) - while joint_id > 0: - if joint_id not in controlled_joint_ids: - return True - joint_id = int(self._model.parents[joint_id]) - return False - - def _validate_frame(self, frame_name: str) -> int: - if not self._model.existFrame(frame_name): - raise ValueError(f"unknown control end-effector frame: {frame_name}") - frame_id = int(self._model.getFrameId(frame_name)) - if frame_id < 0 or frame_id >= len(self._model.frames): - raise ValueError(f"invalid control end-effector frame: {frame_name}") - return frame_id - - def _apply_limits(self, robot: RobotModelConfig) -> None: - if robot.joint_limits_lower is not None or robot.joint_limits_upper is not None: - if robot.joint_limits_lower is None or robot.joint_limits_upper is None: - raise ValueError("both configured joint limit bounds are required") - if len(robot.joint_limits_lower) != len(self._joint_names) or len( - robot.joint_limits_upper - ) != len(self._joint_names): - raise ValueError("configured joint limits do not match control joints") - for index, width, lower, upper in zip( - self._q_indices, - self._q_widths, - robot.joint_limits_lower, - robot.joint_limits_upper, - strict=True, - ): - if not np.isfinite(lower) or not np.isfinite(upper) or lower >= upper: - raise ValueError("configured joint limits must be finite and ordered") - if width == 2: - raise ValueError( - "configured position limits for continuous joints require " - "tangent-space angular limit handling" - ) - self._model.lowerPositionLimit[index] = lower - self._model.upperPositionLimit[index] = upper - if robot.velocity_limits is not None: - if len(robot.velocity_limits) != len(self._joint_names) or any( - not np.isfinite(value) or value <= 0.0 for value in robot.velocity_limits - ): - raise ValueError("configured velocity limits are invalid") - for index, limit in zip(self._v_indices, robot.velocity_limits, strict=True): - self._model.velocityLimit[index] = limit - for index in self._v_indices: - self._model.velocityLimit[index] = min( - self._model.velocityLimit[index], self._config.max_velocity - ) - self._limits = [ConfigurationLimit(self._model), VelocityLimit(self._model)] - def create_pink_control_ik(config: PinkControlIKConfig) -> PinkControlIK: """Construct the default Cartesian control IK backend.""" - return PinkControlIK(config) + return PinkControlIK(_PinkControlIKBuilder(config).build()) diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index 4663931c70..f4d976d6f7 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -15,13 +15,15 @@ from pathlib import Path import numpy as np +from pink import Configuration +from pink.tasks import PostureTask import pytest from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( IKControlRuntimeError, - PinkControlIK, PinkControlIKConfig, + create_pink_control_ik, ) from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -161,7 +163,7 @@ def prepare( prepare, ) - PinkControlIK( + create_pink_control_ik( PinkControlIKConfig(robot_model=robot), ) @@ -177,7 +179,7 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N model_path = _write_urdf(tmp_path) with pytest.raises(ValueError, match="end-effector frame"): - PinkControlIK( + create_pink_control_ik( PinkControlIKConfig(robot_model=_robot(model_path, frame="missing")), ) @@ -185,7 +187,7 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N update={"joint_name_mapping": {"joint1": "missing", "joint2": "joint2"}} ) with pytest.raises(ValueError, match="unknown joint"): - PinkControlIK( + create_pink_control_ik( PinkControlIKConfig(robot_model=mismatched), ) @@ -194,41 +196,40 @@ def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: model_path = _write_urdf(tmp_path) - backend = PinkControlIK( + backend = create_pink_control_ik( PinkControlIKConfig(robot_model=_robot(model_path), qpsolver_options={"eps": 1e-6}), ) measured = np.array([0.3, 0.1]) target = backend.forward_kinematics(measured) - calls: list[tuple[object, list[object], float, dict[str, object]]] = [] + calls: list[tuple[Configuration, list[object], float, dict[str, object]]] = [] def solve( - configuration: object, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[object], dt: float, **kwargs: object ) -> np.ndarray: calls.append((configuration, tasks, dt, kwargs)) - return np.zeros(backend._model.nv) + return np.zeros(configuration.model.nv) monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) result = backend.solve(target, measured, 0.01) assert np.array_equal(result.positions, measured) assert len(calls) == 1 - assert len(calls[0][1]) == 2 - assert calls[0][1] == [backend._frame_task, backend._posture_task] - assert backend._posture_task is not None - assert np.array_equal(backend._posture_task.target_q, backend._full_q(measured)) - assert calls[0][2] == 0.01 - assert calls[0][3] == { - "solver": "proxqp", - "damping": 1e-4, - "limits": backend._limits, - "eps": 1e-6, - } + configuration, tasks, dt, kwargs = calls[0] + assert len(tasks) == 2 + assert isinstance(tasks[1], PostureTask) + assert np.array_equal(tasks[1].target_q, configuration.q) + assert dt == 0.01 + assert kwargs["solver"] == "proxqp" + assert kwargs["damping"] == 1e-4 + assert kwargs["eps"] == 1e-6 + assert isinstance(kwargs["limits"], list) + assert len(kwargs["limits"]) == 2 def test_pink_solver_dependency_failure_is_translated_to_runtime_error( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: - backend = PinkControlIK(PinkControlIKConfig(robot_model=_robot(_write_urdf(tmp_path)))) + backend = create_pink_control_ik(PinkControlIKConfig(robot_model=_robot(_write_urdf(tmp_path)))) def solve( configuration: object, tasks: list[object], dt: float, **kwargs: object @@ -245,16 +246,16 @@ def test_pink_receives_pre_bounded_dt_unchanged( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: model_path = _write_urdf(tmp_path) - backend = PinkControlIK( + backend = create_pink_control_ik( PinkControlIKConfig(robot_model=_robot(model_path)), ) calls: list[float] = [] def solve( - configuration: object, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[object], dt: float, **kwargs: object ) -> np.ndarray: calls.append(dt) - return np.zeros(backend._model.nv) + return np.zeros(configuration.model.nv) monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) measured = np.array([0.3, 0.1]) @@ -265,14 +266,16 @@ def solve( def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None: model_path = _write_urdf(tmp_path) - backend = PinkControlIK(PinkControlIKConfig(robot_model=_robot(model_path), posture_cost=0.0)) + backend = create_pink_control_ik( + PinkControlIKConfig(robot_model=_robot(model_path), posture_cost=0.0) + ) calls: list[list[object]] = [] def solve( - configuration: object, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[object], dt: float, **kwargs: object ) -> np.ndarray: calls.append(tasks) - return np.zeros(backend._model.nv) + return np.zeros(configuration.model.nv) monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) measured = np.array([0.3, 0.1]) @@ -286,46 +289,67 @@ def test_pink_rejects_uncontrolled_end_effector_chain_without_reference( ) -> None: model_path = _write_urdf(tmp_path, "uncontrolled.urdf", _UNCONTROLLED_URDF) with pytest.raises(ValueError, match="reference_q.*uncontrolled joint"): - PinkControlIK( + create_pink_control_ik( PinkControlIKConfig(robot_model=_robot(model_path)), ) -def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(tmp_path: Path) -> None: +def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: model_path = _write_urdf(tmp_path, "continuous.urdf", _CONTINUOUS_URDF) robot = _robot(model_path, joints=["joint1"]) with pytest.raises(ValueError, match="continuous joints.*tangent-space"): - PinkControlIK( + create_pink_control_ik( PinkControlIKConfig(robot_model=robot), ) roundtrip_robot = robot.model_copy( update={"joint_limits_lower": None, "joint_limits_upper": None} ) - backend = PinkControlIK( + backend = create_pink_control_ik( PinkControlIKConfig(robot_model=roundtrip_robot), ) angle = np.array([3.0]) - assert backend._q_widths == [2] - assert np.allclose(backend._project_controlled_positions(backend._full_q(angle), angle), angle) + def solve( + configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + return np.zeros(configuration.model.nv) + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) + result = backend.solve(backend.forward_kinematics(angle), angle, 0.01) -def test_pink_applies_position_velocity_limits_and_finite_output(tmp_path: Path) -> None: + assert np.allclose(result.positions, angle) + + +def test_pink_applies_position_velocity_limits_and_finite_output( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: model_path = _write_urdf(tmp_path) robot = _robot(model_path).model_copy( update={"joint_limits_lower": [-0.5, -0.25], "joint_limits_upper": [0.5, 0.25]} ) - backend = PinkControlIK( + backend = create_pink_control_ik( PinkControlIKConfig(robot_model=robot, max_velocity=0.2), ) + solver_inputs: dict[str, np.ndarray] = {} - assert np.array_equal(backend._model.lowerPositionLimit[:2], np.array([-0.5, -0.25])) - assert np.all(backend._model.velocityLimit[backend._v_indices] <= 0.2) + def solve( + configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + ) -> np.ndarray: + solver_inputs["lower_position"] = configuration.model.lowerPositionLimit.copy() + solver_inputs["velocity"] = configuration.model.velocityLimit.copy() + return np.zeros(configuration.model.nv) + + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) result = backend.solve( backend.forward_kinematics(np.array([0.1, 0.1])), np.array([0.1, 0.1]), 0.01 ) + + assert np.array_equal(solver_inputs["lower_position"][:2], np.array([-0.5, -0.25])) + assert np.all(solver_inputs["velocity"][:2] <= 0.2) assert result.positions.shape == (2,) assert np.all(np.isfinite(result.positions)) @@ -337,7 +361,7 @@ def test_pink_clamps_tiny_position_limit_overshoot( robot = _robot(model_path).model_copy( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) - backend = PinkControlIK(PinkControlIKConfig(robot_model=robot)) + backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot)) measured = np.array([1.22, 0.1]) def solve( @@ -359,7 +383,7 @@ def test_pink_rejects_material_position_limit_violation( robot = _robot(model_path).model_copy( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) - backend = PinkControlIK(PinkControlIKConfig(robot_model=robot)) + backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot)) measured = np.array([1.22, 0.1]) def solve( From c559359dbec02638af77143e58fce237b7ead469 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 3 Aug 2026 23:50:51 -0700 Subject: [PATCH 18/19] refactor(control): use Pydantic robot model validation --- dimos/control/tasks/cartesian_ik_task/pink_control_ik.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index de676edac1..4cd9cdb2a7 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -22,7 +22,7 @@ import numpy as np from numpy.typing import NDArray import pinocchio -from pydantic import Field, FiniteFloat, field_validator +from pydantic import Field, FiniteFloat _PINK_INSTALL_ERROR = "Pink control tasks require the 'pink' dependency. Install it with `uv sync`." @@ -58,13 +58,6 @@ class PinkControlIKConfig(BaseConfig): reference_q: list[float] | None = None qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict) - @field_validator("robot_model", mode="before") - @classmethod - def _accept_robot_model(cls, value: object) -> RobotModelConfig: - if not isinstance(value, RobotModelConfig): - raise TypeError("Pink robot_model must be a RobotModelConfig instance") - return value - @dataclass(frozen=True) class ControlIKResult: From f61e76abb9be50d5640f693e283fac89654433e9 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 3 Aug 2026 23:50:51 -0700 Subject: [PATCH 19/19] fix(xarm): use Viser for RoboPlan teleop --- .../manipulators/xarm/blueprints/teleop.py | 4 +-- .../xarm/blueprints/test_teleop.py | 36 +++++++++++++++++++ 2 files changed, 38 insertions(+), 2 deletions(-) create mode 100644 dimos/robot/manipulators/xarm/blueprints/test_teleop.py diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py index 65bf011f17..b41e615b37 100644 --- a/dimos/robot/manipulators/xarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -64,7 +64,7 @@ ), ManipulationModule.blueprint( robots=[make_xarm6_model_config(add_gripper=True)], - visualization={"backend": "meshcat"}, + visualization={"backend": "viser"}, ), ) @@ -85,7 +85,7 @@ ), ManipulationModule.blueprint( robots=[make_xarm7_model_config(add_gripper=True)], - visualization={"backend": "meshcat"}, + visualization={"backend": "viser"}, ), ) diff --git a/dimos/robot/manipulators/xarm/blueprints/test_teleop.py b/dimos/robot/manipulators/xarm/blueprints/test_teleop.py new file mode 100644 index 0000000000..65ddad5813 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/test_teleop.py @@ -0,0 +1,36 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest + +from dimos.core.coordination.blueprints import Blueprint +from dimos.manipulation.manipulation_module import ( + ManipulationModule, + ManipulationModuleConfig, +) +from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig +from dimos.robot.manipulators.xarm.blueprints.teleop import ( + keyboard_teleop_xarm6, + keyboard_teleop_xarm7, +) + + +@pytest.mark.parametrize("blueprint", [keyboard_teleop_xarm6, keyboard_teleop_xarm7]) +def test_keyboard_teleop_uses_roboplan_compatible_visualization(blueprint: Blueprint) -> None: + manipulation = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule) + config = ManipulationModuleConfig.model_validate(manipulation.kwargs) + + assert config.world_backend == "roboplan" + assert isinstance(config.visualization, ViserVisualizationConfig) + assert config.visualization.requires_world_visualization is False