feat: add manipulation planning groups

[TomCC7]

Summary

• Add first-class manipulation planning groups: public robot/group IDs, global joint names, SRDF/fallback discovery, registry construction, and selector-aware world resolution.
• Extend manipulation planning APIs so kinematics, planners, world monitors, and ManipulationModule can plan to group-scoped pose and joint targets while preserving local model joint names internally.
• Carry generated plans through preview and execution: selected joints are projected in group order, coordinator trajectory tasks are resolved per robot, and multi-arm execution uses coordinator task names.
• Improve IK/planning internals, including Pink IK model-context reuse, multi-frame solve sharing, seed-fallback guardrails, Drake/Jacobian/RRT group support, and optional end-effector handling.
• Align robot/blueprint integration after the manipulator blueprint reorg, including OpenArm planner/coordinator scoped joint-state publishing and current dual-arm blueprint tests.
• Keep only the thin visualization ABI compatibility layer needed by the new planning APIs. The full Viser planning-group UI now lives in stacked PR feat: add planning group Viser panel #2563.
• Remove compatibility shims and clarify helper ownership: planning identifier grammar lives in groups/identifiers.py; joint target/state helpers live in groups/joints.py.

Stack

• Base PR: this PR, core planning groups and execution APIs.
• Stacked UI PR: feat: add planning group Viser panel #2563, full Viser planning-group panel and visual polish.

Verification

• uv run pytest dimos/manipulation/planning/test_planning_group_identifiers.py dimos/manipulation/planning/test_planning_group_joints.py dimos/manipulation/planning/kinematics/test_pink_ik.py dimos/manipulation/test_manipulation_unit.py -q
• uv run pytest dimos/manipulation/planning/kinematics -q
• uv run pytest dimos/e2e_tests/test_manipulation_planning_groups.py -m self_hosted_large -q
• uv run pytest dimos/e2e_tests/test_control_coordinator.py -q
• uv run pytest dimos/robot/test_all_blueprints.py dimos/robot/test_all_blueprints_generation.py dimos/control/blueprints/test_dual.py -q
• uv run pytest dimos/manipulation/visualization/test_factory.py dimos/manipulation/visualization/viser/test_viser_visualization.py dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py dimos/manipulation/test_manipulation_unit.py -q
• uv run --group lint mypy
• uv run ruff check ... / uv run ruff format --check ...
• rtk git diff --check

Notes

• The PR has absorbed follow-up fixes from review and CI: empty target requests no longer fault the manipulation module before planning starts, pose planning now checks for a planner before entering the planning state, and joint-state reads no longer require an end-effector link.
• Old Greptile inline comments may still appear on outdated diff positions, but the latest code includes the P1 fixes called out in the current review cycle.

[codecov]

❌ 2 Tests Failed:

Tests completed	Failed	Passed	Skipped
2343	2	2341	74

View the full list of 2 ❄️ flaky test(s)

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Flake rate in main: 22.22% (Passed 28 times, Failed 8 times)

