Modular manipulation stack #1079

mustafab0 · 2026-01-21T06:40:55Z

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 · 2026-01-21T06:41:13Z

@greptile

greptile-apps · 2026-01-21T06:47:43Z

Greptile Summary

Refactored manipulation planning stack to use protocol-based architecture with clean separation between backend-agnostic and backend-specific components
Implemented modular design comprised of three main components: World (simulation engines), Kinematics (IK/FK solvers), and Planners
Introduced comprehensive factory pattern and specification protocols to enable pluggable backends like Drake while maintaining consistent interfaces

Important Files Changed

Filename	Overview
`dimos/manipulation/manipulation_module.py`	New comprehensive manipulation module implementing state machine-based planning with multi-robot support and orchestrator integration
`dimos/manipulation/planning/world/drake_world.py`	New Drake-based world simulation providing physics, collision detection, and kinematics through protocol interface
`dimos/manipulation/planning/spec/protocols.py`	New protocol definitions establishing interfaces for World, Kinematics, and Planner components with thread-safe context management
`dimos/manipulation/planning/planners/rrt_planner.py`	New RRT-Connect planner implementation with potential infinite loop risk in tree connection algorithm
`dimos/manipulation/planning/kinematics/jacobian_ik.py`	New backend-agnostic Jacobian-based IK solver with adaptive damping and collision checking integration

Confidence score: 4/5

This PR represents a significant architectural improvement that should provide better modularity and maintainability for the manipulation planning system
Score reflects the comprehensive refactoring with good separation of concerns and protocol-based design, but reduced due to potential infinite loop in RRT planner and complexity of changes requiring thorough testing
Pay close attention to dimos/manipulation/planning/planners/rrt_planner.py for the infinite loop risk in the _connect_tree method and ensure proper testing of the new manipulation module state machine

Sequence Diagram

sequenceDiagram
    participant User
    participant ManipulationModule
    participant WorldMonitor
    participant DrakeWorld
    participant RRTPlanner
    participant JacobianIK
    participant Orchestrator
    
    User->>ManipulationModule: "plan_to_joints([0.1, 0.2, 0.3])"
    ManipulationModule->>WorldMonitor: "get_current_positions()"
    WorldMonitor->>DrakeWorld: "get_positions(ctx, robot_id)"
    DrakeWorld-->>WorldMonitor: "current_joints"
    WorldMonitor-->>ManipulationModule: "current_joints"
    
    ManipulationModule->>RRTPlanner: "plan_joint_path(world, robot_id, start, goal)"
    RRTPlanner->>DrakeWorld: "check_config_collision_free(robot_id, q)"
    DrakeWorld-->>RRTPlanner: "collision_free"
    RRTPlanner-->>ManipulationModule: "PlanningResult(path)"
    
    ManipulationModule->>ManipulationModule: "generate_trajectory(path)"
    ManipulationModule-->>User: "plan_success"
    
    User->>ManipulationModule: "execute()"
    ManipulationModule->>ManipulationModule: "translate_trajectory_to_orchestrator()"
    ManipulationModule->>Orchestrator: "execute_trajectory(task_name, trajectory)"
    Orchestrator-->>ManipulationModule: "execution_accepted"
    ManipulationModule-->>User: "execution_started"

greptile-apps

_{33 files reviewed, 14 comments}

_{Edit Code Review Agent Settings | Greptile}

dimos/manipulation/planning/__init__.py

dimos/manipulation/manipulation_blueprints.py

dimos/manipulation/planning/monitor/world_obstacle_monitor.py

dimos/manipulation/planning/examples/manipulation_client.py

dimos/manipulation/planning/utils/kinematics_utils.py

dimos/manipulation/planning/world/drake_world.py

dimos/manipulation/planning/utils/mesh_utils.py

…manipulation scenes

…h files

…cene and owns the scene graph

…ith real world. manages obstacle lifecycle and more

…ner etc specs

…anner and interfaces with other modules

… implementation, split kinematics into JacobianIK and DrakeOptimizationIK

manipulation_blueprint - Added optional add_gripper: bool = True to make xarm6 ans xarm7 config consistent. world_obstacle_monitor - Added warning log when obstacle not found during cleanup manipulation_client - Removed unused numpy impor path_utils - Added explicit tolerances atol=1e-6, rtol=0 to np.allclose() for stricter joint-space duplicate detection jacobian_ik - Added division-by-zero protection for velocity limits using nonzero_mask to skip zero-valued limits drake_world.py - added more specific exception handling (avoids hiding bugs) mesh_utils - regex fixes

