feat: roboplan integration

dimos/manipulation/manipulation_module.py

@@ -38,7 +38,13 @@
 from dimos.core.core import rpc
 from dimos.core.module import Module, ModuleConfig
 from dimos.core.stream import In
-from dimos.manipulation.planning.factory import create_planning_specs, create_world
+from dimos.manipulation.planning.factory import (
+    KinematicsName,
+    PlannerName,
+    WorldBackend,
+    create_planning_specs,
+    create_world,
+)
 from dimos.manipulation.planning.kinematics.config import (
     ManipulationKinematicsConfig,
     PinkKinematicsConfig,
@@ -105,13 +111,14 @@ class ManipulationModuleConfig(ModuleConfig):
 
     robots: list[RobotModelConfig] = Field(default_factory=list)
     planning_timeout: float = 10.0
+    world_backend: WorldBackend = "drake"
     visualization: ManipulationVisualizationConfig = Field(
         default_factory=NoManipulationVisualizationConfig
     )
-    planner_name: str = "rrt_connect"  # "rrt_connect"
+    planner_name: PlannerName = "rrt_connect"
     kinematics: ManipulationKinematicsConfig = Field(default_factory=PinkKinematicsConfig)
     # Deprecated: use kinematics.backend instead.
-    kinematics_name: str | None = None  # "jacobian", "drake_optimization", or "pink"
+    kinematics_name: KinematicsName | None = None
     # Floor plane Z height (meters). When set, a box obstacle is added at startup
     # to prevent the planner from routing trajectories below this height.
     # Set to None to disable.
@@ -186,9 +193,13 @@ def _initialize_planning(self) -> None:
             logger.warning("No robots configured, planning disabled")
             return
 
-        world = create_world(visualization=self.config.visualization)
+        world = create_world(
+            backend=self.config.world_backend,
+            visualization=self.config.visualization,
+        )
         planning_specs = create_planning_specs(
             world=world,
+            world_backend=self.config.world_backend,
             planner_name=self.config.planner_name,
             kinematics_name=self.config.kinematics_name,
             kinematics=self.config.kinematics,

dimos/manipulation/planning/factory.py

@@ -17,7 +17,7 @@
 from __future__ import annotations
 
 from dataclasses import dataclass
-from typing import TYPE_CHECKING, Any
+from typing import TYPE_CHECKING, Any, Literal, TypeAlias, get_args
 
 from dimos.manipulation.planning.kinematics.config import (
     DrakeOptimizationKinematicsConfig,
@@ -26,6 +26,7 @@
     PinkKinematicsConfig,
     kinematics_config_from_name,
 )
+from dimos.manipulation.planning.spec.protocols import PlannerSpec
 from dimos.manipulation.visualization.config import (
     ManipulationVisualizationConfig,
     NoManipulationVisualizationConfig,
@@ -35,7 +36,6 @@
     from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor
     from dimos.manipulation.planning.spec.protocols import (
         KinematicsSpec,
-        PlannerSpec,
         WorldSpec,
     )
 
@@ -49,23 +49,68 @@ class PlanningSpecs:
     planner: PlannerSpec
 
 
+WorldBackend: TypeAlias = Literal["drake", "roboplan"]
+PlannerName: TypeAlias = Literal["rrt_connect", "roboplan"]
+KinematicsName: TypeAlias = Literal["jacobian", "drake_optimization", "pink"]
+
+SUPPORTED_WORLD_BACKENDS = get_args(WorldBackend)
+SUPPORTED_PLANNERS = get_args(PlannerName)
+SUPPORTED_KINEMATICS = get_args(KinematicsName)
+
+_ROBOPLAN_PLANNER_REQUIRES_ROBOPLAN_WORLD = (
+    'planner_name="roboplan" requires world_backend="roboplan"'
+)
+
+DEFAULT_KINEMATICS_NAME: KinematicsName = "pink"
+
+
+def validate_backend_combination(
+    *,
+    world_backend: str = "drake",
+    planner_name: str = "rrt_connect",
+    kinematics_name: str = "jacobian",
+) -> None:
+    """Validate manipulation backend choices before constructing the stack."""
+    if world_backend not in SUPPORTED_WORLD_BACKENDS:
+        raise ValueError(
+            f"Unknown backend: {world_backend}. Available: {list(SUPPORTED_WORLD_BACKENDS)}"
+        )
+    if planner_name not in SUPPORTED_PLANNERS:
+        raise ValueError(f"Unknown planner: {planner_name}. Available: {list(SUPPORTED_PLANNERS)}")
+    if kinematics_name not in SUPPORTED_KINEMATICS:
+        raise ValueError(
+            f"Unknown kinematics solver: {kinematics_name}. Available: {list(SUPPORTED_KINEMATICS)}"
+        )
+
+    if planner_name == "roboplan" and world_backend != "roboplan":
+        raise ValueError(_ROBOPLAN_PLANNER_REQUIRES_ROBOPLAN_WORLD)
+    if kinematics_name == "drake_optimization" and world_backend != "drake":
+        raise ValueError('kinematics_name="drake_optimization" requires world_backend="drake"')
+
+
 def create_world(
     backend: str = "drake",
     visualization: ManipulationVisualizationConfig | None = None,
     **kwargs: Any,
 ) -> WorldSpec:
-    """Create a world instance. backend='drake' only for now."""
+    """Create a world instance for the selected planning backend."""
     visualization = visualization or NoManipulationVisualizationConfig()
+    enable_viz = visualization.requires_world_visualization
+
     if backend == "drake":
         from dimos.manipulation.planning.world.drake_world import DrakeWorld
 
-        return DrakeWorld(enable_viz=visualization.requires_world_visualization, **kwargs)
-    else:
-        raise ValueError(f"Unknown backend: {backend}. Available: ['drake']")
+        return DrakeWorld(enable_viz=enable_viz, **kwargs)
+    if backend == "roboplan":
+        from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld
+
+        return RoboPlanWorld(enable_viz=enable_viz, **kwargs)
+
+    raise ValueError(f"Unknown backend: {backend}. Available: {list(SUPPORTED_WORLD_BACKENDS)}")
 
 
 def create_kinematics(
-    name: str = "pink",
+    name: str = DEFAULT_KINEMATICS_NAME,
     config: ManipulationKinematicsConfig | None = None,
     **kwargs: Any,
 ) -> KinematicsSpec:
@@ -93,19 +138,32 @@ def create_kinematics(
 
 def create_planner(
     name: str = "rrt_connect",
+    world: WorldSpec | None = None,
+    world_backend: str | None = None,
     **kwargs: Any,
 ) -> PlannerSpec:
-    """Create motion planner. name='rrt_connect'."""
+    """Create motion planner. name='rrt_connect'|'roboplan'.
+
+    RoboPlan-native planning is scene/backend-coupled, so `name='roboplan'`
+    returns the RoboPlan world object itself as the planner.
+    """
     if name == "rrt_connect":
         from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner
 
         return RRTConnectPlanner(**kwargs)
-    else:
-        raise ValueError(f"Unknown planner: {name}. Available: ['rrt_connect']")
+    if name == "roboplan":
+        if world_backend != "roboplan" or world is None:
+            raise ValueError(_ROBOPLAN_PLANNER_REQUIRES_ROBOPLAN_WORLD)
+        if not isinstance(world, PlannerSpec):
+            raise ValueError("RoboPlan-native planner requires a RoboPlan world planner object")
+        return world
+
+    raise ValueError(f"Unknown planner: {name}. Available: {list(SUPPORTED_PLANNERS)}")
 
 
 def create_planning_specs(
     world: WorldSpec,
+    world_backend: str = "drake",
     planner_name: str = "rrt_connect",
     kinematics_name: str | None = None,
     kinematics: ManipulationKinematicsConfig | None = None,
@@ -115,25 +173,35 @@ def create_planning_specs(
 
     if kinematics_name is not None:
         kinematics = kinematics_config_from_name(kinematics_name)
+    if kinematics is None:
+        kinematics = kinematics_config_from_name(DEFAULT_KINEMATICS_NAME)
+
+    validate_backend_combination(
+        world_backend=world_backend,
+        planner_name=planner_name,
+        kinematics_name=kinematics.backend,
+    )
 
     return PlanningSpecs(
         world_monitor=WorldMonitor(world=world),
         kinematics=create_kinematics(config=kinematics),
-        planner=create_planner(name=planner_name),
+        planner=create_planner(name=planner_name, world=world, world_backend=world_backend),
     )
 
 
 def create_planning_stack(
     robot_config: Any,
+    world_backend: str = "drake",
     visualization: ManipulationVisualizationConfig | None = None,
     planner_name: str = "rrt_connect",
     kinematics_name: str | None = None,
     kinematics: ManipulationKinematicsConfig | None = None,
 ) -> tuple[WorldSpec, KinematicsSpec, PlannerSpec, str]:
     """Create complete planning stack. Returns (world, kinematics, planner, robot_id)."""
-    world = create_world(visualization=visualization)
+    world = create_world(backend=world_backend, visualization=visualization)
     planning_specs = create_planning_specs(
         world=world,
+        world_backend=world_backend,
         planner_name=planner_name,
         kinematics_name=kinematics_name,
         kinematics=kinematics,

dimos/manipulation/planning/kinematics/test_pink_ik.py

@@ -23,9 +23,11 @@
 
 import numpy as np
 import pytest
+from pytest_mock import MockerFixture
 
 from dimos.manipulation.planning.factory import create_kinematics
 from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig
+import dimos.manipulation.planning.kinematics.pink_ik as pink_ik
 from dimos.manipulation.planning.kinematics.pink_ik import (
     PinkIK,
     PinkIKConfig,
@@ -172,15 +174,11 @@ def _robot_config() -> RobotModelConfig:
     )
 
 
-class _TestPinkIK(PinkIK):
-    def __init__(self, converge: bool = True) -> None:
-        self.config = PinkIKConfig(max_iterations=3)
-        self._modules = _fake_modules(converge=converge)
-        self._robot_contexts = {}
-
-
-def _pink_ik(converge: bool = True) -> PinkIK:
-    return _TestPinkIK(converge=converge)
+def _pink_ik(mocker: MockerFixture, converge: bool = True) -> PinkIK:
+    mocker.patch.object(
+        pink_ik, "_load_optional_dependencies", return_value=_fake_modules(converge=converge)
+    )
+    return PinkIK(PinkIKConfig(max_iterations=3))
 
 
 def _context() -> _PinkRobotContext:
@@ -222,16 +220,14 @@ def check_config_collision_free(self, robot_id: str, joint_state: JointState) ->
 
 
 def test_create_kinematics_pink_missing_dependency_is_actionable(
-    monkeypatch: pytest.MonkeyPatch,
+    mocker: MockerFixture,
 ) -> None:
-    from dimos.manipulation.planning.kinematics import pink_ik
-
     def fake_import_module(name: str) -> ModuleType:
         if name == "pink":
             raise ImportError("missing pink")
         return ModuleType(name)
 
-    monkeypatch.setattr(pink_ik.importlib, "import_module", fake_import_module)
+    mocker.patch.object(pink_ik.importlib, "import_module", side_effect=fake_import_module)
 
     with pytest.raises(PinkIKDependencyError) as exc_info:
         create_kinematics("pink")
@@ -240,36 +236,30 @@ def fake_import_module(name: str) -> ModuleType:
 
 
 def test_create_kinematics_pink_unavailable_solver_mentions_manipulation_extra(
-    monkeypatch: pytest.MonkeyPatch,
+    mocker: MockerFixture,
 ) -> None:
-    from dimos.manipulation.planning.kinematics import pink_ik
-
     def fake_import_module(name: str) -> ModuleType:
         module = ModuleType(name)
         if name == "qpsolvers":
             module.available_solvers = []  # type: ignore[attr-defined]
         return module
 
-    monkeypatch.setattr(pink_ik.importlib, "import_module", fake_import_module)
+    mocker.patch.object(pink_ik.importlib, "import_module", side_effect=fake_import_module)
 
     with pytest.raises(PinkIKDependencyError, match="--extra manipulation"):
         create_kinematics("pink")
 
 
-def test_create_kinematics_pink_returns_backend(monkeypatch: pytest.MonkeyPatch) -> None:
-    from dimos.manipulation.planning.kinematics import pink_ik
-
-    monkeypatch.setattr(pink_ik, "_load_optional_dependencies", lambda solver: _fake_modules())
+def test_create_kinematics_pink_returns_backend(mocker: MockerFixture) -> None:
+    mocker.patch.object(pink_ik, "_load_optional_dependencies", return_value=_fake_modules())
 
     assert isinstance(create_kinematics("pink"), PinkIK)
 
 
 def test_create_kinematics_pink_config_passes_tuning(
-    monkeypatch: pytest.MonkeyPatch,
+    mocker: MockerFixture,
 ) -> None:
-    from dimos.manipulation.planning.kinematics import pink_ik
-
-    monkeypatch.setattr(pink_ik, "_load_optional_dependencies", lambda solver: _fake_modules())
+    mocker.patch.object(pink_ik, "_load_optional_dependencies", return_value=_fake_modules())
 
     ik = create_kinematics(config=PinkKinematicsConfig(max_iterations=7, dt=0.02, posture_cost=0.0))
 
@@ -279,10 +269,8 @@ def test_create_kinematics_pink_config_passes_tuning(
     assert ik.config.posture_cost == 0.0
 
 
-def test_pink_ik_config_overrides_are_applied(monkeypatch: pytest.MonkeyPatch) -> None:
-    from dimos.manipulation.planning.kinematics import pink_ik
-
-    monkeypatch.setattr(pink_ik, "_load_optional_dependencies", lambda solver: _fake_modules())
+def test_pink_ik_config_overrides_are_applied(mocker: MockerFixture) -> None:
+    mocker.patch.object(pink_ik, "_load_optional_dependencies", return_value=_fake_modules())
 
     ik = PinkIK(PinkIKConfig(solver="proxqp", dt=0.1), max_iterations=7, posture_cost=0.0)
 
@@ -310,8 +298,8 @@ def test_mapping_failure_for_missing_joint() -> None:
         _build_joint_mapping(_FakeModel(), config)
 
 
-def test_solve_single_returns_successful_ik_result() -> None:
-    ik = _pink_ik(converge=True)
+def test_solve_single_returns_successful_ik_result(mocker: MockerFixture) -> None:
+    ik = _pink_ik(mocker, converge=True)
     target = np.eye(4)
     target[:3, 3] = [0.1, 0.2, 0.3]
 
@@ -331,8 +319,8 @@ def test_solve_single_returns_successful_ik_result() -> None:
     assert result.joint_state.position == pytest.approx([0.2, 0.1, 0.3])
 
 
-def test_solve_single_reports_non_convergence() -> None:
-    ik = _pink_ik(converge=False)
+def test_solve_single_reports_non_convergence(mocker: MockerFixture) -> None:
+    ik = _pink_ik(mocker, converge=False)
     target = np.eye(4)
     target[:3, 3] = [0.1, 0.0, 0.0]
 
@@ -350,8 +338,8 @@ def test_solve_single_reports_non_convergence() -> None:
     assert "did not converge" in result.message
 
 
-def test_solve_rejects_collision_candidate() -> None:
-    ik = _pink_ik(converge=True)
+def test_solve_rejects_collision_candidate(mocker: MockerFixture) -> None:
+    ik = _pink_ik(mocker, converge=True)
     context = _context()
     ik._robot_contexts = {"robot": context}
 
@@ -370,8 +358,8 @@ def test_solve_rejects_collision_candidate() -> None:
     assert result.joint_state is None
 
 
-def test_solve_retries_after_joint_limit_failure(monkeypatch: pytest.MonkeyPatch) -> None:
-    ik = _pink_ik(converge=True)
+def test_solve_retries_after_joint_limit_failure(mocker: MockerFixture) -> None:
+    ik = _pink_ik(mocker, converge=True)
     context = _context()
     ik._robot_contexts = {"robot": context}
     calls = 0
@@ -396,7 +384,7 @@ def fake_solve_single(**_: object) -> IKResult:
             iterations=1,
         )
 
-    monkeypatch.setattr(ik, "_solve_single", fake_solve_single)
+    solve_single = mocker.patch.object(ik, "_solve_single", side_effect=fake_solve_single)
 
     result = ik.solve(
         world=cast("Any", _FakeWorld(collision_free=True)),
@@ -409,5 +397,5 @@ def fake_solve_single(**_: object) -> IKResult:
         max_attempts=2,
     )
 
-    assert calls == 2
+    assert solve_single.call_count == 2
     assert result.status == IKStatus.SUCCESS

dimos/manipulation/planning/monitor/test_world_monitor.py

@@ -196,7 +196,7 @@ def test_create_planning_specs_wraps_existing_world(monkeypatch) -> None:
         "create_kinematics",
         lambda *args, **kwargs: fake_kinematics,
     )
-    monkeypatch.setattr(planning_factory, "create_planner", lambda name: fake_planner)
+    monkeypatch.setattr(planning_factory, "create_planner", lambda **kwargs: fake_planner)
 
     planning_specs = planning_factory.create_planning_specs(world=fake_world)  # type: ignore[arg-type]