Add planning group world queries

[TomCC7]

Problem

Planning-group definitions exist, but world backends and monitors still answered FK, Jacobian, and state queries through robot-scoped APIs. Callers needed to infer tip frames and local/global joint mappings instead of querying a selected planning group directly.

Closes: N/A

Solution

• Add planning-group FK/Jacobian methods to WorldSpec and implement them for Drake and RoboPlan.
• Preserve legacy robot-scoped wrappers by resolving through the unique pose-targetable planning group, with clear failures for no-pose or ambiguous groups.
• Add WorldMonitor planning-group registry access, current global/group joint-state helpers, and group-scoped FK/Jacobian state normalization.
• Add Drake, RoboPlan, and WorldMonitor coverage for group-local ordering, state mapping, legacy wrapper failures, and duplicate robot-name prevention.
• Keep visualization, Viser/UI, control, ManipulationModule API, and IK/RRT migration out of scope.

How to Test

API change, so only unit tests

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2418	1	2417	78

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

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Flake rate in main: 22.78% (Passed 61 times, Failed 18 times)

Stack Traces | 551s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x733a0c74bda0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x733a0c54b4c0>
human_input = <function human_input.<locals>.send_human_input at 0x733a0c54b560>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x733a0c2e7ce0>
explore_house = <function explore_house.<locals>.explore at 0x733a0c54ba60>

    @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 0x733a0c2e7ce0>
explore_house = <function explore_house.<locals>.explore at 0x733a0c54ba60>
human_input = <function human_input.<locals>.send_human_input at 0x733a0c54b560>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x733a0c74bda0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x733a0c54b4c0>

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 0x733a0c54bc40>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x733a0c74bda0>
        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 0x733a0bce3fb0: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x733a0c54bb00>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x733a0c54bc40>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x733a0c74bda0>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

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

    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 0x733a0bce3fb0: 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 0x733a0c74bda0>
start_time = 1783027244.4152508
timeout    = 180

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 teaches world backends (Drake and RoboPlan) and WorldMonitor to answer FK, Jacobian, and joint-state queries scoped to a named planning group, instead of requiring callers to infer tip frames and joint mappings from robot-level APIs.

• Adds get_group_ee_pose and get_group_jacobian to WorldSpec, DrakeWorld, RoboPlanWorld, and WorldMonitor, with group-local joint ordering and proper tip-link resolution.
• Introduces joint_state_to_ordered_positions in utils.py replacing bespoke name-lookup logic in both backends, and adds current_global_joint_state / current_group_joint_state helpers to WorldMonitor.
• Legacy robot-scoped wrappers (get_ee_pose, get_jacobian) are preserved but now fail explicitly when no unique pose-targetable group exists, backed by duplicate-registration guards and early validation in both backends and the monitor.

Confidence Score: 5/5

Safe to merge — the new group-scoped FK/Jacobian paths are well-isolated behind explicit group IDs, and the legacy wrappers fail loudly rather than silently producing wrong results.

All group-lookup, stale-state, and no-pose-group conditions are tested end-to-end across Drake, RoboPlan, and WorldMonitor. The only finding is a diagnostic message that says 'IK result' on a code path that handles current robot state, which won't affect correctness. The refactored _joint_state_to_q in both backends now uses the shared joint_state_to_ordered_positions, and both validation-before-mutation patterns correctly prevent partial state corruption.

No files require special attention beyond the single diagnostic message in dimos/manipulation/planning/groups/utils.py.

Important Files Changed

