fix(g1): enable MuJoCo navigation with GR00T policy

[raulbastidas1203]

Summary

• Fix unitree-g1-basic-sim navigation wiring by adding MovementManager, so keyboard teleop and map-selected goals are muxed into the cmd_vel stream consumed by G1SimConnection.
• Add an opt-in GR00T ONNX locomotion policy for Unitree G1 MuJoCo simulation via --mujoco-g1-policy groot.
• Filter the simulated G1's own body points from MuJoCo lidar before publishing the point cloud, so click-to-nav does not immediately cancel due to self-obstacles in the costmap.
• Keep existing behavior as the default (mujoco_g1_policy=default) and add tests for blueprint wiring, policy selection, and G1 self-point filtering.

Why

In G1 MuJoCo simulation, keyboard commands and clicked map goals could publish upstream, but the basic sim stack did not include the movement manager that bridges tele_cmd_vel / nav_cmd_vel into cmd_vel. As a result, the robot did not follow keyboard or map navigation commands as expected.

After wiring the movement manager, keyboard teleop worked, but map-selected navigation could still stop early because the MuJoCo depth/lidar output could include parts of the humanoid itself. Those self-points polluted the global map/costmap, causing the local planner to report an obstacle ahead and cancel/replan.

The GR00T policy is included as an optional simulation locomotion backend because it tracks velocity commands better for the humanoid G1 in MuJoCo while preserving the same DimOS navigation interface.

Scope

Simulation only. This PR does not change the real G1 hardware path, and hardware validation is still pending.

How to Test

source .venv/bin/activate
ruff check dimos/core/global_config.py \
  dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py \
  dimos/robot/unitree/g1/blueprints/basic/test_unitree_g1_basic_sim.py \
  dimos/simulation/mujoco/model.py \
  dimos/simulation/mujoco/mujoco_process.py \
  dimos/simulation/mujoco/policy.py \
  dimos/simulation/mujoco/test_model_policy_selection.py \
  dimos/simulation/mujoco/test_mujoco_process.py

pytest dimos/simulation/mujoco/test_mujoco_process.py \
  dimos/robot/unitree/g1/blueprints/basic/test_unitree_g1_basic_sim.py \
  dimos/simulation/mujoco/test_model_policy_selection.py \
  dimos/navigation/movement_manager/test_movement_manager.py \
  dimos/robot/test_all_blueprints_generation.py -q

Manual sim check:

dimos --simulation --mujoco-g1-policy groot run unitree-g1-sim

Verified locally in simulation that:

• keyboard teleop moves the G1;
• publishing a /clicked_point goal produces Found path, path_following, and Arrived at goal;
• lidar/map visualization remains active.

Contributor License Agreement

• I have read and approved the CLA.

[greptile-apps]

Greptile Summary

This PR fixes the G1 MuJoCo simulation navigation stack by wiring MovementManager so that keyboard teleop and click-to-nav commands are both muxed into the cmd_vel stream consumed by G1SimConnection. It also adds an optional GR00T ONNX locomotion policy, filters the G1's own body from MuJoCo lidar depth output, and provides comprehensive tests for all three additions.

• Blueprint fix: MovementManager is added to unitree_g1_basic_sim with a remapping to suppress its internal way_point stream (letting ReplanningAStarPlanner own goals), so both teleop and nav commands reach the robot.
• GR00T policy: G1GrootOnnxController switches between a balance and a walk ONNX model based on commanded speed, converts them to torque control by reconfiguring actuator gain/bias params on first call, and maintains a 6-step observation history; the two previously-flagged issues (linear-velocity slot zeros and shape mismatch with model.nu) are now addressed.
• Self-point filter: filter_g1_self_points removes depth points inside the robot's body-frame XY bounding box (world Z 0.04–1.6 m) before publishing the point cloud, preventing self-obstacle costmap pollution that caused premature navigation cancellation.

Confidence Score: 5/5

Safe to merge — all changes are scoped to MuJoCo simulation, hardware paths are untouched, and the two previously-flagged observation-vector issues are resolved.

The navigation wiring fix is minimal and well-tested via blueprint assertions. The GR00T controller correctly addresses both the linear-velocity slot that was previously zero and the actuator-count mismatch; its PD torque math, observation layout, and dual-policy switching are all consistent with the stated design. The self-point filter uses the correct robot-local frame rotation (verified against the yaw-tracking test) and matches MuJoCo's free-joint quaternion convention. No regressions to existing Go1 or default G1 paths were introduced.

No files require special attention.

Important Files Changed

Filename	Overview
dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py	Adds MovementManager with a way_point remapping to suppress its internal goal stream, correctly wiring both keyboard and nav commands into cmd_vel.
dimos/simulation/mujoco/policy.py	Adds G1GrootOnnxController with dual balance/walk ONNX sessions, a 6-step observation history, torque-actuator configuration, and PD output; previously-flagged linear-velocity slot and nu-mismatch issues are resolved.
dimos/simulation/mujoco/mujoco_process.py	Adds filter_g1_self_points using robot-local XY bounding box and absolute world-Z bounds to strip self-points from depth data before publishing the point cloud.
dimos/simulation/mujoco/model.py	Extends load_model to accept a GlobalConfig and select between G1OnnxController, G1GrootOnnxController, or raise ValueError for unknown policy names.
dimos/core/global_config.py	Adds mujoco_g1_policy (default 'default') and mujoco_groot_policy_dir (default 'data/groot') fields to GlobalConfig.
dimos/simulation/mujoco/test_mujoco_process.py	New tests verifying filter_g1_self_points removes body-interior points, keeps floor and scene objects, and correctly tracks robot yaw rotation.
dimos/simulation/mujoco/test_g1_groot_policy.py	Tests for G1GrootOnnxController: validates body-frame linear velocity populates obs[4:7], actuator-count slicing, and the ValueError guard when nu exceeds target angles.
dimos/simulation/mujoco/test_model_policy_selection.py	Integration tests using monkeypatched mujoco confirming default policy is G1OnnxController and groot policy is opt-in via GlobalConfig.
dimos/robot/unitree/g1/blueprints/basic/test_unitree_g1_basic_sim.py	Blueprint wiring tests asserting MovementManager, ReplanningAStarPlanner, and G1SimConnection are all present and that the way_point remapping is in place.

Sequence Diagram

sequenceDiagram
    participant KB as Keyboard Teleop
    participant Nav as ReplanningAStarPlanner
    participant MM as MovementManager
    participant G1 as G1SimConnection
    participant MJ as MuJoCo Process
    participant PC as Point Cloud Publisher

    KB->>MM: tele_cmd_vel
    Nav->>MM: nav_cmd_vel
    MM->>G1: cmd_vel (muxed)
    G1->>MJ: SHM command write

    MJ->>MJ: policy.get_control(model, data)
    Note over MJ: G1GrootOnnxController or G1OnnxController
    MJ->>MJ: filter_g1_self_points(combined_points, base_pos, base_quat)
    MJ->>PC: filtered PointCloud2

    PC->>Nav: /point_cloud (no self-obstacles)
    Nav->>MM: nav_cmd_vel (goal tracking)

Loading

_{Reviews (4): Last reviewed commit: "Merge branch 'main' into feat/g1-mujoco-..." | Re-trigger Greptile}

[raulbastidas1203]

Real G1 hardware validation is still pending, and I’ll try to complete it next week and update the PR with the results.