Stack Traces | 570s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3e6da00>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x753fc4543560>
human_input = <function human_input.<locals>.send_human_input at 0x753fc4543b00>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x753fc3cbc080>
explore_house = <function explore_house.<locals>.explore at 0x753fc4598540>

    @pytest.mark.self_hosted_large
    def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None:
        start_blueprint(
            "run",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        explore_house()
    
        human_input("go to the bed")
    
>       lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x753fc3cbc080>
explore_house = <function explore_house.<locals>.explore at 0x753fc4598540>
human_input = <function human_input.<locals>.send_human_input at 0x753fc4543b00>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3e6da00>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x753fc4543560>

dimos/e2e_tests/test_dimsim_spatial_memory.py:32: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x753fc4543ec0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3e6da00>
        threshold  = 2
        timeout    = 180
        x          = -3.567
        y          = -1.332
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x753fc3cbd760: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x753fc4598720>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x753fc4543ec0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3e6da00>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3e6da00>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=-3.567, y=-1.332

condition  = <bound method Event.is_set of <threading.Event at 0x753fc3cbd760: unset>>
error_message = 'Failed to get to position x=-3.567, y=-1.332'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3e6da00>
start_time = 1782166809.3396978
timeout    = 180

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

dimos.e2e_tests.test_dimsim_walk_forward::test_walk_forward

Flake rate in main: 28.12% (Passed 23 times, Failed 9 times)

Stack Traces | 204s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3cbec00>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x753fc4598ea0>
human_input = <function human_input.<locals>.send_human_input at 0x753fc4598fe0>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x753fc3cbf2f0>

    @pytest.mark.self_hosted_large
    def test_walk_forward(lcm_spy, start_blueprint, human_input, dim_sim) -> None:
        start_blueprint(
            "run",
            "--disable",
            "spatial-memory",
            "--disable",
            "security-module",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        origin_x, origin_y = 1, 2
        dim_sim.set_agent_position(origin_x, origin_y)
    
        human_input("move forward 3 meter")
    
>       lcm_spy.wait_until_odom_position(origin_x + 3, origin_y, threshold=0.4, timeout=120)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x753fc3cbf2f0>
human_input = <function human_input.<locals>.send_human_input at 0x753fc4598fe0>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3cbec00>
origin_x   = 1
origin_y   = 2
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x753fc4598ea0>

dimos/e2e_tests/test_dimsim_walk_forward.py:37: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x753fc4543a60>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3cbec00>
        threshold  = 0.4
        timeout    = 120
        x          = 4
        y          = 2
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x753fc3cbf8c0: unset>
        fail_message = 'Failed to get to position x=4, y=2'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x753fc4598ae0>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x753fc4543a60>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3cbec00>
        timeout    = 120
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3cbec00>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=4, y=2

condition  = <bound method Event.is_set of <threading.Event at 0x753fc3cbf8c0: unset>>
error_message = 'Failed to get to position x=4, y=2'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x753fc3cbec00>
start_time = 1782167073.3567553
timeout    = 120

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

[greptile-apps]

Greptile Summary

This PR introduces first-class manipulation planning groups — public robot/group IDs with global joint names, SRDF/fallback discovery, a backend-independent registry, and group-scoped APIs across IK solvers, planners, world monitors, and ManipulationModule. It also carries plans through preview and execution for multi-arm use, and adds a unified RobotConfig that auto-generates RobotModelConfig, HardwareComponent, and TaskConfig from a single URDF-backed config.

• New groups/ layer (discovery.py, registry.py, models.py, identifiers.py, joints.py): SRDF chain/joint-list parsing, fallback generation, delimiter-based robot/group and robot/joint IDs, and overlap-checked PlanningGroupSelection. The _ordered_joints_between_links chain traversal fails on valid SRDF chains whose intermediate links have fixed-joint accessories (flagged in a prior review thread and not yet fixed).
• ManipulationModule rewrite: _planned_paths/_planned_trajectories replaced by a single GeneratedPlan; new plan_to_pose_targets, plan_to_joint_targets, inverse_kinematics, forward_kinematics, and check_collision RPCs; sequential multi-arm dispatch and _fail()-after-_begin_planning() state-machine concerns carry over from the previous review.
• Visualization (ViserManipulationVisualizer.animate_plan): The per-robot loop uses return instead of continue when one robot in a multi-arm plan is missing state, silently dropping the preview for all robots rather than just the broken one.

Confidence Score: 4/5

Safe to merge for single-arm use; the dual-arm preview path has a silent abort that will make the Viser ghost appear unresponsive on the first robot-state miss.

The animate_plan early-return in the visualizer means a transient missing joint state for any one arm in a dual-arm plan drops the entire preview animation without any visible indication to the user. The core planning and execution paths (IK, RRT, coordinator dispatch) are unaffected so robot motion is not impacted, but the Viser preview feedback is broken for the primary new use-case.

dimos/manipulation/visualization/viser/visualizer.py (animate_plan loop) and dimos/manipulation/manipulation_module.py (_plan_selected_path missing preview dismiss).

Important Files Changed

Filename	Overview
dimos/manipulation/visualization/viser/visualizer.py	animate_plan uses return instead of continue when a robot in a multi-robot plan has no current state, silently abandoning preview for all remaining robots (flagged as P1).
dimos/manipulation/manipulation_module.py	Major rewrite introducing group-scoped planning APIs; preview ghost is not dismissed on replan (flagged); previously-reported sequential-dispatch partial-execution and _fail()-before-_begin_planning issues persist in this revision.
dimos/manipulation/planning/world/drake_world.py	Group-aware get_group_ee_pose/get_group_jacobian/animate_plan added; redundant PlanningGroupRegistry instance alongside WorldMonitor's; previously-reported misleading error message in get_ee_pose guard is still present.
dimos/manipulation/planning/groups/discovery.py	New file: SRDF/fallback planning-group discovery. Chain traversal in _ordered_joints_between_links rejects valid SRDF chains when any intermediate link has a fixed-joint accessory (already flagged in prior review thread).
dimos/manipulation/planning/monitor/world_monitor.py	Adds current_global_joint_state, check_collision, get_group_ee_pose, get_group_jacobian; logic is sound and the dual-registry population order is safe.
dimos/robot/config.py	New file: unified RobotConfig with lazy model parsing and three converter methods (to_robot_model_config, to_hardware_component, to_task_config); SRDF discovery and global-name generation are wired correctly.
dimos/manipulation/planning/spec/config.py	RobotModelConfig gains srdf_path, strip_model_world_joint, and planning_groups; model_post_init inserts the fallback group when none are provided; end_effector_link is now optional.
dimos/manipulation/planning/planners/rrt_planner.py	plan_selected_joint_path added; single-robot path correctly converts global to local to global names; multi-robot fallback returns UNSUPPORTED which is honest; no new issues.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    A[plan_to_pose / plan_to_pose_targets] --> B[_begin_planning state to PLANNING]
    B --> C[_selected_joint_state collect current global joints]
    C --> D[inverse_kinematics solve_pose_targets per robot]
    D --> E{IK success?}
    E -- No --> F[_fail to FAULT]
    E -- Yes --> G[_plan_selected_path RRTConnect plan_selected_joint_path]
    G --> H{Plan success?}
    H -- No --> F
    H -- Yes --> I[_store_generated_plan GeneratedPlan with global joints]
    I --> J[state to COMPLETED]
    J --> K[preview_plan animate_plan]
    K --> L[execute_plan project local paths per robot]
    L --> M[_invoke_coordinator_task for each affected robot]
    M --> N{All dispatches OK?}
    N -- No --> F
    N -- Yes --> O[state to COMPLETED]
    style F fill:#f99,stroke:#900
    style O fill:#9f9,stroke:#090

Loading

%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
flowchart TD
    A[plan_to_pose / plan_to_pose_targets] --> B[_begin_planning state to PLANNING]
    B --> C[_selected_joint_state collect current global joints]
    C --> D[inverse_kinematics solve_pose_targets per robot]
    D --> E{IK success?}
    E -- No --> F[_fail to FAULT]
    E -- Yes --> G[_plan_selected_path RRTConnect plan_selected_joint_path]
    G --> H{Plan success?}
    H -- No --> F
    H -- Yes --> I[_store_generated_plan GeneratedPlan with global joints]
    I --> J[state to COMPLETED]
    J --> K[preview_plan animate_plan]
    K --> L[execute_plan project local paths per robot]
    L --> M[_invoke_coordinator_task for each affected robot]
    M --> N{All dispatches OK?}
    N -- No --> F
    N -- Yes --> O[state to COMPLETED]
    style F fill:#f99,stroke:#900
    style O fill:#9f9,stroke:#090

Loading

_{Reviews (8): Last reviewed commit: "chore: split bulky viser panel changes" | Re-trigger Greptile}