Skip to content

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

Draft
TomCC7 wants to merge 1 commit into
mainfrom
cc/feat/manip-voxel-grid
Draft

feat(manipulation): add live voxel collision planning#3163
TomCC7 wants to merge 1 commit into
mainfrom
cc/feat/manip-voxel-grid

Conversation

@TomCC7

@TomCC7 TomCC7 commented Jul 24, 2026

Copy link
Copy Markdown
Member

Contribution path

Problem

The xArm voxel-planning demo did not have a responsive, atomic path from live point-cloud occupancy to collision checking. Scene mutation could block IK and collision queries, camera transforms could lag or be unavailable, and the same planning occupancy appeared twice in Viser.

Solution

  • Add a planning collision snapshot pipeline with latest-wins staging and a stable obstacle identity.
  • Align obstacle replacement with WorldSpec.update_obstacle and use copy-on-write RoboPlan scene swaps so readers see the old or new scene atomically.
  • Decouple IK, collision checks, and scene ingestion into independent workers.
  • Use Pink for interactive IK while retaining RoboPlan for planning and collision checking.
  • Add point-cloud self-filtering, a fixed-rate TF pose source, octree support, and the xArm Viser demo blueprint.
  • Show one live blue planning-collision visualization and suppress the duplicate reserved red octree visualization.
  • Document the launch/configuration path and the snapshot lifecycle decision.

How to Test

uv run dimos run xarm-voxel-planning-viser-demo

Focused automated verification:

PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python -m pytest -p pytest_mock -o addopts='' -q \
  dimos/manipulation/planning/monitor/test_planning_collision_snapshot.py \
  dimos/manipulation/planning/monitor/test_world_monitor.py \
  dimos/manipulation/planning/world/test_drake_world_planning_groups.py \
  dimos/manipulation/test_manipulation_unit.py \
  dimos/manipulation/test_roboplan_world.py \
  dimos/manipulation/visualization \
  dimos/perception/test_point_cloud_self_filter.py \
  dimos/robot/manipulators/xarm/blueprints/test_xarm_voxel_planning_viser_demo.py

Result: 214 passed.

The three direct TfPoseSource lifecycle tests require LCM multicast sockets, which are unavailable in the coding sandbox.

AI assistance

Codex with GPT-5 implemented, debugged, tested, and prepared this change under developer direction.

Checklist

  • I have read and approved the CLA.

@mintlify

mintlify Bot commented Jul 24, 2026

Copy link
Copy Markdown
Contributor

Preview deployment for your docs. Learn more about Mintlify Previews.

Project Status Preview Updated (UTC)
dimensional 🟢 Ready View Preview Jul 24, 2026, 5:44 AM

💡 Tip: Enable Workflows to automatically generate PRs for you.

@codecov

codecov Bot commented Jul 24, 2026

Copy link
Copy Markdown

❌ 8 Tests Failed:

Tests completed Failed Passed Skipped
3011 8 3003 176
View the top 3 failed test(s) by shortest run time
dimos.manipulation.test_pick_and_place_unit.TestFindObjectInDetections::test_empty_snapshot_returns_none
Stack Traces | 0.001s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbf5be0>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbf5be0>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbf5be0>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.manipulation.test_pick_and_place_unit.TestFindObjectInDetections::test_find_by_object_id_ambiguous_returns_none
Stack Traces | 0.001s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbf7f80>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbf7f80>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbf7f80>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.manipulation.test_pick_and_place_unit.TestFindObjectInDetections::test_find_by_partial_name
Stack Traces | 0.001s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18db9c2c0>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18db9c2c0>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18db9c2c0>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.manipulation.test_pick_and_place_unit.TestPlaceBack::test_place_back_no_pick_pose_errors
Stack Traces | 0.001s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa187df9040>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa187df9040>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa187df9040>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.manipulation.test_pick_and_place_unit.TestFindObjectInDetections::test_find_by_exact_name
Stack Traces | 0.002s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbe5f70>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbe5f70>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbe5f70>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.manipulation.test_pick_and_place_unit.TestFindObjectInDetections::test_find_by_object_id
Stack Traces | 0.002s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbbc5f0>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbbc5f0>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dbbc5f0>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.manipulation.test_pick_and_place_unit.TestFindObjectInDetections::test_find_missing_returns_none
Stack Traces | 0.002s run time
@pytest.fixture
    def module() -> PickAndPlaceModule:
        """Create a PickAndPlaceModule with heavy base init (RPC, config) patched out."""
        with patch.object(ModuleBase, "__init__", lambda self, config_args: None):
