Modular manipulation stack

[mustafab0]

Summary

• Refactored manipulation planning stack for clean backend-agnostic/backend-specific separation
• Protocol based architecture
• Planning stack is comprised of 3 pieces - World (Sim enginer), Kinematics (IK,FK solvers), Planners

[mustafab0]

@greptile

[greptile-apps]

Greptile Overview

Greptile Summary

Refactored manipulation planning stack into clean protocol-based architecture separating backend-agnostic interfaces from backend-specific implementations.

Architecture Changes:

• Introduced WorldSpec, KinematicsSpec, and PlannerSpec protocols for backend abstraction
• DrakeWorld implements WorldSpec using Drake's MultibodyPlant and SceneGraph
• JacobianIK and DrakeOptimizationIK implement KinematicsSpec for iterative and optimization-based IK
• RRTConnectPlanner implements PlannerSpec with backend-agnostic collision checking
• Factory functions provide clean instantiation API (create_world, create_kinematics, create_planner)

Key Components:

• WorldMonitor manages real-time robot state synchronization via WorldStateMonitor and obstacle updates via WorldObstacleMonitor
• Context management enables thread-safe planning with live context (mirrors real robot) and scratch contexts (for planning/IK)
• ManipulationModule integrates planning stack with ControlCoordinator for trajectory execution
• Comprehensive utilities for kinematics, path manipulation, and URDF/xacro processing

Previous Issues Addressed:

• Division-by-zero checks added to kinematics utilities
• Explicit tolerances for joint-space duplicate detection in path utils
• Exception handling narrowed to RuntimeError in Drake world operations
• Obstacle removal properly handles missing objects with logging

The refactor successfully achieves separation of concerns with protocols defining clear contracts, making it easier to add new backends (MuJoCo, PyBullet) without changing planner/IK implementations.

Confidence Score: 4/5

• This PR is safe to merge with minor attention to documentation
• Well-architected protocol-based refactor with comprehensive tests and previous review issues addressed. The separation of concerns is excellent, making the codebase more maintainable and extensible. Score is 4/5 rather than 5/5 because this is a large architectural change that adds significant new code (~7k+ lines), though the implementation quality is high and tests are thorough.
• All files are well-implemented. Pay attention to dimos/manipulation/planning/utils/mesh_utils.py which uses xacro monkey-patching (acknowledged as acceptable by senior developer)

Important Files Changed

Filename	Overview
dimos/manipulation/planning/spec/protocols.py	Introduces clean protocol-based architecture for manipulation stack with WorldSpec, KinematicsSpec, and PlannerSpec protocols
dimos/manipulation/planning/factory.py	Clean factory functions for creating planning stack components with clear backend selection
dimos/manipulation/planning/world/drake_world.py	Complete Drake-based WorldSpec implementation with robot/obstacle management and collision checking
dimos/manipulation/planning/kinematics/jacobian_ik.py	Backend-agnostic Jacobian IK with iterative and differential modes, division-by-zero checks addressed
dimos/manipulation/planning/planners/rrt_planner.py	Backend-agnostic RRT-Connect planner with comprehensive input validation and collision checking
dimos/manipulation/planning/monitor/world_monitor.py	Thread-safe WorldMonitor for synchronizing real robot state with planning world
dimos/manipulation/manipulation_module.py	Main manipulation module integrating planning stack with coordinator execution
dimos/manipulation/planning/utils/mesh_utils.py	URDF/xacro processing with mesh conversion and xacro monkey-patching for ROS-free usage

Sequence Diagram

sequenceDiagram
    participant Client as Client Code
    participant Factory as Planning Factory
    participant Module as ManipulationModule
    participant Monitor as WorldMonitor
    participant World as DrakeWorld
    participant IK as JacobianIK/DrakeOptimizationIK
    participant Planner as RRTConnectPlanner
    participant Coord as ControlCoordinator

    Note over Client,Coord: Initialization Phase
    Client->>Factory: create_planning_stack(robot_config)
    Factory->>World: create_world(backend="drake")
    Factory->>IK: create_kinematics(name="jacobian")
    Factory->>Planner: create_planner(name="rrt_connect")
    Factory->>World: add_robot(config)
    Factory->>World: finalize()
    Factory-->>Client: (world, kinematics, planner, robot_id)

    Note over Module,Coord: Module Startup
    Module->>Monitor: WorldMonitor(enable_viz=True)
    Monitor->>World: add_robot(config)
    World-->>Monitor: robot_id
    Module->>Monitor: finalize()
    Module->>Monitor: start_state_monitor(robot_id)
    Monitor->>Coord: subscribe to joint_state updates
    
    Note over Client,Coord: Runtime - State Synchronization
    Coord->>Monitor: joint_state updates
    Monitor->>World: sync_from_joint_state(robot_id, positions)
    World->>World: update live_context

    Note over Client,Coord: Runtime - IK Solve
    Client->>Module: solve_ik(target_pose)
    Module->>World: scratch_context()
    World-->>Module: scratch_ctx
    Module->>IK: solve(world, robot_id, target_pose, seed)
    loop Multiple Attempts
        IK->>World: get_positions(ctx, robot_id)
        IK->>World: get_jacobian(ctx, robot_id)
        IK->>World: get_ee_pose(ctx, robot_id)
        IK->>World: set_positions(ctx, robot_id, new_q)
        IK->>World: is_collision_free(ctx, robot_id)
    end
    IK-->>Module: IKResult(joint_positions)
    Module-->>Client: joint_positions

    Note over Client,Coord: Runtime - Motion Planning
    Client->>Module: plan_to_pose(target_pose)
    Module->>IK: solve(world, robot_id, target_pose)
    IK-->>Module: IKResult(q_goal)
    Module->>World: get_positions(live_ctx, robot_id)
    World-->>Module: q_start
    Module->>Planner: plan_joint_path(world, robot_id, q_start, q_goal)
    loop RRT-Connect Iterations
        Planner->>World: check_config_collision_free(robot_id, q)
        Planner->>World: check_edge_collision_free(robot_id, q1, q2)
    end
    Planner-->>Module: PlanningResult(path)
    Module->>Module: generate_trajectory(path)
    Module-->>Client: JointTrajectory

    Note over Client,Coord: Runtime - Execution
    Client->>Module: execute_trajectory(trajectory)
    Module->>Coord: execute_trajectory RPC
    Coord->>Coord: follow trajectory
    Coord-->>Monitor: joint_state updates
    Monitor->>World: sync_from_joint_state()

Loading

[greptile-apps]

_{33 files reviewed, 14 comments}

_{Edit Code Review Agent Settings | Greptile}

[leshy]

get_data("xarm_description/urdf/xarm_device.urdf.xacro") is now supported - returns full Path

[leshy]

I'm never sure if we should use robot_ids or actual instances/classes, it's more easy to not maintain ID -> something layer if you can use something directly

(world.robot1) ?

[mustafab0]

I believe the sim creates a robot object that it manages. perhaps best to use that instance itself. WIll look at this or mark it as an issue for future improvement.

[greptile-apps]

_{No files reviewed, no comments}

_{Edit Code Review Agent Settings | Greptile}

[leshy]

great

[leshy]

😍

[spomichter]

rlly great

[leshy]

stash loves enums

[spomichter]

Factory probably not the best approach here

[spomichter]

@leshy also agrees this is bad

[leshy]

I think all good for the first pass, mustafa will ask if he wants a brainstorm on worlds

[mustafab0]

I agree.

I will use the registry approach similar to the adapter registry