End-to-end test: spatial memory navigation in MuJoCo

[paul-nechifor]

1. Runs dimos --simulation run unitree-go2-agentic
2. Tells the robot to go to the bookcase and then move far away from it (this is accomplished by publishing goals to /goal_request).
3. Tells the robot "go to the bookcase" through the human cli.
4. Asserts that the robot as arrived within 1 meter of the target position.

NOTE: This test is currently extremely flaky because of step 2. Navigation is very unreliable. It often gets close to a target point and fails to realize it reached the goal. I'm currently fixing this.

Run with:

pytest -vvsx dimos/e2e_tests/test_spatial_memory.py

The test itself is quite small, most is done in fixtures:

    start_blueprint("unitree-go2-agentic")

    lcm_spy.save_topic("/rpc/HumanInput/start/res")
    lcm_spy.wait_for_saved_topic("/rpc/HumanInput/start/res", timeout=120.0)
    lcm_spy.save_topic("/agent")
    lcm_spy.wait_for_saved_topic_content("/agent", b"AIMessage", timeout=120.0)

    time.sleep(5)

    follow_points(
        points=[
            # Navigate to the bookcase.
            (1, 1, 0),
            (4, 1, 0),
            (4.2, -1.1, -math.pi / 2),
            (4.2, -3, -math.pi / 2),
            (4.2, -5, -math.pi / 2),
            # Move away, until it's not visible.
            (1, 1, math.pi / 2),
        ],
        fail_message="Failed to get to the bookcase.",
    )

    time.sleep(5)

    human_input("go to the bookcase")

    lcm_spy.wait_until_odom_position(4.2, -5)

[chatgpt-codex-connector]

Codex usage limits have been reached for code reviews. Please check with the admins of this repo to increase the limits by adding credits.
Credits must be used to enable repository wide code reviews.

[greptile-apps]

_{5 files reviewed, 7 comments}

_{Edit Code Review Agent Settings | Greptile}

[paul-nechifor]

Merged into #885