G1 Manipulation Stack: Planning World, Arm Control, Reachability

G1 Manipulation Stack: Planning World, Arm Control, Reachability

Problem

The G1 currently has no manipulation interface. End-effector control is not wired into the coordinator, and the planning stack, including IK and RRT, depends on Drake.

That creates three problems: Drake introduces a second asset pipeline next to our MJCF and scene packages, world alignment depends on fragile odom-sync, and IK currently pays roughly 100 ms for context creation in the hot path. We also do not yet have a clear picture of what the G1 arms can actually reach.

Proposal

The proposal is to add three layers that slot into the existing coordinator. The GR00T WBC remains untouched: it owns legs and waist, while the 14 arm joints remain free.

1. MujocoWorld

Add a WorldSpec backend using MuJoCo as a kinematics and collision library. MuJoCo is never stepped; it is used only for FK, collision queries, and planning-world state.

This backend uses the same robot MJCF and cooked scene packages as the simulator. Scene objects sync through /entity_state_batch, so planning works the same way under simulation and real-robot perception.

The existing RRT-Connect and Jacobian IK code should run unchanged on top of this backend. Drake leaves the online loop.

2. mink arm task

Add a coordinator task that claims the 14 arm joints at priority 20 and runs [mink](https://github.com/kevinzakka/mink) QP differential IK for both arms in one solve.

The task should support joint limits, velocity limits, self-collision avoidance, and posture regularization. Cartesian targets are exposed through the existing command surface.

This layer covers teleoperation, servoing, and trajectory tracking. The global planner from layer 1 remains responsible for clutter and longer-horizon planning.

3. Reachability analysis

Build a fixed-base reachability map for each arm with the robot standing still. The map is generated by FK-sampling the 7-DOF arm on the same MJCF, with the waist fixed at its default pose and self-collision checking enabled.

Cells should be scored rather than treated as purely binary. The main deliverables are standard reachability plots: 2D workspace slices where green means reachable, red means unreachable, and intermediate colors represent the reachability score.

The reachability layer should also provide a query API and an IK-verified accuracy report. This gives us ground truth on what the arms can do before adding locomotion or stance selection.

Phasing

1. Build MujocoWorld core and Drake-parity tests.
2. Add the mink arm task. This is independent of layer 1 and can be developed in parallel.
3. Wire entity sync, config, and flip G1 simulation flows off Drake.
4. Build the fixed-base reachability map and plots.
5. Later, add stance selection by inverting the reachability map to propose pelvis (x, y, heading) for a target pose, IK-verified and feeding into loco-manipulation.

Out of Scope

Stance selection and base placement are out of scope for this issue and should become a separate phase once the fixed-base stack is working.

Planner upgrades such as OMPL 2.0 or VAMP are also out of scope unless RRT latency actually becomes a blocker. WBC and policy changes are out of scope, as are dexterous hands.

Viewer integration is not part of this issue either. Planning previews in Babylon can stay on pim/dev for now.

---

Synced from DIM-1008 by summer