Feature: Detection3D and Object support to Manipulation Module

[mustafab0]

Problem

Detected 3D objects from perception cannot be used as collision obstacles for motion planning.
ManipulationModule lacks TF publishing needed for eye-in-hand camera transform chains.
Solution

ObjectSceneRegistration publishes deduplicated objects via new objects output port with pointclouds.
ManipulationModule subscribes async via observable() and caches objects in WorldObstacleMonitor.

Dedicated 10Hz TF thread publishes EE and tf_extra_links poses using scratch_context().
Eager self.tf init prevents autoconf() blocking in Dask workers.

WorldObstacleMonitor syncs BOX obstacles to Drake on refresh() with opt-in convex hull meshes.
pointcloud_to_convex_hull_obj centers points at origin to avoid double-transform with pose.

ManipulationClient adds refresh(), detections(), goto_object(), and perception status commands.
xarm_perception blueprint wires xArm7 + eye-in-hand RealSense + ObjectSceneRegistration + Foxglove.

Breaking Changes
None.
How to Test

pytest dimos/manipulation/test_manipulation_unit.py -v
dimos run xarm_perception
In client: refresh(5), goto_object("cup", dz=0.1), preview(), execute()

closes DIM-353
closes DIM-397
closes DIM-396

[greptile-apps]

Greptile Overview

Greptile Summary

This PR integrates 3D object detection from perception into the manipulation planning system, enabling collision-aware motion planning around detected objects.

Key additions:

• Object port subscription: ManipulationModule subscribes to deduplicated Object list from ObjectSceneRegistration using observable() for async updates
• Obstacle caching and sync: WorldObstacleMonitor.on_objects() caches objects, refresh_obstacles() performs batch sync to Drake with optional convex hull meshes computed from pointclouds
• TF publishing: Dedicated 10Hz thread publishes EE and tf_extra_links poses using scratch_context() to avoid blocking
• Perception RPC methods: Client commands refresh(), detections(), goto_object() enable interactive object manipulation workflows
• Mesh obstacle support: pointcloud_to_convex_hull_obj() centers points at origin to avoid double-transform; Drake uses Convex shape with proximity properties for collision detection
• Eye-in-hand blueprint: xarm_perception wires XArm7 + RealSense + ObjectSceneRegistration with correct TF chain (world → link7 → camera_link)

The implementation demonstrates good engineering practices: async subscription prevents blocking, convex hull computation happens outside locks, eager TF init avoids Dask worker deadlock, and centered mesh coordinates prevent transform bugs.

Confidence Score: 4/5

• This PR is safe to merge with good engineering practices throughout
• Score reflects solid implementation with proper async handling, thread safety, and good separation of concerns. Main complexity lies in Drake world integration which has lower confidence due to intricate geometry/collision setup
• Pay close attention to dimos/manipulation/planning/world/drake_world.py - ensure proximity properties are correctly set for post-finalization obstacles to enable collision detection

Important Files Changed

Filename	Overview
dimos/manipulation/manipulation_module.py	Adds objects input port subscription using observable() for async object updates from perception, TF publishing thread for EE and extra links at 10Hz, and perception RPC methods for obstacle management
dimos/manipulation/planning/monitor/world_obstacle_monitor.py	New file implementing object-based obstacle caching and Drake world synchronization with on_objects() for caching and refresh_obstacles() for batch sync with optional convex hull meshes
dimos/manipulation/planning/utils/mesh_utils.py	Adds pointcloud_to_convex_hull_obj() function that centers points at origin before computing convex hull to avoid double-transform when combined with obstacle pose
dimos/manipulation/planning/world/drake_world.py	Adds get_link_pose() for arbitrary link transforms, mesh obstacle support via Convex shape, and proximity properties for post-finalization obstacles to enable collision detection

Sequence Diagram

sequenceDiagram
    participant Perception as ObjectSceneRegistration
    participant ManipModule as ManipulationModule
    participant ObsMonitor as WorldObstacleMonitor
    participant Drake as DrakeWorld
    participant Client as ManipulationClient
    
    Note over Perception: Camera captures RGB-D
    Perception->>Perception: YOLO-E detection
    Perception->>Perception: ObjectDB deduplication
    Perception->>ManipModule: publish(objects) via port
    
    ManipModule->>ObsMonitor: on_objects(objects)
    ObsMonitor->>ObsMonitor: Cache objects with timestamps
    
    Note over Client: User calls refresh(min_duration)
    Client->>ManipModule: RPC: refresh_obstacles(5.0)
    ManipModule->>ObsMonitor: refresh_obstacles(5.0)
    
    ObsMonitor->>ObsMonitor: Filter objects by duration
    ObsMonitor->>ObsMonitor: Compute convex hulls (outside lock)
    ObsMonitor->>Drake: add_obstacle(mesh) for each
    Drake->>Drake: Register geometry with proximity props
    ObsMonitor-->>Client: List of added obstacles
    
    Client->>ManipModule: RPC: goto_object("cup", dz=0.1)
    ManipModule->>ManipModule: plan_to_pose(center + offset)
    ManipModule->>Drake: IK + motion planning (with obstacles)
    ManipModule-->>Client: Success
    
    Client->>ManipModule: RPC: preview()
    ManipModule->>Drake: animate_path() on preview robot
    
    Client->>ManipModule: RPC: execute()
    ManipModule->>ManipModule: Coordinator.task_invoke("traj_arm")
    
    loop 10Hz TF publishing
        ManipModule->>Drake: get_ee_pose() + get_link_pose("link7")
        ManipModule->>ManipModule: tf.publish(EE, link7)
    end

Loading

[greptile-apps]

_{12 files reviewed, no comments}

_{Edit Code Review Agent Settings | Greptile}