Pim/feat/g1 reachability#2477
Draft
Nabla7 wants to merge 4 commits into
Draft
Conversation
❌ 2 Tests Failed:
View the top 2 failed test(s) by shortest run time
To view more test analytics, go to the Test Analytics Dashboard |
Nabla7
force-pushed
the
pim/feat/g1-reachability
branch
from
059b829 to
c6dbdc8
Compare
EntityDescriptor (what an entity is) + EntityStateBatch (where every entity is): an authority-agnostic wire contract between whatever owns physics truth (sim today, perception later) and whatever mirrors the scene (planning worlds, visualizers, recorders). Versioned, length-prefixed JSON over LCM — the EntityMarkers pattern — so Rust/browser consumers hand-decode it without generated lcm_msgs types; wire tests pin the schema. msg_name is namespaced simulation_msgs.* (LCM channel keys derive from it) ahead of the type's eventual move out of experimental. entity_scene composes scene entities into an MjSpec — cooked collision hulls when the package provides them, AABB boxes otherwise. The mujoco lint stub gains the spec/kinematics surface this and downstream consumers use.
A second WorldSpec implementation: MuJoCo as a kinematics/collision library (never stepped). MjData scratch contexts are ~62 us to create vs Drake's ~100 ms plant clones; collision checks scope to the planned joints' moving subtree so a humanoid's feet-on-floor contacts don't veto arm plans. RRT-Connect and JacobianIK run on it unchanged; the parity suite pins FK/Jacobian/collision/IK agreement against DrakeWorld. g1_gear_wbc.xml loses its embedded scene (floor, manip table/cube): a planning world attaching the robot file would inherit that geometry at the robot's base pose. The sim now loads g1_gear_wbc_scene.xml — ground and office via include, no manipulables; props come from scene packages. g1_urdf ships the meshes both consumers reference.
ManipulationModule grows world_backend/scene_package config and an entity_states port: scene-entity poses stream from the physics authority into the planning world, so plans route around obstacles where they are now, not where the scene said they started. The G1 catalog (mujoco backend only — DrakeWorld lacks grasp-offset/floating-base support) registers dual arms as views onto one shared MJCF; WorldSpec.add_robot gains the optional share_model_with capability (DrakeWorld raises).
RM4D-style capability map in a gravity-aligned, heading-quotiented pelvis frame, extended with an explicit in-plane wrist-roll dimension (the G1's +/-92.5 deg wrist_yaw breaks RM4D's 4D collapse: measured false-positive rate 4.9% for the 5D map vs 13.9% for the 4D marginal at equal recall). Construction FK-samples the same MJCF the sim uses and rejects self-colliding configurations; an IK-verified evaluation harness reports map accuracy. The viser viewer renders the body-frame workspace red-to-green by dexterity with slice planes and a live drag-to-reach IK ghost (collision-aware, search streamed to the ghost). New [ik] extra: daqp + mink (pinned: 1.1.1+ raises the mujoco floor) + viser. mypy overrides for the untyped ik/viz deps.
Nabla7
force-pushed
the
pim/feat/g1-reachability
branch
from
c6dbdc8 to
a37d624
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Problem
Manipulation planning is currently Drake-only and cannot serve the G1 properly:
Closes DIM-XXX
Solution
This PR adds four layers, split as one commit each.
1. Scene-entity contract
Adds
EntityDescriptorfor what an entity is, andEntityStateBatchfor where everything is now.This gives us a versioned wire format between whatever owns physics truth — sim today, perception later — and whatever mirrors the scene. It uses JSON-over-LCM, following the
EntityMarkerspattern, so Rust/browser consumers can hand-decode it. Wire tests pin the schema.entity_sceneconverts descriptors intoMjSpeccollision bodies, using cooked hulls when available and AABB boxes otherwise.2.
MujocoWorldAdds a second
WorldSpecbackend using MuJoCo purely as a kinematics/collision library. It is never stepped.Key points:
DrakeWorld.3. Wiring
ManipulationModulenow has:entity_statesport, so live poses flow into the planning world;world_backendconfig option.This also adds a new G1 catalog entry:
DrakeWorlddoes not support the required grasp offsets or floating base behavior, so it raises instead of silently aiming IK at the wrist.4. Reachability
Adds a G1 arm capability map in a gravity-aligned, heading-quotiented pelvis frame.
This is valid because the WBC gives us a true SE(2) base. The map is 5D rather than RM4D’s 4D because the G1’s ±92.5° wrist yaw breaks the 4D collapse.
Measured results:
Both are compared at equal recall and IK-verified.
Also included:
New Extras
Adds a new
[ik]extra:daqpminkviserminkis pinned because1.1.1+forces MuJoCo3.5 -> 3.9.Out of Scope
Staged next:
scene_packagehook here is its first consumer;Today’s producer is the
pimsimbranch. The new port is exercised by tests.How to Test
Install dependencies:
Run the test suite:
uv run pytest \ dimos/manipulation/planning \ dimos/manipulation/reachability \ dimos/experimental/pimsim \ dimos/simulation/mujoco/test_entity_scene.py \ -m "mujoco or not mujoco"Expected: 48 tests.
Note: the parity suite needs
pydrakeon Linux.Build a reachability map and explore it:
Reproduce the accuracy numbers:
Sim regression check:
Run the G1 GR00T WBC MuJoCo sim as before. The world should be identical, minus the floating manipulation table/cube.
Contributor License Agreement