feat(manipulation): arm reachability capability map + viser viewer#2477
feat(manipulation): arm reachability capability map + viser viewer#2477Nabla7 wants to merge 3 commits into
Conversation
Codecov Report❌ Patch coverage is @@ Coverage Diff @@
## main #2477 +/- ##
==========================================
- Coverage 69.97% 69.72% -0.25%
==========================================
Files 842 846 +4
Lines 74420 75370 +950
Branches 6668 6777 +109
==========================================
+ Hits 52077 52554 +477
- Misses 20636 21124 +488
+ Partials 1707 1692 -15
Flags with carried forward coverage won't be shown. Click here to find out more.
... and 13 files with indirect coverage changes 🚀 New features to boost your workflow:
|
Nabla7
force-pushed
the
pim/feat/g1-reachability
branch
3 times, most recently
from
a37d624 to
81d32be
Compare
Nabla7
changed the title
Nabla7
force-pushed
the
pim/feat/g1-reachability
branch
2 times, most recently
from
9763050 to
b22fe65
Compare
Nabla7
changed the title
Nabla7
force-pushed
the
pim/feat/g1-reachability
branch
6 times, most recently
from
cbd95b3 to
ce11dcc
Compare
A reachability capability map for robot arms: FK-sample an arm to build a dexterity grid in the base frame, mink-IK-verify it, and view it in viser. Self-contained — loads the arm model directly and does its own FK/IK, with no dependency on the sim/world planning stack or the entity-scene contract. Robot-agnostic via a small registry (reachability/robots.py): MJCF or URDF models (URDF loaded collision-only with package:// expanded), workspace grids auto-sized per arm, and per-arm exclusions for constant structural mesh overlaps. Ships six arms as examples — g1-left/right, xarm7, piper, a750 (URDF), openarm (URDF) — selected by construct/evaluate/viewer --robot <key>. Adds the 'ik' extra (daqp, mink==1.1.0, viser[urdf]) and mujoco MjSpec type stubs.
Nabla7
force-pushed
the
pim/feat/g1-reachability
branch
from
ce11dcc to
829789c
Compare
`_REGISTRY` built its `package_roots` with `str(LfsPath(...))`, and `str()` forces LfsPath's lazy download eagerly — so merely importing `robots` pulled `g1_urdf.tar.gz` (the first entry). That fails on the LFS-guarded main CI runner (bin/git-lfs-guard), taking down every test that imports the module, including `test_registry_is_consistent`, a pure registry check that needs no assets. - Store `LfsPath` objects in `package_roots` (compile_model stringifies them at compile time, so the pull stays deferred); widen to `dict[str, str | Path]`. - Mark `test_registered_model_compiles` `self_hosted`: it compiles real models pulled from the self-hosted LFS, so it belongs on the self-hosted CI job, not the main runner where git-lfs is guarded. Main job: registry + pure-logic tests pass, the 6 compiles deselected. Self-hosted job: the 6 compiles pull + compile + pass (verified locally).
Nabla7
requested review from
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Greptile SummaryThis PR introduces a robot-agnostic reachability capability map for arm manipulation: FK-sampling millions of random configurations builds a 5D voxel grid (
Confidence Score: 4/5Safe to merge. The capability-map math, canonical-value transform, and saturating merge are well-tested; the new issues are limited to a robustness gap in URDF injection and two low-probability viewer threading edge cases. The core map construction, canonical-value transform, body-frame dexterity, mirror symmetry, and save/load are all thoroughly tested and the math checks out. The three findings are in the URDF compile helper and the interactive viewer: duplicate
Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant CLI as construct CLI
participant AS as arm_spec / _autosize
participant W as _worker (spawn)
participant S as _ArmSampler (FK+collision)
participant CM as CapabilityMap
participant NPZ as .npz file
CLI->>AS: arm_spec(robot) — probe 30K samples if no fixed params
AS-->>CLI: ConstructionSpec (with sized MapParams)
CLI->>W: pool.map(_worker, jobs) × n_workers
W->>S: _ArmSampler(spec)
loop per chunk (50K configs)
W->>S: sample_chunk(n, rng)
S->>S: mj_kinematics + mj_collision
S-->>W: (positions, rotations, rejected)
W->>CM: record_batch → saturating uint8 counts + heading_hint + body_counts + body_theta_mask
end
W-->>CLI: (CapabilityMap, rejected)
CLI->>CM: merge workers (OR hints, clip counts to 255)
CM->>NPZ: save(.npz)
Note over CLI,NPZ: O(1) query path
CM->>CM: scores(positions, rotations) — canonical_values → 5D index lookup
CM->>CM: body_dexterity() — bitcount(body_theta_mask) / n_theta
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
participant CLI as construct CLI
participant AS as arm_spec / _autosize
participant W as _worker (spawn)
participant S as _ArmSampler (FK+collision)
participant CM as CapabilityMap
participant NPZ as .npz file
CLI->>AS: arm_spec(robot) — probe 30K samples if no fixed params
AS-->>CLI: ConstructionSpec (with sized MapParams)
CLI->>W: pool.map(_worker, jobs) × n_workers
W->>S: _ArmSampler(spec)
loop per chunk (50K configs)
W->>S: sample_chunk(n, rng)
S->>S: mj_kinematics + mj_collision
S-->>W: (positions, rotations, rejected)
W->>CM: record_batch → saturating uint8 counts + heading_hint + body_counts + body_theta_mask
end
W-->>CLI: (CapabilityMap, rejected)
CLI->>CM: merge workers (OR hints, clip counts to 255)
CM->>NPZ: save(.npz)
Note over CLI,NPZ: O(1) query path
CM->>CM: scores(positions, rotations) — canonical_values → 5D index lookup
CM->>CM: body_dexterity() — bitcount(body_theta_mask) / n_theta
Reviews (1): Last reviewed commit: "Merge branch 'main' into pim/feat/g1-rea..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| text = re.sub( | ||
| r"(<robot\b[^>]*>)", | ||
| r'\1<mujoco><compiler strippath="false" discardvisual="true"/></mujoco>', | ||
| text, | ||
| count=1, | ||
| ) |
There was a problem hiding this comment.
<mujoco>…</mujoco> after the <robot> opening tag without first checking whether the file already contains a <mujoco> element. Any URDF that embeds its own MuJoCo compiler hints (e.g., a community model that ships with strippath or mesh-directory overrides) will end up with two <mujoco> blocks, and MuJoCo's XML parser raises a compile error that is hard to diagnose. A pre-check or conditional replacement prevents the conflict.
| text = re.sub( | |
| r"(<robot\b[^>]*>)", | |
| r'\1<mujoco><compiler strippath="false" discardvisual="true"/></mujoco>', | |
| text, | |
| count=1, | |
| ) | |
| if "<mujoco>" not in text: | |
| text = re.sub( | |
| r"(<robot\b[^>]*>)", | |
| r'\1<mujoco><compiler strippath="false" discardvisual="true"/></mujoco>', | |
| text, | |
| count=1, | |
| ) |
| for name in ("/reachability/core", "/reachability/points"): | ||
| try: | ||
| server.scene.remove_by_name(name) | ||
| except Exception: | ||
| pass |
There was a problem hiding this comment.
except Exception: pass silences every possible failure, not just the "node does not exist" case. Viser can raise other exceptions (e.g., serialization errors, transport failures) that would be silently swallowed here and in refresh_slices, leaving the scene in a stale state with no indication of what went wrong. Narrowing the guard to the expected no-such-node error (or at minimum logging the unexpected ones) makes failures observable.
| def solve_current_pose() -> bool: | ||
| """One solve at the gizmo's current pose; False if it failed.""" | ||
| if gizmo is None: | ||
| return True | ||
| cap = current_map() | ||
| cap_robot = cap.robot or f"g1-{cap.side}" | ||
| if cap_robot not in solvers: | ||
| try: | ||
| solvers[cap_robot] = ArmIK(cap_robot) | ||
| except Exception as e: | ||
| solvers[cap_robot] = None | ||
| logger.warning(f"IK unavailable: {e}") | ||
| solver = solvers[cap_robot] | ||
| if solver is None: | ||
| ik_status.value = "IK unavailable (pip install 'dimos[ik]')" | ||
| return True | ||
| position = np.asarray(gizmo.position, dtype=np.float64) | ||
| wxyz = np.asarray(gizmo.wxyz, dtype=np.float64) |
There was a problem hiding this comment.
gizmo between IK worker and viser callback
solve_current_pose checks if gizmo is None: return True on line 573, then reads gizmo.position and gizmo.wxyz on lines 587–588. The viser callback thread can call refresh_ik between those two points and set gizmo = None (line 549), causing an AttributeError. In practice the outer except Exception in ik_worker catches it, so the viewer survives, but the IK thread silently stops until the next wakeup event. A second if gizmo is None guard immediately before the attribute accesses closes the window.
1 participant
Problem
A robot arm needs a fast, offline answer to "can it reach this pose, and how dexterously?" — for grasp planning, base placement, and feasibility ghosts in the viser GUI — without paying an IK solve per query.
Solution
A reachability capability map.
constructFK-samples an arm over millions of configurations and bins the end-effector poses into a voxel grid in the base frame, recording which orientations are reachable per cell as a dexterity score — a single.npzthat answers reachability/dexterity in O(1).evaluatescores a built map against a mink solve-to-convergence IK oracle;viewerrenders it in viser, colored by dexterity, with the robot posed at the map origin.It's robot-agnostic: a small registry (
reachability/robots.py) maps a--robotkey to a MuJoCo-loadable model — MJCF or URDF (URDF loaded collision-only withpackage://expanded) — with per-arm workspace auto-sizing and exclusions for constant structural mesh overlaps. Ships six arms as examples: g1-left/right, xarm7, piper, a750 (URDF), openarm (URDF). Self-contained — loads the model directly and runs its own FK/IK, with no dependency on the planning/world stack.How to Test
# build + view a map for any registered arm (xarm7 shown) uv run --extra ik python -m dimos.manipulation.reachability.construct --robot xarm7 --samples 200000 uv run --extra ik python -m dimos.manipulation.reachability.viewer --map data/reachability/xarm7_capability.npz--robotacceptsg1-left,g1-right,xarm7,piper,a750,openarm.Contributor License Agreement