leshy · 2026-01-24T04:04:47Z

dimos/manipulation/manipulation_blueprints.py

+# =============================================================================
+
+
+def _get_xarm_urdf_path() -> str:


ideally you can use Path objects for paths, they are super convinient, so no need for str conversion

leshy · 2026-01-24T04:05:12Z

dimos/manipulation/manipulation_blueprints.py

+
+def _get_xarm_urdf_path() -> str:
+    """Get path to xarm URDF."""
+    return str(get_data("xarm_description") / "urdf/xarm_device.urdf.xacro")


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

leshy · 2026-01-24T04:06:23Z

dimos/manipulation/manipulation_blueprints.py

+    return RobotModelConfig(
+        name=name,
+        urdf_path=_get_xarm_urdf_path(),
+        base_pose=np.array(


a bit suspicious, why not Pose

leshy · 2026-01-24T04:10:34Z

dimos/manipulation/planning/spec/types.py

+    Attributes:
+        name: Unique name for the obstacle
+        obstacle_type: Type of geometry (BOX, SPHERE, CYLINDER, MESH)
+        pose: 4x4 homogeneous transform


you have Pose :)

leshy · 2026-01-24T04:14:09Z

dimos/manipulation/planning/spec/protocols.py

+    def solve(
+        self,
+        world: WorldSpec,
+        robot_id: str,


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) ?

leshy · 2026-01-24T04:15:00Z

dimos/manipulation/planning/spec/protocols.py

+        self,
+        world: WorldSpec,
+        robot_id: str,
+        target_pose: NDArray[np.float64],


PoseStamped?

leshy · 2026-01-24T04:22:44Z

dimos/manipulation/planning/spec/protocols.py

+        q_start: NDArray[np.float64],
+        q_goal: NDArray[np.float64],
+        timeout: float = 10.0,
+    ) -> PlanningResult:


should this be a full joint space "path" so that it can be fed into a controller immediately?

because we might want to use planners that plan in full joint space for difficult situations

mustafab0 requested review from a team, paul-nechifor and spomichter January 21, 2026 06:41

greptile-apps bot reviewed Jan 21, 2026

View reviewed changes

mustafab0 force-pushed the modular_manipulation_stack branch 2 times, most recently from 2b839fb to 21a0403 Compare January 21, 2026 18:53

mustafab0 added 19 commits January 22, 2026 15:01

added robot_description folders to data/ folder with lfs tracking

8286042

Base types: RobotModelConfig, Obstacle, Protocol specs for defining …

75b37ed

…manipulation scenes

Mesh conversion utils for converting any urdf to drake compatible mes…

b0df221

…h files

DrakeWorld implementation that maintains all objects, robots in the s…

c13d44e

…cene and owns the scene graph

Monitor system keeps the world model (drake planning world) in synb w…

cfad118

…ith real world. manages obstacle lifecycle and more

FK and IK solver implementation with drake

7cac261

RRT path planner using default drake planner

f2a9e2a

factory for building implementation of the different world, viz, plan…

8fad8e1

…ner etc specs

utils for ik solver and path planning

84ff314

manipulation module manages the world monitor, planner, kinematics pl…

30aed98

…anner and interfaces with other modules

added blueprints and manipulation client for testing

a6c0ac5

added unit test, e2e tests and readme

33882c8

general cleanup

c897a6d

added multi robot management and control to the manipulation module

8bc2a29

refactored planning stack

6a39384

Refactored manipulation planning stack: seperated planners from world…

09095e5

… implementation, split kinematics into JacobianIK and DrakeOptimizationIK

updated README

39309e1

fix mypy import error with manipulation interface (file is deprecated)

8d2020a

mustafab0 force-pushed the modular_manipulation_stack branch from ec17f02 to 8d2020a Compare January 22, 2026 23:01

leshy reviewed Jan 24, 2026

View reviewed changes

		# =============================================================================


		def _get_xarm_urdf_path() -> str:

Modular manipulation stack #1079

Are you sure you want to change the base?

Modular manipulation stack #1079

Uh oh!

Conversation

mustafab0 commented Jan 21, 2026

Summary

Uh oh!

mustafab0 commented Jan 21, 2026

Uh oh!

greptile-apps bot commented Jan 21, 2026

Greptile Summary

Important Files Changed

Confidence score: 4/5

Sequence Diagram

Uh oh!

greptile-apps bot left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

leshy Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants