feat(manip): plan against a live voxel map from a wrist camera by mustafab0 · Pull Request #3714 · dimensionalOS/dimos

mustafab0 · 2026-08-27T01:30:17Z

A wrist camera builds an occupancy map of the workspace; the planner treats those voxels as one octree obstacle.

Stacked on #3712 (the voxel_clear_mask crate port)

The pipeline

wrist RGB-D ──▶ PointCloudSelfFilter ──▶ RayTracingVoxelMap ──▶ GlobalMapObstacleBridge
                  │ (URDF + TF)              (voxel 0.05, world)      │
                  └──▶ voxel_clear_mask ────▶┘                        ▼
                                                        Obstacle(OCTREE, points, res)
                                                          └─▶ roboplan addOcTreeGeometry

`ObstacleType.OCTREE`

Occupied cell centers plus a resolution, into roboplan's addOcTreeGeometry.

Files: 12 (11 planned plus test_roboplan.py

codecov · 2026-08-27T01:38:21Z

❌ 2 Tests Failed:

Tests completed	Failed	Passed	Skipped
4824	2	4822	87

View the top 1 failed test(s) by shortest run time

dimos.e2e_tests.test_manipulation_planning_groups::test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator

Stack Traces | 120s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718e7995bc50>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x718e780dda80>

    def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator(
        lcm_spy: LcmSpy,
        start_blueprint: Callable[..., DimosCliCall],
    ) -> None:
        """Plan one generated plan over both arms and dispatch through one trajectory task."""
>       _start_openarm_mock_planner(start_blueprint, lcm_spy)

lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718e7995bc50>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x718e780dda80>

dimos/e2e_tests/test_manipulation_planning_groups.py:190: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/test_manipulation_planning_groups.py:151: in _start_openarm_mock_planner
    lcm_spy.wait_for_saved_topic(JOINT_STATE_TOPIC, timeout=120.0)
        lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718e7995bc50>
        start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x718e780dda80>
dimos/e2e_tests/lcm_spy.py:97: in wait_for_saved_topic
    wait_until(
        condition  = <function LcmSpy.wait_for_saved_topic.<locals>.condition at 0x718e780de3e0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718e7995bc50>
        timeout    = 120.0
        topic      = '/coordinator_joint_state#sensor_msgs.JointState'
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

predicate = <function LcmSpy.wait_for_saved_topic.<locals>.condition at 0x718e780de3e0>

    def wait_until(
        predicate: Callable[[], bool],
        *,
        timeout: float,
        interval: float = 0.1,
        message: str | None = None,
    ) -> None:
        """Poll ``predicate`` until it returns truthy or ``timeout`` elapses."""
        deadline = time.monotonic() + timeout
        while time.monotonic() < deadline:
            if predicate():
                return
            time.sleep(interval)
>       raise TimeoutError(message or f"Timed out after {timeout}s waiting for condition")
E       TimeoutError: Timeout waiting for topic /coordinator_joint_state#sensor_msgs.JointState

deadline   = 4289699.597411995
interval   = 0.1
message    = 'Timeout waiting for topic /coordinator_joint_state#sensor_msgs.JointState'
predicate  = <function LcmSpy.wait_for_saved_topic.<locals>.condition at 0x718e780de3e0>
timeout    = 120.0

.../utils/testing/waiting.py:35: TimeoutError

View the full list of 1 ❄️ flaky test(s)

dimos.e2e_tests.test_manipulation_planning_groups::test_single_arm_plans_and_executes_through_control_coordinator

Flake rate in main: 7.69% (Passed 12 times, Failed 1 times)

Stack Traces | 120s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718f7d7b7170>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x718e780dd4e0>

    def test_single_arm_plans_and_executes_through_control_coordinator(
        lcm_spy: LcmSpy,
        start_blueprint: Callable[..., DimosCliCall],
    ) -> None:
        """Plan with one arm and execute through its trajectory task."""
>       _start_openarm_mock_planner(start_blueprint, lcm_spy)

lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718f7d7b7170>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x718e780dd4e0>

dimos/e2e_tests/test_manipulation_planning_groups.py:159: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/test_manipulation_planning_groups.py:151: in _start_openarm_mock_planner
    lcm_spy.wait_for_saved_topic(JOINT_STATE_TOPIC, timeout=120.0)
        lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718f7d7b7170>
        start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x718e780dd4e0>
dimos/e2e_tests/lcm_spy.py:97: in wait_for_saved_topic
    wait_until(
        condition  = <function LcmSpy.wait_for_saved_topic.<locals>.condition at 0x718e780dc9a0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x718f7d7b7170>
        timeout    = 120.0
        topic      = '/coordinator_joint_state#sensor_msgs.JointState'
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

predicate = <function LcmSpy.wait_for_saved_topic.<locals>.condition at 0x718e780dc9a0>

    def wait_until(
        predicate: Callable[[], bool],
        *,
        timeout: float,
        interval: float = 0.1,
        message: str | None = None,
    ) -> None:
        """Poll ``predicate`` until it returns truthy or ``timeout`` elapses."""
        deadline = time.monotonic() + timeout
        while time.monotonic() < deadline:
            if predicate():
                return
            time.sleep(interval)
>       raise TimeoutError(message or f"Timed out after {timeout}s waiting for condition")
E       TimeoutError: Timeout waiting for topic /coordinator_joint_state#sensor_msgs.JointState

deadline   = 4289579.43155438
interval   = 0.1
message    = 'Timeout waiting for topic /coordinator_joint_state#sensor_msgs.JointState'
predicate  = <function LcmSpy.wait_for_saved_topic.<locals>.condition at 0x718e780dc9a0>
timeout    = 120.0

.../utils/testing/waiting.py:35: TimeoutError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

greptile-apps · 2026-08-27T01:39:59Z

Greptile Summary

This change adds a bridge that installs complete global voxel-map snapshots in the planning world under a stable obstacle ID, with related octree, point-cloud filtering, and robot transform support.

The bridge currently treats a rejected planning-world update as successful. When the planning interface returns False, the newest map is not applied and the bridge exits instead of retrying; collision planning can therefore continue using an older map until a later snapshot arrives.

T-Rex validation blocked

The focused runtime check could not reach the bridge's async handling path because the local Python environment was missing the plum package required during import. The missing package prevented execution of the false-return scenario.

Confidence Score: 4/5

Not safe to merge until a rejected voxel-obstacle update is routed through retry handling.

The boolean result is explicitly part of the planning interface contract, is discarded by the bridge, and only exceptions reach retry handling. The intended runtime reproduction was blocked during module import, so the conclusion is supported by direct control-flow inspection rather than a completed execution.

Files Needing Attention: dimos/manipulation/planning/global_map_obstacle_bridge.py needs to convert a false planning update result into retryable failure; dimos/manipulation/planning/test_global_map_obstacle_bridge.py should cover a planning fake that returns false.

T-Rex Logs

What T-Rex did

T-Rex authored the focused script named Global-map-false-return-repro.py to inject a planning fake that returns False, set the retry delay to zero, and await handle_global_map.
The run of the script was attempted, but it halted during module import with a ModuleNotFoundError: No module named 'plum' from dimos.msgs.geometry_msgs.Pose.
T-Rex produced a proof for the posted P1 finding.

_{Ran code and verified through T-Rex}

_{Reviews (1): Last reviewed commit: "feat(manip): plan against a live voxel m..." | Re-trigger Greptile}

leshy · 2026-08-27T06:11:05Z

started commenting but realizing this is very arm specific so probably should keep in manipulation/perception or something like this

Manipulation becomes the first non-navigation consumer of the ray-tracing voxel map. A wrist camera builds an occupancy map of the workspace, and the planner treats those voxels as one octree obstacle. Three producers, all model-driven so they work for any URDF: - RobotTfPublisher resolves a complete TF tree from measured joint state. A manipulator's sensor pose comes from forward kinematics, not SLAM, and main had no generic equivalent - only a G1-specific publisher. An incomplete joint state publishes nothing rather than leaving links at a stale pose. - PointCloudSelfFilter cuts the robot out of its own camera's cloud, and emits the map cells the robot occupies as a clear mask. It emits the previous frame's cells too: a link that moved leaves a ghost behind it that ray tracing can never clear, because the link occluded that volume while it was there. The mask floors world coordinates by voxel_size exactly as the mapper does, or it would name cells the map does not hold. - GlobalMapObstacleBridge reconciles each complete map snapshot into the planning world under one stable obstacle ID, so a republished map replaces the obstacle instead of piling up new ones. ObstacleType.OCTREE carries occupied cell centers plus a resolution into roboplan's addOcTreeGeometry. Coal reads a cell as center, edge length, cost and occupancy threshold; every cell here is occupied, so cost is 1.0 against the 0.5 default. Verified against roboplan 0.5.1 and the pinned 0.6.1: adding cells flips a collision-free configuration to colliding and blocks a path through them, while a clear path stays clear. Obstacle.points is a tuple of tuples, not an ndarray. Obstacles are deepcopied into world snapshots, compared, and pickled across worker RPC, and an ndarray field breaks all three. The shape-string obstacle RPCs cannot carry a map, so ManipulationModule gains set_voxel_obstacle alongside them. Additive: no existing caller changes. An empty point list removes the obstacle, which is how a mapper says the space it owns is clear. Validation caps an octree at 200k points, since every point crosses worker RPC as a pickled tuple and an unbounded map would stall the pipe. Depends on the voxel_clear_mask port (#3712) for the mask to land anywhere, and on the TFLookup forward_tolerance keyword (#3711) for the self filter to wait on a wrist transform still in flight.

The bridge retried a failed snapshot in a loop, which I carried over without checking how these handlers are dispatched. They run through a single-slot latest mailbox, one at a time: while the loop retried, newer maps could not reach the planner, and on eventual success it installed geometry that no longer described the world. The mapper republishes a complete map every frame, so a failed snapshot is already superseded by the time a retry could fire. Log it and drop it. Also hand numpy's tolist() to the RPC rather than building a tuple per point in python; this runs at map cadence.

The map reached the planning world through a module whose only job was to forward it: subscribe to a cloud, call an RPC. That is an adapter, and naming it a bridge did not make it one less. Worse, the hop dictated the design. WorldMonitor is a plain class inside ManipulationModule, so from a separate module the only way in is the RPC surface - which meant pushing a whole occupancy map through the worker pipe as a pickle. The point budget and the tuple-per-point conversion existed to make that survivable. Neither is needed now: an In port carries the cloud over the normal transport, which is built for payloads this size. ManipulationModule takes voxel_map: In[PointCloud2] directly. Each message is a complete map, so it replaces one octree obstacle under a stable id rather than accumulating; an empty cloud removes it. The port's async handler gives latest-wins coalescing and runs off the transport thread, so building the octree cannot stall other port callbacks. The octree itself is not something this module holds. It comes into existence inside roboplan's Scene via addOcTreeGeometry, and coal's OcTree is a binding object that cannot be pickled or published - so there is no earlier stage that could produce one. Nulling a port drops it from Module.inputs, which is how the test harnesses already avoid binding coordinator_joint_state; voxel_map follows the same pattern.

…y does it RobotTfPublisher resolved a TF tree from joint state through yourdfpy. That is what ManipulationModule._tf_publish_loop has been doing since #1236: for every name in RobotModelConfig.tf_extra_links it takes the link pose from the planning world's forward kinematics and publishes world -> link at 10 Hz. So the self filter does not need a new module, it needs the arm's collision links listed in tf_extra_links. Listing them is a blueprint concern, and the demo blueprint is a follow-up. Also fixes two things the rebase onto #3431 left behind: RobotModelConfig lost its name field, and _make_world returns a world rather than a tuple.

Co-authored-by: Paul Nechifor <paul@nechifor.net>

Two review findings. paul-nechifor: the filter_config property was noise. The class already declares config: PointCloudSelfFilterConfig, so self.config is typed without it. Removed, five call sites read self.config directly. greptile: update_obstacle returns False and add_obstacle returns "" when the planning world declines, and both results were discarded. The map silently did not land and planning carried on against the previous one with nothing to say so. Now logged. Not retried: the mapper republishes a complete map every frame, so a superseded snapshot is not worth reinstalling. The finding was raised against the obstacle bridge, which is gone; the same discard had followed the code into the port handler.

One asserted a warning was logged, which is not behaviour. The other called _apply_voxel_map with no world monitor and asserted nothing at all.

PointCloudSelfFilter is a processing module wired into someone else's blueprint, not a runnable blueprint or a skill container, so the module-level alias had nothing to expose. Nothing imported it and it never reached all_blueprints.py. It was also the only one of its kind left in the repo.

Three CI failures, all in the self filter. Primitive collision shapes were being measured with trimesh.proximity.signed_distance, which needs rtree. rtree is not declared in pyproject and is only present here as a transitive dependency, so this passed locally and failed in CI with ModuleNotFoundError. Both call sites now go through one _points_inside: box, sphere and cylinder answer analytically, and only a real mesh element reaches trimesh.proximity. Verified by blocking rtree at the import hook, where the tests now pass. That import failure was also what left modules half-constructed and tripped the non-closed-thread check; teardown drops from 5s to 0.05s with it gone. _ = self.tfbuffer tripped the no-underscore-assignment check. The point is the side effect of touching the property, so touch it. all_blueprints.py regenerated: the module registers itself, independent of the module-level alias that was removed earlier.

planning_frame and voxel_map_frame were both mine and both wrong; the repo says world_frame in 119 places.

object means "has no attributes", so every attribute access needed a type: ignore to get past it. Any says the same thing about our knowledge without the noise. Fifteen ignores down to three, and the three left are an untyped import and two untyped trimesh calls. The config annotation did not need its ignore either; 119 other modules declare config: SomeConfig plainly.

It takes a manipulation RobotModelConfig, so it is not the general self-filter perception would want. Parked next to the planning utils that already handle robot geometry until perception has a model-agnostic design for this. Also dropped a mask test the previous-position test already covers.

It only ever wanted a robot model, not manipulation's RobotModelConfig, so the config takes a RobotModel from dimos.robot.assets directly. prepare_urdf_for_drake goes with it. That strips transmission blocks and converts DAE/STL to OBJ, both for Drake's benefit; yourdfpy parses transmission blocks without complaint and trimesh reads DAE and STL natively. No manipulation imports left. Staying under planning/utils for now — where a general self filter should live is a perception question.

tf is an ordinary input, so build the buffer over it in start() rather than relying on the lazy tfbuffer property constructing itself as a side effect of the first lookup — which is both odd to read and cold exactly when the first cloud needs it. TF takes the port directly, which is all tfbuffer was doing. _tf is the conventional slot for it, so the base class still disposes it.

mustafab0 requested review from Dreamsorcerer, aclauer, leshy, paul-nechifor and spomichter as code owners August 27, 2026 01:30

mustafab0 mentioned this pull request Aug 27, 2026

feat(manipulation): add live voxel collision planning #3163

Draft

1 task

greptile-apps Bot reviewed Aug 27, 2026

View reviewed changes

Comment thread dimos/manipulation/planning/global_map_obstacle_bridge.py Outdated

mustafab0 force-pushed the mustafa/feat/manip-voxel-planning branch 2 times, most recently from 0b4faf3 to 4be41fb Compare August 27, 2026 02:28

paul-nechifor reviewed Aug 27, 2026

View reviewed changes

Comment thread dimos/perception/test_point_cloud_self_filter.py Outdated

paul-nechifor reviewed Aug 27, 2026

View reviewed changes