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

• openspec validate planning-world-monitor-groups --type change --strict --no-interactive
• uv run pytest dimos/manipulation/planning/world/test_drake_world_planning_groups.py dimos/manipulation/test_roboplan_world.py dimos/manipulation/planning/monitor/test_world_monitor.py -q
  • Local result: 27 passed, 1 skipped because pydrake is not installed.
• uv run --group lint mypy dimos/manipulation/planning/spec/protocols.py dimos/manipulation/planning/world/drake_world.py dimos/manipulation/planning/world/roboplan_world.py dimos/manipulation/planning/monitor/world_monitor.py

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2404	1	2403	75

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

dimos.e2e_tests.test_dimsim_walk_forward::test_walk_forward

Flake rate in main: 28.00% (Passed 54 times, Failed 21 times)

Stack Traces | 204s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x78354aa9c680>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x78354790aa20>
human_input = <function human_input.<locals>.send_human_input at 0x78354790a340>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x783549eee420>

    @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 0x783549eee420>
human_input = <function human_input.<locals>.send_human_input at 0x78354790a340>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x78354aa9c680>
origin_x   = 1
origin_y   = 2
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x78354790aa20>

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 0x783547909120>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x78354aa9c680>
        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 0x783549eefd10: unset>
        fail_message = 'Failed to get to position x=4, y=2'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x78354790b060>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x783547909120>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x78354aa9c680>
        timeout    = 120
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

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

    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 0x783549eefd10: 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 0x78354aa9c680>
start_time = 1782971915.6063612
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.}

[TomCC7]

what is this for if we have self._planning_groups?

[TomCC7]

naming should be coherent with current_global_joint_state

[TomCC7]

shouldn't this be a util function in groups folder?

[TomCC7]

we should have only one single planning group registry across the program

[TomCC7]

same here, planning group registy should stay in world monitor