>           return PickAndPlaceModule()


dimos/manipulation/test_pick_and_place_unit.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/manipulation/pick_and_place_module.py:84: in __init__
    super().__init__(**kwargs)
        __class__  = <class 'dimos.manipulation.pick_and_place_module.PickAndPlaceModule'>
        kwargs     = {}
        self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dc04b60>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dc04b60>
kwargs = {}

    def __init__(self, **kwargs: Any) -> None:
        super().__init__(**kwargs)
    
        # State machine
        self._state = ManipulationState.IDLE
        self._lock = threading.Lock()
        # State transitions are local, but coordinator calls must be serialized
        # with cancellation.  In particular, do not let cancel race between
        # two arms of a multi-arm dispatch.
        self._execution_dispatch_lock = threading.Lock()
        self._execution_generation = 0
        self._possibly_active_tasks: set[str] = set()
        self._cancellation_in_progress = False
        self._error_message = ""
        self._planning_epoch = 0
    
        # Planning components (initialized in start())
        self._world_monitor: WorldMonitor | None = None
        self._planning_collision_snapshot = PlanningCollisionSnapshot(
>           resolution=float(getattr(self.config, "planning_voxel_resolution", 0.05)),
            planning_frame=str(getattr(self.config, "planning_world_frame", "world")),
        )
E       AttributeError: 'PickAndPlaceModule' object has no attribute 'config'

__class__  = <class 'dimos.manipulation.manipulation_module.ManipulationModule'>
kwargs     = {}
self       = <dimos.manipulation.pick_and_place_module.PickAndPlaceModule object at 0xffa18dc04b60>

dimos/manipulation/manipulation_module.py:178: AttributeError
dimos.codebase_checks.test_no_dunder_new::test_no_dunder_new
Stack Traces | 1.52s run time
def test_no_dunder_new() -> None:
        """Fail if any test file calls `__new__` to bypass `__init__`."""
        dimos_dir = DIMOS_PROJECT_ROOT / "dimos"
        hits = find_dunder_new_calls()
        if hits:
            listing = "\n".join(
                f"  - {p.relative_to(dimos_dir)}:{lineno}: {line.strip()}" for p, lineno, line in hits
            )
>           raise AssertionError(
                f"Found __new__ call(s) in test files:\n{listing}\n\n"
                "Tests must construct objects with the real constructor: __init__ is "
                "code under test too, and an object assembled by hand silently rots "
                "when the constructor changes. If __init__ does heavy work, mock the "
                "collaborators it needs instead of skipping it. Only if that is truly "
                "impossible, add the call to the WHITELIST in "
                "dimos/codebase_checks/test_no_dunder_new.py."
            )
E           AssertionError: Found __new__ call(s) in test files:
E             - .../planning/world/test_drake_world_planning_groups.py:47: world = object.__new__(DrakeWorld)
E             - .../planning/world/test_drake_world_planning_groups.py:187: DrakeWorld._create_shape(object.__new__(DrakeWorld), obstacle)
E             - perception/test_point_cloud_self_filter.py:30: module = object.__new__(PointCloudSelfFilter)
E           
E           Tests must construct objects with the real constructor: __init__ is code under test too, and an object assembled by hand silently rots when the constructor changes. If __init__ does heavy work, mock the collaborators it needs instead of skipping it. Only if that is truly impossible, add the call to the WHITELIST in dimos/codebase_checks/test_no_dunder_new.py.

dimos_dir  = PosixPath('.../dimos/dimos/dimos')
hits       = [(PosixPath('.../dimos/dimos/dimos/.../planning/world/test_drake_world_planning_groups.py'), 47....../dimos/perception/test_point_cloud_self_filter.py'), 30, '    module = object.__new__(PointCloudSelfFilter)')]
listing    = '  - .../planning/world/test_drake_world_planning_groups.py:47: world = object.__new__(DrakeWorld)\n  - manip...akeWorld), obstacle)\n  - perception/test_point_cloud_self_filter.py:30: module = object.__new__(PointCloudSelfFilter)'

dimos/codebase_checks/test_no_dunder_new.py:67: AssertionError

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

@mintlify

mintlify Bot commented Jul 24, 2026

Copy link
Copy Markdown
Contributor

Preview deployment for your docs. Learn more about Mintlify Previews.

Project Status Preview Updated (UTC)
dimensional 🟡 Building Jul 24, 2026, 5:43 AM

💡 Tip: Enable Workflows to automatically generate PRs for you.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant