WIP spec: planning group and bimanual/multi-target motion planning

[TomCC7]

Proposal for adding planning group concept into manipulation module and naturally enable bimanual/mult-target IK and motion planning.

check design

[codecov]

❌ 3 Tests Failed:

Tests completed	Failed	Passed	Skipped
2166	3	2163	70

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

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Flake rate in main: 36.36% (Passed 7 times, Failed 4 times)

Stack Traces | 576s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7d42124e2570>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7d420f743880>
human_input = <function human_input.<locals>.send_human_input at 0x7d420f7bc220>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x7d4211a1ce30>
explore_house = <function explore_house.<locals>.explore at 0x7d420f7bc860>

    @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 0x7d4211a1ce30>
explore_house = <function explore_house.<locals>.explore at 0x7d420f7bc860>
human_input = <function human_input.<locals>.send_human_input at 0x7d420f7bc220>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7d42124e2570>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7d420f743880>

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

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

    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 0x7d4211a1d940: 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 0x7d42124e2570>
start_time = 1781375112.8662891
timeout    = 180

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

dimos.e2e_tests.test_dimsim_walk_forward::test_walk_forward

Flake rate in main: 57.14% (Passed 3 times, Failed 4 times)

Stack Traces | 205s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7d4211a1f260>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7d420f7bd1c0>
human_input = <function human_input.<locals>.send_human_input at 0x7d420f7bd300>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x7d4211a1f9b0>

    @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 0x7d4211a1f9b0>
human_input = <function human_input.<locals>.send_human_input at 0x7d420f7bd300>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7d4211a1f260>
origin_x   = 1
origin_y   = 2
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7d420f7bd1c0>

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

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

    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 0x7d4211a44200: 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 0x7d4211a1f260>
start_time = 1781375378.0150328
timeout    = 120

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

dimos.protocol.pubsub.test_spec::test_high_volume_messages[ros_context-topic1-values1]

Flake rate in main: 16.67% (Passed 15 times, Failed 3 times)

Stack Traces | 3.73s run time

pubsub_context = <function ros_context at 0x73869a7c37e0>
topic = RawROSTopic(topic='/test_ros_topic', ros_type=<class 'geometry_msgs.msg._vector3.Vector3'>, qos=<rclpy.qos.QoSProfile object at 0x73869a6b2420>)
values = [geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0), geometry_msgs.msg.Vector3(x=4.0, y=5.0, z=6.0), geometry_msgs.msg.Vector3(x=7.0, y=8.0, z=9.0)]

    @pytest.mark.self_hosted
    @pytest.mark.skipif_macos_bug
    @pytest.mark.parametrize("pubsub_context, topic, values", testdata)
    def test_high_volume_messages(
        pubsub_context: Callable[[], Any], topic: Any, values: list[Any]
    ) -> None:
        """Test that all 5k messages are received correctly.
        Limited to 5k because ros transport cannot handle more.
        Might want to have separate expectations per transport later
        """
        with pubsub_context() as x:
            # Create a list to capture received messages
            received_messages: list[Any] = []
            last_message_time = [time.time()]  # Use list to allow modification in callback
    
            # Define callback function
            def callback(message: Any, topic: Any) -> None:
                received_messages.append(message)
                last_message_time[0] = time.time()
    
            # Subscribe to the topic
            x.subscribe(topic, callback)
    
            # Publish 5000 messages
            num_messages = 5000
            for _ in range(num_messages):
                x.publish(topic, values[0])
    
            # Wait until no messages received for 0.5 seconds
            timeout = 2.0  # Maximum time to wait
            stable_duration = 0.1  # Time without new messages to consider done
            start_time = time.time()
    
            while time.time() - start_time < timeout:
                if time.time() - last_message_time[0] >= stable_duration:
                    break
                time.sleep(0.1)
    
            # Capture count and clear list to avoid printing huge list on failure
            received_len = len(received_messages)
            received_messages.clear()
>           assert received_len == num_messages, f"Expected {num_messages} messages, got {received_len}"
E           AssertionError: Expected 5000 messages, got 4604
E           assert 4604 == 5000

_          = 4999
callback   = <function test_high_volume_messages.<locals>.callback at 0x73860720cea0>
last_message_time = [1781375702.5274153]
num_messages = 5000
pubsub_context = <function ros_context at 0x73869a7c37e0>
received_len = 4604
received_messages = [geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0), geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0), geometry_msgs.msg.Vec....0, y=2.0, z=3.0), geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0), geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0), ...]
stable_duration = 0.1
start_time = 1781375700.2611203
timeout    = 2.0
topic      = RawROSTopic(topic='/test_ros_topic', ros_type=<class 'geometry_msgs.msg._vector3.Vector3'>, qos=<rclpy.qos.QoSProfile object at 0x73869a6b2420>)
values     = [geometry_msgs.msg.Vector3(x=1.0, y=2.0, z=3.0), geometry_msgs.msg.Vector3(x=4.0, y=5.0, z=6.0), geometry_msgs.msg.Vector3(x=7.0, y=8.0, z=9.0)]
x          = <dimos.protocol.pubsub.impl.rospubsub.RawROS object at 0x738607223320>

.../protocol/pubsub/test_spec.py:325: AssertionError

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