Filename	Overview
dimos/manipulation/planning/groups/utils.py	Adds joint_state_to_ordered_positions (new canonical name-resolution helper) and renames the filter_joint_state_to_selected_joints error message to 'IK result' — misleading now that the function is also called from non-IK paths.
dimos/manipulation/planning/monitor/world_monitor.py	Adds planning-group registry, duplicate-name guard, group-scoped state helpers, and get_group_ee_pose/get_group_jacobian. Stale/unavailable checks are consistent; early validation prevents backend mutation on invalid configs.
dimos/manipulation/planning/world/drake_world.py	Implements group FK/Jacobian using tip-link name lookups and group-local joint ordering. Legacy wrappers correctly delegate through _primary_pose_group_id_for_config. Finalization guard is preserved via delegation.
dimos/manipulation/planning/world/roboplan_world.py	Implements group FK/Jacobian with scene-order or config-order fallback projection. Duplicate-name guard is unreachable (pre-existing issue noted in prior reviews). _joint_state_to_q refactored to use the shared joint_state_to_ordered_positions utility.
dimos/manipulation/planning/spec/protocols.py	Adds get_group_ee_pose and get_group_jacobian to WorldSpec protocol; clean additions with no issues.
dimos/manipulation/planning/world/test_drake_world_planning_groups.py	New test file covering group config helpers, FK/Jacobian shape and ordering, legacy wrapper failures, and non-controllable joint rejection — good coverage.
dimos/manipulation/test_roboplan_world.py	Adds tests for group FK/Jacobian projection, global-name mapping, duplicate resolved names, and legacy wrapper requirements — comprehensive.
dimos/manipulation/planning/monitor/test_world_monitor.py	Adds coverage for duplicate robot names, invalid group configs, stale/unavailable state guards, group state ordering, and legacy wrapper failures; tests are thorough.

Sequence Diagram

%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
    participant Caller
    participant WorldMonitor
    participant PlanningGroupRegistry
    participant WorldSpec

    Caller->>WorldMonitor: get_group_ee_pose(group_id, joint_state?)
    WorldMonitor->>PlanningGroupRegistry: get(group_id) → PlanningGroup
    WorldMonitor->>WorldMonitor: _robot_ids_by_name[group.robot_name] → robot_id
    alt joint_state is None
        WorldMonitor->>WorldMonitor: is_state_stale? → raise stale
        WorldMonitor->>WorldMonitor: get_current_joint_state(robot_id) → None? raise unavailable
    end
    WorldMonitor->>WorldSpec: scratch_context() → ctx
    WorldMonitor->>WorldSpec: set_joint_state(ctx, robot_id, joint_state)
    WorldMonitor->>WorldSpec: get_group_ee_pose(ctx, group_id)
    WorldSpec->>WorldSpec: _planning_group_from_id(group_id) → group
    WorldSpec->>WorldSpec: GetBodyByName(group.tip_link) → ee_body
    WorldSpec-->>WorldMonitor: PoseStamped
    WorldMonitor-->>Caller: PoseStamped

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"}}}%%
sequenceDiagram
    participant Caller
    participant WorldMonitor
    participant PlanningGroupRegistry
    participant WorldSpec

    Caller->>WorldMonitor: get_group_ee_pose(group_id, joint_state?)
    WorldMonitor->>PlanningGroupRegistry: get(group_id) → PlanningGroup
    WorldMonitor->>WorldMonitor: _robot_ids_by_name[group.robot_name] → robot_id
    alt joint_state is None
        WorldMonitor->>WorldMonitor: is_state_stale? → raise stale
        WorldMonitor->>WorldMonitor: get_current_joint_state(robot_id) → None? raise unavailable
    end
    WorldMonitor->>WorldSpec: scratch_context() → ctx
    WorldMonitor->>WorldSpec: set_joint_state(ctx, robot_id, joint_state)
    WorldMonitor->>WorldSpec: get_group_ee_pose(ctx, group_id)
    WorldSpec->>WorldSpec: _planning_group_from_id(group_id) → group
    WorldSpec->>WorldSpec: GetBodyByName(group.tip_link) → ee_body
    WorldSpec-->>WorldMonitor: PoseStamped
    WorldMonitor-->>Caller: PoseStamped

Loading

_{Reviews (3): Last reviewed commit: "test: fix joint state ordering assertion" | Re-trigger Greptile}